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

#include <boost/hana/ext/std/integer_sequence.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/iterable.hpp>

#include <cstddef>
#include <utility>
using namespace boost::hana;

using T = int;
using U = long long;

int main() {
    //////////////////////////////////////////////////////////////////////////
    // Setup for the laws below
    //////////////////////////////////////////////////////////////////////////
    auto sequences = make<Tuple>(
        std::integer_sequence<T>{},
        std::integer_sequence<U>{},

        std::integer_sequence<T, 0>{},
        std::integer_sequence<U, 0>{},

        std::integer_sequence<T, 0, 1>{},
        std::integer_sequence<U, 0, 1>{},

        std::integer_sequence<T, 1, 0, 2>{},
        std::integer_sequence<U, 1, 0, 2>{}
    );

    //////////////////////////////////////////////////////////////////////////
    // Iterable
    //////////////////////////////////////////////////////////////////////////
    // head
    {
        BOOST_HANA_CONSTANT_CHECK(equal(
            head(std::index_sequence<0>{}),
            std::integral_constant<std::size_t, 0>{}
        ));
        BOOST_HANA_CONSTANT_CHECK(equal(
            head(std::index_sequence<0, 1>{}),
            std::integral_constant<std::size_t, 0>{}
        ));
    }

    // is_empty
    {
        BOOST_HANA_CONSTANT_CHECK(is_empty(std::index_sequence<>{}));
        BOOST_HANA_CONSTANT_CHECK(not_(is_empty(std::index_sequence<0>{})));
        BOOST_HANA_CONSTANT_CHECK(not_(is_empty(std::index_sequence<1>{})));
    }

    // tail
    {
        BOOST_HANA_CONSTANT_CHECK(equal(
            tail(std::index_sequence<0>{}),
            std::index_sequence<>{}
        ));
        BOOST_HANA_CONSTANT_CHECK(equal(
            tail(std::index_sequence<0, 1>{}),
            std::index_sequence<1>{}
        ));
        BOOST_HANA_CONSTANT_CHECK(equal(
            tail(std::index_sequence<0, 1, 2>{}),
            std::index_sequence<1, 2>{}
        ));
    }

    // laws
    test::TestIterable<ext::std::IntegerSequence>{sequences};
}


/// @file py_coord_zcorn_tools.cpp
/// @brief Python bindnings for COORD & ZCORN tools
/// @author uentity
/// @date 10.05.2011
/// @copyright This source code is released under the terms of
///            the BSD License. See LICENSE for more details.
#include "coord_zcorn_tools.h"
#include "export_python_wrapper.h"
#include "py_pair_converter.h"
#include <boost/python/tuple.hpp>

#ifdef BSPY_EXPORTING_PLUGIN
using namespace boost::python;
namespace czt = blue_sky::coord_zcorn_tools;

namespace {

typedef t_long int_t;
typedef czt::uint_t uint_t;
typedef t_double fp_t;
typedef t_float fp_stor_t;
typedef v_float fp_storarr_t;
typedef spv_float spfp_storarr_t;
typedef spv_long spi_arr_t;
typedef spi_arr_t::pure_pointed_t int_arr_t;
typedef std::pair< spv_float, spv_float > coord_zcorn_pair;

tuple wave_mesh_deltas_s2_o1(
	fp_stor_t cell_dx, fp_stor_t cell_dy,
	fp_stor_t min_dx, fp_stor_t min_dy,
	spfp_storarr_t dx, spfp_storarr_t dy,
	fp_t max_sz_tol = 0.3, bool strict_max_sz = false)
{
	// Make copies of delta arrays as thay can be referenced by Python
	spfp_storarr_t dx_cpy = BS_KERNEL.create_object(fp_storarr_t::bs_type());
	dx_cpy->assign(*dx);
	spfp_storarr_t dy_cpy = BS_KERNEL.create_object(fp_storarr_t::bs_type());
	dy_cpy->assign(*dy);
	// invoke stage 2
	czt::wave_mesh_deltas_s2(
		cell_dx, cell_dy, min_dx, min_dy,
		dx_cpy, dy_cpy, max_sz_tol, strict_max_sz
	);
	return make_tuple(dx_cpy, dy_cpy);
}

tuple wave_mesh_deltas_s2_o2(
	fp_stor_t cell_dx, fp_stor_t cell_dy,
	spfp_storarr_t points_param,
	spfp_storarr_t dx, spfp_storarr_t dy,
	fp_t max_sz_tol = 0.3, bool strict_max_sz = false)
{
	// Make copies of delta arrays as thay can be referenced by Python
	spfp_storarr_t dx_cpy = BS_KERNEL.create_object(fp_storarr_t::bs_type());
	dx_cpy->assign(*dx);
	spfp_storarr_t dy_cpy = BS_KERNEL.create_object(fp_storarr_t::bs_type());
	dy_cpy->assign(*dy);
	// invoke stage 2
	czt::wave_mesh_deltas_s2(
		cell_dx, cell_dy, points_param,
		dx, dy, max_sz_tol, strict_max_sz
	);
	return make_tuple(dx_cpy, dy_cpy);
}

// wave_mesh_deltas with overloads
tuple wave_mesh_deltas(fp_stor_t max_dx, fp_stor_t max_dy,
		fp_stor_t len_x, fp_stor_t len_y,
		spfp_storarr_t points_pos, spfp_storarr_t points_param)
{
	spi_arr_t hit_idx = BS_KERNEL.create_object(int_arr_t::bs_type());
	coord_zcorn_pair r = czt::wave_mesh_deltas(
		max_dx, max_dy, len_x, len_y, points_pos, points_param,
		hit_idx
	);
	return make_tuple(r.first, r.second, hit_idx);
}

tuple wave_mesh(
	fp_stor_t max_dx, fp_stor_t max_dy,
	fp_stor_t len_x, fp_stor_t len_y, spfp_storarr_t points_pos, spfp_storarr_t points_param,
	int_t nz, spfp_storarr_t dz,
	fp_stor_t x0 = 0, fp_stor_t y0 = 0, fp_stor_t z0 = 0)
{
	int_t nx, ny;
	coord_zcorn_pair r = czt::wave_mesh(
		nx, ny, max_dx, max_dy, len_x, len_y, points_pos, points_param,
		nz, dz, x0, y0, z0
	);
	return make_tuple(r.first, r.second, nx, ny);
}

tuple wave_mesh_ij(
	spfp_storarr_t coord,
	int_t nx, int_t ny,
	fp_stor_t max_dx, fp_stor_t max_dy,
	spi_arr_t points_pos, spfp_storarr_t points_param,
	int_t nz, spfp_storarr_t dz,
	fp_stor_t x0 = 0, fp_stor_t y0 = 0, fp_stor_t z0 = 0)
{
	coord_zcorn_pair r = czt::wave_mesh(
		coord,
		nx, ny, max_dx, max_dy, points_pos, points_param,
		nz, dz, x0, y0, z0
	);
	return make_tuple(r.first, r.second, nx, ny);
}

spi_arr_t find_hit_idx1(
	spfp_storarr_t dx, spfp_storarr_t dy, spfp_storarr_t points_pos,
	fp_stor_t x0 = 0, fp_stor_t y0 = 0)
{
	return czt::find_hit_idx(dx, dy, points_pos, x0, y0);
}

spi_arr_t find_hit_idx2(
	uint_t nx, uint_t ny, spfp_storarr_t coord,
	spfp_storarr_t points_pos)
{
	return czt::find_hit_idx(nx, ny, coord, points_pos);
}

}  // eof hidden namespace

BOOST_PYTHON_FUNCTION_OVERLOADS(wave_mesh_deltas_s2_overl1, wave_mesh_deltas_s2_o1, 6, 8)
BOOST_PYTHON_FUNCTION_OVERLOADS(wave_mesh_deltas_s2_overl2, wave_mesh_deltas_s2_o2, 5, 7)
BOOST_PYTHON_FUNCTION_OVERLOADS(wave_mesh_overl, wave_mesh, 8, 11)
BOOST_PYTHON_FUNCTION_OVERLOADS(wave_mesh_ij_overl, wave_mesh_ij, 9, 12)
BOOST_PYTHON_FUNCTION_OVERLOADS(find_hit_idx_overl, find_hit_idx1, 3, 5)
BOOST_PYTHON_FUNCTION_OVERLOADS(gen_sgrid_overl, czt::gen_sgrid, 6, 10)
BOOST_PYTHON_FUNCTION_OVERLOADS(gen_sgrid_2d_overl, czt::gen_sgrid_2d, 4, 7)

namespace blue_sky { namespace python {

void py_export_czt() {
	def("gen_coord", &czt::gen_coord);
	def("gen_coord2", &czt::gen_coord2);
	def("gen_coord_zcorn", &czt::gen_coord_zcorn);

	def("wave_mesh_deltas_s1", &czt::wave_mesh_deltas_s1);
	def("wave_mesh_deltas_s2", &wave_mesh_deltas_s2_o1, wave_mesh_deltas_s2_overl1());
	def("wave_mesh_deltas_s2", &wave_mesh_deltas_s2_o2, wave_mesh_deltas_s2_overl2());
	def("wave_mesh_deltas", &wave_mesh_deltas);
	def("wave_mesh", &wave_mesh, wave_mesh_overl());
	def("wave_mesh", &wave_mesh_ij, wave_mesh_ij_overl());
	def("find_hit_idx", &find_hit_idx1, find_hit_idx_overl());
	def("find_hit_idx", &find_hit_idx2);
	def("refine_wave_mesh", &czt::refine_wave_mesh);
	def("refine_wave_mesh_deltas", &czt::refine_wave_mesh_deltas);

	spv_float (*tops2sgrid1)(uint_t, uint_t, spv_float) = &czt::tops2struct_grid;
	spv_float (*tops2sgrid2)(uint_t, uint_t, spv_float, spv_float) = &czt::tops2struct_grid;
	spv_float (*tops2sgrid3)(smart_ptr< rs_smesh_iface >) = &czt::tops2struct_grid;
	def("tops2struct_grid", tops2sgrid1);
	def("tops2struct_grid", tops2sgrid2);
	def("tops2struct_grid", tops2sgrid3);

	def("gen_sgrid", &czt::gen_sgrid, gen_sgrid_overl());
	def("gen_sgrid_2d", &czt::gen_sgrid_2d, gen_sgrid_2d_overl());
}

}} 	// eof blue_sky::python

#endif


///////////////////////////////////////////////////////////////////////////////
// statistics::model::wrap::unary::parameter.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_MODEL_WRAP_UNARY_PARAMETER_HPP_ER_2009
#define BOOST_STATISTICS_MODEL_WRAP_UNARY_PARAMETER_HPP_ER_2009
#include <boost/statistics/model/wrap/unary/detail/base.hpp>

namespace boost{
namespace statistics{
namespace model{

    template<typename X>
    class parameter_wrapper : detail::base_<X>{
        typedef detail::base_<X> base_;
    public:
        typedef X                       parameter_type;
        typedef parameter_wrapper<X>    parameter_wrapper_type;

        // Construction
        parameter_wrapper();
        parameter_wrapper(const X& x);
        parameter_wrapper(const parameter_wrapper&);

        // Access
        const X& parameter()const;
    };

    // Implementation //

    template<typename X>
    parameter_wrapper<X>::parameter_wrapper() : base_(){}

    template<typename X>
    parameter_wrapper<X>::parameter_wrapper( const X& x ) : base_(x){}

    template<typename X>
    parameter_wrapper<X>::parameter_wrapper(const parameter_wrapper& that)
    :base_(that){}

    template<typename X>
    const X& parameter_wrapper<X>::parameter()const{ return (this->value); }

    // Free functions

    template<typename X>
    parameter_wrapper<X>
    make_parameter_wrapper(const X& x){
        typedef parameter_wrapper<X> result_type;
        return result_type(x);
    }

}// model
}// statistics
}// boost

#endif


#include <chrono>
#include <iostream>

#include <boost/function.hpp>
#include <cxxopts.hpp>

#include <mrpt/poses/CPoint2D.h>
#include <mrpt/poses/CPose2D.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>

#include <a4wd2/config.h>

static const std::string log_prefix = "a4wd2_visualisation - ";
static constexpr int QUEUE_SIZE = 100;

class pose_tracker
{
public:
    using seconds = std::chrono::duration<double, std::chrono::seconds::period>;

    pose_tracker()
        : n_samples(0),
          a_x_sum(0),
          a_y_sum(0),
          v_phi_sum(0),
          m_timestamp(0),
          m_pose(0, 0, 0),
          v_x(0),
          v_y(0),
          v_phi(0)
    {
    }

    void accelerate_and_rotate(seconds timestamp, double a_x, double a_y, double vel_phi)
    {
        // Special case to avoid a glitch when no timestamp is available
        if (m_timestamp.count() == 0)
        {
            m_timestamp = timestamp;
            return;
        }

        n_samples++;
        a_x_sum += a_x;
        a_y_sum += a_y;
        v_phi_sum += v_phi;
        std::cout << "running avg a_x, a_y, v_phi: " << a_x_sum / n_samples << ", "
                  << a_y_sum / n_samples << ", " << v_phi_sum / n_samples << std::endl;

        double dt_sec = (timestamp - m_timestamp).count();
        std::cout << "dt_sec: " << dt_sec << std::endl;

        m_pose += mrpt::poses::CPose2D(v_x * dt_sec, v_y * dt_sec, v_phi * dt_sec);
        v_x += a_x * dt_sec;
        v_y += a_y * dt_sec;
        v_phi = vel_phi;

        m_timestamp = timestamp;
    }

    const mrpt::poses::CPose2D& get_pose() const { return m_pose; }

private:
    seconds m_timestamp;
    mrpt::poses::CPose2D m_pose;
    double v_x, v_y, v_phi;
    double a_x_sum, a_y_sum, v_phi_sum;
    int n_samples;
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "a4wd2_visualisation");
    ros::NodeHandle nh;
    ros::Subscriber imu_subscriber;

    // clang-format off
    cxxopts::Options options("a4wd2_visualisation", "ROS visualisation module for A4WD2");
    options.add_options()
        ("h,help", "Print this help")
        ("v,verbosity",
            "Verbosity level [trace, debug, info, warning, error, fatal]",
            cxxopts::value<std::string>()->default_value("disabled"));
    // clang-format on
    const char** parse_argv = const_cast<const char**>(argv);
    auto options_result = options.parse(argc, parse_argv);

    if (options_result.count("help"))
    {
        std::cout << options.help();
        return EXIT_SUCCESS;
    }
    if (!a4wd2::config::setup_verbosity(options_result))
    {
        std::cout << options.help();
        return EXIT_FAILURE;
    }

    pose_tracker tracker;
    boost::function<void(const sensor_msgs::Imu&)> callback =
            [&tracker](const sensor_msgs::Imu& imu_msg) {
                pose_tracker::seconds ts{imu_msg.header.stamp.toSec()};
                tracker.accelerate_and_rotate(ts, imu_msg.linear_acceleration.x,
                                              imu_msg.linear_acceleration.y,
                                              imu_msg.angular_velocity.z);
                std::cout << tracker.get_pose().asString() << std::endl;
            };

    imu_subscriber = nh.subscribe<sensor_msgs::Imu>("imu", QUEUE_SIZE, callback);
    ros::spin();

    return EXIT_SUCCESS;
}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/trigonometric/include/functions/cospi.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/mhalf.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/quarter.hpp>
#include <nt2/include/constants/sqrt_2.hpp>
#include <nt2/include/constants/sqrt_2o_2.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( cospi_real,  NT2_REAL_TYPES)
{
  using nt2::cospi;
  using nt2::tag::cospi_;

  typedef typename nt2::meta::call<cospi_(T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(cospi(nt2::Inf<T>()), nt2::Nan<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(cospi(nt2::Minf<T>()), nt2::Nan<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(cospi(nt2::Nan<T>()), nt2::Nan<r_t>(), 0.5);
#endif
  NT2_TEST_ULP_EQUAL(cospi(-nt2::Quarter<T>()), nt2::Sqrt_2o_2<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(cospi(nt2::Half<T>()), nt2::Zero<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(cospi(nt2::Mhalf<T>()), nt2::Zero<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(cospi(nt2::One<T>()), nt2::Mone<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(cospi(nt2::Quarter<T>()), nt2::Sqrt_2o_2<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(cospi(nt2::Zero<T>()), nt2::One<r_t>(), 0.5);
}

NT2_TEST_CASE_TPL ( cospi_unsigned_int__1_0,  NT2_UNSIGNED_TYPES)
{

  using nt2::cospi;
  using nt2::tag::cospi_;
  typedef typename nt2::meta::call<cospi_(T)>::type r_t;
  typedef typename boost::dispatch::meta::as_floating<T>::type wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_ULP_EQUAL(cospi(nt2::One<T>()), nt2::Mone<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(cospi(nt2::Zero<T>()), nt2::One<r_t>(), 0.5);
} // end of test for unsigned_int_

NT2_TEST_CASE_TPL ( cospi_signed_int__1_0,  NT2_INTEGRAL_SIGNED_TYPES)
{

  using nt2::cospi;
  using nt2::tag::cospi_;
  typedef typename nt2::meta::call<cospi_(T)>::type r_t;
  typedef typename boost::dispatch::meta::as_floating<T>::type wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_ULP_EQUAL(cospi(nt2::Mone<T>()), nt2::Mone<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(cospi(nt2::One<T>()), nt2::Mone<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(cospi(nt2::Zero<T>()), nt2::One<r_t>(), 0.5);
} // end of test for signed_int_


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_OPERATOR_FUNCTIONS_DETAILS_ASSERT_UTILS_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_DETAILS_ASSERT_UTILS_HPP_INCLUDED

#include <boost/simd/include/functions/simd/extract.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/allbits.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>

namespace boost { namespace simd
{

  template<class T>
  bool assert_all( T const& t )
  {
    for(std::size_t i = 0; i != meta::cardinal_of<T>::value; ++i)
      if(!extract(t, i))
        return false;
    return true;
  }

  template<class T>
  bool assert_is_mask( T const& t )
  {
    for(std::size_t i = 0; i != meta::cardinal_of<T>::value; ++i)
    {
      typedef typename meta::scalar_of<T>::type sT;
      typedef typename dispatch::meta::as_integer<sT, unsigned>::type uT;

      sT v = extract(t, i);
      uT uv = bitwise_cast<uT>(v);

      if(uv != Zero<uT>() && uv != Allbits<uT>())
        return false;
    }
    return true;
  }

} }

#endif


/**
 * Dumps out a csv file with information on ring hypergraphs
 */

#include <ostream>
#include <istream>
#include <iostream>
#include <chrono>
#include <sstream>
#include <vector>

#include <boost/hana.hpp>
#include <hypergraph/approx.hpp>
#include <generators/generators.hpp>
#include <hypergraph/certificate.hpp>

namespace hana = boost::hana;

struct InputInfo {
  BOOST_HANA_DEFINE_STRUCT(
      InputInfo,
      (size_t, num_vertices),
      (size_t, num_edges),
      (double, radius),
      (uint64_t, seed),
      (double, epsilon),

      (size_t, min_cut_value),
      (size_t, eps_cut_value),
      (size_t, size_before),
      (size_t, size_after),
      (size_t, p1),
      (size_t, p2)
  );
  /*
  size_t num_vertices;
  size_t num_edges;
  double radius;
  double epsilon;
  // Suboptimality factor of cut found by running the approximate algorithm
  double suboptimality_factor;
  size_t size_before;
  // Size after sparsifying with the cut value found by the approximate algorithm
  size_t size_after;
  // Size of partition 1
  size_t p1;
  // Size of partition 2
  size_t p2;
   */

  static std::string header() {
    std::stringstream s;
    hana::for_each(InputInfo{}, hana::fuse([&s](const auto &name, const auto &member) {
      s << name.c_str() << ",";
    }));
    std::string str = s.str();
    return str.substr(0, str.size() - 1);
  }
};

// Comma delimit
template<typename T>
std::string comma_delimit(const T &data) {
  std::stringstream s;
  hana::for_each(data, hana::fuse([&s](const auto &name, const auto &member) {
    s << member << ",";
  }));
  std::string str = s.str();
  return str.substr(0, str.size() - 1);
}

std::ostream &operator<<(std::ostream &out, const InputInfo &info) {
  out << comma_delimit(info) << std::endl;
  return out;
}

/*
std::istream &operator>>(std::istream &in, InputInfo &info) {
  std::string line;
  std::getline(in, line);
  std::stringstream line_stream(line);
  auto map = hana::to<hana::map_tag>(info);
  hana::for_each(info, hana::fuse([&line_stream, &map](auto name, auto member) {
    std::string field;
    std::getline(line_stream, field, ',');
    std::stringstream field_stream(field);
    decltype(member) m;
    field_stream >> m;
    map[name] = m;
  }));

  info.num_vertices = map[BOOST_HANA_STRING("num_vertices")];
  info.num_edges = map[BOOST_HANA_STRING("num_edges")];

  return in;
}
 */

struct Timer {
  using TimePoint = decltype(std::chrono::high_resolution_clock::now());

  Timer() {
    start = std::chrono::high_resolution_clock::now();
  }

  auto stop() {
    return std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::high_resolution_clock::now() - start).count();
  }
private:
  TimePoint start;
};

// Compare just the input parts of InputInfo
struct CompareInput {
  static auto to_tuple(const InputInfo &a) {
    return std::make_tuple(a.num_vertices, a.num_edges, a.radius);
  }

  bool operator()(const InputInfo &a, const InputInfo &b) const {
    return to_tuple(a) < to_tuple(b);
  }
};

hypergraphlib::HypergraphCut<size_t> memoized_cut(const InputInfo &info, const hypergraphlib::Hypergraph &hypergraph) {
  static std::map<InputInfo, hypergraphlib::HypergraphCut<size_t>, CompareInput> cuts;

  auto it = cuts.find(info);
  if (it == cuts.end()) {
    hypergraphlib::Hypergraph h(hypergraph);
    const auto cut = hypergraphlib::MW_min_cut(h);
    cuts.insert({info, cut});
    return cut;
  }
  return it->second;
}

int main() {
  std::vector<InputInfo> infos;

  for (size_t num_vertices = 100; num_vertices <= 500; num_vertices += 25) {
    for (size_t num_edges = 100; num_edges <= num_vertices * 30; num_edges += 250) {
      for (double radius = 5; radius <= 90; radius += 5) {
        for (double epsilon: {0.1, 0.2, 0.4, 0.8, 1., 2., 4., 8., 16., 32., 64., 128.}) {
          for (int i = 0; i < 10; ++i) {
            std::mt19937_64 rd;
            std::uniform_int_distribution<uint64_t> dis;
            uint64_t seed;
            InputInfo info = {
                .num_vertices = num_vertices,
                .num_edges = num_edges,
                .radius = radius,
                .seed = dis(rd),
                .epsilon = epsilon
            };
            infos.emplace_back(std::move(info));
          }
        }
      }
    }
  }

  std::cout << InputInfo::header() << std::endl;

  for (auto &info : infos) {
    RandomRingConstantEdgeHypergraph gen(info.num_vertices, info.num_edges, info.radius, 777);
    auto[h, _] = gen.generate();
    // TODO do this before in generate or something
    h.remove_singleton_and_empty_hyperedges();
    const hypergraphlib::Hypergraph hypergraph(h);

    const auto cut = memoized_cut(info, hypergraph);
    hypergraphlib::Hypergraph temp2(hypergraph);
    const auto eps_cut = hypergraphlib::approximate_minimizer(temp2, info.epsilon);
    hypergraphlib::KTrimmedCertificate k(hypergraph);
    auto hypergraph_after = k.certificate(eps_cut.value);

    info.eps_cut_value = eps_cut.value;
    info.min_cut_value = cut.value;
    info.size_before = hypergraph.size();
    info.size_after = hypergraph_after.size();
    info.p1 = cut.partitions.at(0).size();
    info.p2 = cut.partitions.at(1).size();

    std::cout << info;
  }
}


// Copyright (c) fortiss GmbH
//
// Authors: Patrick Hart, Julian Bernhard, Klemens Esterle, and
// Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef BARK_ML_OBSERVERS_FRENET_OBSERVER_HPP_
#define BARK_ML_OBSERVERS_FRENET_OBSERVER_HPP_

#include <memory>
#include <vector>
#include <tuple>
#include <map>
#include <functional>
#include <Eigen/Dense>

#include "bark/commons/params/params.hpp"
#include "bark/world/world.hpp"
#include "bark/world/observed_world.hpp"
#include "bark/world/goal_definition/goal_definition_state_limits_frenet.hpp"
#include "bark/models/dynamic/dynamic_model.hpp"
#include "bark_ml/commons/spaces.hpp"
#include "bark_ml/commons/commons.hpp"
#include "bark_ml/observers/base_observer.hpp"

namespace bark_ml {
namespace observers {

using bark::commons::ParamsPtr;
using bark::world::Agent;
using spaces::Box;
using commons::Norm;
using spaces::Matrix_t;
using bark::world::AgentMap;
using bark::world::AgentPtr;
using bark::world::objects::AgentId;
using bark::world::WorldPtr;
using bark::world::goal_definition::GoalDefinitionStateLimitsFrenet;
using bark::world::map::LaneCorridorPtr;
using bark::world::ObservedWorldPtr;
using bark::world::ObservedWorld;
using bark::geometry::Point2d;
using bark::commons::transformation::FrenetState;
using bark::commons::transformation::FrenetStateDifference;
using bark::geometry::B_2PI;
using bark::geometry::B_PI;
using bark::geometry::Line;
using bark::geometry::Distance;
using bark::geometry::NormToPI;
using bark::models::dynamic::StateDefinition::X_POSITION;
using bark::models::dynamic::StateDefinition::Y_POSITION;
using bark::models::dynamic::StateDefinition::THETA_POSITION;
using bark::models::dynamic::StateDefinition::VEL_POSITION;
using bark::commons::transformation::FrenetPosition;

class FrenetObserver {
 public:
  explicit FrenetObserver(const ParamsPtr& params) {
      nearest_agent_num_ =
        params->GetInt(
          "ML::FrenetObserver::NNearestAgents", "Nearest agents number", 2);
      min_vel_lon_ = params->GetReal("ML::FrenetObserver::MinVelLon", "", -20.0);
      max_vel_lon_ = params->GetReal("ML::FrenetObserver::MaxVelLon", "", 20.0);
      min_vel_lat_ = params->GetReal("ML::FrenetObserver::MinVelLat", "", -10.0);
      max_vel_lat_ = params->GetReal("ML::FrenetObserver::MaxVelLat", "", 10.0);
      max_dist_ = params->GetReal("ML::FrenetObserver::MaxDist", "", 75.0);
      min_s_ = params->GetReal("ML::FrenetObserver::MinS", "", 0.0);
      max_s_ = params->GetReal("ML::FrenetObserver::MaxS", "", 100.0);
      min_d_ = params->GetReal("ML::FrenetObserver::MinD", "", -10.0);
      max_d_ = params->GetReal("ML::FrenetObserver::MaxD", "", 10.0);
      min_theta_ = params->GetReal("ML::FrenetObserver::MinTheta", "", -B_PI);
      max_theta_ = params->GetReal("ML::FrenetObserver::MaxTheta", "", B_PI);
      use_rel_s_values_ = params->GetBool(
        "ML::FrenetObserver::RelativeLonValues", "", false);
      observation_len_ = nearest_agent_num_ * 4 + 5;
  }

  double Norm(const double val, const double mi, const double ma) const {
    LOG_IF_EVERY_N(WARNING, (val < mi), 100) << "Val=" << val <<
      " < Lower Limit=" << mi;
    LOG_IF_EVERY_N(WARNING, (val > ma), 100) << "Val=" << val <<
      " > Upper Limit=" << ma;
    // Normalize to be within [-1, 1]
    return (val - (mi+ma)/2.0)/((ma-mi)/2.0);
  }

  FrenetState GetEgoFrenet(const ObservedWorld& observed_world) const {
    const auto ego_state = observed_world.CurrentEgoState();
    const auto ego_pos = observed_world.CurrentEgoPosition();
    const auto ego_corridor = observed_world.GetEgoAgent()
      ->GetRoadCorridor()->GetNearestLaneCorridor(ego_pos);
    FrenetState current_ego_frenet(ego_state, ego_corridor->GetCenterLine());
    return current_ego_frenet;
  }

  ObservedState GetEgoState(const ObservedWorld& observed_world) const {
    const auto current_ego_frenet = GetEgoFrenet(observed_world);
    ObservedState ego_nn_state(1, 5);
    const double normalized_angle = NormToPI(current_ego_frenet.angle);
    double ego_s = 0.0;
    if (use_rel_s_values_) {
      ego_s -= current_ego_frenet.lon;
    }
    ego_nn_state << Norm(current_ego_frenet.lon + ego_s, min_s_, max_s_),
                    Norm(current_ego_frenet.lat, min_d_, max_d_),
                    Norm(normalized_angle, min_theta_, max_theta_),
                    Norm(current_ego_frenet.vlon, min_vel_lon_, max_vel_lon_),
                    Norm(current_ego_frenet.vlat, min_vel_lat_, max_vel_lat_);
    return ego_nn_state;
  }

  ObservedState GetOtherAgentState(
    const AgentId& agent_id, const ObservedWorld& observed_world) const {
    const auto ego_pos = observed_world.CurrentEgoPosition();
    const auto lane_corridor = observed_world.GetEgoAgent()
      ->GetRoadCorridor()->GetNearestLaneCorridor(ego_pos);
    const auto ego_state = observed_world.CurrentEgoState();
    FrenetState current_ego_frenet(
      ego_state, lane_corridor->GetCenterLine());
    const auto ego_shape = observed_world.GetEgoAgent()->GetShape();
    const auto& other_state = observed_world.GetAgent(
      agent_id)->GetCurrentState();
    FrenetState other_frenet(
      other_state, lane_corridor->GetCenterLine());
    const auto other_shape = observed_world.GetAgent(agent_id)->GetShape();
    FrenetStateDifference state_diff(
      current_ego_frenet, ego_shape, other_frenet, other_shape);

    double ego_s = 0.0;
    if (use_rel_s_values_) {
      const auto current_ego_frenet = GetEgoFrenet(observed_world);
      ego_s -= current_ego_frenet.lon;
    }
    ObservedState other_nn_state(1, 4);
    other_nn_state <<
      Norm(state_diff.lon_zeroed ? 0.0 : state_diff.lon + ego_s, min_s_, max_s_),
      Norm(state_diff.lat_zeroed ? 0.0 : state_diff.lat, min_d_, max_d_),
      Norm(state_diff.vlon, min_vel_lon_, max_vel_lon_),
      Norm(state_diff.vlat, min_vel_lat_, max_vel_lat_);
    return other_nn_state;
  }

  ObservedState Observe(const ObservedWorld& observed_world) const {
    // find near agents (n)
    AgentMap nearest_agents = observed_world.GetNearestAgents(
      observed_world.CurrentEgoPosition(), nearest_agent_num_ + 1);

    // Build state
    int state_start_idx = 0;
    ObservedState state(1, observation_len_);
    state.setZero();

    // add ego agent state
    ObservedState obs_ego_agent_state = GetEgoState(observed_world);
    state.block(
      0, state_start_idx, 1, obs_ego_agent_state.cols()) = obs_ego_agent_state;
    state_start_idx = obs_ego_agent_state.cols();

    // add other states
    for (const auto& agent : nearest_agents) {
      if(agent.first == observed_world.GetEgoAgentId()) continue;

      const auto& agent_state = agent.second->GetCurrentPosition();
      double distance = Distance(
        observed_world.CurrentEgoPosition(), agent_state);
      if (distance > max_dist_) continue;

      ObservedState other_agent_state = GetOtherAgentState(
        agent.second->GetAgentId(), observed_world);
      state.block(
        0, state_start_idx, 1, other_agent_state.cols()) = other_agent_state;
      state_start_idx += other_agent_state.cols();
    }
    return state;
  }

  WorldPtr Reset(const WorldPtr& world) {
    return world;
  }

  Box<double> ObservationSpace() const {
    Matrix_t<double> low(1, observation_len_);
    low.setOnes();
    low = -1*low;
    Matrix_t<double> high(1, observation_len_);
    high.setOnes();
    std::tuple<int> shape{observation_len_};
    return Box<double>(low, high, shape);
  }

 private:
  int nearest_agent_num_, observation_len_;
  double min_theta_, max_theta_, min_vel_lon_,
         max_vel_lon_, min_vel_lat_,
         max_vel_lat_, max_dist_,
         min_d_, max_d_, min_s_, max_s_;
  bool use_rel_s_values_;
};

}  // namespace observers
}  // namespace bark_ml

#endif  // BARK_ML_OBSERVERS_FRENET_OBSERVER_HPP_


#include <boost/preprocessor/arithmetic/sub.hpp>


#include "CallChainChecker.hpp"

#include <llvm/Support/raw_ostream.h>
#include <unordered_map>

#include <clang/AST/Decl.h>
#include <clang/ASTMatchers/ASTMatchers.h>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/property_map/property_map.hpp>

using namespace clang;
using namespace clang::ast_matchers;

using namespace boost;

using StaticCallGraph = adjacency_list<vecS, vecS, directedS>;

namespace ccc {

bool callChainExists(const std::vector<CallPair> &Calls, std::string From,
                     std::string To) {
    std::unordered_map<std::string, size_t> FunctionToIdx;
    size_t idx = 0;
    StaticCallGraph SCG;
    for (const auto &[Caller, Callee] : Calls) {
        if (not FunctionToIdx.count(Caller)) {
            SCG.added_vertex(idx);
            FunctionToIdx[Caller] = idx++;
        }
        if (not FunctionToIdx.count(Callee)) {
            SCG.added_vertex(idx);
            FunctionToIdx[Callee] = idx++;
        }
        boost::add_edge(FunctionToIdx[Caller], FunctionToIdx[Callee], SCG);
    }
    if (not FunctionToIdx.count(From)) {
        llvm::errs() << From << " is not part of the call graph\n";
        return false;
    }
    if (not FunctionToIdx.count(To)) {
        llvm::errs() << To << " is not part of the call graph\n";
        return false;
    }

    std::vector<default_color_type> Colors(num_vertices(SCG));
    iterator_property_map ColorMap(Colors.begin(),
                                   boost::get(boost::vertex_index, SCG));
    breadth_first_search(SCG, FunctionToIdx.at(From), color_map(ColorMap));
    return Colors[FunctionToIdx.at(To)] == default_color_type::black_color;
}

void CallChainCollector::registerMatchers(
    clang::ast_matchers::MatchFinder &Finder) {
    Finder.addMatcher(callExpr(clang::ast_matchers::isExpansionInMainFile(),
                               callee(functionDecl().bind("callee")),
                               hasAncestor(functionDecl().bind("caller"))),
                      this);
}

void CallChainCollector::run(
    const clang::ast_matchers::MatchFinder::MatchResult &Result) {
    if (const auto *Callee = Result.Nodes.getNodeAs<FunctionDecl>("callee"))
        if (const auto *Caller = Result.Nodes.getNodeAs<FunctionDecl>("caller"))
            Calls.emplace_back(Caller->getNameAsString(),
                               Callee->getNameAsString());
}

} // namespace ccc


#ifndef DCS_DES_CLOUD_CONFIG_OPERATION_MAKE_ENERGY_MODEL_HPP
#define DCS_DES_CLOUD_CONFIG_OPERATION_MAKE_ENERGY_MODEL_HPP

#include <boost/variant.hpp>
#include <dcs/des/cloud/config/energy_model.hpp>
#include <dcs/memory.hpp>
#include <dcs/perfeval/energy.hpp>

namespace dcs { namespace des { namespace cloud { namespace config {

template <typename RealT, typename VariantT>
::dcs::shared_ptr<
	::dcs::perfeval::energy::base_model<RealT>
> make_energy_model(energy_model_category category,
					VariantT const& variant_config)
{
	typedef ::dcs::perfeval::energy::base_model<RealT> base_model_type;

	::dcs::shared_ptr<base_model_type> ptr_model;

	switch (category)
	{
		case constant_energy_model:
			{
				typedef constant_energy_model_config<RealT> model_config_type;
				typedef ::dcs::perfeval::energy::constant_model<RealT> model_type;
				model_config_type const& model = ::boost::get<model_config_type>(variant_config);
				ptr_model = ::dcs::make_shared<model_type>(model.c0);
			}
			break;
		case fan2007_energy_model:
			{
				typedef fan2007_energy_model_config<RealT> model_config_type;
				typedef ::dcs::perfeval::energy::fan2007_model<RealT> model_type;
				model_config_type const& model = ::boost::get<model_config_type>(variant_config);
				ptr_model = ::dcs::make_shared<model_type>(
							model.c0,
							model.c1,
							model.c2,
							model.r
					);
			}
			break;
	}

	return ptr_model;
}

}}}} // Namespace dcs::des::cloud::config

#endif // DCS_DES_CLOUD_CONFIG_OPERATION_MAKE_ENERGY_MODEL_HPP


/*
BSD 2-Clause License

Copyright (c) 2021, Young-min Song,
Machine Learning and Vision Lab (https://sites.google.com/view/mlv/),
Gwangju Institute of Science and Technology(GIST), South Korea.
All rights reserved.

This software is an implementation of the GMPHD_MAF tracker,
which not only refers to the paper entitled
"Online Multi-Object Tracking and Segmentation with GMPHD Filter and Mask-Based Affinity Fusion"
but also is available at https://github.com/SonginCV/GMPHD_MAF.

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

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

2. 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.

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

// user-defined pre-processor,...
// user-defined containters, data-type,..

// #include <opencv2\opencv.hpp>

#include <opencv2\core.hpp>
#include <opencv2\highgui.hpp>
#include "opencv2\video\tracking.hpp"	// Kalman Filtering
#include <opencv2/imgproc.hpp>			// image transformation

// IOU between 3D Boxes
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>

#include "VOT.h"
#include "kcftracker.hpp"
#include <unordered_map>
//#include "siamRPN_tracker.hpp"
//#include "DaSiamTracker.h"

#define MAX_OBJECTS	27

#define SIZE_CONSTRAINT_RATIO	2

// Parameters for the GMPHD filter
#define PI_8				3.14159265
#define e_8				2.71828182
#define T_th				(0.0)
#define W_th				(0.0)
#define Q_TH_LOW_20			0.00000000000000000001f		// 0.1^20
#define Q_TH_LOW_20_INVERSE		100000000000000000000.0f	// 10^20
#define Q_TH_LOW_15			0.00000000000001f		// 0.1^15
#define Q_TH_LOW_15_INVERSE		1000000000000000.0f		// 10^15
#define Q_TH_LOW_10			0.0000000001f			// 0.1^10
#define Q_TH_LOW_10_INVERSE		10000000000.0f			// 10^10
#define Q_TH_LOW_8			0.00000001f			// 0.1^8
#define Q_TH_LOW_8_INVERSE		100000000.0f			// 10^8
#define P_SURVIVE_LOW			0.99				// object number >=2 : 0.99, else 0.95
#define P_SURVIVE_MID			0.95
#define VAR_X				25 //42.96 // 279.13 // 1116.520// 0.1 //25 // 171.87
#define VAR_Y				100//29.27// 47.685 // 190.740 //0.1 //100 // 117.09
#define VAR_D				25//0.1 //25
#define VAR_R				0.09 // 0.1, 0.04, 0.01
#define VAR_X_VEL			25// 42.96 // 279.13 // 1116.520// 0.1 //25 // 171.871
#define VAR_Y_VEL			100// 29.27//47.685 // 190.740//0.1//100 // 117.090
#define VAR_D_VEL			25//0.1 // 25	// 1, 4, 9, 16, 25, 100, 225, 400, 900
#define VAR_R_VEL			100 // 0.1, 0.04, 0.01
#define VAR_WIDTH			100
#define VAR_HEIGHT			400
#define VAR_X_MID			100
#define VAR_Y_MID			400
#define VAR_X_VEL_MID			100
#define VAR_Y_VEL_MID			400

//#define VELOCITY_UPDATE_ALPHA	0.4f // moving camera scene with low fps ( <= 15)
#define CONFIDENCE_UPDATE_ALPHA	0.95f

// Parameters for Data Association
#define TRACK_ASSOCIATION_FRAME_DIFFERENCE	0	// 0: equal or later, 1:later
#define ASSOCIATION_STAGE_1_GATING_ON		0
#define ASSOCIATION_STAGE_2_GATING_ON		0
#define AFFINITY_COST_L1_OR_L2_NORM_FW		1	// 0: L1-norm, 1: L2-norm, frame-wise
#define AFFINITY_COST_L1_OR_L2_NORM_TW		1	// 0: L1-norm, 1: L2-norm, tracklet-wise
// SOT Tracker Options
#define SOT_USE_KCF_TRACKER			1
#define SOT_USE_SIAMRPN_TRACKER			2
#define SOT_USE_DASIAMRPN_TRACKER		3
#define SOT_TRACK_OPT				SOT_USE_KCF_TRACKER

// (D2TA) GMPHD-KCF Fusion (D2TA)
#define APPEARANCE_STRICT_UPDATE_ON		0
#define APPEARANCE_UPDATE_DET_TH		0.85f

// (T2TA) GMPHD-KCF Fusion (T2TA)
//#define SAF_MASK_T2TA_ON			1

// #define KCF_SOT_T2TA_ON			0
//#define IOU_UPPER_TH_T2TA_ON			1

// MOTS
#define COV_UPDATE_T2TA				0

#define USE_LINEAR_MOTION			0
#define USE_KALMAN_MOTION			1
#define T2TA_MOTION_OPT				USE_LINEAR_MOTION
#define KALMAN_INIT_BY_LINEAR			1

// Parameters for Occlusion Group Management (Merge and Occlusion Group Energy Minimization)
#define MERGE_DET_ON			1
#define MERGE_TRACK_ON			1
#define MERGE_METRIC_OPT		2	// 0: SIOA, 1:IOU, 2:sIOU
#define MERGE_THRESHOLD_RATIO	0.4f
#define MERGE_METRIC_SIOA		0
#define MERGE_METRIC_IOU		1
#define MERGE_METRIC_mIOU		2

#define GROUP_MANAGEMENT_FRAME_WISE_ON	1

#define USE_GMPHD_NAIVE			0
#define USE_GMPHD_HDA			1
#define GMPHD_TRACKER_MODE		USE_GMPHD_HDA

// Tracking Results Writing Option
#define EXCLUDE_BBOX_OUT_OF_FRAME	0

// Visualization Option (main function)
#define VISUALIZATION_MAIN_ON		1
#define SKIP_FRAME_BY_FRAME		0
#define VISUALIZATION_RESIZE_ON		0
#define BOUNDING_BOX_THICK		5
#define TRAJECTORY_THICK		5
#define	ID_CONFIDENCE_FONT_SIZE		2
#define	ID_CONFIDENCE_FONT_THICK	2
#define FRAME_COUNT_FONT_SIZE		3
#define FRAME_COUNT_FONT_THICK		3
// Visualization Option (tracker class)
#define VIS_D2TA_DETAIL			0
#define VIS_T2TA_DETAIL			0
#define VIS_TRACK_MERGE			0

// Image Save Option (main function)
#define SAVE_IMG_ON			0

// Dataset Indices
#define DB_TYPE_MOT15			0	// MOT Challenge 2015 Dataset (-1)
#define DB_TYPE_MOT17			1	// MOT Challenge 2017 Dataset
#define DB_TYPE_CVPR19			2   	// MOT Challenge 2019 (CVPR 2019) Dataset
#define DB_TYPE_MOT20			2   	// MOT Challenge 2020
#define DB_TYPE_KITTI			3	// KITTI 	(2D box, 3D box, 3D Point Cloud)
#define DB_TYPE_KITTI_MOTS		4	// KITTI-MOTS 	(2D Instance Segment)
#define DB_TYPE_MOTS20			5	// MOTS20	(2D Instacne Segment)

#define DEBUG_PRINT			0

/*--------------------------------------------------------------------------------------*/

typedef struct boundingbox_id {
	int id;
	int idx;
	int min_id;		// minimum value ID in a group (is considered as group ID)
	double weight = 0.0;
	cv::Rect rec;	// t, t-1, t-2
	boundingbox_id() {
	}
	boundingbox_id(int id, cv::Rect occRect = cv::Rect()) :id(id) {
		// Deep Copy
		id = id;
		rec = occRect;
	}
	boundingbox_id& operator=(const boundingbox_id& copy) { // overloading the operator = for deep copy
		if (this == &copy) // if same instance (mermory address)
			return *this;

		this->idx = copy.idx;
		this->id = copy.id;
		this->min_id = copy.min_id;
		this->rec = copy.rec;
		this->weight = copy.weight;
	}
	bool operator<(const boundingbox_id& rect) const {
		return (id < rect.id);
	}
}RectID;

typedef struct track_id_bb_mask {
	int id;
	int fn;
	int object_type;
	cv::Rect bb;
	cv::Mat mask;

	//track_id_bb_mask() {

	//}
	//track_id_bb_mask(int id_, int fn_, cv::Rect bb, cv::Mat mask_=cv::Mat()) {
	//	this->id = id_;
	//	this->fn = fn_;
	//	this->bb = bb;

	//	this->mask = mask_.clone();
	//}

	track_id_bb_mask& operator=(const track_id_bb_mask& copy) { // overloading the operator = for deep copy
		if (this == &copy) // if same instance (mermory address)
			return *this;

		this->id = copy.id;
		this->fn = copy.fn;
		this->object_type = copy.object_type;
		this->bb = copy.bb;

		if (!this->mask.empty()) this->mask.release();
		this->mask = copy.mask.clone();

		return *this;
	}
} track_info;

typedef struct bbTrack {
	//VOT *papp ;
	KCFTracker papp;
	cv::Mat segMask;
	std::string segMaskRle;
	std::vector<float> reid_features;

	int fn;
	int id;
	int id_associated = -1; // (index) it is able to be used in Tracklet-wise association
	/*----MOTS Tracking Only---*/
	int det_id;
	float det_confidence;
	/*----MOTS Tracking Only---*/
	int fn_latest_T2TA = 0;
	cv::Rect rec;
	cv::Rect rec_corr;
	float rec6D[6];
	std::vector<cv::Vec3f> top_corners;
	std::vector<cv::Vec3f> bottom_corners;
	float vx;
	float vy;
	float vd;
	float vr;
	float weight;
	float conf;
	cv::KalmanFilter KF;
	cv::Mat cov;
	cv::Mat tmpl;
	cv::Mat hist;
	float density;
	float depth;
	float ratio_yx;
	float rotation_y;
	bool isNew;
	bool isAlive;
	bool isMerged = false;
	int isInterpolated = 0; // 0: Online, 1: Interpolated, -1:Ignored (Falsely Interpolated)
	int iGroupID = -1;
	bool isOcc = false;
	int size = 0;
	int objType;
	std::vector<RectID> occTargets;
	bbTrack() {}
	bbTrack(int fn, int id, int isOcc, cv::Rect rec, cv::Mat obj = cv::Mat(), cv::Mat hist = cv::Mat()) :
		fn(fn), id(id), isOcc(isOcc), rec(rec) {
		if (!hist.empty()) {
			this->hist.release();
			this->hist = hist.clone(); // deep copy
		}
		else {
			//this->hist.release();
			//printf("[ERROR]target_bb's parameter \"hist\" is empty!\n");
			this->hist = hist;
		}
		if (!obj.empty()) {
			this->tmpl.release();
			this->tmpl = obj.clone(); // deep copy
		}
		else {
			//this->obj_tmpl.release();
			this->tmpl = obj;
		}
		//isOccCorrNeeded = false; // default

	}
	bool operator<(const bbTrack& trk) const {
		return (id < trk.id);
	}
	bbTrack& operator=(const bbTrack& copy) { // overloading the operator = for deep copy
		if (this == &copy) // if same instance (mermory address)
			return *this;

		//this->KCFfeatures = copy.KCFfeatures;
		this->papp = copy.papp;
		if (!this->segMask.empty()) this->segMask.release();
		if (!copy.segMask.empty())	this->segMask = copy.segMask;
		if (!this->reid_features.empty()) this->reid_features.clear();
		this->reid_features = copy.reid_features;

		this->fn = copy.fn;
		this->id = copy.id;
		this->id_associated = copy.id_associated;
		this->size = copy.size;
		this->fn_latest_T2TA = copy.fn_latest_T2TA;
		this->rec = copy.rec;
		this->rec6D[0] = copy.rec6D[0];
		this->rec6D[1] = copy.rec6D[1];
		this->rec6D[2] = copy.rec6D[2];
		this->rec6D[3] = copy.rec6D[3];
		this->rec6D[4] = copy.rec6D[4];
		this->rec6D[5] = copy.rec6D[5];
		this->top_corners = copy.top_corners;
		this->bottom_corners = copy.bottom_corners;
		this->vx = copy.vx;
		this->vy = copy.vy;
		this->vd = copy.vd;
		this->vr = copy.vr;
		this->rec_corr = copy.rec_corr;
		this->density = copy.density;
		this->depth = copy.depth;
		this->rotation_y = copy.rotation_y;
		this->ratio_yx = copy.ratio_yx;
		this->isNew = copy.isNew;
		this->isAlive = copy.isAlive;
		this->isMerged = copy.isMerged;
		this->isInterpolated = copy.isInterpolated;
		this->iGroupID = copy.iGroupID;
		this->isOcc = copy.isOcc;
		this->weight = copy.weight;
		this->conf = copy.conf;
		this->objType = copy.objType;
		this->segMaskRle = copy.segMaskRle;

		if (!this->cov.empty()) this->cov.release();
		if (!this->tmpl.empty()) this->tmpl.release();
		if (!this->hist.empty()) this->hist.release();

		this->cov = copy.cov.clone();
		this->tmpl = copy.tmpl.clone();
		this->hist = copy.hist.clone();

		/*----MOTS Tracking Only---*/
		this->det_id = copy.det_id;
		this->det_confidence = copy.det_confidence;
		/*----MOTS Tracking Only---*/

		return *this;
	}
	void CopyTo(bbTrack& dst) {
		dst.papp = this->papp;
		if (!this->segMask.empty()) dst.segMask = this->segMask.clone();
		if (!this->reid_features.empty()) dst.reid_features = this->reid_features;

		dst.fn = this->fn;
		dst.id = this->id;
		dst.id_associated = this->id_associated;
		dst.rec = this->rec;
		dst.rec6D[0] = this->rec6D[0];
		dst.rec6D[1] = this->rec6D[1];
		dst.rec6D[2] = this->rec6D[2];
		dst.rec6D[3] = this->rec6D[3];
		dst.rec6D[4] = this->rec6D[4];
		dst.rec6D[5] = this->rec6D[5];
		dst.top_corners = this->top_corners;
		dst.bottom_corners = this->bottom_corners;
		dst.vx = this->vx;
		dst.vy = this->vy;
		dst.vd = this->vd;
		dst.vr = this->vr;
		dst.rec_corr = this->rec_corr;
		dst.density = this->density;
		dst.depth = this->depth;
		dst.rotation_y = this->rotation_y;
		dst.isNew = this->isNew;
		dst.isAlive = this->isAlive;
		dst.isMerged = this->isMerged;
		dst.isInterpolated = this->isInterpolated;
		dst.iGroupID = this->iGroupID;
		dst.isOcc = this->isOcc;
		dst.weight = this->weight;
		dst.conf = this->conf;
		dst.objType = this->objType;
		dst.segMaskRle = this->segMaskRle;

		if (!this->cov.empty()) dst.cov = this->cov.clone();
		if (!this->tmpl.empty()) dst.tmpl = this->tmpl.clone();
		if (!this->hist.empty()) dst.hist = this->hist.clone();

		/*----MOTS Tracking Only-- - */
		dst.det_id = this->det_id;
		dst.det_confidence = this->det_confidence;
		/*----MOTS Tracking Only---*/
	}
	void Destroy() {
		if (!this->cov.empty()) this->cov.release();
		if (!this->tmpl.empty()) this->tmpl.release();
		if (!this->hist.empty()) this->hist.release();
	}
	void InitKF(int nStats, int mObs, float x_var, float y_var) {
		this->KF = cv::KalmanFilter(nStats, mObs, 0);

		// F, constant
		setIdentity(KF.transitionMatrix);
		this->KF.transitionMatrix.at<float>(0, 2) = 1;
		this->KF.transitionMatrix.at<float>(1, 3) = 1;

		// H, constant
		this->KF.measurementMatrix = (cv::Mat_<float>(2, 4) << 1, 0, 0, 0, 0, 1, 0, 0);

		setIdentity(this->KF.processNoiseCov, cv::Scalar::all(x_var / 2));	// Q, constant, 4^2/2, 4x4
		this->KF.processNoiseCov.at<float>(1, 1) = y_var / 2;
		this->KF.processNoiseCov.at<float>(3, 3) = y_var / 2;
		//cout << this->KF.processNoiseCov.rows << "x" << this->KF.processNoiseCov.cols << endl;

		setIdentity(this->KF.measurementNoiseCov, cv::Scalar::all(x_var));	// R, constant, 4^2, 2x2
		this->KF.measurementNoiseCov.at<float>(1, 1) = y_var;
		//cout << this->KF.measurementNoiseCov.rows << "x" << this->KF.measurementNoiseCov.cols << endl;

		setIdentity(this->KF.errorCovPre, cv::Scalar::all(1));				// P_t|t-1, predicted from P_t-1
		setIdentity(this->KF.errorCovPost, cv::Scalar::all(1));				// P_0 or P_t,	just for init (variable)
	}
}BBTrk;

typedef struct bbDet {
	int fn;
	int object_type = -1;
	cv::Rect rec;
	float depth;		// used to represent the 2.5D Bounding Box (2D + Depth from Point Cloud)
	float rotation_y;
	float ratio_yx;
	float rec6D[6];
	std::vector<cv::Vec3f> top_corners;
	std::vector<cv::Vec3f> bottom_corners;
	std::vector<float> distances;
	float confidence;
	float weight;		// normalization value of confidence at time t
	int id;				// Used in Looking Back Association
	int input_type;		// 2D Box, 3D Box, 3D Point Cloud, 2D Instance
	cv::Mat segMask;
	std::string segMaskRle;
	std::vector<float> reid_features;

	bbDet& operator=(const bbDet& copy) { // overloading the operator = for deep copy
		if (this == &copy) // if same instance (mermory address)
			return *this;

		if (!this->segMask.empty()) this->segMask.release();
		this->segMask = copy.segMask;
		this->segMaskRle = copy.segMaskRle;

		if (!this->reid_features.empty()) this->reid_features.clear();
		this->reid_features = copy.reid_features;
		if (!this->distances.empty()) this->distances.clear();
		this->distances = copy.distances;

		this->fn = copy.fn;
		this->object_type = copy.object_type;
		this->rec = copy.rec;
		this->rec6D[0] = copy.rec6D[0];
		this->rec6D[1] = copy.rec6D[1];
		this->rec6D[2] = copy.rec6D[2];
		this->rec6D[3] = copy.rec6D[3];
		this->rec6D[4] = copy.rec6D[4];
		this->rec6D[5] = copy.rec6D[5];

		if (!this->top_corners.empty()) this->top_corners.clear();
		this->top_corners = copy.top_corners;
		if (!this->bottom_corners.empty()) this->bottom_corners.clear();
		this->bottom_corners = copy.bottom_corners;

		this->depth = copy.depth;
		this->rotation_y = copy.rotation_y;
		this->ratio_yx = copy.ratio_yx;
		this->confidence = copy.confidence;

		this->weight = copy.weight;
		this->id = copy.id;
		this->input_type = copy.input_type;

		return *this;
	}
}BBDet;

// 함수의 정의, 변수의 초기화를 header 파일에서 하면서
// 다른 파일(소스)에서 해당 함수, 변수를 가져다 쓰고 싶으면 static 밖에는 방법이 없다.
// 아니면, 선언은 헤더파일, 정의 및 초기화는 cpp 에서 한뒤
// extern 키워드를 쓰면된다.
// https://zerobell.tistory.com/22 참고
// 어짜피 값의 변동이 없을 때는 static으로 하는게 제일 편하다
// 함수는 자동으로 extern 이 생략되어있다. (파일 내 전역 변수도 마찬가지)

namespace sym {
	static enum MODEL_VECTOR {
		XY, XYWH, XYRyx
	};

	static int DIMS_STATE[3] = { 4, 6, 6};
	static int DIMS_OBS[3] = { 2, 4, 3};

	static enum OBJECT_TYPE {
		CAR, VAN,
		PEDESTRIAN, PERSON_SITTING,
		CYCLIST, TRUCK, TRAM,
		MISC, DONTCARE, BUS
	};
	static std::string OBJECT_STRINGS[10] = \
	{ "Car", "Van",
		"Pedestrian", "Person_sitting",
		"Cyclist", "Truck", "Tram",
		"Misc", "DontCare", "Bus" };

	static std::string DB_NAMES[6] = { "MOT15", "MOT17", "MOT20", "KITTI", "KITTI", "MOTS20" };
	static int FRAME_OFFSETS[6] = { 1,1,1,0,0,1 };

	static CvScalar OBJECT_TYPE_COLORS[9] = {
		cvScalar(255, 255, 0),	/*Car: Lite Blue (Mint)*/
		cvScalar(255, 0, 0),	/*Van: Blue*/
		cvScalar(255, 0, 255),	/*Pedestrian: Pink*/
		cvScalar(0, 0, 255),	/*Person_sitting: Red*/
		cvScalar(42, 42, 165),	/*Cyclist: Brown*/
		cvScalar(0, 255, 0),	/*Truck: Lite Green*/
		cvScalar(0, 255, 255),	/*Tram: Yellow*/
		cvScalar(64, 64, 64),	/*Misc: Gray*/
		cvScalar(0, 0, 0) };	/*DontCare: Black*/

	// Parameters: 11*10*7 = 770 cases * num of scenes * num of detectors * num of object classes, too many.
	static float DET_SCORE_THS[15] = { -100.0, 0.0, 0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,0.95,0.98,0.99,1.0 };
	//static float DET_SCORE_THS[11] = { 0.0,0.52,0.54,0.55,0.56,0.58,0.62,0.64,0.65,0.66,0.68};
	static int TRACK_INIT_MIN[10] = { 1,2,3,4,5,6,7,8,9,10 };
	static int TRACK_T2TA_MAX[8] = { 5,10,15,20,30,60,80,100 };
	static float VEL_UP_ALPHAS[12] = { 0.0,0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,0.95,0.99 };
	static float MERGE_THS[8] = { 0.3,0.4,0.5,0.6,0.7,0.8,0.9,1.0 };
	static float DET_SCORE_THS_LOWER[7] = { 0.0,0.1,0.2,0.3,0.4,0.5,0.6 };

	static std::string MERGE_METRICS_STR[3] = { "SIOA","IOU", "mIOU" };

	static enum AFFINITY_OPT { GMPHD, KCF, MAF };
}

typedef std::vector<std::vector<std::vector<std::vector<float>>>> VECx4xFLT;
typedef std::vector<std::vector<std::vector<float>>> VECx3xFLT;
typedef std::vector<std::vector<std::vector<cv::Mat>>> VECx3xMAT;
typedef std::vector<std::vector<std::vector<std::string>>> VECx3xSTR;
typedef std::vector<std::vector<std::vector<BBDet>>> VECx3xBBDet;
typedef std::vector<std::vector<BBDet>> VECx2xBBDet;
typedef std::vector<std::vector<std::vector<BBTrk>>> VECx3xBBTrk;
typedef std::vector<std::vector<BBTrk>> VECx2xBBTrk;

#define IS_DONTCARE(x) (((int)x)==sym::OBJECT_TYPE::DONTCARE)
#define IS_VEHICLE_EVAL(x) (((int)x==sym::OBJECT_TYPE::CAR) ||((int)x==sym::OBJECT_TYPE::VAN))
#define IS_VEHICLE_ALL(x) (((int)x==sym::OBJECT_TYPE::CAR) ||((int)x==sym::OBJECT_TYPE::VAN)||((int)x==sym::OBJECT_TYPE::TRUCK)||((int)x==sym::OBJECT_TYPE::BUS))
#define IS_PERSON_KITTI(x) (((int)x==sym::OBJECT_TYPE::PEDESTRIAN) ||((int)x==sym::OBJECT_TYPE::PERSON_SITTING))
#define IS_PERSON_EVAL(x) (((int)x==sym::OBJECT_TYPE::PEDESTRIAN) ||((int)x==sym::OBJECT_TYPE::PERSON_SITTING)||((int)x==sym::OBJECT_TYPE::CYCLIST))

typedef struct MOTparams {
	/*TARGET CLASS*/
	int OBJ_TYPE = sym::OBJECT_TYPE::PEDESTRIAN;
	/*PARAMS*/
	float DET_MIN_CONF = 0.0f; // upper bound
	int TRACK_MIN_SIZE = 1;
	int QUEUE_SIZE = 10;
	int FRAMES_DELAY_SIZE = TRACK_MIN_SIZE - 1;
	int T2TA_MAX_INTERVAL = 10;
	float VEL_UP_ALPHA = 0.5;
	int MERGE_METRIC = MERGE_METRIC_OPT;
	float MERGE_RATIO_THRESHOLD = MERGE_THRESHOLD_RATIO;
	int GROUP_QUEUE_SIZE = TRACK_MIN_SIZE * 10;
	int FRAME_OFFSET = 0;
	/*Gating */
	bool GATE_D2TA_ON = true;
	bool GATE_T2TA_ON = true;

	/*Simple Affinity Fusion*/
	/// 0: GMPHD (Position and Motion) only, 1: KCF (Appearance) Only, 2: Simple Affinity Fuion On
	int SAF_D2TA_MODE = 2;
	int SAF_T2TA_MODE = 2;
	bool SAF_MASK_D2TA = true; // compute appearance considering pixel-wise mask
	bool SAF_MASK_T2TA = true;
	cv::Vec2f KCF_BOUNDS_D2TA;
	cv::Vec2f KCF_BOUNDS_T2TA;
	cv::Vec2f IOU_BOUNDS_D2TA;
	cv::Vec2f IOU_BOUNDS_T2TA;
	/*Simple Affinity Fusion*/

	MOTparams() {
	}
	MOTparams(int obj_type, float dConf_th, int trk_min, int t2ta_max, int mg_metric, float mg_th, float vel_alpha, int group_q_size, int frames_offset,
		int saf_d2ta = 2, int saf_t2ta = 2, bool mask_d2ta = true, bool mask_t2ta = true,
		const cv::Vec2f& kcf_bounds_d2ta = cv::Vec2f(0.5, 0.9), const cv::Vec2f& kcf_bounds_t2ta = cv::Vec2f(0.5, 0.9),
		const cv::Vec2f& iou_bounds_d2ta = cv::Vec2f(0.1, 0.9), const cv::Vec2f& iou_bounds_t2ta = cv::Vec2f(0.1, 0.9),
		const cv::Vec2b& GATES_ONOFF = cv::Vec2b(true, true)) {

		this->OBJ_TYPE = obj_type;

		this->DET_MIN_CONF = dConf_th;
		this->TRACK_MIN_SIZE = trk_min;
		this->FRAMES_DELAY_SIZE = trk_min - 1;
		this->T2TA_MAX_INTERVAL = t2ta_max;
		this->VEL_UP_ALPHA = vel_alpha;
		this->MERGE_METRIC = mg_metric;
		this->MERGE_RATIO_THRESHOLD = mg_th;
		this->GROUP_QUEUE_SIZE = group_q_size;
		this->FRAME_OFFSET = frames_offset;

		this->GATE_D2TA_ON = GATES_ONOFF[0];
		this->GATE_T2TA_ON = GATES_ONOFF[1];

		this->SAF_D2TA_MODE = saf_d2ta;
		this->SAF_T2TA_MODE = saf_t2ta;
		this->SAF_MASK_D2TA = mask_d2ta;
		this->SAF_MASK_T2TA = mask_t2ta;

		this->KCF_BOUNDS_D2TA = kcf_bounds_d2ta;
		this->KCF_BOUNDS_T2TA = kcf_bounds_t2ta;
		this->IOU_BOUNDS_D2TA = iou_bounds_d2ta;
		this->IOU_BOUNDS_T2TA = iou_bounds_t2ta;
	}
	MOTparams& operator=(const MOTparams& copy) { // overloading the operator = for deep copy
		if (this == &copy) // if same instance (mermory address)
			return *this;

		this->OBJ_TYPE = copy.OBJ_TYPE;
		this->DET_MIN_CONF = copy.DET_MIN_CONF;
		this->TRACK_MIN_SIZE = copy.TRACK_MIN_SIZE;
		this->FRAMES_DELAY_SIZE = copy.FRAMES_DELAY_SIZE;
		this->T2TA_MAX_INTERVAL = copy.T2TA_MAX_INTERVAL;
		this->VEL_UP_ALPHA = copy.VEL_UP_ALPHA;
		this->MERGE_METRIC = copy.MERGE_METRIC;
		this->MERGE_RATIO_THRESHOLD = copy.MERGE_RATIO_THRESHOLD;
		this->GROUP_QUEUE_SIZE = copy.GROUP_QUEUE_SIZE;
		this->FRAME_OFFSET = copy.FRAME_OFFSET;

		this->GATE_D2TA_ON = copy.GATE_D2TA_ON;
		this->GATE_T2TA_ON = copy.GATE_T2TA_ON;

		this->SAF_D2TA_MODE = copy.SAF_D2TA_MODE;
		this->SAF_T2TA_MODE = copy.SAF_T2TA_MODE;
		this->SAF_MASK_D2TA = copy.SAF_MASK_D2TA;
		this->SAF_MASK_T2TA = copy.SAF_MASK_T2TA;

		this->KCF_BOUNDS_D2TA = copy.KCF_BOUNDS_D2TA;
		this->KCF_BOUNDS_T2TA = copy.KCF_BOUNDS_T2TA;
		this->IOU_BOUNDS_D2TA = copy.IOU_BOUNDS_D2TA;
		this->IOU_BOUNDS_T2TA = copy.IOU_BOUNDS_T2TA;

		return *this;
	}
} GMPHDMAFparams;


/*
 * parallel_planner.cc
 * Copyright (C) 2017 Roman C. Podolski <maito:roman.podolski@tum.de>
 *
 * Distributed under terms of the MIT license.
 */

#include "planner/planner.h"
#include <glog/logging.h>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <fstream>
#include <iostream>
#include <memory>
#include <vector>
#include "planner/baseframe.h"
#include "planner/maneuver.h"
#include "planner/map.h"

DEFINE_double(max_manouver_offset, 4, "maximum offset from baseframe");
DEFINE_uint64(number_manouvers, 30, "how many manouvers should be considered");

/// TODO(roman): add debug output using glog
namespace planner {

typedef bg::model::box<point> box;
typedef std::pair<box, std::shared_ptr<polygon>> value;
typedef bgi::rtree<value, bgi::quadratic<16>> Rtree;

Planner::Planner(std::shared_ptr<Baseframe> const &base)
    : _m_max{static_cast<double>(FLAGS_number_manouvers)},
      _q_max{static_cast<double>(FLAGS_max_manouver_offset)},
      _baseframe{base},
      _rtree{new Rtree()},
      _last_known_s{0.0},
      _maneuvers(),
      _maneuver(_baseframe->P(0.0), _baseframe->theta(0.0), 0.0, 0.0, 0.0, 0.0,
                _baseframe, _rtree) {}

Planner::Planner(const Map &map)
    : Planner(std::shared_ptr<Baseframe>(new Baseframe(map))) {}

Planner::Planner(const std::vector<std::pair<double, double>> &wp)
    : Planner(std::shared_ptr<Baseframe>(new Baseframe(wp))) {}

Planner::Planner(const std::vector<point> &wp)
    : Planner(std::shared_ptr<Baseframe>(new Baseframe(wp))) {}

const Maneuver &Planner::update(const point position, const double heading,
                                const double velocity,
                                const double dt,  // TODO(roman): braucht es dt?
                                std::vector<polygon> obstacles) {
  _maneuvers.clear();

  // add obstacles to spatial index
  // TODO(roman): move to own function
  for (auto obstacle : obstacles) {
    box b = bg::return_envelope<box>(obstacle);
    auto p = std::make_shared<polygon>(obstacle);
    _rtree->insert(std::make_pair(b, p));
  }

  std::pair<double, double> p =
      _baseframe->localize(position, velocity, heading, dt, _last_known_s);
  _last_known_s = p.first;      // arc-length at current position
  const double q_i = p.second;  // lateral offset at current position

  for (double q_f = -_q_max; q_f <= _q_max; q_f += (2 * _q_max / _m_max)) {
    Maneuver m = Maneuver(position, heading, velocity, q_i, q_f, _last_known_s,
                          _baseframe, _rtree);

    if (m.driveable()) {  // only keep reasonable maneuvers
      _maneuvers.push_back(m);
    }
  }

  // throw exception on this
  CHECK(!_maneuvers.empty());

  std::map<double, Maneuver> candidates;
  std::map<double, Maneuver> collisions_paths;

  // collision check
  for (auto &m : _maneuvers) {
    if (m.collision()) {
      collisions_paths.insert(std::make_pair(m.collision(), m));
    } else {  // calculate path costs
      double cost =
          m.J(_maneuver, std::make_shared<std::vector<Maneuver>>(_maneuvers));
      candidates.insert(std::make_pair(cost, m));
    }
  }

  _maneuver = candidates.empty() ? collisions_paths.rbegin()->second
                                 : candidates.begin()->second;

  return _maneuver;
}
}  // namespace planner


#include <boost/graph/sequential_vertex_coloring.hpp>


// 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)

// Copyright (c) 2006 Johan Rade
// Copyright (c) 2011 Paul A. Bristow comments
// Copyright (c) 2011 John Maddock
/*!
\file
\brief Basic tests of the nonfinite num facets.

\detail If has_infinity and has_nan, then
basic_test outputs using nonfinite_num_put facet
and reads back in using nonfinite_num_ facet,
and checks loopback OK.

Also checks that output of infinity, -infinity and NaN are as expected,
using C99 specification  "nan -nan nan -nan" and "inf -inf".
Also includes a few combinations of display manipulators
(left, right, internal, showpos)
and checks that can input C99 infinity and NaN too.

*/

#ifdef _MSC_VER
#   pragma warning(disable : 4702) // Unreachable code.
#endif

#include <iomanip>
#include <locale>
#include <sstream>

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "almost_equal.ipp"
#include "s_.ipp"

#include <boost/math/special_functions/nonfinite_num_facets.hpp>

namespace
{  // The anonymous namespace resolves ambiguities on
   // platforms with fpclassify etc functions at global scope.

using namespace boost::math;
using boost::math::signbit;
using boost::math::changesign;
using boost::math::isnan;

//------------------------------------------------------------------------------

void basic_test_finite();
void basic_test_inf();
void basic_test_nan();
void basic_test_format();

BOOST_AUTO_TEST_CASE(basic_test)
{
    basic_test_finite();
    basic_test_inf();
    basic_test_nan();
    basic_test_format();
}

//------------------------------------------------------------------------------

template<class CharType, class ValType> void basic_test_finite_impl();

void basic_test_finite()
{
    basic_test_finite_impl<char, float>();
    basic_test_finite_impl<char, double>();
    basic_test_finite_impl<char, long double>();
    basic_test_finite_impl<wchar_t, float>();
    basic_test_finite_impl<wchar_t, double>();
    basic_test_finite_impl<wchar_t, long double>();
}

template<class CharType, class ValType> void basic_test_finite_impl()
{
    if((std::numeric_limits<ValType>::has_infinity == 0) || (std::numeric_limits<ValType>::infinity() == 0))
       return;

    std::locale old_locale;
    std::locale tmp_locale(old_locale, new nonfinite_num_put<CharType>);
    std::locale new_locale(tmp_locale, new nonfinite_num_get<CharType>);

    std::basic_stringstream<CharType> ss;
    ss.imbue(new_locale);

    ValType a1 = (ValType)1.2;
    ValType a2 = (ValType)-3.5;
    ValType a3 = (std::numeric_limits<ValType>::max)();
    ValType a4 = -(std::numeric_limits<ValType>::max)();
    ss << a1 << ' ' << a2 << ' ' << a3 << ' ' << a4;

    ValType b1, b2, b3, b4;
    ss >> b1 >> b2 >> b3 >> b4;

    BOOST_CHECK(almost_equal(b1, a1));
    BOOST_CHECK(almost_equal(b2, a2));
    BOOST_CHECK(almost_equal(b3, a3));
    BOOST_CHECK(almost_equal(b4, a4));
    BOOST_CHECK(b3 != std::numeric_limits<ValType>::infinity());
    BOOST_CHECK(b4 != -std::numeric_limits<ValType>::infinity());
    BOOST_CHECK(ss.rdstate() == std::ios_base::eofbit);

    ss.clear();
    ss.str(S_(""));

    ss << "++5";
    ValType b5;
    ss >> b5;
    BOOST_CHECK(ss.rdstate() == std::ios_base::failbit);
}

//------------------------------------------------------------------------------

template<class CharType, class ValType> void basic_test_inf_impl();

void basic_test_inf()
{
    basic_test_inf_impl<char, float>();
    basic_test_inf_impl<char, double>();
    basic_test_inf_impl<char, long double>();
    basic_test_inf_impl<wchar_t, float>();
    basic_test_inf_impl<wchar_t, double>();
    basic_test_inf_impl<wchar_t, long double>();
}

template<class CharType, class ValType> void basic_test_inf_impl()
{
    if((std::numeric_limits<ValType>::has_infinity == 0) || (std::numeric_limits<ValType>::infinity() == 0))
       return;

    std::locale old_locale;
    std::locale tmp_locale(old_locale, new nonfinite_num_put<CharType>);
    std::locale new_locale(tmp_locale, new nonfinite_num_get<CharType>);

    std::basic_stringstream<CharType> ss;
    ss.imbue(new_locale);

    ValType a1 = std::numeric_limits<ValType>::infinity();
    ValType a2 = -std::numeric_limits<ValType>::infinity();

    BOOST_CHECK((boost::math::isinf)(a1));
    BOOST_CHECK((boost::math::isinf)(a2));

    ss << a1 << ' ' << a2;

    std::basic_string<CharType> s = S_("inf -inf");
    BOOST_CHECK(ss.str() == s);

    ss << " infinity";          // Alternative C99 representation of infinity.

    ValType b1, b2, b3;
    ss >> b1;
    ss >> b2;
    ss >> b3;

    BOOST_CHECK(b1 == a1);
    BOOST_CHECK(b2 == a2);
    BOOST_CHECK(b3 == std::numeric_limits<ValType>::infinity());
    BOOST_CHECK(ss.rdstate() == std::ios_base::eofbit);
}

//------------------------------------------------------------------------------

template<class CharType, class ValType> void basic_test_nan_impl();

void basic_test_nan()
{
    basic_test_nan_impl<char, float>();
    basic_test_nan_impl<char, double>();
    basic_test_nan_impl<char, long double>();
    basic_test_nan_impl<wchar_t, float>();
    basic_test_nan_impl<wchar_t, double>();
    basic_test_nan_impl<wchar_t, long double>();
}

template<class CharType, class ValType> void basic_test_nan_impl()
{
    if((std::numeric_limits<ValType>::has_quiet_NaN == 0) || (std::numeric_limits<ValType>::quiet_NaN() == 0))
       return;

    std::locale old_locale;
    std::locale tmp_locale(old_locale, new nonfinite_num_put<CharType>);
    std::locale new_locale(tmp_locale, new nonfinite_num_get<CharType>);

    std::basic_stringstream<CharType> ss;
    ss.imbue(new_locale);

    ValType a1 = std::numeric_limits<ValType>::quiet_NaN();
    ValType a2 = (boost::math::changesign)(std::numeric_limits<ValType>::quiet_NaN());
    ValType a3 = std::numeric_limits<ValType>::signaling_NaN();
    ValType a4 = (boost::math::changesign)(std::numeric_limits<ValType>::signaling_NaN());
    ss << a1 << ' ' << a2 << ' ' << a3 << ' ' << a4;

    BOOST_CHECK((boost::math::isnan)(a1) && (boost::math::isnan)(a2) && (boost::math::isnan)(a3) && (boost::math::isnan)(a4));

    std::basic_string<CharType> s = S_("nan -nan nan -nan");
    BOOST_CHECK(ss.str() == s);

    // Alternative C99 representation of NaN.
    ss << " nan(foo)";

    ValType b1, b2, b3, b4, b5;
    ss >> b1 >> b2 >> b3 >> b4 >> b5;

    BOOST_CHECK((isnan)(b1));
    BOOST_CHECK((isnan)(b2));
    BOOST_CHECK((isnan)(b3));
    BOOST_CHECK((isnan)(b4));
    BOOST_CHECK((isnan)(b5));

    BOOST_CHECK(!(signbit)(b1));
    BOOST_CHECK((signbit)(b2));
    BOOST_CHECK(!(signbit)(b3));
    BOOST_CHECK((signbit)(b4));
    BOOST_CHECK(!(signbit)(b5));

    BOOST_CHECK(ss.rdstate() == std::ios_base::eofbit);
}

//------------------------------------------------------------------------------

template<class CharType, class ValType> void basic_test_format_impl();

void basic_test_format()
{
    basic_test_format_impl<char, float>();
    basic_test_format_impl<char, double>();
    basic_test_format_impl<char, long double>();
    basic_test_format_impl<wchar_t, float>();
    basic_test_format_impl<wchar_t, double>();
    basic_test_format_impl<wchar_t, long double>();
}

template<class CharType, class ValType> void basic_test_format_impl()
{
    if((std::numeric_limits<ValType>::has_infinity == 0) || (std::numeric_limits<ValType>::infinity() == 0))
       return;

    std::locale old_locale;
    std::locale tmp_locale(old_locale, new nonfinite_num_put<CharType>);
    std::locale new_locale(tmp_locale, new nonfinite_num_get<CharType>);

    std::basic_stringstream<CharType> ss;
    ss.imbue(new_locale);

    ValType a = std::numeric_limits<ValType>::infinity();

    BOOST_CHECK((boost::math::isinf)(a));

    ss << std::setw(6) << a; // Expect right justified in field of six, so 3 leading spaces.
    ss << '|';
    ss << std::setw(2) << a; // Too narrow for "inf", but should still be "inf".
    ss << '|';
    ss << std::left << std::setw(5) << a; // 5 - 3 leaves two trailing spaces.
    ss << '|';
    ss << std::showpos << std::internal << std::setw(7) << a; // 3 internal spaces between + and "inf".
    ss << '|';
    ss << std::uppercase << std::right << std::setw(6) << a; // still showpos, so "space, space, +INF".

    std::basic_string<CharType> s = S_("   inf|inf|inf  |+   inf|  +INF");
    BOOST_CHECK(ss.str() == s);
}

//------------------------------------------------------------------------------

}   // anonymous namespace


/*
 * OperatorConstraint.cpp
 * @brief OperatorConstraint constraint
 * @date Mar 20, 2022
 * @author Yoonwoo Kim
 */

#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <cpp/planning/SingleValueConstraint.h>
#include <cpp/planning/OperatorAddConstraint.h>
#include <cpp/planning/MultiValueConstraint.h>
#include <cpp/planning/NullConstraint.h>

#include <boost/make_shared.hpp>
using namespace gtsam;
using namespace std;

namespace gtsam_planner {

/* ************************************************************************* */
OperatorAddConstraint::OperatorAddConstraint(const DiscreteKey& dkey,
    const DiscreteKeys& dkeys, const vector<MultiValueConstraint>& factors,
    const vector<NullConstraint>& null_factors) : DiscreteFactor(dkeys.indices()) {
  for (const MultiValueConstraint& factor: factors) factors_.push_back(factor);
  for (const NullConstraint& factor: null_factors) null_factors_.push_back(factor);
  dkey_ = dkey;
  dkeys_ = dkeys;
  cardinality_ = dkey_.second;
}

/* ************************************************************************* */
void OperatorAddConstraint::print(const string& s, const KeyFormatter& formatter) const {
  cout << s << "OperatorAddConstraint on "
       << "j=" << formatter(dkey_.first) << endl;
}
/* ************************************************************************* */
double OperatorAddConstraint::operator()(const DiscreteValues& values) const {
  for (size_t i=0; i < cardinality_; i++) {
    SingleValueConstraint single(dkey_, i);
    if (single(values)==1.0 && factors_[i](values)==1.0
      && null_factors_[i](values)==1.0) return 1.0;
  }
  return 0.0;
}

/* ************************************************************************* */
size_t OperatorAddConstraint::operatorKey() const {
  return dkey_.first;
}

/* ************************************************************************* */
double add_(const double& a, const double& b) {
    return a + b;
}

DecisionTreeFactor OperatorAddConstraint::toDecisionTreeFactor() const {
  vector<DecisionTreeFactor> operators;
  for (size_t i=1; i<cardinality_; i++) {
    SingleValueConstraint single(dkey_, i);
    DecisionTreeFactor single_op = single * factors_[i].toDecisionTreeFactor()
      * null_factors_[i].toDecisionTreeFactor();
    operators.push_back(single_op);
  }

  SingleValueConstraint single_comb(dkey_, 0);
  DecisionTreeFactor combined = single_comb * factors_[0].toDecisionTreeFactor()
    * null_factors_[0].toDecisionTreeFactor();
  for (DecisionTreeFactor single_op : operators) {
    combined = combined.apply(single_op, &add_);
  }

  return combined;
}

/* ************************************************************************* */
DecisionTreeFactor OperatorAddConstraint::operator*(const DecisionTreeFactor& f) const {
  // TODO: can we do this more efficiently?
  return toDecisionTreeFactor() * f;
}

}  // namespace gtsam_planner


#pragma once
#include <unordered_map>
#include <boost/heap/fibonacci_heap.hpp>
//#include <boost/heap/binomal_heap.hpp>

template <class id_t, class hash_t = std::hash<id_t>>
struct PrioritizedHolder{

	id_t top();
	void pop();

	void update(const id_t& k, const float& p);
	void reset(const id_t& k);

	size_t size() const { return heap.size(); }
	bool empty() const { return heap.empty(); }
	void clear() {
		heap.clear();
		khm.clear();
	}
	void reserve(const size_t n) {
		khm.reserve(n);
	}

private:
	struct Unit{
		id_t k;
		float p;
	};
	struct CmpUnit{
		bool operator()(const Unit& a, const Unit& b) const {
			return a.p < b.p;
		}
	};
	struct HashID{
		size_t operator()(const id_t& k) const{
			return (k.first << 5) | (k.second);
		}
	};
	using heap_t = boost::heap::fibonacci_heap<Unit, boost::heap::compare<CmpUnit> >;
	//using heap_t = boost::heap::binomal_heap<Unit, boost::heap::compare<CmpUnit> >;
	using handle_t = typename heap_t::handle_type;
	heap_t heap;
	std::unordered_map<id_t, handle_t, hash_t> khm; // key-handler mapper
};

template <class id_t, class hash_t>
id_t PrioritizedHolder<id_t, hash_t>::top(){
	return heap.top().k;
}

template <class id_t, class hash_t>
void PrioritizedHolder<id_t, hash_t>::pop(){
	id_t k = top();
	heap.pop();
	khm.erase(k);
}

template <class id_t, class hash_t>
void PrioritizedHolder<id_t, hash_t>::update(const id_t& k, const float& p){
	auto it = khm.find(k);
	if(it == khm.end()){ // new key -> push
		khm[k] = heap.push(Unit{ k, p });
	} else{ // exist key -> update
		heap.update(it->second, Unit{ k, p });
	}
}

template <class id_t, class hash_t>
void PrioritizedHolder<id_t, hash_t>::reset(const id_t& k){
	auto it = khm.find(k);
	if(it != khm.end()){
		heap.erase(it->second);
		khm.erase(it);
	}
}


#include <iterator>
#include <fstream>
#include <string>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/qi_parse.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include "binparse.h"

namespace qi = boost::spirit::qi;

std::string leb128_encode_s(std::intmax_t value)
{
	std::string encoding;
	std::uintmax_t uvalue;
	std::memcpy(&uvalue, &value, sizeof(uvalue));
	const bool neg = value < 0;
	for(bool more = true; more; )
	{
		unsigned char byte = uvalue & std::uintmax_t(0b0111'1111u);
		uvalue >>= 7;
		if(neg)
			uvalue |= (~std::uintmax_t(0u)) << (CHAR_BIT * sizeof(std::uintmax_t) - 7);
		bool signbit = static_cast<bool>(byte & 0b0100'0000u);
		if((uvalue == 0 and (not signbit)) or ((uvalue == (~std::uintmax_t(0u))) and signbit))
			more = false;
		else
			byte |= 0b1000'0000u;
		encoding.push_back(byte);
	}
	return encoding;
}

std::string leb128_encode_u(std::uintmax_t value)
{
	std::string encoding;
	do {
		unsigned char byte = static_cast<unsigned char>(value & 0b0111'1111u);
		value >>= 7;
		if(value > 0u)
			byte |= 0b1000'0000u;
		encoding.push_back(byte);
	} while(value > 0u);
	return encoding;
}

void print_encoding(std::string_view sv)
{
	for(auto chr: sv)
		std::cout << (unsigned)chr << ", ";
}

int main(int argc, char** argv)
{
	for(std::uint8_t uv = 0; uv < 0b0111'1111u; ++uv)
	{
		std::string uenc(leb128_encode_u(uv));
		std::uintmax_t v{0};
		assert(qi::parse(begin(uenc), end(uenc), wasm::parse::varuint7[boost::phoenix::ref(v) = qi::_1]));
		if(v != uv)
			std::cerr << std::uintmax_t(uv) << " != " << v << std::endl;
		assert(v == uv);
	}

	for(std::uint8_t uv = 0b1000'0000u; uv < 0b1111'1111; ++uv)
	{
		std::string uenc(leb128_encode_u(uv));
		assert(not qi::parse(begin(uenc), end(uenc), wasm::parse::varuint7));
	}

	for(std::int8_t sv = -(1 << 6); sv < ((1 << 6) - 1); ++sv)
	{
		std::string senc(leb128_encode_s(sv));
		signed char v;
		bool matched = qi::parse(begin(senc), end(senc), wasm::parse::varint7[boost::phoenix::ref(v) = qi::_1]);
		if(not matched)
		{
			std::cerr << int(sv) << " -> ";
			for(auto chr: senc)
				std::cerr << int(chr) << ", ";
			std::cerr << std::endl;
		}
		assert(matched);
		assert(v == sv);
	}

	std::string enc;
	std::uintmax_t uv_in{0};
	std::uintmax_t uv_out{0};
	std::intmax_t sv_in{0};
	std::intmax_t sv_out{0};
	bool matched = false;

	enc = leb128_encode_u(uv_in);
	matched = qi::parse(
		begin(enc), end(enc),
		wasm::parse::varuint32[boost::phoenix::ref(uv_out) = qi::_1]
	);
	assert(matched);
	assert(uv_in == uv_out);

	uv_in = std::numeric_limits<std::uint32_t>::max();
	enc = leb128_encode_u(uv_in);
	matched = qi::parse(
		begin(enc), end(enc),
		wasm::parse::varuint32[boost::phoenix::ref(uv_out) = qi::_1]
	);
	assert(matched);
	assert(uv_in == uv_out);

	uv_in = static_cast<std::uintmax_t>(12345678);
	enc = leb128_encode_u(uv_in);
	matched = qi::parse(
		begin(enc), end(enc),
		wasm::parse::varuint32[boost::phoenix::ref(uv_out) = qi::_1]
	);
	assert(matched);
	assert(uv_in == uv_out);

	uv_in = static_cast<std::uintmax_t>(std::numeric_limits<std::uint32_t>::max()) + 1;
	enc = leb128_encode_u(uv_in);
	matched = qi::parse(
		begin(enc), end(enc),
		wasm::parse::varuint32[boost::phoenix::ref(uv_out) = qi::_1]
	);
	assert(not matched);

	sv_in = 0;
	enc = leb128_encode_s(sv_in);
	matched = qi::parse(
		begin(enc), end(enc),
		wasm::parse::varint32[boost::phoenix::ref(sv_out) = qi::_1]
	);
	assert(matched);
	assert(sv_in == sv_out);

	sv_in = std::numeric_limits<std::int32_t>::max();
	enc = leb128_encode_u(sv_in);
	matched = qi::parse(
		begin(enc), end(enc),
		wasm::parse::varint32[boost::phoenix::ref(sv_out) = qi::_1]
	);
	assert(matched);
	assert(sv_in == sv_out);

	sv_in = std::numeric_limits<std::int32_t>::min();
	enc = leb128_encode_u(sv_in);
	matched = qi::parse(
		begin(enc), end(enc),
		wasm::parse::varint32[boost::phoenix::ref(sv_out) = qi::_1]
	);
	assert(matched);
	assert(sv_in == sv_out);

	sv_in = 12345678;
	enc = leb128_encode_u(sv_in);
	matched = qi::parse(
		begin(enc), end(enc),
		wasm::parse::varint32[boost::phoenix::ref(sv_out) = qi::_1]
	);
	assert(matched);
	assert(sv_in == sv_out);

	sv_in = static_cast<std::uintmax_t>(std::numeric_limits<std::uint32_t>::max()) + 1;
	enc = leb128_encode_u(sv_in);
	matched = qi::parse(
		begin(enc), end(enc),
		wasm::parse::varint32[boost::phoenix::ref(sv_out) = qi::_1]
	);
	assert(not matched);

	return 0;
}


// (C) Copyright 2013 Louis Dionne
//
// Modified from `tiernan_all_cycles.cpp`.
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GRAPH_TEST_CYCLE_TEST_HPP
#define BOOST_GRAPH_TEST_CYCLE_TEST_HPP

#include <boost/assert.hpp>
#include <boost/graph/directed_graph.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/undirected_graph.hpp>
#include <boost/next_prior.hpp>
#include <boost/random/linear_congruential.hpp>
#include <cstddef>
#include <iostream>

namespace cycle_test_detail
{
using namespace boost;

struct cycle_validator
{
    explicit cycle_validator(std::size_t& number_of_cycles)
    : cycles(number_of_cycles)
    {
    }

    template < typename Path, typename Graph >
    void cycle(Path const& p, Graph const& g)
    {
        ++cycles;
        // Check to make sure that each of the vertices in the path
        // is truly connected and that the back is connected to the
        // front - it's not validating that we find all paths, just
        // that the paths are valid.
        typename Path::const_iterator i, j, last = prior(p.end());
        for (i = p.begin(); i != last; ++i)
        {
            j = boost::next(i);
            BOOST_ASSERT(edge(*i, *j, g).second);
        }
        BOOST_ASSERT(edge(p.back(), p.front(), g).second);
    }

    std::size_t& cycles;
};

template < typename Graph, typename Algorithm >
void test_one(Algorithm algorithm)
{
    typedef erdos_renyi_iterator< minstd_rand, Graph > er;

    // Generate random graphs with 15 vertices and 15% probability
    // of edge connection.
    static std::size_t const N = 20;
    static double const P = 0.1;
    minstd_rand rng;

    Graph g(er(rng, N, P), er(), N);
    renumber_indices(g);
    print_edges(g, get(vertex_index, g));

    std::size_t cycles = 0;
    cycle_validator vis(cycles);
    algorithm(g, vis);
    std::cout << "# cycles: " << vis.cycles << "\n";
}
} // end namespace cycle_test_detail

template < typename Algorithm > void cycle_test(Algorithm const& algorithm)
{
    std::cout << "*** undirected ***\n";
    cycle_test_detail::test_one< boost::undirected_graph<> >(algorithm);

    std::cout << "*** directed ***\n";
    cycle_test_detail::test_one< boost::directed_graph<> >(algorithm);
}

#endif // !BOOST_GRAPH_TEST_CYCLE_TEST_HPP