#include <boost_adaptbx/graph/graph_type.hpp>
#include <boost_adaptbx/graph/graph_export_adaptor.hpp>

#include <boost_adaptbx/exporting.hpp>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/tuple.hpp>
#include <boost/python/iterator.hpp>

#include <boost/graph/graph_traits.hpp>

#include <boost/mpl/transform.hpp>
#include <boost/mpl/string.hpp>

#include <boost/iterator/transform_iterator.hpp>
#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <boost/functional/hash.hpp>

#include <string>

namespace boost_adaptbx
{
namespace
{

template< typename Graph >
struct graph_export
{
  typedef boost::graph_traits< Graph > graph_traits;
  typedef typename graph_traits::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits::edge_descriptor edge_descriptor;
  typedef typename graph_traits::vertex_iterator vertex_iterator;
  typedef typename graph_traits::adjacency_iterator adjacency_iterator;
  typedef typename graph_traits::edge_iterator edge_iterator;
  typedef typename graph_traits::out_edge_iterator out_edge_iterator;
  typedef graph_export_adaptor::vertex_descriptor_converter< vertex_descriptor > converter;
  typedef boost::transform_iterator< converter, vertex_iterator > transformed_vertex_iterator;
  typedef boost::transform_iterator< converter, adjacency_iterator > transformed_adjacency_iterator;

  static
  typename converter::type
  add_vertex(Graph& graph, boost::python::object const& prop)
  {
    return converter::forward(
      boost::add_vertex( graph_type::vertex_property( prop ), graph )
      );
  }

  static
  boost::python::tuple
  add_edge(
    Graph& graph,
    typename converter::type u,
    typename converter::type v,
    boost::python::object const& prop
    )
  {
    std::pair< edge_descriptor, bool > res = boost::add_edge(
      converter::backward( u ),
      converter::backward( v ),
      graph_type::edge_property( prop ),
      graph
      );
    return boost::python::make_tuple( res.first, res.second );
  }

  static
  typename converter::type
  source_vertex(Graph const& graph, edge_descriptor edge)
  {
    return converter::forward( boost::source( edge, graph ) );
  }

  static
  typename converter::type
  target_vertex(Graph const& graph, edge_descriptor edge)
  {
    return converter::forward( boost::target( edge, graph ) );
  }

  static
  transformed_vertex_iterator
  vertex_iterator_begin(Graph const& graph)
  {
    return transformed_vertex_iterator(
      boost::vertices( graph ).first,
      converter()
      );
  }

  static
  transformed_vertex_iterator
  vertex_iterator_end(Graph const& graph)
  {
    return transformed_vertex_iterator(
      boost::vertices( graph ).second,
      converter()
      );
  }

  static
  transformed_adjacency_iterator
  adjacent_vertex_iterator_begin(Graph const& graph, typename converter::type vertex)
  {
    return transformed_adjacency_iterator(
      boost::adjacent_vertices( converter::backward( vertex ), graph ).first,
      converter()
      );
  }

  static
  transformed_adjacency_iterator
  adjacent_vertex_iterator_end(Graph const& graph, typename converter::type vertex)
  {
    return transformed_adjacency_iterator(
      boost::adjacent_vertices( converter::backward( vertex ), graph ).second,
      converter()
      );
  }

  static
  boost::python::object
  adjacent_vertices(Graph const& graph, typename converter::type vertex)
  {
    return boost::python::range< boost::python::default_call_policies, Graph const>(
      boost::bind( &graph_export< Graph >::adjacent_vertex_iterator_begin, _1, vertex ),
      boost::bind( &graph_export< Graph >::adjacent_vertex_iterator_end, _1, vertex )
      )( boost::cref( graph ) );
  }

  static
  boost::python::object
  vertex_label(Graph const& graph, typename converter::type vertex)
  {
    return boost::get( boost::vertex_name_t(), graph, converter::backward( vertex ) );
  }

  static
  void
  set_vertex_label(
    Graph& graph,
    typename converter::type vertex,
    boost::python::object label
    )
  {
    boost::put( boost::vertex_name_t(), graph, converter::backward( vertex ), label );
  }

  static
  boost::python::object
  edge_weight(Graph const& graph, edge_descriptor edge)
  {
    return boost::get( boost::edge_weight_t(), graph, edge );
  }

  static
  void
  set_edge_weight(
    Graph& graph,
    edge_descriptor edge,
    boost::python::object weight
    )
  {
    boost::put( boost::edge_weight_t(), graph, edge, weight );
  }

  static
  edge_iterator
  edge_iterator_begin(Graph const& graph)
  {
    return boost::edges( graph ).first;
  }

  static
  edge_iterator
  edge_iterator_end(Graph const& graph)
  {
    return boost::edges( graph ).second;
  }

  static
  out_edge_iterator
  out_edge_iterator_begin(Graph const& graph, typename converter::type vertex)
  {
    return boost::out_edges( converter::backward( vertex ), graph ).first;
  }

  static
  out_edge_iterator
  out_edge_iterator_end(Graph const& graph, typename converter::type vertex)
  {
    return boost::out_edges( converter::backward( vertex ), graph ).second;
  }

  static
  boost::python::object
  out_edges(Graph const& graph, typename converter::type vertex)
  {
    return boost::python::range< boost::python::default_call_policies, Graph const>(
      boost::bind( &graph_export< Graph >::out_edge_iterator_begin, _1, vertex ),
      boost::bind( &graph_export< Graph >::out_edge_iterator_end, _1, vertex )
      )( boost::cref( graph ) );
  }

  static
  void
  remove_vertex(Graph& graph, typename converter::type vertex)
  {
    vertex_descriptor vd = converter::backward( vertex );
    boost::clear_vertex( vd, graph );
    boost::remove_vertex( vd, graph );
  }

  static
  std::size_t
  num_vertices(Graph const& graph)
  {
    return boost::num_vertices( graph );
  }

  static
  std::size_t
  num_edges(Graph const& graph)
  {
    return boost::num_edges( graph );
  }

  static
  void
  remove_edge(Graph& graph, edge_descriptor edge)
  {
    boost::remove_edge( edge, graph );
  }

  static
  void
  process(std::string const& name)
  {
    using namespace boost::python;

    class_< Graph >( ( "graph_" + name ).c_str(), no_init )
      .def( init<>() )
      .def( "vertices", boost::python::range( vertex_iterator_begin, vertex_iterator_end ) )
      .def( "source", source_vertex, arg( "edge" ) )
      .def( "target", target_vertex, arg( "edge" ) )
      .def( "adjacent_vertices", adjacent_vertices, arg( "vertex" ) )
      .def( "edges", boost::python::range( edge_iterator_begin, edge_iterator_end ) )
      .def( "out_edges", out_edges, arg( "vertex" ) )
      .def( "vertex_label", vertex_label, arg( "vertex" ) )
      .def( "set_vertex_label", set_vertex_label, ( arg( "vertex" ), arg( "label" ) ) )
      .def( "edge_weight", edge_weight, arg( "edge" ) )
      .def( "set_edge_weight", set_edge_weight, ( arg( "edge" ), arg( "weight" ) ) )
      .def( "add_vertex", add_vertex, arg( "label" ) = object() )
      .def(
        "add_edge",
        &add_edge,
        ( arg( "vertex1" ), arg( "vertex2" ), arg( "weight" ) = object() )
        )
      .def( "remove_vertex", remove_vertex, arg( "vertex" ) )
      .def( "remove_edge", remove_edge, arg( "edge" ) )
      .def( "num_vertices", num_vertices )
      .def( "num_edges", num_edges )
      ;
  }
};

struct graph_exporter
{
  template< typename Export >
  void operator ()(boost::mpl::identity< Export > myexport) const
  {
    typedef typename Export::first graph_type;
    typedef typename Export::second name_type;

    graph_export< graph_type >::process(
      std::string( boost::mpl::c_str< name_type >::value )
      );
  }
};

struct get_graph_vertex_descriptor_type
{
  template< class Export >
  struct apply
  {
    typedef typename Export::first graph_type;
    typedef typename boost::graph_traits< graph_type >::vertex_descriptor type;
  };
};

struct get_graph_edge_descriptor_type
{
  template< class Export >
  struct apply
  {
    typedef typename Export::first graph_type;
    typedef typename boost::graph_traits< graph_type >::edge_descriptor type;
  };
};

} // namespace <anonymous>

namespace exporting
{

template< typename DirectedCatg, typename VertexDesc >
struct python_type_export_traits<
  boost::detail::edge_desc_impl< DirectedCatg, VertexDesc >
  >
{
  typedef boost::detail::edge_desc_impl< DirectedCatg, VertexDesc > edge_descriptor;

  static
  std::size_t edge_descriptor_hash_value(edge_descriptor const& edge)
  {
    return boost::hash_value( edge.get_property() );
  }

  static
  void process(boost::python::class_< edge_descriptor >& myclass)
  {
    myclass.def( "__hash__", edge_descriptor_hash_value );
  }
};

} // namespace exporting

} // namespace boost_adaptbx

BOOST_PYTHON_MODULE(boost_adaptbx_graph_ext)
{
  boost_adaptbx::exporting::class_list< boost_adaptbx::graph_type::exports >::process(
    boost_adaptbx::graph_exporter()
    );

  //
  // Dependent types
  //
  #pragma clang diagnostic push
  #pragma clang diagnostic ignored "-Wmultichar"

  // Vertex descriptors
  typedef boost::mpl::transform<
    boost_adaptbx::graph_type::exports,
    boost_adaptbx::get_graph_vertex_descriptor_type
    >::type graph_vertex_descriptor_types;
  typedef boost_adaptbx::exporting::unique_type_set<
    graph_vertex_descriptor_types
    >::type unique_graph_vertex_descriptor_types;
  typedef boost_adaptbx::exporting::novel_python_type<
     unique_graph_vertex_descriptor_types
     >::type novel_unique_graph_vertex_descriptor_types;

  boost_adaptbx::exporting::class_list< novel_unique_graph_vertex_descriptor_types >::process(
    boost_adaptbx::exporting::python_type_export< boost::mpl::string< 'vert', 'ex_' > >()
    );

  // Edge descriptors
  typedef boost::mpl::transform<
    boost_adaptbx::graph_type::exports,
    boost_adaptbx::get_graph_edge_descriptor_type
    >::type graph_edge_descriptor_types;
  typedef boost_adaptbx::exporting::unique_type_set<
    graph_edge_descriptor_types
    >::type unique_graph_edge_descriptor_types;

  boost_adaptbx::exporting::class_list< unique_graph_edge_descriptor_types >::process(
    boost_adaptbx::exporting::python_type_export<
      boost::mpl::string< 'edge', '_' >
      >().enable_equality_operators()
    );

  #pragma clang diagnostic pop
}


#include <boost/test/unit_test.hpp>
#include "test_utils.hpp"

#include <templ/solvers/temporal/TemporalConstraintNetwork.hpp>
#include <templ/solvers/temporal/QualitativeTemporalConstraintNetwork.hpp>
#include <templ/solvers/temporal/point_algebra/TimePointComparator.hpp>
#include <numeric/Combinatorics.hpp>
#include <graph_analysis/WeightedEdge.hpp>
#include <graph_analysis/VertexTypeManager.hpp>
#include <graph_analysis/EdgeTypeManager.hpp>
#include <graph_analysis/GraphIO.hpp>

using namespace templ::solvers;
using namespace templ::solvers::temporal;
using namespace graph_analysis;

BOOST_AUTO_TEST_SUITE(temporal_constraint_network)

BOOST_AUTO_TEST_CASE(ult)
{
// Input (a * near a vertex means that it is a target vertex):
// .-------------------------------------------------------
// .                                                      .
// v0 -------[10,20] [30,40]---> v1                       .
// .                          .     .                     .
// .                       .          .                   .
//[20,130]             [0,40]       [20,40]            [30,110]
//[140,170]        [60,90]            [60,70]             .
//[180,200]    [130,150]                  .               .
// .         .                              .             .
// .     .                                    .           .
// *  *                                         *         .
// v3 <---------------[0,50]--------------------v2 <-------
//
//
//Expected result:
// .-------------------------------------------------------
// .                                                      .
// v0 ------[10,20] [30,40]--> v1                         .
// .                          .  .                        .
// .                       .      .                       .
// .                [20,40]     [20,40]                [30,110]
//[30,130]       [60,90]          [60,70]                 .
// .            .                    .                    .
// .         .                        .                   .
// .     .                             .                  .
// * *                                  *                 .
// v3 <-----------[0,50]----------------v2 <---------------

    TemporalConstraintNetwork tcn,expected;

    point_algebra::TimePoint::Ptr v0(new point_algebra::TimePoint(0,std::numeric_limits<uint64_t>::max()));
    point_algebra::TimePoint::Ptr v1(new point_algebra::TimePoint(0,std::numeric_limits<uint64_t>::max()));
    point_algebra::TimePoint::Ptr v2(new point_algebra::TimePoint(0,std::numeric_limits<uint64_t>::max()));
    point_algebra::TimePoint::Ptr v3(new point_algebra::TimePoint(0,std::numeric_limits<uint64_t>::max()));

    IntervalConstraint::Ptr i0(new IntervalConstraint(v0,v1));
    i0->addInterval(Bounds(10,20));
    i0->addInterval(Bounds(30,40));
    IntervalConstraint::Ptr i1(new IntervalConstraint(v1,v2));
    i1->addInterval(Bounds(20,40));
    i1->addInterval(Bounds(60,70));
    IntervalConstraint::Ptr i2(new IntervalConstraint(v2,v3));
    i2->addInterval(Bounds(0,50));
    IntervalConstraint::Ptr i3(new IntervalConstraint(v1,v3));
    i3->addInterval(Bounds(0,40));
    i3->addInterval(Bounds(60,90));
    i3->addInterval(Bounds(130,150));
    IntervalConstraint::Ptr i4(new IntervalConstraint(v0,v3));
    i4->addInterval(Bounds(20,130));
    i4->addInterval(Bounds(140,170));
    i4->addInterval(Bounds(180,200));
    IntervalConstraint::Ptr i5(new IntervalConstraint(v0,v2));
    i5->addInterval(Bounds(30,110));

    tcn.addIntervalConstraint(i0);
    tcn.addIntervalConstraint(i1);
    tcn.addIntervalConstraint(i2);
    tcn.addIntervalConstraint(i3);
    tcn.addIntervalConstraint(i4);
    tcn.addIntervalConstraint(i5);

    BOOST_REQUIRE_MESSAGE(tcn.getEdgeNumber() == 6,"Expected: 6, Actual: "<<tcn.getEdgeNumber());

    std::string dotFilename = "/tmp/templ-test-pre-ult.dot";
    graph_analysis::io::GraphIO::write(dotFilename, tcn.getDistanceGraph());
    BOOST_TEST_MESSAGE("Dot file written to: " << dotFilename);

    tcn.upperLowerTightening();
    BaseGraph::Ptr graph = tcn.getDistanceGraph();

    dotFilename = "/tmp/templ-test-post-ult.dot";
    graph_analysis::io::GraphIO::write(dotFilename, tcn.getDistanceGraph());
    BOOST_TEST_MESSAGE("Dot file written to: " << dotFilename);

    IntervalConstraint::Ptr e0(new IntervalConstraint(v0,v1));
    e0->addInterval(Bounds(10,20));
    e0->addInterval(Bounds(30,40));
    IntervalConstraint::Ptr e1(new IntervalConstraint(v1,v2));
    e1->addInterval(Bounds(20,40));
    e1->addInterval(Bounds(60,70));
    IntervalConstraint::Ptr e2(new IntervalConstraint(v2,v3));
    e2->addInterval(Bounds(0,50));
    IntervalConstraint::Ptr e3(new IntervalConstraint(v1,v3));
    e3->addInterval(Bounds(20,40));
    e3->addInterval(Bounds(60,90));
    IntervalConstraint::Ptr e4(new IntervalConstraint(v0,v3));
    e4->addInterval(Bounds(30,130));
    IntervalConstraint::Ptr e5(new IntervalConstraint(v0,v2));
    e5->addInterval(Bounds(30,110));

    expected.addIntervalConstraint(e0);
    expected.addIntervalConstraint(e1);
    expected.addIntervalConstraint(e2);
    expected.addIntervalConstraint(e3);
    expected.addIntervalConstraint(e4);
    expected.addIntervalConstraint(e5);

    dotFilename = "/tmp/templ-test-expected-ult.dot";
    graph_analysis::io::GraphIO::write(dotFilename, expected.getDistanceGraph());
    BOOST_TEST_MESSAGE("Dot file written to: " << dotFilename);

//  check if the expected result and the actual result are the same
    bool ok = expected.equals(graph);
    BOOST_REQUIRE_MESSAGE(ok, "Expected graph equality -- but graphs are different");

}

BOOST_AUTO_TEST_CASE(io)
{
    using namespace templ::solvers::temporal;
    using namespace graph_analysis;

    QualitativeTemporalConstraintNetwork::Ptr tpc(new QualitativeTemporalConstraintNetwork());

    Vertex::Ptr t0 = VertexTypeManager::getInstance()->createVertex("QualitativeTimePoint", "t0");
    Vertex::Ptr t1 = VertexTypeManager::getInstance()->createVertex("QualitativeTimePoint", "t1");
    BOOST_REQUIRE_MESSAGE(t0->getClassName() == "QualitativeTimePoint", "QualitativeTimePoint is instanciated for t0");
    BOOST_REQUIRE_MESSAGE(t1->getClassName() == "QualitativeTimePoint", "QualitativeTimePoint is instanciated for t1");

    Edge::Ptr e0 = EdgeTypeManager::getInstance()->createEdge("QualitativeTimePointConstraint", t0,t1,"<");
    BOOST_REQUIRE_MESSAGE(e0->getClassName() == "QualitativeTimePointConstraint", "QualitativeTimePointConstraint is instanciated for e0");
}

BOOST_AUTO_TEST_CASE(io_non_overlapping_intervals)
{
    std::string filename = getRootDir() + "/test/data/temporal_constraint_networks/sequential-non-overlapping-intervals.gexf";

    using namespace templ::solvers::temporal;
    using namespace graph_analysis;

    QualitativeTemporalConstraintNetwork::Ptr tpc(new QualitativeTemporalConstraintNetwork());

    std::string dotFilename = "/tmp/test-sequential-non-overlapping-intervals.dot";
    graph_analysis::io::GraphIO::read(filename, tpc->getGraph());
    graph_analysis::io::GraphIO::write(dotFilename, tpc->getGraph());
    BOOST_TEST_MESSAGE("Dot file written to: " << dotFilename);

    std::map<std::string, point_algebra::TimePoint::Ptr> timepoints;

    VertexIterator::Ptr vertexIt = tpc->getGraph()->getVertexIterator();
    while(vertexIt->next())
    {
        point_algebra::TimePoint::Ptr tp = dynamic_pointer_cast<point_algebra::TimePoint>(vertexIt->current());
        BOOST_REQUIRE_MESSAGE(tp, "Timepoint: " << tp->toString() << " found");
        timepoints[tp->getLabel()] = tp;
    }

    point_algebra::TimePointComparator comparator(tpc);
    BOOST_REQUIRE_MESSAGE( comparator.greaterThan(timepoints["t9"], timepoints["t0"]), "t9 > t0");
    BOOST_REQUIRE_MESSAGE( !comparator.greaterThan(timepoints["t6"], timepoints["t9"]), "not t6 > t9");
    BOOST_REQUIRE_MESSAGE( !comparator.greaterThan(timepoints["t0"], timepoints["t9"]), "not t0 > t9");
    BOOST_REQUIRE_MESSAGE( !comparator.lessThan(timepoints["t9"], timepoints["t0"]), "not t9 < t0");
}

BOOST_AUTO_TEST_CASE(comparator_validation)
{
    using namespace templ::solvers::temporal;
    using namespace templ::solvers::temporal::point_algebra;

    using namespace graph_analysis;

    QualitativeTemporalConstraintNetwork::Ptr tpc(new QualitativeTemporalConstraintNetwork());
    typedef QualitativeTimePointConstraint QTPC;

    QualitativeTimePoint::Ptr t0 = make_shared<QualitativeTimePoint>("t0");
    QualitativeTimePoint::Ptr t1 = make_shared<QualitativeTimePoint>("t1");
    QualitativeTimePoint::Ptr t2 = make_shared<QualitativeTimePoint>("t2");
    QualitativeTimePoint::Ptr t3 = make_shared<QualitativeTimePoint>("t3");
    QualitativeTimePoint::Ptr t4 = make_shared<QualitativeTimePoint>("t4");
    QualitativeTimePoint::Ptr t5 = make_shared<QualitativeTimePoint>("t5");
    QualitativeTimePoint::Ptr t6 = make_shared<QualitativeTimePoint>("t6");
    QualitativeTimePoint::Ptr t7 = make_shared<QualitativeTimePoint>("t7");
    QualitativeTimePoint::Ptr t8 = make_shared<QualitativeTimePoint>("t8");

    TimePoint::PtrList timepoints = { t0, t1, t2, t3, t4, t5, t6, t7, t8 };

    tpc->addQualitativeConstraint(t1, t0, QTPC::Greater);
    tpc->addQualitativeConstraint(t2, t1, QTPC::Greater);
    tpc->addQualitativeConstraint(t3, t2, QTPC::Greater);
    tpc->addQualitativeConstraint(t4, t3, QTPC::Greater);
    tpc->addQualitativeConstraint(t5, t4, QTPC::Greater);
    tpc->addQualitativeConstraint(t6, t5, QTPC::Greater);
    tpc->addQualitativeConstraint(t7, t6, QTPC::Greater);
    tpc->addQualitativeConstraint(t8, t7, QTPC::Greater);

    point_algebra::TimePointComparator comparator(tpc);
    for(size_t i = 0; i < timepoints.size() -1; ++i)
    {
        TimePoint::Ptr ti = timepoints[i];
        for(size_t j = i+1; j < timepoints.size(); ++j)
        {
            TimePoint::Ptr tj = timepoints[j];

            BOOST_REQUIRE_MESSAGE(comparator.lessThan(ti, tj), "Expected " << ti->toString()
                    << " lessThan " << tj->toString());
            BOOST_REQUIRE_MESSAGE(!comparator.greaterThan(ti, tj), "Expected " << ti->toString()
                    << " not greaterThan " << tj->toString());
        }
    }

}

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_COMPUTATIONAL_TRSYL_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_COMPUTATIONAL_TRSYL_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/is_column_major.hpp>
#include <boost/numeric/bindings/is_complex.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/is_real.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/utility/enable_if.hpp>

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

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

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

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * float value-type.
//
inline std::ptrdiff_t trsyl( const char trana, const char tranb,
        const fortran_int_t isgn, const fortran_int_t m,
        const fortran_int_t n, const float* a, const fortran_int_t lda,
        const float* b, const fortran_int_t ldb, float* c,
        const fortran_int_t ldc, float& scale ) {
    fortran_int_t info(0);
    LAPACK_STRSYL( &trana, &tranb, &isgn, &m, &n, a, &lda, b, &ldb, c, &ldc,
            &scale, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * double value-type.
//
inline std::ptrdiff_t trsyl( const char trana, const char tranb,
        const fortran_int_t isgn, const fortran_int_t m,
        const fortran_int_t n, const double* a, const fortran_int_t lda,
        const double* b, const fortran_int_t ldb, double* c,
        const fortran_int_t ldc, double& scale ) {
    fortran_int_t info(0);
    LAPACK_DTRSYL( &trana, &tranb, &isgn, &m, &n, a, &lda, b, &ldb, c, &ldc,
            &scale, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<float> value-type.
//
inline std::ptrdiff_t trsyl( const char trana, const char tranb,
        const fortran_int_t isgn, const fortran_int_t m,
        const fortran_int_t n, const std::complex<float>* a,
        const fortran_int_t lda, const std::complex<float>* b,
        const fortran_int_t ldb, std::complex<float>* c,
        const fortran_int_t ldc, float& scale ) {
    fortran_int_t info(0);
    LAPACK_CTRSYL( &trana, &tranb, &isgn, &m, &n, a, &lda, b, &ldb, c, &ldc,
            &scale, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<double> value-type.
//
inline std::ptrdiff_t trsyl( const char trana, const char tranb,
        const fortran_int_t isgn, const fortran_int_t m,
        const fortran_int_t n, const std::complex<double>* a,
        const fortran_int_t lda, const std::complex<double>* b,
        const fortran_int_t ldb, std::complex<double>* c,
        const fortran_int_t ldc, double& scale ) {
    fortran_int_t info(0);
    LAPACK_ZTRSYL( &trana, &tranb, &isgn, &m, &n, a, &lda, b, &ldb, c, &ldc,
            &scale, &info );
    return info;
}

} // namespace detail

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

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

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

    //
    // Static member function, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixA, typename MatrixB, typename MatrixC >
    static std::ptrdiff_t invoke( const char trana, const char tranb,
            const fortran_int_t isgn, const fortran_int_t m,
            const fortran_int_t n, const MatrixA& a, const MatrixB& b,
            MatrixC& c, real_type& scale ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixB >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixC >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixB >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixC >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixC >::value) );
        BOOST_ASSERT( bindings::size_minor(a) == 1 ||
                bindings::stride_minor(a) == 1 );
        BOOST_ASSERT( bindings::size_minor(b) == 1 ||
                bindings::stride_minor(b) == 1 );
        BOOST_ASSERT( bindings::size_minor(c) == 1 ||
                bindings::stride_minor(c) == 1 );
        BOOST_ASSERT( bindings::stride_major(a) >= std::max< std::ptrdiff_t >(1,
                m) );
        BOOST_ASSERT( bindings::stride_major(b) >= std::max< std::ptrdiff_t >(1,
                n) );
        BOOST_ASSERT( bindings::stride_major(c) >= std::max< std::ptrdiff_t >(1,
                m) );
        BOOST_ASSERT( m >= 0 );
        BOOST_ASSERT( n >= 0 );
        BOOST_ASSERT( trana == 'N' || trana == 'T' || trana == 'C' );
        BOOST_ASSERT( tranb == 'N' || tranb == 'T' || tranb == 'C' );
        return detail::trsyl( trana, tranb, isgn, m, n,
                bindings::begin_value(a), bindings::stride_major(a),
                bindings::begin_value(b), bindings::stride_major(b),
                bindings::begin_value(c), bindings::stride_major(c), scale );
    }

};

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

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

    //
    // Static member function, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixA, typename MatrixB, typename MatrixC >
    static std::ptrdiff_t invoke( const char trana, const char tranb,
            const fortran_int_t isgn, const fortran_int_t m,
            const fortran_int_t n, const MatrixA& a, const MatrixB& b,
            MatrixC& c, real_type& scale ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixA >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixB >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixC >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixB >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixC >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixC >::value) );
        BOOST_ASSERT( bindings::size_minor(a) == 1 ||
                bindings::stride_minor(a) == 1 );
        BOOST_ASSERT( bindings::size_minor(b) == 1 ||
                bindings::stride_minor(b) == 1 );
        BOOST_ASSERT( bindings::size_minor(c) == 1 ||
                bindings::stride_minor(c) == 1 );
        BOOST_ASSERT( bindings::stride_major(a) >= std::max< std::ptrdiff_t >(1,
                m) );
        BOOST_ASSERT( bindings::stride_major(b) >= std::max< std::ptrdiff_t >(1,
                n) );
        BOOST_ASSERT( bindings::stride_major(c) >= std::max< std::ptrdiff_t >(1,
                m) );
        BOOST_ASSERT( m >= 0 );
        BOOST_ASSERT( n >= 0 );
        BOOST_ASSERT( trana == 'N' || trana == 'C' );
        BOOST_ASSERT( tranb == 'N' || tranb == 'C' );
        return detail::trsyl( trana, tranb, isgn, m, n,
                bindings::begin_value(a), bindings::stride_major(a),
                bindings::begin_value(b), bindings::stride_major(b),
                bindings::begin_value(c), bindings::stride_major(c), scale );
    }

};

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

//
// Overloaded function for trsyl. Its overload differs for
//
template< typename MatrixA, typename MatrixB, typename MatrixC >
inline std::ptrdiff_t trsyl( const char trana, const char tranb,
        const fortran_int_t isgn, const fortran_int_t m,
        const fortran_int_t n, const MatrixA& a, const MatrixB& b,
        MatrixC& c, typename remove_imaginary< typename bindings::value_type<
        MatrixA >::type >::type& scale ) {
    return trsyl_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( trana, tranb, isgn, m, n, a, b, c,
            scale );
}

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

#endif


/*ckwg +29
 * Copyright 2013-2015 by Kitware, Inc.
 * 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 name of Kitware, Inc. nor the names of any 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 AUTHORS 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.
 */

/**
 * \file
 * \brief test core rotation class
 */

#include <test_common.h>

#include <iostream>
#include <vector>

#include <maptk/rotation.h>

#include <boost/foreach.hpp>
#include <boost/math/constants/constants.hpp>

#define TEST_ARGS ()

DECLARE_TEST_MAP();

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

  testname_t const testname = argv[1];

  RUN_TEST(testname);
}

IMPLEMENT_TEST(default_constructor)
{
  maptk::rotation_d rot;
  Eigen::Quaterniond I;
  I.setIdentity();
  if (rot.quaternion().coeffs() != I.coeffs())
  {
    TEST_ERROR("The default rotation is not the identity");
  }
}

IMPLEMENT_TEST(convert_matrix)
{
  maptk::rotation_d rot;
  maptk::matrix_3x3d R = maptk::matrix_3x3d(rot);
  maptk::matrix_3x3d I = maptk::matrix_3x3d().setIdentity();
  std::cout << R << I << std::endl;

  if (R != I)
  {
    TEST_ERROR("Default rotation is not identity matrix");
  }

}

IMPLEMENT_TEST(convert_rodrigues)
{
  using namespace maptk;

  rotation_d rot;
  TEST_EQUAL("Rodrigues vector of identity is zero",
              rot.rodrigues().norm(), 0.0);

  rotation_d rot2(vector_3d(0,0,0));
  TEST_EQUAL("Rotation from zero Rodrigues vector is identity",
              rotation_d(vector_3d(0,0,0)),
              rotation_d());

  vector_3d rvec(2,-1,0.5);
  rotation_d rot3(rvec);
  using namespace std;
  cerr << "rvec magnitude: " << rvec.norm() << endl;
  cerr << "rot3 magnitude: " << rot3.rodrigues().norm() << endl;
  TEST_NEAR("Rodrigues convert to and from rotation is same",
            (rot3.rodrigues() - rvec).norm(), 0.0, 1e-14);

  TEST_NEAR("Rodrigues direction == rotation axis",
            (rvec.normalized() - rot3.axis()).norm(), 0.0, 1e-14);
}

IMPLEMENT_TEST(convert_axis_angle)
{
  using namespace maptk;

  double angle = 0.8;
  vector_3d axis = vector_3d(-3,2,1).normalized();

  rotation_d rot(angle, axis);

  TEST_NEAR("Angle extracted from rotation matches input",
            rot.angle(), angle, 1e-14);

  TEST_NEAR("Axis extracted from rotation matches input",
            (rot.axis() - axis).norm(), 0.0, 1e-14);
}

IMPLEMENT_TEST(convert_yaw_pitch_roll)
{
  using namespace maptk;

  const double yaw = 1.2;
  const double pitch = 0.3;
  const double roll = -1.0;
  double y,p,r;

  {
    rotation_d rot(yaw, pitch, roll);
    std::cout <<"(xxx) rotation mat:\n"<<matrix_3x3d(rot)<<std::endl;
    rot.get_yaw_pitch_roll(y, p, r);

    TEST_NEAR("(xxx) Yaw angle", y, yaw, 1e-14);
    TEST_NEAR("(xxx) Pitch angle", p, pitch, 1e-14);
    TEST_NEAR("(xxx) Roll angle", r, roll, 1e-14);
  }
  {
    rotation_d rot(0, pitch, roll);
    std::cout <<"(0xx) rotation mat:\n"<<matrix_3x3d(rot)<<std::endl;
    rot.get_yaw_pitch_roll(y, p, r);

    TEST_NEAR("(0xx) Yaw angle", y, 0, 1e-14);
    TEST_NEAR("(0xx) Pitch angle", p, pitch, 1e-14);
    TEST_NEAR("(0xx) Roll angle", r, roll, 1e-14);
  }
  {
    rotation_d rot(yaw, 0, roll);
    std::cout <<"(x0x) rotation mat:\n"<<matrix_3x3d(rot)<<std::endl;
    rot.get_yaw_pitch_roll(y, p, r);

    TEST_NEAR("(x0x) Yaw angle", y, yaw, 1e-14);
    TEST_NEAR("(x0x) Pitch angle", p, 0, 1e-14);
    TEST_NEAR("(x0x) Roll angle", r, roll, 1e-14);
  }
  {
    rotation_d rot(yaw, pitch, 0);
    std::cout <<"(xx0) rotation mat:\n"<<matrix_3x3d(rot)<<std::endl;
    rot.get_yaw_pitch_roll(y, p, r);

    TEST_NEAR("(xx0) Yaw angle", y, yaw, 1e-14);
    TEST_NEAR("(xx0) Pitch angle", p, pitch, 1e-14);
    TEST_NEAR("(xx0) Roll angle", r, 0, 1e-14);
  }
  {
    rotation_d rot(0, 0, roll);
    std::cout <<"(00x) rotation mat:\n"<<matrix_3x3d(rot)<<std::endl;
    rot.get_yaw_pitch_roll(y, p, r);

    TEST_NEAR("(00x) Yaw angle", y, 0, 1e-14);
    TEST_NEAR("(00x) Pitch angle", p, 0, 1e-14);
    TEST_NEAR("(00x) Roll angle", r, roll, 1e-14);
  }
  {
    rotation_d rot(0, pitch, 0);
    std::cout <<"(0x0) rotation mat:\n"<<matrix_3x3d(rot)<<std::endl;
    rot.get_yaw_pitch_roll(y, p, r);

    TEST_NEAR("(0x0) Yaw angle", y, 0, 1e-14);
    TEST_NEAR("(0x0) Pitch angle", p, pitch, 1e-14);
    TEST_NEAR("(0x0) Roll angle", r, 0, 1e-14);
  }
  {
    rotation_d rot(yaw, 0, 0);
    std::cout <<"(x00) rotation mat:\n"<<matrix_3x3d(rot)<<std::endl;
    rot.get_yaw_pitch_roll(y, p, r);

    TEST_NEAR("(x00) Yaw angle", y, yaw, 1e-14);
    TEST_NEAR("(x00) Pitch angle", p, 0, 1e-14);
    TEST_NEAR("(x00) Roll angle", r, 0, 1e-14);
  }
  {
    rotation_d rot(0, 0, 0);
    std::cout <<"(000) rotation mat:\n"<<matrix_3x3d(rot)<<std::endl;
    rot.get_yaw_pitch_roll(y, p, r);

    TEST_NEAR("(000) Yaw angle", y, 0, 1e-14);
    TEST_NEAR("(000) Pitch angle", p, 0, 1e-14);
    TEST_NEAR("(000) Roll angle", r, 0, 1e-14);
  }
}

IMPLEMENT_TEST(compose)
{
  using namespace maptk;

  rotation_d rot1(vector_3d(0.1, -1.5, 2.0));
  rotation_d rot2(vector_3d(-0.5, -0.5, 1.0));

  matrix_3x3d quat_comp_rot = rot1 * rot2;
  matrix_3x3d mat_comp_rot = matrix_3x3d(rot1) * matrix_3x3d(rot2);
  std::cout << "quaternion composition: "<<quat_comp_rot<<std::endl;
  std::cout << "matrix composition: "<<mat_comp_rot<<std::endl;

  TEST_NEAR("Matrix multiplication matches quaternion composition",
            (quat_comp_rot - mat_comp_rot).norm(),
            0.0, 1e-14);
}

IMPLEMENT_TEST(interpolation)
{
  using namespace maptk;

  double pi = boost::math::constants::pi<double>();
  rotation_d x(0, vector_3d(1, 0, 0)),
             y(pi / 2, vector_3d(0, 1, 0)),
             z;
  z = interpolate_rotation(x, y, 0.5);

  using namespace std;
  cerr << "x: " << x.axis() << " " << x.angle() << endl
       << "y: " << y.axis() << " " << y.angle() << endl
       << "z: " << z.axis() << " " << z.angle() << endl;

  TEST_NEAR("z-axis 0", z.axis()[0], 0, 1e-15);
  TEST_NEAR("z-axis 1", z.axis()[1], 1, 1e-15);
  TEST_NEAR("z-axis 2", z.axis()[2], 0, 1e-15);
  TEST_NEAR("z-angle",  z.angle(), pi / 4, 1e-15);
}

IMPLEMENT_TEST(multiple_interpolations)
{
  using namespace maptk;
  using namespace std;

  double pi = boost::math::constants::pi<double>();
  rotation_d x(0, vector_3d(1, 0, 0)),
             y(pi / 2, vector_3d(0, 1, 0));
  vector<rotation_d> rots;

  rots.push_back(x);
  interpolated_rotations(x, y, 3, rots);
  rots.push_back(y);

  cerr << "Vector size: " << rots.size() << endl;
  TEST_EQUAL("vector size", rots.size(), 5);
  BOOST_FOREACH(rotation_d rot, rots)
  {
    cerr << "\t" << rot.axis() << ' ' << rot.angle() << endl;
  }

  rotation_d i1 = rots[1],
             i2 = rots[2],
             i3 = rots[3];
  cerr << "i1 .25 : " << i1.axis() << ' ' << i1.angle() << endl;
  cerr << "i2 .50 : " << i2.axis() << ' ' << i2.angle() << endl;
  cerr << "i3 .75 : " << i3.axis() << ' ' << i3.angle() << endl;

  TEST_NEAR("i1 axis x", i1.axis().x(), 0, 1e-15);
  TEST_NEAR("i1 axis y", i1.axis().y(), 1, 1e-15);
  TEST_NEAR("i1 axis z", i1.axis().z(), 0, 1e-15);
  TEST_NEAR("i1 andgle", i1.angle(), pi / 8, 1e-15);

  TEST_NEAR("i2 axis x", i2.axis().x(), 0, 1e-15);
  TEST_NEAR("i2 axis y", i2.axis().y(), 1, 1e-15);
  TEST_NEAR("i2 axis z", i2.axis().z(), 0, 1e-15);
  TEST_NEAR("i2 andgle", i2.angle(), pi / 4, 1e-15);

  TEST_NEAR("i3 axis x", i3.axis().x(), 0, 1e-15);
  TEST_NEAR("i3 axis y", i3.axis().y(), 1, 1e-15);
  TEST_NEAR("i3 axis z", i3.axis().z(), 0, 1e-15);
  TEST_NEAR("i3 andgle", i3.angle(), (3*pi) / 8, 1e-15);
}


#include <lfant/util/lexical_cast.h>

// Internal
#include <lfant/util/String.h>
#include <lfant/Range.h>
#include <lfant/Console.h>
#include <lfant/Rect.h>

// External
#include <glm/gtx/quaternion.hpp>
#include <boost/lexical_cast.hpp>
#include <string>

using namespace std;

namespace lfant
{

/*
 *	Defaults
 */

template<typename Target, typename Source>
Target lexical_cast(const Source& src)
{
	return boost::lexical_cast<Target>(src);
}

template<>
string lexical_cast<string, string>(const string& src)
{
	return src;
}

template<>
string lexical_cast<string, const char*>(const char* const& src)
{
	return string(src);
}

template<>
string lexical_cast<string, void*>(void* const& src)
{
	char str[12];
	sprintf(str, "%p", src);
	return str;
}

/*
 *	Primitives from string
 */

template<>
byte lexical_cast<byte, string>(const string& src)
{
	return (byte)strtoul(src.c_str(), 0, 0);
}

template<>
short lexical_cast<short, string>(const string& src)
{
	return atoi(src.c_str());
}

template<>
unsigned short lexical_cast<unsigned short, string>(const string& src)
{
	return (unsigned short)strtoul(src.c_str(), 0, 0);
}

template<>
int lexical_cast<int, string>(const string& src)
{
	return atoi(src.c_str());
}

template<>
unsigned int lexical_cast<unsigned int, string>(const string& src)
{
	return (unsigned int)strtoul(src.c_str(), 0, 0);
}

template<>
long lexical_cast<long, string>(const string& src)
{
	return atol(src.c_str());
}

template<>
unsigned long lexical_cast<unsigned long, string>(const string& src)
{
	return strtoul(src.c_str(), 0, 0);
}

template<>
long long lexical_cast<long long, string>(const string& src)
{
	return atoll(src.c_str());
}

template<>
unsigned long long lexical_cast<unsigned long long, string>(const string& src)
{
	return strtoull(src.c_str(), 0, 0);
}

template<>
float lexical_cast<float, string>(const string& src)
{
	return strtof(src.c_str(), 0);
}

template<>
double lexical_cast<double, string>(const string& src)
{
	return atof(src.c_str());
}

template<>
bool lexical_cast<bool, string>(const string& src)
{
	if(src == "true")
	{
		return true;
	}
	else if(src == "false")
	{
		return false;
	}

	int i = lexical_cast<int>(src);
	if(i > 0)
	{
		return true;
	}
	return false;
}

/*
 *	Primitives _to_ string
 */

template<>
string lexical_cast<string, int>(const int& src)
{
	char str[12];
	sprintf(str, "%i", src);
	return str;
}

template<>
string lexical_cast<string, unsigned char>(const unsigned char& src)
{
	char str[4];
	sprintf(str, "%u", src);
	return str;
}

template<>
string lexical_cast<string, unsigned short>(const unsigned short& src)
{
	char str[6];
	sprintf(str, "%u", src);
	return str;
}

template<>
string lexical_cast<string, unsigned int>(const unsigned int& src)
{
	char str[12];
	sprintf(str, "%u", src);
	return str;
}

template<>
string lexical_cast<string, unsigned long>(const unsigned long& src)
{
	char str[20];
	sprintf(str, "%lu", src);
	return str;
}

template<>
string lexical_cast<string, unsigned long long>(const unsigned long long& src)
{
	char str[40];
	sprintf(str, "%llu", src);
	return str;
}
template<>
string lexical_cast<string, bool>(const bool& src)
{
	if(src)
	{
		return "true";
	}
	return "false";
}

string cut_zeroes(string orig)
{
	uint pos = 0;
	string str = orig;
	for(uint i = 0; i < str.size(); ++i)
	{
		if(str[str.size()-1-i] != '0')
		{
			pos = str.size()-i+1;
			break;
		}
	}
	if(pos < str.size())
	{
		str.erase(str.begin()+pos, str.end());
	}
	return str;
}

template<>
string lexical_cast<string, float>(const float& src)
{
	return cut_zeroes(std::to_string(src));
}

template<>
string lexical_cast<string, double>(const double& src)
{
	return cut_zeroes(std::to_string(src));
}

template<>
string lexical_cast<string, vec2>(const vec2& src)
{
	return "("+lexical_cast<string>(src.x)+","+lexical_cast<string>(src.y)+")";
}

template<>
string lexical_cast<string, ivec2>(const ivec2& src)
{
	return "("+lexical_cast<string>(src.x)+","+lexical_cast<string>(src.y)+")";
}

template<>
string lexical_cast<string, vec3>(const vec3& src)
{
	return "("+lexical_cast<string>(src.x)+","+lexical_cast<string>(src.y)+","+
			lexical_cast<string>(src.z)+")";
}

template<>
string lexical_cast<string, dvec3>(const dvec3& src)
{
	return "("+lexical_cast<string>(src.x)+","+lexical_cast<string>(src.y)+","+
			lexical_cast<string>(src.z)+")";
}

template<>
string lexical_cast<string, bvec3>(const bvec3& src)
{
	return "("+lexical_cast<string>(src.x)+","+lexical_cast<string>(src.y)+","+
			lexical_cast<string>(src.z)+")";
}

template<>
string lexical_cast<string, vec4>(const vec4& src)
{
	return "("+lexical_cast<string>(src.x)+","+lexical_cast<string>(src.y)+","+
			lexical_cast<string>(src.z)+","+lexical_cast<string>(src.w)+")";
}

template<>
string lexical_cast<string, u8vec4>(const u8vec4& src)
{
	return "("+lexical_cast<string>(src.r)+","+lexical_cast<string>(src.g)+","+
			lexical_cast<string>(src.b)+","+lexical_cast<string>(src.a)+")";
}

template<>
string lexical_cast<string, mat4>(const mat4& src)
{
	string result = "\n[\n";
	for(uint i=0; i<4; ++i)
	{
		//if(i != 0)
		{
			result.append(" ");
		}
		for(uint k=0; k<4; ++k)
		{
			result.append(lexical_cast<string>(src[k][i]));
			if(k != 3)
			{
				result.append(", ");
			}
		}
		if(i != 3)
		{
			result.append("\n");
		}
	}
	result.append("\n]");
	return result;
}

template<>
string lexical_cast<string, Range<int> >(const Range<int>& src)
{
	return lexical_cast<string>(src.min)+"-"+lexical_cast<string>(src.max);
}

template<>
string lexical_cast<string, Range<float> >(const Range<float>& src)
{
	return lexical_cast<string>(src.min)+"-"+lexical_cast<string>(src.max);
}

template<>
string lexical_cast<string, Range<Range<float> > >(const Range<Range<float> >& src)
{
	return "("+lexical_cast<string>(src.start)+")-("+lexical_cast<string>(src.end)+")";
}

template<>
string lexical_cast<string, Range<u8vec4> >(const Range<u8vec4>& src)
{
	return "("+lexical_cast<string>(src.min)+")-("+lexical_cast<string>(src.max)+")";
}

template<>
string lexical_cast<string, Range<vec3>>(const Range<vec3>& src)
{
	return "("+lexical_cast<string>(src.min)+")-("+lexical_cast<string>(src.max)+")";
}

template<>
string lexical_cast<string, Range<Range<vec3>>>(const Range<Range<vec3>>& src)
{
	return "("+lexical_cast<string>(src.start)+")-("+lexical_cast<string>(src.end)+")";
}

template<>
string lexical_cast<string, Range<vec4>>(const Range<vec4>& src)
{
	return "("+lexical_cast<string>(src.min)+")-("+lexical_cast<string>(src.max)+")";
}

template<>
string lexical_cast<string, Range<Range<vec4>>>(const Range<Range<vec4>>& src)
{
	return "("+lexical_cast<string>(src.start)+")-("+lexical_cast<string>(src.end)+")";
}

template<>
string lexical_cast<string, Rect>(const Rect& src)
{
	return "("+lexical_cast<string>(src.x)+","+
		lexical_cast<string>(src.y)+","+
		lexical_cast<string>(src.width)+","+
		lexical_cast<string>(src.height)+")";
}

/*
 *	Primitive to primitive
 */
template<>
unsigned short lexical_cast<unsigned short, char>(const char& src)
{
	return src;
}

/*
 *	GLM Types from string
 */

template<>
ivec2 lexical_cast<ivec2, string>(const string& val)
{
	ivec2 result(0);
	std::deque<string> str = lfant::Split(val, " x:,()");
	result.x = lexical_cast<int>(str[0]);
	if(str.size() > 1)
	{
		result.y = lexical_cast<int>(str[1]);
	}
	return result;
}

template<>
vec2 lexical_cast<vec2, string>(const string& val)
{
	vec2 result(0);
	std::deque<string> str = lfant::Split(val, " x:,()");
	result.x = lexical_cast<float>(str[0]);
	if(str.size() > 1)
	{
		result.y = lexical_cast<float>(str[1]);
	}
	return result;
}

template<>
ivec3 lexical_cast<ivec3, string>(const string& val)
{
	ivec3 result(0);
	std::deque<string> str = lfant::Split(val, " x:,()");
	result.x = lexical_cast<int>(str[0]);
	if(str.size() > 1)
	{
		result.y = lexical_cast<int>(str[1]);
		if(str.size() > 2)
		{
			result.z = lexical_cast<int>(str[2]);
		}
	}
	return result;
}

template<>
bvec3 lexical_cast<bvec3, string>(const string& val)
{
	bvec3 result(0);
	std::deque<string> str = lfant::Split(val, " x:,()");
	result.x = lexical_cast<bool>(str[0]);
	if(str.size() > 1)
	{
		result.y = lexical_cast<bool>(str[1]);
		if(str.size() > 2)
		{
			result.z = lexical_cast<bool>(str[2]);
		}
	}
	return result;
}

template<>
vec3 lexical_cast<vec3, string>(const string& val)
{
	vec3 result(0);
	string valstr = val;
/*	for(uint i = 0; i < valstr.size(); ++i)
	{
		if(valstr[i] == -7)
		{
			valstr.erase(i, 1);
		}
	}*/
	std::deque<string> str = lfant::Split(valstr, " x:,()");
	result.x = lexical_cast<float>(str[0]);
	if(str.size() > 1)
	{
		result.y = lexical_cast<float>(str[1]);
		if(str.size() > 2)
		{
			result.z = lexical_cast<float>(str[2]);
		}
	}
	else
	{
		result.y = result.x;
		result.z = result.x;
	}
	return result;
}

template<>
dvec3 lexical_cast<dvec3, string>(const string& val)
{
	dvec3 result(0);
	std::deque<string> str = lfant::Split(val, " x:,()");
	for(int i = 0; i < str.size(); ++i)
	{
		result[i] = lexical_cast<double>(str[i]);
	}
	return result;
}

template<>
u8vec4 lexical_cast<u8vec4, string>(const string& src)
{
	u8vec4 result(0);
	std::deque<string> str = lfant::Split(src, " x,()");
	result.r = lexical_cast<byte>(str[0]);
	if(str.size() > 1)
	{
		result.g = lexical_cast<byte>(str[1]);
		if(str.size() > 2)
		{
			result.b = lexical_cast<byte>(str[2]);
			if(str.size() > 3)
			{
				result.a = lexical_cast<byte>(str[3]);
			}
		}
	}
	return result;
}

template<>
vec4 lexical_cast<vec4, string>(const string& src)
{
	vec4 result(0);
	deque<string> str = lfant::Split(src, " x,()");
	result.r = lexical_cast<float>(str[0]);
	if(str.size() > 1)
	{
		result.g = lexical_cast<float>(str[1]);
		if(str.size() > 2)
		{
			result.b = lexical_cast<float>(str[2]);
			if(str.size() > 3)
			{
				result.a = lexical_cast<float>(str[3]);
			}
		}
	}
	return result;
}

/*
 *	Lfant Types from string
 */

template<>
Range<int> lexical_cast<Range<int>, string>(const string& src)
{
	Range<int> result(0);
	deque<string> str = lfant::Split(src, " .-()");
	result.min = lexical_cast<int>(str[0]);
	if(str.size() > 1)
	{
		result.max = lexical_cast<int>(str[1]);
	}
	return result;
}

template<>
Range<float> lexical_cast<Range<float>, string>(const string& src)
{
	Range<float> result(0);
	deque<string> str = lfant::Split(src, " ,-()");
	result.min = lexical_cast<float>(str[0]);
	if(str.size() > 1)
	{
		result.max = lexical_cast<float>(str[1]);
	}
	return result;
}

template<>
Range<vec3> lexical_cast<Range<vec3>, string>(const string &src)
{
	Range<vec3> result(vec3(0));
	deque<string> str = lfant::Split(src, " -");
	result.min = lexical_cast<vec3>(str[0]);
	if(str.size() > 1)
	{
		result.max = lexical_cast<vec3>(str[1]);
	}
	return result;
}

template<>
Range<vec4> lexical_cast<Range<vec4>, string>(const string &src)
{
	Range<vec4> result(vec4(0));
	deque<string> str = lfant::Split(src, " -");
	result.min = lexical_cast<vec4>(str[0]);
	if(str.size() > 1)
	{
		result.max = lexical_cast<vec4>(str[1]);
	}
	return result;
}

template<>
Range<Range<float>> lexical_cast<Range<Range<float>>, string>(const string& src)
{
	Range<Range<float>> result(0);
	deque<string> str = lfant::Split(src, " ,-()");
	result.min = lexical_cast<Range<float>>(str[0]+"-"+str[1]);
	if(str.size() > 2)
	{
		result.max = lexical_cast<Range<float>>(str[2]+"-"+str[3]);
	}
	return result;
}

template<>
Range<Range<vec4>> lexical_cast<Range<Range<vec4>>, string>(const string& src)
{
	Range<Range<vec4>> result;
	deque<string> str = lfant::Split(src, " -");
	result.min = lexical_cast<Range<vec4>>(str[0]+"-"+str[1]);
	if(str.size() > 2)
	{
		result.max = lexical_cast<Range<vec4>>(str[2]+"-"+str[3]);
	}
	return result;
}

template<>
Range<Range<vec3>> lexical_cast<Range<Range<vec3>>, string>(const string& src)
{
	Range<Range<vec3>> result;
	deque<string> str = lfant::Split(src, " -");
	result.min = lexical_cast<Range<vec3>>(str[0]+"-"+str[1]);
	if(str.size() > 2)
	{
		result.max = lexical_cast<Range<vec3>>(str[2]+"-"+str[3]);
	}
	return result;
}

/*
 *	String to Containers
 */

template<>
string lexical_cast<string, vector<string>>(const vector<string>& src)
{
	string result = "{";
	for(uint i = 0; i < src.size(); ++i)
	{
		result.append(src[i]);
		if(i != src.size()-1)
		{
			result.append(",");
		}
	}
	result.append("}");
	return result;
}

template<>
vector<string> lexical_cast<vector<string>, string>(const string& src)
{
	deque<string> toks = Split(src, "\n{}", ", \t");
	vector<string> result;
	bool indent = false;
	for(uint i = 0; i < toks.size(); ++i)
	{
		if(!indent && i == 0)
		{
			while(toks[i] == " " || toks[i] == "\t")
			{
				++i;
			}
			indent = true;
		}
		if(toks[i] == ",")
		{
			++i;
			while(toks[i] == " " || toks[i] == "\t")
			{
				++i;
			}
		}

		result.push_back(toks[i]);
	}
	return result;
}

static string TrimSpace(const string& str)
{
	std::size_t firstLetter = str.find_first_not_of(" \t\n");
	std::size_t lastLetter = str.find_last_not_of(" \t\n");

	if(firstLetter == string::npos)
	{
		return "";
	}
	else
	{
		return str.substr(firstLetter, lastLetter-firstLetter+1);
	}
}

template<>
deque<string> lexical_cast<deque<string>, string>(const string& src)
{
	deque<string> toks = Split(src, "\n{}[]", ", \t");
	deque<string> result;
	bool indent = false;
	for(uint i = 0; i < toks.size(); ++i)
	{
		if(!indent && i == 0)
		{
			while(toks[i] == " " || toks[i] == "\t")
			{
				++i;
			}
			indent = true;
		}
		if(toks[i] == ",")
		{
			++i;
			while(toks[i] == " " || toks[i] == "\t")
			{
				++i;
			}
		}

		if(!toks[i].empty())
		{
			toks[i] = TrimSpace(toks[i]);
		}

		if(!toks[i].empty()) result.push_back(toks[i]);
	}
	return result;
}

template<>
string lexical_cast<string, deque<string> >(const deque<string>& src)
{
	string result = "{ ";
	for(uint i = 0; i < src.size(); ++i)
	{
		result.append(src[i]);
		if(i+1 < src.size())
		{
			result.append(", ");
		}
	}
	result.append(" }");
	return result;
}

}


#pragma once

// #include "geometry.hpp"
// #include "segmentation.hpp"
// #include "typedefs.hpp"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "geometry_msgs/Pose.h"
#include "ros/ros.h"

namespace sub
{
// ******* 3D Visualization *******
class RvizVisualizer
{
public:
  ros::Publisher rviz_pub;
  ros::NodeHandle nh;
  RvizVisualizer(std::string rviz_topic);
  void visualize_buoy(geometry_msgs::Pose& pose, std::string& frame_id);
  void visualize_torpedo_board(geometry_msgs::Pose& pose, Eigen::Quaterniond orientation,
                               std::vector<Eigen::Vector3d>& targets, std::vector<Eigen::Vector3d>& corners3d,
                               std::string& frame_id);
};
}


/* vim: set tabstop=4 : */
#include "var_int.hpp"
#include <assert.h>
#include <stdexcept>

#if defined(_MSC_VER)
# include <intrin.h>
#pragma intrinsic(_BitScanReverse)
//#pragma intrinsic(_BitScanReverse64)
#endif

#include <boost/version.hpp>
#if BOOST_VERSION < 103301
# include <boost/limits.hpp>
# include <boost/detail/limits.hpp>
#else
# include <boost/predef/other/endian.h>
#endif

#ifdef _MSC_VER
/* Always compile this module for speed, not size */
#pragma optimize("t", on)
#endif

namespace terark {

#include "var_int_inline.hpp"

uint32_t load_var_uint32(const unsigned char* buf, const unsigned char** endp)
{
	return gg_load_var_uint<uint32_t>(buf, endp, BOOST_CURRENT_FUNCTION);
}

uint64_t load_var_uint64(const unsigned char* buf, const unsigned char** endp)
{
	return gg_load_var_uint<uint64_t>(buf, endp, BOOST_CURRENT_FUNCTION);
}

int32_t load_var_int32(const unsigned char* buf, const unsigned char** endp)
{
	uint32_t x = load_var_uint32(buf, endp);
	return var_int32_u2s(x);
}

int64_t load_var_int64(const unsigned char* buf, const unsigned char** endp)
{
	return var_int64_u2s(load_var_uint64(buf, endp));
}

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

unsigned char* save_var_uint32(unsigned char* buf, uint32_t x)
{
   	return gg_save_var_uint(buf, x);
}

unsigned char* save_var_uint64(unsigned char* p, uint64_t x)
{
#if 0 // save max to 9 bytes
	for (int bytes = 0; bytes < 8; ++bytes)
	{
		if (x & ~(uint64_t)0x7F) {
			*p++ = (unsigned char)((x & 0x7f) | 0x80);
			x >>= 7; //doing unsigned shift
		} else
			break;
	}
	*p++ = (unsigned char)x;
	return p;
#else // save max to 10 bytes
	return gg_save_var_uint(p, x);
#endif
}

unsigned char* save_var_int32(unsigned char* buf, int32_t x) { return save_var_uint32(buf, var_int32_s2u(x)); }
unsigned char* save_var_int64(unsigned char* buf, int64_t x) { return save_var_uint64(buf, var_int64_s2u(x)); }

//##########################################################################################

uint32_t load_var_uint30(const unsigned char* buf, const unsigned char** endp) { return gg_load_var_uint30(buf, endp); }
uint64_t load_var_uint61(const unsigned char* buf, const unsigned char** endp) { return gg_load_var_uint61(buf, endp); }

int32_t load_var_int30(const unsigned char* buf, const unsigned char** endp) { return var_int30_u2s(load_var_uint30(buf, endp)); }
int64_t load_var_int61(const unsigned char* buf, const unsigned char** endp) { return var_int61_u2s(load_var_uint61(buf, endp)); }

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

unsigned char* save_var_uint30(unsigned char* buf, uint32_t x) { return gg_save_var_uint30(buf, x); }
unsigned char* save_var_uint61(unsigned char* buf, uint64_t x) { return gg_save_var_uint61(buf, x); }

unsigned char* save_var_int30(unsigned char* buf, int32_t x) { return save_var_uint30(buf, var_int30_s2u(x)); }
unsigned char* save_var_int61(unsigned char* buf, int64_t x) { return save_var_uint61(buf, var_int61_s2u(x)); }

//##########################################################################################

/**
 @brief reverse get var_uint32_t

 @note
   - if first var_int has read, *cur == buf-1
   - the sequence must all stored var_int, if not, the dilimeter byte's high bit must be 0
 */
uint32_t reverse_get_var_uint32(const unsigned char* buf, unsigned char const ** cur)
{
	assert(cur);
	assert(*cur);
	assert(*cur >= buf);

	const unsigned char* p = *cur;
	uint32_t x = 0;
	uint32_t w = *p;
	assert(!(x & 0x80));
	int shift = 0;
	--p;
	while (p >= buf && *p & 0x80)
	{
		x = x << 7 | (uint32_t)(*p & 0x7F);
		shift += 7;
		--p;
	}
	x |= w << shift;
	*cur = p;

	return x;
}

/**
 @brief reverse get var_int32_t

 @note if first var_int has read, *cur == buf-1
 */
int32_t reverse_get_var_int32(const unsigned char* buf, unsigned char const ** cur)
{
	return var_int32_u2s(reverse_get_var_uint32(buf, cur));
}

#if !defined(BOOST_NO_INT64_T)
/**
 @brief reverse get var_uint64_t

 @note if first var_int has read, *cur == buf-1
 */
uint64_t reverse_get_var_uint64(const unsigned char* buf, unsigned char const ** cur)
{
	assert(cur);
	assert(*cur);
	assert(*cur >= buf);

	const unsigned char* p = *cur;
	uint64_t x = 0;
	uint64_t w = *p;
	int shift = 0;
	--p;
	while (p >= buf && shift < 56 && *p & 0x80)
	{
		x = x << 7 | (uint64_t)(*p & 0x7F);
		shift += 7;
		--p;
	}
	assert(shift <= 56);

	x |= w << shift;

	*cur = p; // p now point to last byte of prev var_int

	return x;
}

/**
 @brief reverse get var_int64_t

 @note if first var_int has read, *cur == buf-1
 */
int64_t reverse_get_var_int64(const unsigned char* buf, unsigned char const ** cur)
{
	return var_int64_u2s(reverse_get_var_uint64(buf, cur));
}

#endif //BOOST_NO_INT64_T

} // namespace terark


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate the constant true.

    @return The True constant for the proper type
  **/
  template<typename T> T True();
} }
#endif

#include <boost/simd/constant/scalar/true.hpp>
#include <boost/simd/constant/simd/true.hpp>

#endif


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  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 Willow Garage 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: Melonee Wise */

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <image_transport/image_transport.h>
#include <image_geometry/pinhole_camera_model.h>
#include <tf/transform_listener.h>
#include <opencv2/core/core.hpp>
#include <cv_bridge/cv_bridge.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include "turtlebot_actions/FindFiducialAction.h"
#include "turtlebot_actions/detect_calibration_pattern.h"

class FindFiducialAction
{
public:

  FindFiducialAction(std::string name) :
    as_(nh_, name, true),
    action_name_(name),
    it_(nh_)
  {
    //register the goal and feeback callbacks
    as_.registerGoalCallback(boost::bind(&FindFiducialAction::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&FindFiducialAction::preemptCB, this));

  }

  ~FindFiducialAction(void)
  {
  }

  void goalCB()
  {
    ROS_INFO("%s: Received new goal", action_name_.c_str());

    typedef boost::shared_ptr<const turtlebot_actions::FindFiducialGoal> GoalPtr;
    GoalPtr goal = as_.acceptNewGoal();

    cv::Size grid_size(goal->pattern_width,goal->pattern_height);
    detector_.setPattern(grid_size, goal->pattern_size, Pattern(goal->pattern_type));

    ros::Duration(1.0).sleep();
    //subscribe to the image topic of interest
    std::string image_topic = goal->camera_name + "/image_rect_mono";
    sub_ = it_.subscribeCamera(image_topic, 1, &FindFiducialAction::detectCB, this);

    pub_timer_ = nh_.createTimer(tf_listener_.getCacheLength() - ros::Duration(1.0), boost::bind(&FindFiducialAction::timeoutCB, this, _1),true);
  }

  void timeoutCB(const ros::TimerEvent& e)
  {
    if(sub_.getNumPublishers() == 0)
      ROS_INFO("%s: Aborted, there are no publishers on goal topic.", action_name_.c_str());
    else
      ROS_INFO("%s: Aborted, there are publishers on goal topic, but detection took too long.", action_name_.c_str());

    sub_.shutdown();
    as_.setAborted();
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // set the action state to preempted
    pub_timer_.stop();
    as_.setPreempted();
    sub_.shutdown();
  }

  void detectCB(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg)
  {
    // make sure that the action is active
    if (!as_.isActive())
      return;

    ROS_INFO("%s: Received image, performing detection", action_name_.c_str());
    // Convert image message

    try
    {
      img_bridge_ = cv_bridge::toCvCopy(image_msg, "mono8");
    }
    catch (cv_bridge::Exception& ex)
    {
      ROS_ERROR("[calibrate] Failed to convert image");
      return;
    }

    ROS_INFO("%s: created cv::Mat", action_name_.c_str());
    cam_model_.fromCameraInfo(info_msg);
    detector_.setCameraMatrices(cam_model_.intrinsicMatrix(), cam_model_.distortionCoeffs());

    Eigen::Vector3f translation;
    Eigen::Quaternionf orientation;

    if (detector_.detectPattern(img_bridge_->image, translation, orientation))
    {
      ROS_INFO("detected fiducial");
      tf::Transform fiducial_transform;
      fiducial_transform.setOrigin( tf::Vector3(translation.x(), translation.y(), translation.z()) );
      fiducial_transform.setRotation( tf::Quaternion(orientation.x(), orientation.y(), orientation.z(), orientation.w()) );

      tf::StampedTransform fiducial_transform_stamped(fiducial_transform, image_msg->header.stamp, cam_model_.tfFrame(), "fiducial_frame");
      tf_broadcaster_.sendTransform(fiducial_transform_stamped);

      tf::Pose fiducial_pose;
      fiducial_pose.setRotation(tf::Quaternion(0, 0, 0, 1.0));
      fiducial_pose = fiducial_transform * fiducial_pose;
      tf::Stamped<tf::Pose> fiducial_pose_stamped(fiducial_pose, image_msg->header.stamp, cam_model_.tfFrame());

      tf::poseStampedTFToMsg(fiducial_pose_stamped, result_.pose);
      as_.setSucceeded(result_);
      pub_timer_.stop();
      sub_.shutdown();
    }

  }

protected:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<turtlebot_actions::FindFiducialAction> as_;
  std::string action_name_;
  PatternDetector detector_;

  image_transport::ImageTransport it_;
  image_transport::CameraSubscriber sub_;
  cv_bridge::CvImagePtr img_bridge_;
  image_geometry::PinholeCameraModel cam_model_;

  tf::TransformListener tf_listener_;
  tf::TransformBroadcaster tf_broadcaster_;

  ros::Timer pub_timer_;

  // create messages that are used to published feedback/result
  turtlebot_actions::FindFiducialFeedback feedback_;
  turtlebot_actions::FindFiducialResult result_;

};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "find_fiducial_pose");

  FindFiducialAction find_fiducial(ros::this_node::getName());
  ros::spin();

  return 0;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>

template<typename T>
class SufficientStatistics
{

};

/* sufficient statistics for gaussian distribution
 * has outer product, sum over data and count
 */
template<typename T>
class ssGauss : public SufficientStatistics<T>
{

};

/* sufficient statistics for categorical distribution
 * mainly just counts
 */
template<typename T>
class ssCat : public SufficientStatistics<T>
{

};


/*!
@file
Defines `boost::hana::Product::mcd`.

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

#ifndef BOOST_HANA_PRODUCT_MCD_HPP
#define BOOST_HANA_PRODUCT_MCD_HPP

#include <boost/hana/product/product.hpp>

#include <boost/hana/comparable/equal_mcd.hpp>
#include <boost/hana/core/is_a.hpp>
#include <boost/hana/logical/logical.hpp>

namespace boost { namespace hana {
    //! Minimal complete definition: `first` and `second`
    struct Product::mcd { };

    //! Instance of `Comparable` for `Product`s.
    //!
    //! Two products `x` and `y` are equal iff they are equal element-wise,
    //! i.e. iff
    //! @code
    //!     first(x) == first(y) && second(x) == second(y)
    //! @endcode
    //!
    //! ### Example
    //! @snippet example/product/comparable.cpp main
    template <typename T, typename U>
    struct Comparable::instance<T, U, when<
        is_a<Product, T>() && is_a<Product, U>()
    >> : Comparable::equal_mcd {
        template <typename X, typename Y>
        static constexpr auto equal_impl(X x, Y y) {
            return and_(
                equal(first(x), first(y)),
                equal(second(x), second(y))
            );
        }
    };

#if 0
    template <typename T>
    struct Functor::instance<T, when<is_a<Pair, T>()>>
        : Functor::fmap_mcd
    {
        template <typename F, typename P>
        static constexpr auto fmap_impl(F f, P p)
        { return pair(f(first(p)), f(second(p))); }
    };

    template <typename T>
    struct Applicative::instance<T, when<is_a<Pair, T>()>>
        : Applicative::mcd
    {
        template <typename X>
        static constexpr auto lift_impl(X x)
        { return pair(x, x); }

        template <typename F, typename P>
        static constexpr auto ap_impl(F f, P p)
        { return pair(first(f)(first(p), second(f)(second(p)))); }
    };

    template <typename T>
    struct Monad::instance<T, when<is_a<Pair, T>()>>
        : Monad::bind_mcd<T>
    {
        template <typename P, typename F>
        static constexpr auto bind_impl(P p, F f) {
            return pair(
                bind(first(p), compose(first, f)),
                bind(second(p), compose(second, f))
            );
        }
    };

    template <typename T>
    struct Foldable::instance<T, when<is_a<Pair, T>()>>
        : Foldable::mcd
    {
        template <typename P, typename S, typename F>
        static constexpr auto foldr_impl(P p, S s, F f) {
            return f(first(p), f(second(p), s));
        }

        template <typename P, typename S, typename F>
        static constexpr auto foldl_impl(P p, S s, F f) {
            return f(f(s, first(p)), second(p));
        }
    };
#endif
}} // end namespace boost::hana

#endif // !BOOST_HANA_PRODUCT_MCD_HPP


/*
 * graph.cpp
 *
 * Purpose: Graph
 * Created by: Keisuke Okumura <okumura.k@coord.c.titech.ac.jp>
 */

#include "graph.h"
#include <random>
#include <unordered_set>
#include "util.h"
#include <boost/heap/fibonacci_heap.hpp>

Graph::Graph() {
  std::random_device seed_gen;
  MT = new std::mt19937(seed_gen());
  init();
}

Graph::Graph(std::mt19937* _MT) : MT(_MT) {
  init();
}

void Graph::init() {
  directed = false;
  regFlg = true;
}

Graph::~Graph() {
  for (auto v : nodes) delete v;
  nodes.clear();
  for (auto p : knownPaths) delete p.second;
  knownPaths.clear();
}

Node* Graph::getNode(int id) {
  auto itr = std::find_if(nodes.begin(), nodes.end(),
                          [id](Node* v){ return v->getId() == id; });
  // error check
  if (itr == nodes.end()) {
    std::cout << "error@Graph::getNode, "
              << "node index is over, " << id << "\n";
    std::exit(1);
  }

  return *itr;
}

Node* Graph::getNode(int x, int y) {
  auto itr = std::find_if(nodes.begin(), nodes.end(),
                          [x, y](Node* v)
                          { return v->getPos().x == x
                              && v->getPos().y == y; });
  if (itr == nodes.end()) return nullptr;
  return *itr;
}

bool Graph::existNode(int id) {
  auto itr = std::find_if(nodes.begin(), nodes.end(),
                          [id](Node* v){ return v->getId() == id; });
  return itr != nodes.end();
}

int Graph::getNodeIndex(Node* v) {
  return v->getIndex();
}

Nodes Graph::neighbor(Node* v) {
  return v->getNeighbor();
}

Nodes Graph::neighbor(int i) {
  return getNode(i)->getNeighbor();
}

Nodes Graph::getPath(Node* s, Node* g, Nodes &prohibitedNodes) {
  return {};
}

// regFlg : whether register
Nodes Graph::getPath(Node* _s, Node* _g,
                     Nodes &prohibitedNodes, int (*dist) (Node*, Node*))
{
  bool prohibited = !prohibitedNodes.empty();
  Nodes path, C;
  std::string key;

  // ==== fast implementation ====
  if (regFlg && !prohibited) {
    key = getKey(_s, _g);
    auto itrK = knownPaths.find(key);
    if (itrK != knownPaths.end()) {  // known
      path = itrK->second->path;
      return path;
    }
  }
  // =============================

  int f;
  bool invalid = true;

  // prepare node open hashtable
  boost::heap::fibonacci_heap<Fib_AN> OPEN;
  std::unordered_map<int, boost::heap::fibonacci_heap<Fib_AN>::handle_type> SEARCHED;
  std::unordered_set<int> CLOSE;
  AN* n = new AN { _s, 0, dist(_s, _g), nullptr };
  auto handle = OPEN.push(Fib_AN(n));
  SEARCHED.emplace(n->v->getId(), handle);

  while (!OPEN.empty()) {
    // argmin
    n = OPEN.top().node;

    // check goal condition
    if (n->v == _g) {
      invalid = false;
      break;
    }

    // ==== fast implementation ====
    key = getKey(n->v, _g);
    auto itrK = knownPaths.find(key);
    if (itrK != knownPaths.end()) {  // known
      Nodes kPath = itrK->second->path;
      bool valid = true;
      if (prohibited) {
        for (auto v : kPath) {
          if (inArray(v, prohibitedNodes)) {
            valid = false;
            break;
          }
        }
      }
      if (valid) {
        for (int i = 1; i < kPath.size(); ++i) {
          n = new AN { kPath[i], 0, 0, n };
        }
        invalid = false;
        break;
      }
    }
    // =============================

    // update list
    OPEN.pop();
    CLOSE.emplace(n->v->getId());

    // search neighbor
    C = neighbor(n->v);

    for (auto m : C) {
      if (prohibited && inArray(m, prohibitedNodes)) continue;
      if (CLOSE.find(m->getId()) != CLOSE.end()) continue;
      f = n->g + 1 + dist(m, _g);

      // ==== fast implementation ====
      if (regFlg) {
        key = getKey(m, _g);
        auto itrK = knownPaths.find(key);
        if (itrK != knownPaths.end()) {
          f = n->g + 1 + itrK->second->path.size() - 1;
        }
      }
      // =============================

      auto itrS = SEARCHED.find(m->getId());
      if (itrS == SEARCHED.end()) {  // new node
        AN* l = new AN { m, n->g + 1, f, n };
        auto handle = OPEN.push(Fib_AN(l));
        SEARCHED.emplace(l->v->getId(), handle);
      } else {
        auto handle = itrS->second;
        AN* l = (*handle).node;
        if (l->f > f) {
          l->g = n->g + 1;
          l->f = f;
          l->p = n;
          OPEN.increase(handle);
        }
      }
    }
  }

  if (invalid) return path;

  // back tracking
  while (n != nullptr) {
    path.push_back(n->v);
    n = n->p;
  }
  std::reverse(path.begin(), path.end());

  // register path
  if (regFlg && !prohibited) registerPath(path);

  return path;
}

std::string Graph::getKey(Node* s, Node* g) {
  int sIndex = getNodeIndex(s);
  int gIndex = getNodeIndex(g);
  std::string key = "";
  key += std::to_string(sIndex);
  key += "-";
  key += std::to_string(gIndex);
  return key;
}

void Graph::registerPath(const Nodes &path) {
  if (path.empty()) return;

  Nodes tmp = path;
  std::string key;

  Node *v1, *v2;
  do {
    v1 = tmp[0];
    v2 = tmp[tmp.size() - 1];
    key = getKey(v1, v2);
    KnownPath* knownPath = new KnownPath { v1, v2, tmp };
    knownPaths.emplace(key, knownPath);
    tmp.erase(tmp.begin());
  } while (tmp.size() > 2);
}

Paths Graph::getRandomStartGoal(int num) {
  if (num > starts.size() || num > goals.size()) {
    std::cout << "error@Graph::getStartGoal, over node size" << "\n";
    std::exit(1);
  }

  Paths points;
  Nodes ss(starts.size());
  Nodes gs(goals.size());
  bool flg;

  std::copy(starts.begin(), starts.end(), ss.begin());
  std::copy(goals.begin(),  goals.end(), gs.begin());

  while (true) {
    points.clear();
    std::shuffle(ss.begin(), ss.end(), *MT);
    std::shuffle(gs.begin(), gs.end(), *MT);

    flg = true;
    for (int i = 0; i < num; ++i) {
      if (ss[i] != gs[i]) {
        points.push_back({ ss[i], gs[i] });
      } else {
        flg = false;
        break;
      }
    }

    if (flg) break;
  }

  return points;
}


///////////////////////////////////////////////////////////////////////////////
// distribution::survival::record::function::include.hpp               		 //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RECORD_FUNCTION_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RECORD_FUNCTION_INCLUDE_HPP_ER_2009

#include <boost/statistics/detail/distribution/survival/record/function/time_since_entry.hpp>
#include <boost/statistics/detail/distribution/survival/record/function/less_than.hpp>
#include <boost/statistics/detail/distribution/survival/record/function/description.hpp>

// TODO is_at_risk

#endif


#include <iostream>
#include <fstream>
#include <vector>
#include <Eigen/Geometry>

int32_t main(int32_t argc_, char** argv_) {
  if (argc_ < 3) {
    std::cerr << "usage: ./trajectory_converter -g2o <pose_graph.g2o> | -tum <tum_trajectory>" << std::endl;
    return 0;
  }

  //ds determine and log configuration
  std::string input_file          = argv_[1];
  std::string input_format        = "";
  const std::string output_file   = "kitti_trajectory.txt";
  const std::string output_format = "KITTI";
  const std::string pose_keyword  = "VERTEX_SE3:QUAT";

  int32_t number_of_checked_parameters = 1;
  while (number_of_checked_parameters < argc_) {
    if (!std::strcmp(argv_[number_of_checked_parameters], "-g2o")) {
      input_format = "g2o";
      ++number_of_checked_parameters;
      if (number_of_checked_parameters == argc_) {break;}
      input_file = argv_[number_of_checked_parameters];
    } else if (!std::strcmp(argv_[number_of_checked_parameters], "-tum")) {
      input_format = "tum";
      ++number_of_checked_parameters;
      if (number_of_checked_parameters == argc_) {break;}
      input_file = argv_[number_of_checked_parameters];
    }
    ++number_of_checked_parameters;
  }

  std::cerr << "< converting trajectory format '" << output_format << "': '" << input_file << "'" << std::endl;
  std::cerr << "> to trajectory of format '" << output_format << "': '" << output_file << "'" << std::endl;

  if (input_format == "g2o") {
    std::cerr << "with pose keyword: '" << pose_keyword << "'" << std::endl;
  }

  //ds parse g2o file by hand (avoiding to link against it)
  std::ifstream input_stream(input_file);
  if (!input_stream.good() || !input_stream.is_open()) {
    std::cerr << "ERROR: unable to open: '" << input_file << "'" << std::endl;
    return 0;
  }

  //ds parsing buffers
  std::vector<Eigen::Isometry3d> poses(0);
  std::string line;

  //ds parse the complete input file (assuming continuous, sequential indexing)
  while (std::getline(input_stream, line)) {

    //ds for g2o
    if (input_format == "g2o") {

      //ds check if the current line contains pose information
      if (line.find(pose_keyword) != std::string::npos) {

        //ds get line to a string stream object
        std::istringstream stringstream(line);

        //ds possible values
        std::string entry_name;
        uint64_t entry_identifier = 0;
        double translation_x = 0;
        double translation_y = 0;
        double translation_z = 0;
        double quaternion_w  = 0;
        double quaternion_x  = 0;
        double quaternion_y  = 0;
        double quaternion_z  = 0;

        //ds parse the full line and check for failure
        if (!(stringstream >> entry_name >> entry_identifier >> translation_x >> translation_y >> translation_z
                                                             >> quaternion_x >> quaternion_y >> quaternion_z >> quaternion_w)) {
          std::cerr << "ERROR: unable to parse pose lines with keyword: '" << pose_keyword << "'" << std::endl;
          input_stream.close();
          return 0;
        }

        //ds set pose value
        Eigen::Isometry3d parsed_pose(Eigen::Isometry3d::Identity());
        parsed_pose.translation() = Eigen::Vector3d(translation_x, translation_y, translation_z);
        parsed_pose.linear()      = Eigen::Quaterniond(quaternion_w, quaternion_x, quaternion_y, quaternion_z).toRotationMatrix();
        poses.push_back(parsed_pose);
      }
    } else if (input_format == "tum") {

      //ds get line to a string stream object
      std::istringstream stringstream(line);

      //ds possible values
      double time_stamp = 0;
      double translation_x = 0;
      double translation_y = 0;
      double translation_z = 0;
      double quaternion_w  = 0;
      double quaternion_x  = 0;
      double quaternion_y  = 0;
      double quaternion_z  = 0;

      //ds parse the full line and check for failure
      if (!(stringstream >> time_stamp >> translation_x >> translation_y >> translation_z
                                       >> quaternion_x >> quaternion_y >> quaternion_z >> quaternion_w)) {
        std::cerr << "ERROR: unable to parse pose lines" << std::endl;
        input_stream.close();
        return 0;
      }

      //ds set pose value
      Eigen::Isometry3d parsed_pose(Eigen::Isometry3d::Identity());
      parsed_pose.translation() = Eigen::Vector3d(translation_x, translation_y, translation_z);
      parsed_pose.linear()      = Eigen::Quaterniond(quaternion_w, quaternion_x, quaternion_y, quaternion_z).toRotationMatrix();
      poses.push_back(parsed_pose);
    }
  }
  input_stream.close();
  std::cerr << "parsed poses: " << poses.size() << std::endl;

  //ds open output file (assuming continuous, sequential indexing)
  std::ofstream output_stream(output_file, std::ifstream::out);
  for (const Eigen::Isometry3d& pose: poses) {

    //ds dump transform according to KITTI format
    for (uint8_t u = 0; u < 3; ++u) {
      for (uint8_t v = 0; v < 4; ++v) {
        output_stream << pose(u,v) << " ";
      }
    }
    output_stream << "\n";
  }
  output_stream.close();
  std::cerr << "to trajectory of format '" << output_format << "' to: '" << output_file << "'" << std::endl;
  return 0;
}


#include "Utils.hpp"

#include <boost/container/flat_map.hpp>
#include <nlohmann/json.hpp>
#include <variant>

#include "gtest/gtest.h"

TEST(TemplateCharReplace, replaceOneInt)
{
    nlohmann::json j = {{"foo", "$bus"}};
    auto it = j.begin();
    boost::container::flat_map<std::string, BasicVariantType> data;
    data["BUS"] = 23;

    templateCharReplace(it, data, 0);

    nlohmann::json expected = 23;
    EXPECT_EQ(expected, j["foo"]);
}

TEST(TemplateCharReplace, replaceOneStr)
{
    nlohmann::json j = {{"foo", "$TEST"}};
    auto it = j.begin();
    boost::container::flat_map<std::string, BasicVariantType> data;
    data["TEST"] = std::string("Test");

    templateCharReplace(it, data, 0);

    nlohmann::json expected = "Test";
    EXPECT_EQ(expected, j["foo"]);
}

TEST(TemplateCharReplace, replaceSecondStr)
{
    nlohmann::json j = {{"foo", "the $TEST"}};
    auto it = j.begin();
    boost::container::flat_map<std::string, BasicVariantType> data;
    data["TEST"] = std::string("Test");

    templateCharReplace(it, data, 0);

    nlohmann::json expected = "the Test";
    EXPECT_EQ(expected, j["foo"]);
}

TEST(TemplateCharReplace, replaceMiddleStr)
{
    nlohmann::json j = {{"foo", "the $TEST worked"}};
    auto it = j.begin();
    boost::container::flat_map<std::string, BasicVariantType> data;
    data["TEST"] = std::string("Test");

    templateCharReplace(it, data, 0);

    nlohmann::json expected = "the Test worked";
    EXPECT_EQ(expected, j["foo"]);
}

TEST(TemplateCharReplace, replaceLastStr)
{
    nlohmann::json j = {{"foo", "the Test $TEST"}};
    auto it = j.begin();
    boost::container::flat_map<std::string, BasicVariantType> data;
    data["TEST"] = 23;

    templateCharReplace(it, data, 0);

    nlohmann::json expected = "the Test 23";
    EXPECT_EQ(expected, j["foo"]);
}

TEST(TemplateCharReplace, increment)
{
    nlohmann::json j = {{"foo", "3 plus 1 equals $TEST + 1"}};
    auto it = j.begin();
    boost::container::flat_map<std::string, BasicVariantType> data;
    data["TEST"] = 3;

    templateCharReplace(it, data, 0);

    nlohmann::json expected = "3 plus 1 equals 4";
    EXPECT_EQ(expected, j["foo"]);
}

TEST(TemplateCharReplace, decrement)
{
    nlohmann::json j = {{"foo", "3 minus 1 equals $TEST - 1 !"}};
    auto it = j.begin();
    boost::container::flat_map<std::string, BasicVariantType> data;
    data["TEST"] = 3;

    templateCharReplace(it, data, 0);

    nlohmann::json expected = "3 minus 1 equals 2 !";
    EXPECT_EQ(expected, j["foo"]);
}

TEST(TemplateCharReplace, modulus)
{
    nlohmann::json j = {{"foo", "3 mod 2 equals $TEST % 2"}};
    auto it = j.begin();
    boost::container::flat_map<std::string, BasicVariantType> data;
    data["TEST"] = 3;

    templateCharReplace(it, data, 0);

    nlohmann::json expected = "3 mod 2 equals 1";
    EXPECT_EQ(expected, j["foo"]);
}

TEST(TemplateCharReplace, multiply)
{
    nlohmann::json j = {{"foo", "3 * 2 equals $TEST * 2"}};
    auto it = j.begin();
    boost::container::flat_map<std::string, BasicVariantType> data;
    data["TEST"] = 3;

    templateCharReplace(it, data, 0);

    nlohmann::json expected = "3 * 2 equals 6";
    EXPECT_EQ(expected, j["foo"]);
}

TEST(TemplateCharReplace, divide)
{
    nlohmann::json j = {{"foo", "4 / 2 equals $TEST / 2"}};
    auto it = j.begin();
    boost::container::flat_map<std::string, BasicVariantType> data;
    data["TEST"] = 4;

    templateCharReplace(it, data, 0);

    nlohmann::json expected = "4 / 2 equals 2";
    EXPECT_EQ(expected, j["foo"]);
}

TEST(TemplateCharReplace, multiMath)
{
    nlohmann::json j = {{"foo", "4 * 2 % 6 equals $TEST * 2 % 6"}};
    auto it = j.begin();
    boost::container::flat_map<std::string, BasicVariantType> data;
    data["TEST"] = 4;

    templateCharReplace(it, data, 0);

    nlohmann::json expected = "4 * 2 % 6 equals 2";
    EXPECT_EQ(expected, j["foo"]);
}

TEST(TemplateCharReplace, twoReplacements)
{
    nlohmann::json j = {{"foo", "$FOO $BAR"}};
    auto it = j.begin();
    boost::container::flat_map<std::string, BasicVariantType> data;
    data["FOO"] = std::string("foo");
    data["BAR"] = std::string("bar");

    templateCharReplace(it, data, 0);

    nlohmann::json expected = "foo bar";
    EXPECT_EQ(expected, j["foo"]);
}

TEST(TemplateCharReplace, twoReplacementsWithMath)
{
    nlohmann::json j = {{"foo", "4 / 2 equals $TEST / 2 $BAR"}};
    auto it = j.begin();
    boost::container::flat_map<std::string, BasicVariantType> data;
    data["TEST"] = 4;
    data["BAR"] = std::string("bar");

    templateCharReplace(it, data, 0);

    nlohmann::json expected = "4 / 2 equals 2 bar";
    EXPECT_EQ(expected, j["foo"]);
}

TEST(TemplateCharReplace, hexAndWrongCase)
{
    nlohmann::json j = {{"Address", "0x54"},
                        {"Bus", 15},
                        {"Name", "$bus sensor 0"},
                        {"Type", "SomeType"}};

    boost::container::flat_map<std::string, BasicVariantType> data;
    data["BUS"] = 15;

    for (auto it = j.begin(); it != j.end(); it++)
    {
        templateCharReplace(it, data, 0);
    }
    nlohmann::json expected = {{"Address", 84},
                               {"Bus", 15},
                               {"Name", "15 sensor 0"},
                               {"Type", "SomeType"}};
    EXPECT_EQ(expected, j);
}

TEST(TemplateCharReplace, replaceSecondAsInt)
{
    nlohmann::json j = {{"foo", "twelve is $TEST"}};
    auto it = j.begin();
    boost::container::flat_map<std::string, BasicVariantType> data;
    data["test"] = 12;

    templateCharReplace(it, data, 0);

    nlohmann::json expected = "twelve is 12";
    EXPECT_EQ(expected, j["foo"]);
}

TEST(TemplateCharReplace, singleHex)
{
    nlohmann::json j = {{"foo", "0x54"}};
    auto it = j.begin();
    boost::container::flat_map<std::string, BasicVariantType> data;

    templateCharReplace(it, data, 0);

    nlohmann::json expected = 84;
    EXPECT_EQ(expected, j["foo"]);
}


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

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

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

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

 /*!

    @ingroup group-swar
    Function object implementing deinterleave_first capabilities

    Computes a vector from a combination of the two inputs.

    @par Semantic:

    For every parameters of type T
    @code
    T0 r = deinterleave_first(x,y);
    @endcode

    is equivalent to :

    @code
    r = [ x[0] x[1] ... x[n/2] y[0] y[1] ... y[n/2] ]
    @endcode

    with <tt> n = cardinal_of<T>::value </tt>

  **/
  const boost::dispatch::functor<tag::deinterleave_first_> deinterleave_first = {};
} }
#endif

#include <boost/simd/function/scalar/deinterleave_first.hpp>
#include <boost/simd/function/simd/deinterleave_first.hpp>

#endif


#define BOOST_TEST_MODULE test_graph

#include <iostream>
#include <boost/test/unit_test.hpp>
#include "graph.h"

using namespace dpct;

BOOST_AUTO_TEST_CASE(node_arcs)
{
    Node n1(std::vector<double>(), nullptr);
    Node n2(std::vector<double>(), nullptr);
    BOOST_CHECK_EQUAL(n1.getNumInArcs(),  0);
    BOOST_CHECK_EQUAL(n1.getNumOutArcs(), 0);

    BOOST_CHECK_EQUAL(n2.getNumInArcs(),  0);
    BOOST_CHECK_EQUAL(n2.getNumOutArcs(), 0);

    Arc a(&n1, &n2, Arc::Move, {0.0});

    BOOST_CHECK_EQUAL(n1.getNumInArcs(),  0);
    BOOST_CHECK_EQUAL(n1.getNumOutArcs(), 1);

    BOOST_CHECK_EQUAL(n2.getNumInArcs(),  1);
    BOOST_CHECK_EQUAL(n2.getNumOutArcs(), 0);

    a.getSourceNode()->removeOutArc(&a);
    BOOST_CHECK_EQUAL(n1.getNumInArcs(),  0);
    BOOST_CHECK_EQUAL(n1.getNumOutArcs(), 0);

    BOOST_CHECK_EQUAL(n2.getNumInArcs(),  1);
    BOOST_CHECK_EQUAL(n2.getNumOutArcs(), 0);

    a.getTargetNode()->removeInArc(&a);
    BOOST_CHECK_EQUAL(n1.getNumInArcs(),  0);
    BOOST_CHECK_EQUAL(n1.getNumOutArcs(), 0);

    BOOST_CHECK_EQUAL(n2.getNumInArcs(),  0);
    BOOST_CHECK_EQUAL(n2.getNumOutArcs(), 0);
}

BOOST_AUTO_TEST_CASE(build_graph)
{
    Graph::Configuration config(true, true, true);
    Graph g(config);
    BOOST_CHECK_EQUAL(g.getNumArcs(), 0);
    BOOST_CHECK_EQUAL(g.getNumNodes(), 0);

    std::vector<double> appearanceScore(2, -200);
    std::vector<double> disappearanceScore(2, -200);
    std::vector<double> divisionScore(2, -200);

    // -----------------------------------------------------
    // Timestep 1
    Graph::NodePtr n_1_1 = g.addNode(0, {3, -1}, appearanceScore, disappearanceScore,
                                        true, false, std::make_shared<NameData>("Timestep 1: Node 1"));

    Graph::NodePtr n_1_2 = g.addNode(0, {2, 5, -2}, appearanceScore, disappearanceScore,
                                        true, false, std::make_shared<NameData>("Timestep 1: Node 2"));

    Graph::NodePtr n_1_3 = g.addNode(0, {3, -5}, appearanceScore, disappearanceScore,
                                        true, false, std::make_shared<NameData>("Timestep 1: Node 3"));

    BOOST_CHECK_EQUAL(g.getNumArcs(), 6);
    BOOST_CHECK_EQUAL(g.getNumNodes(), 3);

    // -----------------------------------------------------
    // Timestep 2
    Graph::NodePtr n_2_1 = g.addNode(1, {4, -2}, appearanceScore, disappearanceScore,
                                        false, false, std::make_shared<NameData>("Timestep 2: Node 1"));

    Graph::NodePtr n_2_2 = g.addNode(1, {2, -2}, appearanceScore, disappearanceScore,
                                        false, false, std::make_shared<NameData>("Timestep 2: Node 2"));

    Graph::NodePtr n_2_3 = g.addNode(1, {2, -4}, appearanceScore, disappearanceScore,
                                        false, false, std::make_shared<NameData>("Timestep 2: Node 3"));

    Graph::NodePtr n_2_4 = g.addNode(1, {2,-2}, appearanceScore, disappearanceScore,
                                        false, false, std::make_shared<NameData>("Timestep 2: Node 4"));

    BOOST_CHECK_EQUAL(g.getNumArcs(), 14);
    BOOST_CHECK_EQUAL(g.getNumNodes(), 7);

    g.addMoveArc(n_1_1, n_2_1, {0.0}, std::make_shared<NameData>("Arc 1"));
    g.addMoveArc(n_1_2, n_2_2, {0.0});
    g.addMoveArc(n_1_2, n_2_3, {0.0});
    g.addMoveArc(n_1_3, n_2_4, {0.0});

    BOOST_CHECK_EQUAL(g.getNumArcs(), 18);
    BOOST_CHECK_EQUAL(g.getNumNodes(), 7);

    // -----------------------------------------------------
    // Timestep 3
    Graph::NodePtr n_3_1 = g.addNode(2, {3, 2}, appearanceScore, disappearanceScore,
                                        false, false);

    Graph::NodePtr n_3_2 = g.addNode(2, {2, 0}, appearanceScore, disappearanceScore,
                                        false, false);

    Graph::NodePtr n_3_3 = g.addNode(2, {3, -3}, appearanceScore, disappearanceScore,
                                        false, false);

    BOOST_CHECK_EQUAL(g.getNumArcs(), 24);
    BOOST_CHECK_EQUAL(g.getNumNodes(), 10);

    g.addMoveArc(n_2_1, n_3_1, {0.0});
    g.addMoveArc(n_2_2, n_3_2, {0.0});
    g.addMoveArc(n_2_3, n_3_3, {0.0});
    BOOST_CHECK_EQUAL(g.getNumArcs(), 27);

    // -----------------------------------------------------
    // Timestep 4
    Graph::NodePtr n_4_1 = g.addNode(3, {4, -1}, appearanceScore, disappearanceScore,
                                        false, true);

    Graph::NodePtr n_4_2 = g.addNode(3, {2, -1}, appearanceScore, disappearanceScore,
                                        false, true);

    Graph::NodePtr n_4_3 = g.addNode(3, {2, -6}, appearanceScore, disappearanceScore,
                                        false, true);

    Graph::NodePtr n_4_4 = g.addNode(3, {4, -2}, appearanceScore, disappearanceScore,
                                        false, true);

    BOOST_CHECK_EQUAL(g.getNumArcs(), 35); // doesn't add disappearance moves at sink
    BOOST_CHECK_EQUAL(g.getNumNodes(), 14);

    g.addMoveArc(n_3_1, n_4_1, {0.0});
    g.addMoveArc(n_3_1, n_4_2, {0.0});
    Graph::ArcPtr m1 = g.addMoveArc(n_3_2, n_4_2, {0.0});
    Graph::ArcPtr m2 = g.addMoveArc(n_3_2, n_4_3, {0.0});
    g.addMoveArc(n_3_3, n_4_3, {0.0});
    g.addMoveArc(n_3_3, n_4_4, {0.0});
    BOOST_CHECK_EQUAL(g.getNumArcs(), 41);

    Graph::ArcPtr d1 = g.allowMitosis(n_3_2, n_4_3, -1);
    Graph::ArcPtr d2 = g.allowMitosis(n_3_3, n_4_3, 2);

    BOOST_CHECK_EQUAL(g.getNumArcs(), 43);
    BOOST_CHECK_EQUAL(d1->isEnabled(), false);
    BOOST_CHECK_EQUAL(d2->isEnabled(), false);
    BOOST_CHECK_EQUAL(m1->isEnabled(), true);

    // add a cell to a division's mother and activate one of its out arc to enable a division
    BOOST_CHECK_EQUAL(n_3_2->getCellCount(), 0);
    n_3_2->increaseCellCount();
    BOOST_CHECK_EQUAL(n_3_2->getCellCount(), 1);
    BOOST_CHECK_EQUAL(m1->getUseCount(), 0);
    m1->markUsed();
    BOOST_CHECK_EQUAL(m1->getUseCount(), 1);
    // this should enable the division arc
    BOOST_CHECK_EQUAL(d1->isEnabled(), true);
    BOOST_CHECK_EQUAL(m2->isEnabled(), true);
    // now let's use the division arc and see whether the other transitions become invalid
    d1->markUsed();
    BOOST_CHECK_EQUAL(m1->isEnabled(), false);
    BOOST_CHECK_EQUAL(m2->isEnabled(), false);
    BOOST_CHECK_EQUAL(d1->isEnabled(), false);

    // unuse again
    d1->markUsed(false);
    BOOST_CHECK_EQUAL(m1->isEnabled(), true);
    BOOST_CHECK_EQUAL(m2->isEnabled(), true);
    BOOST_CHECK_EQUAL(d1->isEnabled(), true);

    // use other out arc and add another cell, then the division should be disabled
    n_3_2->increaseCellCount();
    m2->markUsed();
    BOOST_CHECK_EQUAL(d1->isEnabled(), false);
}

BOOST_AUTO_TEST_CASE(node_active_arc_counts)
{
    Node n1(std::vector<double>(), nullptr);
    Node n2(std::vector<double>(), nullptr);

    Arc a(&n1, &n2, Arc::Move, {0.0});
    BOOST_CHECK_EQUAL(n1.getNumActiveDivisions(), 0);
    BOOST_CHECK_EQUAL(n1.getMoveInArcUsedSum(), 0);
    BOOST_CHECK_EQUAL(n1.getMoveOutArcUsedSum(), 0);

    a.markUsed();
    BOOST_CHECK_EQUAL(n1.getNumActiveDivisions(), 0);
    BOOST_CHECK_EQUAL(n1.getMoveInArcUsedSum(), 0);
    BOOST_CHECK_EQUAL(n1.getMoveOutArcUsedSum(), 1);

    BOOST_CHECK_EQUAL(n2.getNumActiveDivisions(), 0);
    BOOST_CHECK_EQUAL(n2.getMoveInArcUsedSum(), 1);
    BOOST_CHECK_EQUAL(n2.getMoveOutArcUsedSum(), 0);

    a.markUsed();
    BOOST_CHECK_EQUAL(n1.getNumActiveDivisions(), 0);
    BOOST_CHECK_EQUAL(n1.getMoveInArcUsedSum(), 0);
    BOOST_CHECK_EQUAL(n1.getMoveOutArcUsedSum(), 2);

    BOOST_CHECK_EQUAL(n2.getNumActiveDivisions(), 0);
    BOOST_CHECK_EQUAL(n2.getMoveInArcUsedSum(), 2);
    BOOST_CHECK_EQUAL(n2.getMoveOutArcUsedSum(), 0);

    a.markUsed(false);
    BOOST_CHECK_EQUAL(n1.getNumActiveDivisions(), 0);
    BOOST_CHECK_EQUAL(n1.getMoveInArcUsedSum(), 0);
    BOOST_CHECK_EQUAL(n1.getMoveOutArcUsedSum(), 1);

    BOOST_CHECK_EQUAL(n2.getNumActiveDivisions(), 0);
    BOOST_CHECK_EQUAL(n2.getMoveInArcUsedSum(), 1);
    BOOST_CHECK_EQUAL(n2.getMoveOutArcUsedSum(), 0);

    a.markUsed(false);
    BOOST_CHECK_EQUAL(n1.getNumActiveDivisions(), 0);
    BOOST_CHECK_EQUAL(n1.getMoveInArcUsedSum(), 0);
    BOOST_CHECK_EQUAL(n1.getMoveOutArcUsedSum(), 0);

    BOOST_CHECK_EQUAL(n2.getNumActiveDivisions(), 0);
    BOOST_CHECK_EQUAL(n2.getMoveInArcUsedSum(), 0);
    BOOST_CHECK_EQUAL(n2.getMoveOutArcUsedSum(), 0);
}