/*! \file demo_2d_plot.cpp
    \brief Demonstration of some 2D plot features.
    \details Uses some simple math functions to generate curves.
    The detailed output shows the plot settings for each plot.

    \author Jacob Voytko and Paul A. Bristow
*/

// Copyright Jacob Voytko 2007
// Copyright Paul A. Bristow 2007, 2008

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

#ifdef _MSC_VER
#  pragma warning (disable : 4800) // forcing value to bool 'true' or 'false' (performance warning)
#  pragma warning (disable : 4180) // qualifier applied to function type has no meaning; ignored
#  pragma warning (disable : 4503) // decorated name length exceeded, name was truncated
#  pragma warning (disable : 4512) // assignment operator could not be generated
#  pragma warning (disable : 4100) // unreferenced formal parameter
#  pragma warning (disable : 4224) // formal parameter 'hash' was previously defined as a type
#endif

#include <boost/svg_plot/svg_2d_plot.hpp>

#include <boost/svg_plot/show_2d_settings.hpp>
// for show_2d_plot_settings - Only needed for showing which settings in use.

//#include ".\axis_scaling.hpp"

#include <boost/array.hpp>
  using boost::array;

#include <map>
  using std::map;
#include <vector>
   using std::vector;

#include <cmath>
   using std::sqrt;
   using std::pair;
#include <iostream>
   using std::cout;
   using std::endl;
   using std::scientific;
   using std::hex;
   using std::ios;
   using std::boolalpha;

using namespace boost::svg;
// Convenient to use namespace because there are so many functions,
// and very many named colors in this namespace.

  // Use map with both x and y double.
  map<double, double> data1;
  map<double, double> data2;
  map<double, double> data3;

 // Several simple math functions to demonstrate:
double f(double x)
{
  return sqrt(x);
}

double g(double x)
{
  return -2 + x * x;
}

double h(double x)
{
  return -1 + 2 * x;
}

double sq(double x)
{
  return x * x;
}

double recip(double x)
{
  return 1. / x;
}

void plot(const std::string& title, const std::string& file,
    const std::string& x_label = std::string(), // default is ""
    double xmin = 0., double xmax = 0.,
    const std::string& y_label = std::string(),
    double ymin = 0., double ymax = 0.,
    int x_major_label = -1, int y_major_label = -1, // None
    int x_rotation = horizontal, int y_rotation = upward) //
{
  using namespace boost::svg;

  svg_2d_plot my_plot;
  // Size/scale settings.
  my_plot.size(700, 500);
  //my_plot.legend_top_left(175, 100);
  my_plot.title(title)
          .title_font_size(20)
          .x_label(x_label)
          .y_label(y_label)
          ;
  // Legend settings.
  my_plot.legend_on(true)
          //.legend_place(outside_bottom)
          //.legend_place(outside_right)
          .title_on(true)
          .plot_window_on(true)
          .plot_border_color(magenta)
          .x_label_on(true)
          .y_label_on(true)
          .y_major_labels_side(left_side)
          .x_major_labels_side(left_side)
          //.legend_title("Function")
          .legend_title("Unicode ΩΦ") // Omega Phi
          .legend_title_font_size(16);

  // Color settings chosen for visibility rather than taste!

  // Color settings.
  my_plot.background_color(ghostwhite) // image
          .legend_background_color(lightyellow)
          .legend_border_color(yellow)
          .plot_background_color(svg_color(white))
          .plot_border_color(svg_color(green))
          .plot_border_width(1)
          .title_color(red)
          ;
  // Options for x and/or y num_minor_ticks.
  // .y_num_minor_ticks(4)  // 0 major, 2,4,6,7,8 minor, 10 major ...
  // .y_num_minor_ticks(1) // 0 major, 5, minor, 10 major ...
  // .y_num_minor_ticks(2) // show half points.
  // .y_num_minor_ticks(3) // show half and quarter points.
  // .y_num_minor_ticks(4) // 0 major, 2,4,6,7,8 minor, 10 major ...
  // .y_num_minor_ticks(9) // 0 major, 1,2,3,4,5,6,7,8,9 minor, 10 major ...

  // X axis settings.
  my_plot.x_range(xmin, xmax)
          .x_major_interval(2.)
          .x_major_tick_length(10) // pixels
          .x_major_tick_width(2) // pixels
          .x_minor_tick_length(5) // pixels
          .x_minor_tick_width(1) // pixels
          .x_num_minor_ticks(2) // plus 1 major = 5 ticks per major step.
          ;

  // Y-axis settings.
 my_plot
    .y_range(ymin, ymax)
    .y_major_interval(2.)
    .y_num_minor_ticks(2) // plus 1 major = 5 ticks per major step.
    .y_major_tick_length(10) // pixels
    .y_major_tick_width(2) // pixels
    .y_minor_tick_length(5) // pixels
    .y_minor_tick_width(1) // pixels
    ;

  // Very pale blue grid - like old fashioned graph paper.
  my_plot.x_major_grid_color(svg_color(200, 220, 255))
         .x_minor_grid_color(svg_color(240, 240, 255))
         .y_major_grid_color(svg_color(200, 220, 255))
         .y_minor_grid_color(svg_color(240, 240, 255))
         .y_major_grid_width(2)
         .y_minor_grid_width(1)
     // But nothing shows - until you make .major_grid_on(true)!
         .x_major_grid_on(true)
         .x_minor_grid_on(true)
         .y_major_grid_on(true)
         .y_minor_grid_on(true);

  my_plot.x_ticks_down_on(true); // X-axis.
  my_plot.y_ticks_left_on(true); // Y-axis.

  // Where the ticks (and labels if any) go, left/right, on axis, or bottom/top.
  // Default x_ticks_on_window_or_axis == -1 left or bottom, +1 right to top, 0 = on axis.
  // my_plot.x_ticks_on_window_or_axis(+1); //
  // my_plot.y_ticks_on_window_or_axis(+1);
  // my_plot.x_ticks_on_window_or_axis(-1); // right or top.
  // my_plot.y_ticks_on_window_or_axis(-1);
  // x_ticks_on_window_or_axis == 0 : on axes, if possible.
  my_plot.x_ticks_on_window_or_axis(0); // ticks on axes.
  my_plot.y_ticks_on_window_or_axis(0); // ticks on axes.

  // Which side of axis line or plot window the value labels go.
  //my_plot.x_major_labels_side(0); // NO value labels.
  //my_plot.y_major_labels_side(0); // NO value labels.

  //my_plot.x_major_labels_side(top_side); // Top side value labels.
  //my_plot.x_major_labels_side(bottom_side); // Bottom side value labels (default).
  //my_plot.y_major_labels_side(no_labels); // NO value labels.
  //my_plot.y_major_labels_side(right_side); // Right side of axis value labels.
  //my_plot.y_major_labels_side(left_side); // Left side value labels (default).

  //my_plot.x_major_label_rotation(rotate_style(x_rotation));// Use this plot function's defaults.
  //my_plot.x_major_label_rotation(horizontal); // svg_2d_plot default.
  //my_plot.x_major_label_rotation(upward);
  //my_plot.x_major_label_rotation(downward);
  //my_plot.x_major_label_rotation(uphill);
  //my_plot.x_major_label_rotation(downhill);

  //my_plot.y_major_label_rotation(rotate_style(y_rotation)); // Use this plot function's defaults.
  //my_plot.y_major_label_rotation(upward); // svg_2d_plot default.
  //my_plot.y_major_label_rotation(downhill); // sloping.
  //my_plot.y_major_label_rotation(uphill);
  //my_plot.y_major_label_rotation(horizontal);.
  //my_plot.y_major_label_rotation(downward);

  my_plot.y_value_ioflags(ios::dec | ios::fixed).y_value_precision(1);
  my_plot.x_value_ioflags(ios::dec | ios::scientific).x_value_precision(2);
  //  my_plot.x_value_ioflags(ios::dec).x_value_precision(2);

  my_plot.plot(data1, "Sqrt(x)").fill_color(red);
  my_plot.plot(data2, "-2 + x^2").fill_color(orange).size(5).bezier_on(true);
  my_plot.plot(data3, "-1 + 2x").fill_color(yellow).line_color(blue).line_width(3).shape(square);

  cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> " << file << endl;
  my_plot.write(file);
  show_2d_plot_settings(my_plot);

  } // plot

int main()
{
  try
  {
  // boost::array or anything in boost such that pair_type has
  // std and boost as associated namespaces.
  //typedef ::std::pair< ::boost::array<int, 1>, int> pair_type;
  //pair_type p1;
  //pair_type p2;

  //p1.first[0] = p1.second = 0;
  //p2.first[0] = p2.second = 1;

  //array<pair<double, double>, 10> ddpairs;
  //ddpairs[0].first = 1.; ddpairs[0].second = 2.;

  for(double i = -10; i <= 10.; i += 1.)
  {
    data1[i] = f(i);
    data2[i] = g(i);
    data3[i] = h(i);
  }

   // Demonstrate/test plots with various range of x and y, some *not* including zero.

   plot("Plot of Mathematical Functions XYPM", "./demo_2d_plot_XYPM.svg", "X-axis", -10., +10., "Y-axis", -10., +10.); // Both X & Y include zero.
   plot("Plot of Mathematical Functions XP", "./demo_2d_plot_XP.svg", "X-axis", +1., +10., "Y-axis", -10., 10.); // X all > 0
   plot("Plot of Mathematical Functions XN", "./demo_2d_plot_XN.svg", "X-axis", -10., -1., "Y-axis", -10., 10.); // x all < 0
   plot("Plot of Mathematical Functions YP", "./demo_2d_plot_YP.svg", "X-axis", -1., +10., "Y-axis", +1., +10.); // Y all > 0
   plot("Plot of Mathematical Functions YN", "./demo_2d_plot_YN.svg", "X-axis", -1., +10., "Y-axis", -10., -1.); // y all < 0
   plot("Plot of Mathematical Functions XYP", "./demo_2d_plot_XYP.svg", "X-axis", +1., +10., "Y-axis", +1., +10.); // x & y all > 0
   plot("Plot of Mathematical Functions XYN", "./demo_2d_plot_XYN.svg", "X-axis", -10., -1., "Y-axis", -10., -1.); // x & y all < 0
   plot("Title with Unicode <sup>-α </sup> Ω Φ √ ∞ ζ ± ₀ ₁ ₂₃", "./demo_2d_plot_XYPM.svg", "X-axis ±Ω", -10., +10., "Y-axis ∞√", -10., +10.); // Both X & Y include zero.
  }
  catch (const std::exception& e)
  {
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
} // int main()

/*

Output :

Autorun "j:\Cpp\SVG\Debug\demo_2d_plot.exe"
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> ./demo_2d_plot_XYPM.svg
axes_on true
background_border_width 2
background_border_color RGB(255,255,0)
background_color RGB(248,248,255)
image_border_margin() 3
image_border_width() 2
coord_precision 3
copyright_date
copyright_holder
description
document_title
x_size 700
image_y_size 500
image_size 700, 500
image_filename
legend_on true
legend_place 2
legend_top_left 538.1, 58, legend_bottom_right 692, 202
legend_background_color RGB(255,255,224)
legend_border_color RGB(255,255,0)
legend_color blank
legend_title
legend_title_font_size 16
legend_font_weight
legend_width 153.9
legend_lines true
limit points stroke color RGB(128,128,128)
limit points fill color blank
license_on false
license_reproduction permits
license_distribution permits
license_attribution requires
license_commercialuse permits
plot_background_color RGB(255,255,255)
plot_border_color RGB(0,128,0)
plot_border_width 1
plot_window_on true
plot_window_x 46, 524.1
plot_window_x_left 46
plot_window_x_right 524.1
plot_window_y 58, 454
plot_window_y_top 58
plot_window_y_bottom 454
title_on true
title ""
title_color RGB(255,0,0)
title_font_alignment 2
title_font_decoration
title_font_family Verdana
title_font_rotation 0
title_font_size 20
title_font_stretch
title_font_style
title_font_weight
x_values_on false
x_values_font_size 12
x_values_font_family
x_values_precision 3
x_values_ioflags 200 IOS format flags (0x1200) dec, scientific.
y_values_precision 3
y_values_font_size() 1
y_values_ioflags 200 IOS format flags (0x200) dec.
y_values_color blank
y_values_font_family()
y_values_font_size() 12
x_max 10
x_min -10
x_autoscale false
y_autoscale false
xy_autoscale false
x_autoscale_check_limits true
x_axis_on true
x_axis_color() RGB(0,0,0)
x_axis_label_color blank
x_values_color blank
x_axis_width 1
x_label_on true
x_label X-axis
x_label_color blank
x_label_font_family Verdana
x_label_font_size 14
x_label_units
x_label_units_on false
x_major_labels_side left
x_major_label_rotation 0
x_major_grid_color RGB(200,220,255)
x_major_grid_on true
x_major_grid_width 1
x_major_interval 2
x_major_tick 2
x_major_tick_color RGB(0,0,0)
x_major_tick_length 10
x_major_tick_width 2
x_minor_interval 0
x_minor_tick_color RGB(0,0,0)
x_minor_tick_length 5
x_minor_tick_width 1
x_minor_grid_on true
x_minor_grid_color RGB(240,240,255)
x_minor_grid_width 0.5
x_range() -10, 10
x_num_minor_ticks 2
x_ticks_down_on true
x_ticks_up_on false
x_ticks_on_window_or_axis none
y_axis_position y_axis_position intersects X axis (X range includes zero)
x_axis_position x_axis_position intersects Y axis (Y range includes zero)
x_plusminus_on false
x_plusminus_color RGB(0,0,255)
x_df_on false
x_df_color RGB(0,0,255)
x_prefix
x_separator
x_suffix
xy_values_on false
y_label_on true
y_label_axis Y-axis
y_axis_color RGB(0,0,0)
y_axis_label_color blank
y_axis_on true
axes_on true
y_axis_value_color RGB(0,0,0)
y_axis_width 1
y_label Y-axis
y_label_color blank
y_label_font_family Verdana
y_label_font_size 14
y_label_on true
y_label_units
y_label_units_on false
y_label_width 0
y_major_grid_on true
y_major_grid_color RGB(200,220,255)
y_major_grid_width 2
y_major_interval 2
y_major_labels_side bottom
y_major_label_rotation 0
y_major_tick_color RGB(0,0,0)
y_major_tick_length  10
y_major_tick_width  2
y_minor_grid_on true
y_minor_grid_color  RGB(240,240,255)
y_minor_grid_width 1
y_minor_interval 0
y_minor_tick_color RGB(0,0,0)
y_minor_tick_length 5
y_minor_tick_width 1
y_range() -10, 10
y_num_minor_ticks
y_ticks_left_on true
y_ticks_right_on false
y_ticks_on_window_or_axis none
y_max 10
y_min -10
y_values_on false
y_plusminus_on false
y_plusminus_color RGB(0,0,255)
y_df_on false
y_df_color RGB(0,0,255)
y_prefix
y_separator
y_suffix
data lines width 2
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> ./demo_2d_plot_XP.svg
axes_on true
background_border_width 2
background_border_color RGB(255,255,0)
background_color RGB(248,248,255)
image_border_margin() 3
image_border_width() 2
coord_precision 3
copyright_date
copyright_holder
description
document_title
x_size 700
image_y_size 500
image_size 700, 500
image_filename
legend_on true
legend_place 2
legend_top_left 538.1, 58, legend_bottom_right 692, 202
legend_background_color RGB(255,255,224)
legend_border_color RGB(255,255,0)
legend_color blank
legend_title
legend_title_font_size 16
legend_font_weight
legend_width 153.9
legend_lines true
limit points stroke color RGB(128,128,128)
limit points fill color blank
license_on false
license_reproduction permits
license_distribution permits
license_attribution requires
license_commercialuse permits
plot_background_color RGB(255,255,255)
plot_border_color RGB(0,128,0)
plot_border_width 1
plot_window_on true
plot_window_x 88, 524.1
plot_window_x_left 88
plot_window_x_right 524.1
plot_window_y 58, 454
plot_window_y_top 58
plot_window_y_bottom 454
title_on true
title ""
title_color RGB(255,0,0)
title_font_alignment 2
title_font_decoration
title_font_family Verdana
title_font_rotation 0
title_font_size 20
title_font_stretch
title_font_style
title_font_weight
x_values_on false
x_values_font_size 12
x_values_font_family
x_values_precision 3
x_values_ioflags 200 IOS format flags (0x1200) dec, scientific.
y_values_precision 3
y_values_font_size() 1
y_values_ioflags 200 IOS format flags (0x200) dec.
y_values_color blank
y_values_font_family()
y_values_font_size() 12
x_max 10
x_min 1
x_autoscale false
y_autoscale false
xy_autoscale false
x_autoscale_check_limits true
x_axis_on true
x_axis_color() RGB(0,0,0)
x_axis_label_color blank
x_values_color blank
x_axis_width 1
x_label_on true
x_label X-axis
x_label_color blank
x_label_font_family Verdana
x_label_font_size 14
x_label_units
x_label_units_on false
x_major_labels_side left
x_major_label_rotation 0
x_major_grid_color RGB(200,220,255)
x_major_grid_on true
x_major_grid_width 1
x_major_interval 2
x_major_tick 2
x_major_tick_color RGB(0,0,0)
x_major_tick_length 10
x_major_tick_width 2
x_minor_interval 0
x_minor_tick_color RGB(0,0,0)
x_minor_tick_length 5
x_minor_tick_width 1
x_minor_grid_on true
x_minor_grid_color RGB(240,240,255)
x_minor_grid_width 0.5
x_range() 1, 10
x_num_minor_ticks 2
x_ticks_down_on true
x_ticks_up_on false
x_ticks_on_window_or_axis none
y_axis_position y_axis_position left (all X values < 0)
x_axis_position x_axis_position intersects Y axis (Y range includes zero)
x_plusminus_on false
x_plusminus_color RGB(0,0,255)
x_df_on false
x_df_color RGB(0,0,255)
x_prefix
x_separator
x_suffix
xy_values_on false
y_label_on true
y_label_axis Y-axis
y_axis_color RGB(0,0,0)
y_axis_label_color blank
y_axis_on true
axes_on true
y_axis_value_color RGB(0,0,0)
y_axis_width 1
y_label Y-axis
y_label_color blank
y_label_font_family Verdana
y_label_font_size 14
y_label_on true
y_label_units
y_label_units_on false
y_label_width 0
y_major_grid_on true
y_major_grid_color RGB(200,220,255)
y_major_grid_width 2
y_major_interval 2
y_major_labels_side bottom
y_major_label_rotation 0
y_major_tick_color RGB(0,0,0)
y_major_tick_length  10
y_major_tick_width  2
y_minor_grid_on true
y_minor_grid_color  RGB(240,240,255)
y_minor_grid_width 1
y_minor_interval 0
y_minor_tick_color RGB(0,0,0)
y_minor_tick_length 5
y_minor_tick_width 1
y_range() -10, 10
y_num_minor_ticks
y_ticks_left_on true
y_ticks_right_on false
y_ticks_on_window_or_axis left
y_max 10
y_min -10
y_values_on false
y_plusminus_on false
y_plusminus_color RGB(0,0,255)
y_df_on false
y_df_color RGB(0,0,255)
y_prefix
y_separator
y_suffix
data lines width 2
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> ./demo_2d_plot_XN.svg
axes_on true
background_border_width 2
background_border_color RGB(255,255,0)
background_color RGB(248,248,255)
image_border_margin() 3
image_border_width() 2
coord_precision 3
copyright_date
copyright_holder
description
document_title
x_size 700
image_y_size 500
image_size 700, 500
image_filename
legend_on true
legend_place 2
legend_top_left 538.1, 58, legend_bottom_right 692, 202
legend_background_color RGB(255,255,224)
legend_border_color RGB(255,255,0)
legend_color blank
legend_title
legend_title_font_size 16
legend_font_weight
legend_width 153.9
legend_lines true
limit points stroke color RGB(128,128,128)
limit points fill color blank
license_on false
license_reproduction permits
license_distribution permits
license_attribution requires
license_commercialuse permits
plot_background_color RGB(255,255,255)
plot_border_color RGB(0,128,0)
plot_border_width 1
plot_window_on true
plot_window_x 46, 524.1
plot_window_x_left 46
plot_window_x_right 524.1
plot_window_y 58, 454
plot_window_y_top 58
plot_window_y_bottom 454
title_on true
title ""
title_color RGB(255,0,0)
title_font_alignment 2
title_font_decoration
title_font_family Verdana
title_font_rotation 0
title_font_size 20
title_font_stretch
title_font_style
title_font_weight
x_values_on false
x_values_font_size 12
x_values_font_family
x_values_precision 3
x_values_ioflags 200 IOS format flags (0x1200) dec, scientific.
y_values_precision 3
y_values_font_size() 1
y_values_ioflags 200 IOS format flags (0x200) dec.
y_values_color blank
y_values_font_family()
y_values_font_size() 12
x_max -1
x_min -10
x_autoscale false
y_autoscale false
xy_autoscale false
x_autoscale_check_limits true
x_axis_on true
x_axis_color() RGB(0,0,0)
x_axis_label_color blank
x_values_color blank
x_axis_width 1
x_label_on true
x_label X-axis
x_label_color blank
x_label_font_family Verdana
x_label_font_size 14
x_label_units
x_label_units_on false
x_major_labels_side left
x_major_label_rotation 0
x_major_grid_color RGB(200,220,255)
x_major_grid_on true
x_major_grid_width 1
x_major_interval 2
x_major_tick 2
x_major_tick_color RGB(0,0,0)
x_major_tick_length 10
x_major_tick_width 2
x_minor_interval 0
x_minor_tick_color RGB(0,0,0)
x_minor_tick_length 5
x_minor_tick_width 1
x_minor_grid_on true
x_minor_grid_color RGB(240,240,255)
x_minor_grid_width 0.5
x_range() -10, -1
x_num_minor_ticks 2
x_ticks_down_on true
x_ticks_up_on false
x_ticks_on_window_or_axis none
y_axis_position y_axis_position right (all X values > 0)
x_axis_position x_axis_position intersects Y axis (Y range includes zero)
x_plusminus_on false
x_plusminus_color RGB(0,0,255)
x_df_on false
x_df_color RGB(0,0,255)
x_prefix
x_separator
x_suffix
xy_values_on false
y_label_on true
y_label_axis Y-axis
y_axis_color RGB(0,0,0)
y_axis_label_color blank
y_axis_on true
axes_on true
y_axis_value_color RGB(0,0,0)
y_axis_width 1
y_label Y-axis
y_label_color blank
y_label_font_family Verdana
y_label_font_size 14
y_label_on true
y_label_units
y_label_units_on false
y_label_width 0
y_major_grid_on true
y_major_grid_color RGB(200,220,255)
y_major_grid_width 2
y_major_interval 2
y_major_labels_side bottom
y_major_label_rotation 0
y_major_tick_color RGB(0,0,0)
y_major_tick_length  10
y_major_tick_width  2
y_minor_grid_on true
y_minor_grid_color  RGB(240,240,255)
y_minor_grid_width 1
y_minor_interval 0
y_minor_tick_color RGB(0,0,0)
y_minor_tick_length 5
y_minor_tick_width 1
y_range() -10, 10
y_num_minor_ticks
y_ticks_left_on true
y_ticks_right_on false
y_ticks_on_window_or_axis right
y_max 10
y_min -10
y_values_on false
y_plusminus_on false
y_plusminus_color RGB(0,0,255)
y_df_on false
y_df_color RGB(0,0,255)
y_prefix
y_separator
y_suffix
data lines width 2
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> ./demo_2d_plot_YP.svg
axes_on true
background_border_width 2
background_border_color RGB(255,255,0)
background_color RGB(248,248,255)
image_border_margin() 3
image_border_width() 2
coord_precision 3
copyright_date
copyright_holder
description
document_title
x_size 700
image_y_size 500
image_size 700, 500
image_filename
legend_on true
legend_place 2
legend_top_left 538.1, 58, legend_bottom_right 692, 202
legend_background_color RGB(255,255,224)
legend_border_color RGB(255,255,0)
legend_color blank
legend_title
legend_title_font_size 16
legend_font_weight
legend_width 153.9
legend_lines true
limit points stroke color RGB(128,128,128)
limit points fill color blank
license_on false
license_reproduction permits
license_distribution permits
license_attribution requires
license_commercialuse permits
plot_background_color RGB(255,255,255)
plot_border_color RGB(0,128,0)
plot_border_width 1
plot_window_on true
plot_window_x 46, 524.1
plot_window_x_left 46
plot_window_x_right 524.1
plot_window_y 58, 436
plot_window_y_top 58
plot_window_y_bottom 436
title_on true
title ""
title_color RGB(255,0,0)
title_font_alignment 2
title_font_decoration
title_font_family Verdana
title_font_rotation 0
title_font_size 20
title_font_stretch
title_font_style
title_font_weight
x_values_on false
x_values_font_size 12
x_values_font_family
x_values_precision 3
x_values_ioflags 200 IOS format flags (0x1200) dec, scientific.
y_values_precision 3
y_values_font_size() 1
y_values_ioflags 200 IOS format flags (0x200) dec.
y_values_color blank
y_values_font_family()
y_values_font_size() 12
x_max 10
x_min -1
x_autoscale false
y_autoscale false
xy_autoscale false
x_autoscale_check_limits true
x_axis_on true
x_axis_color() RGB(0,0,0)
x_axis_label_color blank
x_values_color blank
x_axis_width 1
x_label_on true
x_label X-axis
x_label_color blank
x_label_font_family Verdana
x_label_font_size 14
x_label_units
x_label_units_on false
x_major_labels_side left
x_major_label_rotation 0
x_major_grid_color RGB(200,220,255)
x_major_grid_on true
x_major_grid_width 1
x_major_interval 2
x_major_tick 2
x_major_tick_color RGB(0,0,0)
x_major_tick_length 10
x_major_tick_width 2
x_minor_interval 0
x_minor_tick_color RGB(0,0,0)
x_minor_tick_length 5
x_minor_tick_width 1
x_minor_grid_on true
x_minor_grid_color RGB(240,240,255)
x_minor_grid_width 0.5
x_range() -1, 10
x_num_minor_ticks 2
x_ticks_down_on true
x_ticks_up_on false
x_ticks_on_window_or_axis bottom
y_axis_position y_axis_position intersects X axis (X range includes zero)
x_axis_position x_axis_position bottom (all Y values > 0)
x_plusminus_on false
x_plusminus_color RGB(0,0,255)
x_df_on false
x_df_color RGB(0,0,255)
x_prefix
x_separator
x_suffix
xy_values_on false
y_label_on true
y_label_axis Y-axis
y_axis_color RGB(0,0,0)
y_axis_label_color blank
y_axis_on true
axes_on true
y_axis_value_color RGB(0,0,0)
y_axis_width 1
y_label Y-axis
y_label_color blank
y_label_font_family Verdana
y_label_font_size 14
y_label_on true
y_label_units
y_label_units_on false
y_label_width 0
y_major_grid_on true
y_major_grid_color RGB(200,220,255)
y_major_grid_width 2
y_major_interval 2
y_major_labels_side bottom
y_major_label_rotation 0
y_major_tick_color RGB(0,0,0)
y_major_tick_length  10
y_major_tick_width  2
y_minor_grid_on true
y_minor_grid_color  RGB(240,240,255)
y_minor_grid_width 1
y_minor_interval 0
y_minor_tick_color RGB(0,0,0)
y_minor_tick_length 5
y_minor_tick_width 1
y_range() 1, 10
y_num_minor_ticks
y_ticks_left_on true
y_ticks_right_on false
y_ticks_on_window_or_axis none
y_max 10
y_min 1
y_values_on false
y_plusminus_on false
y_plusminus_color RGB(0,0,255)
y_df_on false
y_df_color RGB(0,0,255)
y_prefix
y_separator
y_suffix
data lines width 2
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> ./demo_2d_plot_YN.svg
axes_on true
background_border_width 2
background_border_color RGB(255,255,0)
background_color RGB(248,248,255)
image_border_margin() 3
image_border_width() 2
coord_precision 3
copyright_date
copyright_holder
description
document_title
x_size 700
image_y_size 500
image_size 700, 500
image_filename
legend_on true
legend_place 2
legend_top_left 538.1, 58, legend_bottom_right 692, 202
legend_background_color RGB(255,255,224)
legend_border_color RGB(255,255,0)
legend_color blank
legend_title
legend_title_font_size 16
legend_font_weight
legend_width 153.9
legend_lines true
limit points stroke color RGB(128,128,128)
limit points fill color blank
license_on false
license_reproduction permits
license_distribution permits
license_attribution requires
license_commercialuse permits
plot_background_color RGB(255,255,255)
plot_border_color RGB(0,128,0)
plot_border_width 1
plot_window_on true
plot_window_x 46, 524.1
plot_window_x_left 46
plot_window_x_right 524.1
plot_window_y 58, 454
plot_window_y_top 58
plot_window_y_bottom 454
title_on true
title ""
title_color RGB(255,0,0)
title_font_alignment 2
title_font_decoration
title_font_family Verdana
title_font_rotation 0
title_font_size 20
title_font_stretch
title_font_style
title_font_weight
x_values_on false
x_values_font_size 12
x_values_font_family
x_values_precision 3
x_values_ioflags 200 IOS format flags (0x1200) dec, scientific.
y_values_precision 3
y_values_font_size() 1
y_values_ioflags 200 IOS format flags (0x200) dec.
y_values_color blank
y_values_font_family()
y_values_font_size() 12
x_max 10
x_min -1
x_autoscale false
y_autoscale false
xy_autoscale false
x_autoscale_check_limits true
x_axis_on true
x_axis_color() RGB(0,0,0)
x_axis_label_color blank
x_values_color blank
x_axis_width 1
x_label_on true
x_label X-axis
x_label_color blank
x_label_font_family Verdana
x_label_font_size 14
x_label_units
x_label_units_on false
x_major_labels_side left
x_major_label_rotation 0
x_major_grid_color RGB(200,220,255)
x_major_grid_on true
x_major_grid_width 1
x_major_interval 2
x_major_tick 2
x_major_tick_color RGB(0,0,0)
x_major_tick_length 10
x_major_tick_width 2
x_minor_interval 0
x_minor_tick_color RGB(0,0,0)
x_minor_tick_length 5
x_minor_tick_width 1
x_minor_grid_on true
x_minor_grid_color RGB(240,240,255)
x_minor_grid_width 0.5
x_range() -1, 10
x_num_minor_ticks 2
x_ticks_down_on true
x_ticks_up_on false
x_ticks_on_window_or_axis top
y_axis_position y_axis_position intersects X axis (X range includes zero)
x_axis_position x_axis_position top (all Y values < 0)
x_plusminus_on false
x_plusminus_color RGB(0,0,255)
x_df_on false
x_df_color RGB(0,0,255)
x_prefix
x_separator
x_suffix
xy_values_on false
y_label_on true
y_label_axis Y-axis
y_axis_color RGB(0,0,0)
y_axis_label_color blank
y_axis_on true
axes_on true
y_axis_value_color RGB(0,0,0)
y_axis_width 1
y_label Y-axis
y_label_color blank
y_label_font_family Verdana
y_label_font_size 14
y_label_on true
y_label_units
y_label_units_on false
y_label_width 0
y_major_grid_on true
y_major_grid_color RGB(200,220,255)
y_major_grid_width 2
y_major_interval 2
y_major_labels_side bottom
y_major_label_rotation 0
y_major_tick_color RGB(0,0,0)
y_major_tick_length  10
y_major_tick_width  2
y_minor_grid_on true
y_minor_grid_color  RGB(240,240,255)
y_minor_grid_width 1
y_minor_interval 0
y_minor_tick_color RGB(0,0,0)
y_minor_tick_length 5
y_minor_tick_width 1
y_range() -10, -1
y_num_minor_ticks
y_ticks_left_on true
y_ticks_right_on false
y_ticks_on_window_or_axis none
y_max -1
y_min -10
y_values_on false
y_plusminus_on false
y_plusminus_color RGB(0,0,255)
y_df_on false
y_df_color RGB(0,0,255)
y_prefix
y_separator
y_suffix
data lines width 2
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> ./demo_2d_plot_XYP.svg
axes_on true
background_border_width 2
background_border_color RGB(255,255,0)
background_color RGB(248,248,255)
image_border_margin() 3
image_border_width() 2
coord_precision 3
copyright_date
copyright_holder
description
document_title
x_size 700
image_y_size 500
image_size 700, 500
image_filename
legend_on true
legend_place 2
legend_top_left 538.1, 58, legend_bottom_right 692, 202
legend_background_color RGB(255,255,224)
legend_border_color RGB(255,255,0)
legend_color blank
legend_title
legend_title_font_size 16
legend_font_weight
legend_width 153.9
legend_lines true
limit points stroke color RGB(128,128,128)
limit points fill color blank
license_on false
license_reproduction permits
license_distribution permits
license_attribution requires
license_commercialuse permits
plot_background_color RGB(255,255,255)
plot_border_color RGB(0,128,0)
plot_border_width 1
plot_window_on true
plot_window_x 79.6, 524.1
plot_window_x_left 79.6
plot_window_x_right 524.1
plot_window_y 58, 436
plot_window_y_top 58
plot_window_y_bottom 436
title_on true
title ""
title_color RGB(255,0,0)
title_font_alignment 2
title_font_decoration
title_font_family Verdana
title_font_rotation 0
title_font_size 20
title_font_stretch
title_font_style
title_font_weight
x_values_on false
x_values_font_size 12
x_values_font_family
x_values_precision 3
x_values_ioflags 200 IOS format flags (0x1200) dec, scientific.
y_values_precision 3
y_values_font_size() 1
y_values_ioflags 200 IOS format flags (0x200) dec.
y_values_color blank
y_values_font_family()
y_values_font_size() 12
x_max 10
x_min 1
x_autoscale false
y_autoscale false
xy_autoscale false
x_autoscale_check_limits true
x_axis_on true
x_axis_color() RGB(0,0,0)
x_axis_label_color blank
x_values_color blank
x_axis_width 1
x_label_on true
x_label X-axis
x_label_color blank
x_label_font_family Verdana
x_label_font_size 14
x_label_units
x_label_units_on false
x_major_labels_side left
x_major_label_rotation 0
x_major_grid_color RGB(200,220,255)
x_major_grid_on true
x_major_grid_width 1
x_major_interval 2
x_major_tick 2
x_major_tick_color RGB(0,0,0)
x_major_tick_length 10
x_major_tick_width 2
x_minor_interval 0
x_minor_tick_color RGB(0,0,0)
x_minor_tick_length 5
x_minor_tick_width 1
x_minor_grid_on true
x_minor_grid_color RGB(240,240,255)
x_minor_grid_width 0.5
x_range() 1, 10
x_num_minor_ticks 2
x_ticks_down_on true
x_ticks_up_on false
x_ticks_on_window_or_axis bottom
y_axis_position y_axis_position left (all X values < 0)
x_axis_position x_axis_position bottom (all Y values > 0)
x_plusminus_on false
x_plusminus_color RGB(0,0,255)
x_df_on false
x_df_color RGB(0,0,255)
x_prefix
x_separator
x_suffix
xy_values_on false
y_label_on true
y_label_axis Y-axis
y_axis_color RGB(0,0,0)
y_axis_label_color blank
y_axis_on true
axes_on true
y_axis_value_color RGB(0,0,0)
y_axis_width 1
y_label Y-axis
y_label_color blank
y_label_font_family Verdana
y_label_font_size 14
y_label_on true
y_label_units
y_label_units_on false
y_label_width 0
y_major_grid_on true
y_major_grid_color RGB(200,220,255)
y_major_grid_width 2
y_major_interval 2
y_major_labels_side bottom
y_major_label_rotation 0
y_major_tick_color RGB(0,0,0)
y_major_tick_length  10
y_major_tick_width  2
y_minor_grid_on true
y_minor_grid_color  RGB(240,240,255)
y_minor_grid_width 1
y_minor_interval 0
y_minor_tick_color RGB(0,0,0)
y_minor_tick_length 5
y_minor_tick_width 1
y_range() 1, 10
y_num_minor_ticks
y_ticks_left_on true
y_ticks_right_on false
y_ticks_on_window_or_axis left
y_max 10
y_min 1
y_values_on false
y_plusminus_on false
y_plusminus_color RGB(0,0,255)
y_df_on false
y_df_color RGB(0,0,255)
y_prefix
y_separator
y_suffix
data lines width 2
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> ./demo_2d_plot_XYN.svg
axes_on true
background_border_width 2
background_border_color RGB(255,255,0)
background_color RGB(248,248,255)
image_border_margin() 3
image_border_width() 2
coord_precision 3
copyright_date
copyright_holder
description
document_title
x_size 700
image_y_size 500
image_size 700, 500
image_filename
legend_on true
legend_place 2
legend_top_left 538.1, 58, legend_bottom_right 692, 202
legend_background_color RGB(255,255,224)
legend_border_color RGB(255,255,0)
legend_color blank
legend_title
legend_title_font_size 16
legend_font_weight
legend_width 153.9
legend_lines true
limit points stroke color RGB(128,128,128)
limit points fill color blank
license_on false
license_reproduction permits
license_distribution permits
license_attribution requires
license_commercialuse permits
plot_background_color RGB(255,255,255)
plot_border_color RGB(0,128,0)
plot_border_width 1
plot_window_on true
plot_window_x 46, 524.1
plot_window_x_left 46
plot_window_x_right 524.1
plot_window_y 58, 454
plot_window_y_top 58
plot_window_y_bottom 454
title_on true
title ""
title_color RGB(255,0,0)
title_font_alignment 2
title_font_decoration
title_font_family Verdana
title_font_rotation 0
title_font_size 20
title_font_stretch
title_font_style
title_font_weight
x_values_on false
x_values_font_size 12
x_values_font_family
x_values_precision 3
x_values_ioflags 200 IOS format flags (0x1200) dec, scientific.
y_values_precision 3
y_values_font_size() 1
y_values_ioflags 200 IOS format flags (0x200) dec.
y_values_color blank
y_values_font_family()
y_values_font_size() 12
x_max -1
x_min -10
x_autoscale false
y_autoscale false
xy_autoscale false
x_autoscale_check_limits true
x_axis_on true
x_axis_color() RGB(0,0,0)
x_axis_label_color blank
x_values_color blank
x_axis_width 1
x_label_on true
x_label X-axis
x_label_color blank
x_label_font_family Verdana
x_label_font_size 14
x_label_units
x_label_units_on false
x_major_labels_side left
x_major_label_rotation 0
x_major_grid_color RGB(200,220,255)
x_major_grid_on true
x_major_grid_width 1
x_major_interval 2
x_major_tick 2
x_major_tick_color RGB(0,0,0)
x_major_tick_length 10
x_major_tick_width 2
x_minor_interval 0
x_minor_tick_color RGB(0,0,0)
x_minor_tick_length 5
x_minor_tick_width 1
x_minor_grid_on true
x_minor_grid_color RGB(240,240,255)
x_minor_grid_width 0.5
x_range() -10, -1
x_num_minor_ticks 2
x_ticks_down_on true
x_ticks_up_on false
x_ticks_on_window_or_axis top
y_axis_position y_axis_position right (all X values > 0)
x_axis_position x_axis_position top (all Y values < 0)
x_plusminus_on false
x_plusminus_color RGB(0,0,255)
x_df_on false
x_df_color RGB(0,0,255)
x_prefix
x_separator
x_suffix
xy_values_on false
y_label_on true
y_label_axis Y-axis
y_axis_color RGB(0,0,0)
y_axis_label_color blank
y_axis_on true
axes_on true
y_axis_value_color RGB(0,0,0)
y_axis_width 1
y_label Y-axis
y_label_color blank
y_label_font_family Verdana
y_label_font_size 14
y_label_on true
y_label_units
y_label_units_on false
y_label_width 0
y_major_grid_on true
y_major_grid_color RGB(200,220,255)
y_major_grid_width 2
y_major_interval 2
y_major_labels_side bottom
y_major_label_rotation 0
y_major_tick_color RGB(0,0,0)
y_major_tick_length  10
y_major_tick_width  2
y_minor_grid_on true
y_minor_grid_color  RGB(240,240,255)
y_minor_grid_width 1
y_minor_interval 0
y_minor_tick_color RGB(0,0,0)
y_minor_tick_length 5
y_minor_tick_width 1
y_range() -10, -1
y_num_minor_ticks
y_ticks_left_on true
y_ticks_right_on false
y_ticks_on_window_or_axis right
y_max -1
y_min -10
y_values_on false
y_plusminus_on false
y_plusminus_color RGB(0,0,255)
y_df_on false
y_df_color RGB(0,0,255)
y_prefix
y_separator
y_suffix
data lines width 2
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> ./demo_2d_plot_XYPM.svg
axes_on true
background_border_width 2
background_border_color RGB(255,255,0)
background_color RGB(248,248,255)
image_border_margin() 3
image_border_width() 2
coord_precision 3
copyright_date
copyright_holder
description
document_title
x_size 700
image_y_size 500
image_size 700, 500
image_filename
legend_on true
legend_place 2
legend_top_left 538.1, 58, legend_bottom_right 692, 202
legend_background_color RGB(255,255,224)
legend_border_color RGB(255,255,0)
legend_color blank
legend_title
legend_title_font_size 16
legend_font_weight
legend_width 153.9
legend_lines true
limit points stroke color RGB(128,128,128)
limit points fill color blank
license_on false
license_reproduction permits
license_distribution permits
license_attribution requires
license_commercialuse permits
plot_background_color RGB(255,255,255)
plot_border_color RGB(0,128,0)
plot_border_width 1
plot_window_on true
plot_window_x 46, 524.1
plot_window_x_left 46
plot_window_x_right 524.1
plot_window_y 58, 454
plot_window_y_top 58
plot_window_y_bottom 454
title_on true
title ""
title_color RGB(255,0,0)
title_font_alignment 2
title_font_decoration
title_font_family Verdana
title_font_rotation 0
title_font_size 20
title_font_stretch
title_font_style
title_font_weight
x_values_on false
x_values_font_size 12
x_values_font_family
x_values_precision 3
x_values_ioflags 200 IOS format flags (0x1200) dec, scientific.
y_values_precision 3
y_values_font_size() 1
y_values_ioflags 200 IOS format flags (0x200) dec.
y_values_color blank
y_values_font_family()
y_values_font_size() 12
x_max 10
x_min -10
x_autoscale false
y_autoscale false
xy_autoscale false
x_autoscale_check_limits true
x_axis_on true
x_axis_color() RGB(0,0,0)
x_axis_label_color blank
x_values_color blank
x_axis_width 1
x_label_on true
x_label X-axis ±Ω
x_label_color blank
x_label_font_family Verdana
x_label_font_size 14
x_label_units
x_label_units_on false
x_major_labels_side left
x_major_label_rotation 0
x_major_grid_color RGB(200,220,255)
x_major_grid_on true
x_major_grid_width 1
x_major_interval 2
x_major_tick 2
x_major_tick_color RGB(0,0,0)
x_major_tick_length 10
x_major_tick_width 2
x_minor_interval 0
x_minor_tick_color RGB(0,0,0)
x_minor_tick_length 5
x_minor_tick_width 1
x_minor_grid_on true
x_minor_grid_color RGB(240,240,255)
x_minor_grid_width 0.5
x_range() -10, 10
x_num_minor_ticks 2
x_ticks_down_on true
x_ticks_up_on false
x_ticks_on_window_or_axis none
y_axis_position y_axis_position intersects X axis (X range includes zero)
x_axis_position x_axis_position intersects Y axis (Y range includes zero)
x_plusminus_on false
x_plusminus_color RGB(0,0,255)
x_df_on false
x_df_color RGB(0,0,255)
x_prefix
x_separator
x_suffix
xy_values_on false
y_label_on true
y_label_axis Y-axis ∞√
y_axis_color RGB(0,0,0)
y_axis_label_color blank
y_axis_on true
axes_on true
y_axis_value_color RGB(0,0,0)
y_axis_width 1
y_label Y-axis ∞√
y_label_color blank
y_label_font_family Verdana
y_label_font_size 14
y_label_on true
y_label_units
y_label_units_on false
y_label_width 0
y_major_grid_on true
y_major_grid_color RGB(200,220,255)
y_major_grid_width 2
y_major_interval 2
y_major_labels_side bottom
y_major_label_rotation 0
y_major_tick_color RGB(0,0,0)
y_major_tick_length  10
y_major_tick_width  2
y_minor_grid_on true
y_minor_grid_color  RGB(240,240,255)
y_minor_grid_width 1
y_minor_interval 0
y_minor_tick_color RGB(0,0,0)
y_minor_tick_length 5
y_minor_tick_width 1
y_range() -10, 10
y_num_minor_ticks
y_ticks_left_on true
y_ticks_right_on false
y_ticks_on_window_or_axis none
y_max 10
y_min -10
y_values_on false
y_plusminus_on false
y_plusminus_color RGB(0,0,255)
y_df_on false
y_df_color RGB(0,0,255)
y_prefix
y_separator
y_suffix
data lines width 2
Build Time 0:05

*/

/*

Parked stuff:

  // TODO autoscale
  // int scale_axis(double min_value, double max_value, // input
  //             double* axis_min_value,  double* axis_max_value, double* axis_tick_increment, // updated.
  //             bool origin, double tight);

  // Use map with x int and y double.
  map<int, double> datai1;
  map<int, double> datai2;

  other functions

    datai1[i] = sq(i);
    datai2[i] = recip(i);

  unsigned int legend_title_font_size = my_plot.legend_title_font_size();
  cout << "Legend title font size is " << legend_title_font_size << endl;
   unsigned int title_font_size = my_plot.title_font_size();
  cout << "Title font size is " << title_font_size << endl;
  cout << "Title color is " << my_plot.title_color() << endl;

  //text_style label_style(40, "sans"); // Or use all defaults.
  //cout << label_style.font_size() << endl;
  //cout << label_style.font_family() << endl;
  //label_style.font_weight("bold");
 // cout << label_style.font_weight() << endl;
  //label_style.font_size(30);
//  cout << label_style.font_size() << endl;

 // text_style& x_label_style(); // returns ref to the style.

//Then the user could write:
//
//x_label_style().font_size(20);
//x_label_style().font_size(20).font_family("Arial");
//
//x_label_style().font_family("Arial");
//
//which is fairly readable and logical, and doesn't clutter the plot classes
//with so many methods or data members for that matter.  One could even create
//a text_style object and reuse it for multiple graphs or multiple items
//within one graph:
//
//my_plot1.x_label_style = my_plot1.y_label_style = myplot2.x_label_style =
//mystyle_object; // etc

  */


// Copyright 2010 The Trustees of Indiana University.

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

//  Authors: Jeremiah Willcock
//           Andrew Lumsdaine

#include <boost/graph/random_spanning_tree.hpp>
#include <boost/graph/grid_graph.hpp>
#include <boost/array.hpp>
#include <boost/random.hpp>
#include <boost/property_map/shared_array_property_map.hpp>
#include <boost/property_map/dynamic_property_map.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/graph/property_maps/constant_property_map.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include <boost/graph/iteration_macros.hpp>

using namespace boost;
using namespace std;

typedef grid_graph< 2 > graph_type;
typedef graph_traits< graph_type > gt;

template < typename Graph, typename PredMap, typename WeightMap >
void write_spanning_tree(
    const Graph& g, PredMap pred, WeightMap weight, string filename)
{
    shared_array_property_map< string,
        typename property_map< Graph, edge_index_t >::const_type >
        edge_style(num_edges(g), get(edge_index, g));
    shared_array_property_map< string,
        typename property_map< Graph, vertex_index_t >::const_type >
        vertex_pos(num_vertices(g), get(vertex_index, g));
    BGL_FORALL_EDGES_T(e, g, Graph)
    {
        put(edge_style, e,
            (get(pred, target(e, g)) == source(e, g)
                || get(pred, source(e, g)) == target(e, g))
                ? "bold"
                : "dotted");
    }
    BGL_FORALL_VERTICES_T(v, g, Graph)
    {
        put(vertex_pos, v,
            lexical_cast< string >(v[0]) + "," + lexical_cast< string >(v[1]));
    }
    dynamic_properties dp;
    dp.property("style", edge_style);
    dp.property("pos", vertex_pos);
    dp.property("len", weight);
    dp.property("node_id", get(vertex_index, g));
    ofstream out(filename.c_str());
    write_graphviz_dp(out, g, dp);
}

int main(int, char**)
{

    boost::array< size_t, 2 > sizes = { { 5, 5 } };
    graph_type g(sizes);

    shared_array_property_map< gt::vertex_descriptor,
        property_map< graph_type, vertex_index_t >::const_type >
        pred(num_vertices(g), get(vertex_index, g));
    shared_array_property_map< double,
        property_map< graph_type, edge_index_t >::const_type >
        weight(num_edges(g), get(edge_index, g));

    BGL_FORALL_EDGES(e, g, graph_type)
    {
        put(weight, e, (1. + get(edge_index, g, e)) / num_edges(g));
    }

    boost::mt19937 gen;
    random_spanning_tree(g, gen, predecessor_map(pred));
    // write_spanning_tree(g, pred, constant_property_map<gt::edge_descriptor,
    // double>(1.), "unweight_random_st.dot");
    random_spanning_tree(g, gen, predecessor_map(pred));
    // write_spanning_tree(g, pred, constant_property_map<gt::edge_descriptor,
    // double>(1.), "unweight_random_st2.dot");

    random_spanning_tree(g, gen, predecessor_map(pred).weight_map(weight));
    // write_spanning_tree(g, pred, weight, "weight_random_st.dot");

    return 0;
}


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

#include <sstream>
#include <string>
#include <tuple>

#include "cta.hpp"

using namespace cta;

static auto x0 = make_var<double, 0>();
static auto x1 = make_var<double, 1>();

template <typename T>
std::string stringify(const T& t) {
	std::stringstream r;
	r << t;
	return r.str();
}

BOOST_AUTO_TEST_CASE(construct) {
	BOOST_CHECK_EQUAL("x0", stringify(x0));
	BOOST_CHECK_EQUAL("(-x0)", stringify(-x0));
	BOOST_CHECK_EQUAL("(x0+1)", stringify(x0 + 1.0));
	BOOST_CHECK_EQUAL("(x0-1)", stringify(x0 - 1.0));
	BOOST_CHECK_EQUAL("(x0*2)", stringify(x0 * 2.0));
	BOOST_CHECK_EQUAL("(x0*x1)", stringify(x0 * x1));
	BOOST_CHECK_EQUAL("(x0/3)", stringify(x0 / 3.0));
	BOOST_CHECK_EQUAL("sin(x0)", stringify(sin(x0)));
	BOOST_CHECK_EQUAL("cos(x0)", stringify(cos(x0)));
	BOOST_CHECK_EQUAL("atan(x0)", stringify(atan(x0)));
	BOOST_CHECK_EQUAL("pow(x0, 2)", stringify(pow(x0, 2)));
}

BOOST_AUTO_TEST_CASE(eval) {
	BOOST_CHECK_EQUAL(1, x0(1));
	BOOST_CHECK_EQUAL(-2, (-x0)(2));
	BOOST_CHECK_EQUAL(3, (x0 + x1)(1, 2));
	BOOST_CHECK_EQUAL(-1, (x0 - x1)(1, 2));
	BOOST_CHECK_EQUAL(2, (x0 / x1)(4, 2));
	BOOST_CHECK_EQUAL(8, (x0 * x1)(4, 2));
	BOOST_CHECK_EQUAL(0, sin(x0)(0));
	BOOST_CHECK_EQUAL(1, cos(x0)(0));
	BOOST_CHECK_EQUAL(9, pow(x0, 2)(3));
	BOOST_CHECK_CLOSE(M_PI / 8, atan(x0)(sqrt(2)-1), 1e-10);
}

BOOST_AUTO_TEST_CASE(diff) {
	auto test = [](const std::string& s, auto exp) {
		BOOST_CHECK_EQUAL(s, stringify(differentiate(exp, x0)));
	};

	test("1", x0);
	test("0", x1);
	test("-1", -x0);
	test("0", -x1);
	test("1", x0+x1);
	test("2", x0*2.0);
	test("0.5", x0/2.0);
	test("x1", x0*x1);
	test("(2*x0)", pow<2>(x0));
	test("(2*pow(x0, 1))", pow(x0, 2));
	test("cos(x0)", sin(x0));
	test("(-sin(x0))", cos(x0));
	test("(1/(pow(x0, 2)+1))", atan(x0));
}

BOOST_AUTO_TEST_CASE(dvar) {
	auto t = make_var<double, 0>();
	auto g = make_dvar<double, 1>();
	auto f = t + pow<2>(g);
	auto fdt = differentiate(f, t);
	BOOST_CHECK_CLOSE(101, fdt(1, std::make_tuple(5, 10)), 1e-10);
}


//==================================================================================================
/**
  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)
**/
//==================================================================================================
#include <boost/simd/function/slide.hpp>
#include <boost/simd/logical.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

namespace bs = boost::simd;

template <typename T, int N, typename Env> void test(Env& $)
{
  std::array<bs::logical<T>,3*N> data;
  for(std::size_t i = 0;i < data.size(); ++i) data[i] = false;
  for(int i = N;i < 2*N; ++i) data[i] = (i%2 != 0);

  using p_t = bs::pack<bs::logical<T>,N>;
  p_t ref, p = bs::load<p_t>(&data[N], 0);

  STF_EQUAL( bs::slide<-N>(p), bs::Zero<p_t>() );

  ref = bs::load<p_t>(&data[N], -1);
  STF_EQUAL( bs::slide<-1>(p), ref );

  ref = bs::load<p_t>(&data[N]);
  STF_EQUAL( bs::slide<0>(p), ref );

  ref = bs::load<p_t>(&data[N], +1);
  STF_EQUAL( bs::slide<+1>(p), ref );

  STF_EQUAL( bs::slide<N>(p), bs::Zero<p_t>() );
}

STF_CASE_TPL( "Check unary slide behavior", STF_NUMERIC_TYPES )
{
  static const int N = bs::pack<bs::logical<T>>::static_size;

  test<T, N  >($);
  test<T, N/2>($);
  test<T, N*2>($);
}


#include "ackermann_ekf/ackermann_ekf.h"

#include <boost/algorithm/clamp.hpp>
#include <math.h>
#include <vector>

namespace ackermann_ekf {

AckermannEkf::AckermannEkf(const Eigen::VectorXd x_min,
                           const Eigen::VectorXd x_max, double wheelbase,
                           double control_acceleration_gain,
                           double max_control_acceleration,
                           double control_angle_speed_gain,
                           double max_control_angle_speed)
    : x(STATE_SIZE),
      P(STATE_SIZE, STATE_SIZE),
      Q(STATE_SIZE, STATE_SIZE),
      x_min_(x_min),
      x_max_(x_max),
      wheelbase_(wheelbase),
      control_acceleration_gain_(control_acceleration_gain),
      max_control_acceleration_(max_control_acceleration),
      control_angle_speed_gain_(control_angle_speed_gain),
      max_control_angle_speed_(max_control_angle_speed) {
    // Initialize the state to zeros, this is compensated for with a high
    // initial state covariance (P), see below
    // \todo Read initial state vector from a parameter
    x.setZero();

    // Set the initial state covariance to a constant matrix
    // \todo Read the initial state covariance matrix from a parameter
    P.setIdentity();
    P *= 1e1;

    // Set the process noise covariance to a constant matrix
    // \todo Read the process noise covariance matrix from a parameter
    Q.setZero();
    Q(State::X, State::X) = 1e-3;
    Q(State::Y, State::Y) = 1e-3;
    Q(State::Z, State::Z) = 1e-4;
    Q(State::speed, State::speed) = 1e-0;
    Q(State::accel, State::accel) = 1e+1;
    Q(State::Roll, State::Roll) = 1e-4;
    Q(State::Pitch, State::Pitch) = 1e-4;
    Q(State::Yaw, State::Yaw) = 1e-3;
    Q(State::droll_dx, State::droll_dx) = 1e-6;
    Q(State::dpitch_dx, State::dpitch_dx) = 1e-6;
    Q(State::dyaw_dx, State::dyaw_dx) = 1e+0;
}

void AckermannEkf::process_measurement(const Measurement &measurement) {
    bring_time_forward_to(measurement.time);

    correct(measurement);
}

void AckermannEkf::process_control_signal(const ControlSignal &control_signal) {
    bring_time_forward_to(control_signal.time);

    // Enable control signals for prediction step as soon as one is recieved
    control_signal_enabled_ = true;
    control_signal_ = control_signal;
}

void AckermannEkf::bring_time_forward_to(double time) {
    if (time > this->time) {
        // Only predict if the filter time has been initialized, otherwise dt
        // may be VERY large
        if (this->time >= 0) {
            predict(time - this->time);
        }
        this->time = time;
    }
}

void AckermannEkf::predict(double dt) {
    if (control_signal_enabled_) {
        // Accelration is linear gain on velocity error
        x(State::accel) = boost::algorithm::clamp(
            (control_signal_.u(ControlSignal::speed) - x(State::speed)) *
                control_acceleration_gain_,
            -max_control_acceleration_, max_control_acceleration_);

        double steering_angle = atan(wheelbase_ * x(State::dyaw_dx));
        // Add (steering angle speed) * dt to steering angle, where steering
        // angle speed is linear gain on steering angle error
        steering_angle +=
            boost::algorithm::clamp(
                (control_signal_.u(ControlSignal::angle) - steering_angle) *
                    control_angle_speed_gain_,
                -max_control_angle_speed_, max_control_angle_speed_) *
            dt;
        steering_angle = boost::algorithm::clamp(steering_angle, -M_PI * 0.499,
                                                 M_PI * 0.499);

        x(State::dyaw_dx) = tan(steering_angle) / wheelbase_;

        constrain_state();
    }

    Eigen::VectorXd f(STATE_SIZE);
    Eigen::MatrixXd F(STATE_SIZE, STATE_SIZE);
    f.setZero();
    F.setZero();

    // clang-format off

    // The code here has been autogenerated by matlab and manually pasted in.
    // Do not edit directly!

    f(State::X) = x(State::X)-((dt*dt)*(x(State::speed)*(x(State::dpitch_dx)*x(State::speed)*(sin(x(State::Roll))*sin(x(State::Yaw))+cos(x(State::Roll))*cos(x(State::Yaw))*sin(x(State::Pitch)))+x(State::dyaw_dx)*x(State::speed)*(cos(x(State::Roll))*sin(x(State::Yaw))-cos(x(State::Yaw))*sin(x(State::Pitch))*sin(x(State::Roll))))-x(State::accel)*cos(x(State::Pitch))*cos(x(State::Yaw))))/2.0+dt*x(State::speed)*cos(x(State::Pitch))*cos(x(State::Yaw));
    f(State::Y) = x(State::Y)+((dt*dt)*(x(State::speed)*(x(State::dpitch_dx)*x(State::speed)*(cos(x(State::Yaw))*sin(x(State::Roll))-cos(x(State::Roll))*sin(x(State::Pitch))*sin(x(State::Yaw)))+x(State::dyaw_dx)*x(State::speed)*(cos(x(State::Roll))*cos(x(State::Yaw))+sin(x(State::Pitch))*sin(x(State::Roll))*sin(x(State::Yaw))))+x(State::accel)*cos(x(State::Pitch))*sin(x(State::Yaw))))/2.0+dt*x(State::speed)*cos(x(State::Pitch))*sin(x(State::Yaw));
    f(State::Z) = x(State::Z)-((dt*dt)*(x(State::speed)*(x(State::dpitch_dx)*x(State::speed)*cos(x(State::Pitch))*cos(x(State::Roll))-x(State::dyaw_dx)*x(State::speed)*cos(x(State::Pitch))*sin(x(State::Roll)))+x(State::accel)*sin(x(State::Pitch))))/2.0-dt*x(State::speed)*sin(x(State::Pitch));
    f(State::speed) = x(State::speed)+x(State::accel)*dt;
    f(State::accel) = x(State::accel);
    f(State::Roll) = (x(State::Roll)*cos(x(State::Pitch))+x(State::droll_dx)*dt*x(State::speed)*cos(x(State::Pitch))+dt*x(State::dyaw_dx)*x(State::speed)*cos(x(State::Roll))*sin(x(State::Pitch))+x(State::dpitch_dx)*dt*x(State::speed)*sin(x(State::Pitch))*sin(x(State::Roll)))/cos(x(State::Pitch));
    f(State::Pitch) = x(State::Pitch)+dt*x(State::speed)*(x(State::dpitch_dx)*cos(x(State::Roll))-x(State::dyaw_dx)*sin(x(State::Roll)));
    f(State::Yaw) = (x(State::Yaw)*cos(x(State::Pitch))+dt*x(State::dyaw_dx)*x(State::speed)*cos(x(State::Roll))+x(State::dpitch_dx)*dt*x(State::speed)*sin(x(State::Roll)))/cos(x(State::Pitch));
    f(State::droll_dx) = x(State::droll_dx);
    f(State::dpitch_dx) = x(State::dpitch_dx);
    f(State::dyaw_dx) = x(State::dyaw_dx);

    F(State::X, State::X) = 1.0;
    F(State::X, State::speed) = -dt*(-cos(x(State::Pitch))*cos(x(State::Yaw))+dt*x(State::dyaw_dx)*x(State::speed)*cos(x(State::Roll))*sin(x(State::Yaw))+x(State::dpitch_dx)*dt*x(State::speed)*sin(x(State::Roll))*sin(x(State::Yaw))+x(State::dpitch_dx)*dt*x(State::speed)*cos(x(State::Roll))*cos(x(State::Yaw))*sin(x(State::Pitch))-dt*x(State::dyaw_dx)*x(State::speed)*cos(x(State::Yaw))*sin(x(State::Pitch))*sin(x(State::Roll)));
    F(State::X, State::accel) = ((dt*dt)*cos(x(State::Pitch))*cos(x(State::Yaw)))/2.0;
    F(State::X, State::Roll) = (dt*dt)*x(State::speed)*(x(State::dpitch_dx)*x(State::speed)*(cos(x(State::Roll))*sin(x(State::Yaw))-cos(x(State::Yaw))*sin(x(State::Pitch))*sin(x(State::Roll)))-x(State::dyaw_dx)*x(State::speed)*(sin(x(State::Roll))*sin(x(State::Yaw))+cos(x(State::Roll))*cos(x(State::Yaw))*sin(x(State::Pitch))))*(-1.0/2.0);
    F(State::X, State::Pitch) = dt*cos(x(State::Yaw))*(x(State::speed)*sin(x(State::Pitch))*2.0+x(State::accel)*dt*sin(x(State::Pitch))+x(State::dpitch_dx)*dt*(x(State::speed)*x(State::speed))*cos(x(State::Pitch))*cos(x(State::Roll))-dt*x(State::dyaw_dx)*(x(State::speed)*x(State::speed))*cos(x(State::Pitch))*sin(x(State::Roll)))*(-1.0/2.0);
    F(State::X, State::Yaw) = (dt*dt)*(x(State::speed)*(x(State::dpitch_dx)*x(State::speed)*(cos(x(State::Yaw))*sin(x(State::Roll))-cos(x(State::Roll))*sin(x(State::Pitch))*sin(x(State::Yaw)))+x(State::dyaw_dx)*x(State::speed)*(cos(x(State::Roll))*cos(x(State::Yaw))+sin(x(State::Pitch))*sin(x(State::Roll))*sin(x(State::Yaw))))+x(State::accel)*cos(x(State::Pitch))*sin(x(State::Yaw)))*(-1.0/2.0)-dt*x(State::speed)*cos(x(State::Pitch))*sin(x(State::Yaw));
    F(State::X, State::dpitch_dx) = (dt*dt)*(x(State::speed)*x(State::speed))*(sin(x(State::Roll))*sin(x(State::Yaw))+cos(x(State::Roll))*cos(x(State::Yaw))*sin(x(State::Pitch)))*(-1.0/2.0);
    F(State::X, State::dyaw_dx) = (dt*dt)*(x(State::speed)*x(State::speed))*(cos(x(State::Roll))*sin(x(State::Yaw))-cos(x(State::Yaw))*sin(x(State::Pitch))*sin(x(State::Roll)))*(-1.0/2.0);
    F(State::Y, State::Y) = 1.0;
    F(State::Y, State::speed) = dt*(cos(x(State::Pitch))*sin(x(State::Yaw))+dt*x(State::dyaw_dx)*x(State::speed)*cos(x(State::Roll))*cos(x(State::Yaw))+x(State::dpitch_dx)*dt*x(State::speed)*cos(x(State::Yaw))*sin(x(State::Roll))-x(State::dpitch_dx)*dt*x(State::speed)*cos(x(State::Roll))*sin(x(State::Pitch))*sin(x(State::Yaw))+dt*x(State::dyaw_dx)*x(State::speed)*sin(x(State::Pitch))*sin(x(State::Roll))*sin(x(State::Yaw)));
    F(State::Y, State::accel) = ((dt*dt)*cos(x(State::Pitch))*sin(x(State::Yaw)))/2.0;
    F(State::Y, State::Roll) = ((dt*dt)*x(State::speed)*(x(State::dpitch_dx)*x(State::speed)*(cos(x(State::Roll))*cos(x(State::Yaw))+sin(x(State::Pitch))*sin(x(State::Roll))*sin(x(State::Yaw)))-x(State::dyaw_dx)*x(State::speed)*(cos(x(State::Yaw))*sin(x(State::Roll))-cos(x(State::Roll))*sin(x(State::Pitch))*sin(x(State::Yaw)))))/2.0;
    F(State::Y, State::Pitch) = dt*sin(x(State::Yaw))*(x(State::speed)*sin(x(State::Pitch))*2.0+x(State::accel)*dt*sin(x(State::Pitch))+x(State::dpitch_dx)*dt*(x(State::speed)*x(State::speed))*cos(x(State::Pitch))*cos(x(State::Roll))-dt*x(State::dyaw_dx)*(x(State::speed)*x(State::speed))*cos(x(State::Pitch))*sin(x(State::Roll)))*(-1.0/2.0);
    F(State::Y, State::Yaw) = (dt*dt)*(x(State::speed)*(x(State::dpitch_dx)*x(State::speed)*(sin(x(State::Roll))*sin(x(State::Yaw))+cos(x(State::Roll))*cos(x(State::Yaw))*sin(x(State::Pitch)))+x(State::dyaw_dx)*x(State::speed)*(cos(x(State::Roll))*sin(x(State::Yaw))-cos(x(State::Yaw))*sin(x(State::Pitch))*sin(x(State::Roll))))-x(State::accel)*cos(x(State::Pitch))*cos(x(State::Yaw)))*(-1.0/2.0)+dt*x(State::speed)*cos(x(State::Pitch))*cos(x(State::Yaw));
    F(State::Y, State::dpitch_dx) = ((dt*dt)*(x(State::speed)*x(State::speed))*(cos(x(State::Yaw))*sin(x(State::Roll))-cos(x(State::Roll))*sin(x(State::Pitch))*sin(x(State::Yaw))))/2.0;
    F(State::Y, State::dyaw_dx) = ((dt*dt)*(x(State::speed)*x(State::speed))*(cos(x(State::Roll))*cos(x(State::Yaw))+sin(x(State::Pitch))*sin(x(State::Roll))*sin(x(State::Yaw))))/2.0;
    F(State::Z, State::Z) = 1.0;
    F(State::Z, State::speed) = -dt*(sin(x(State::Pitch))+x(State::dpitch_dx)*dt*x(State::speed)*cos(x(State::Pitch))*cos(x(State::Roll))-dt*x(State::dyaw_dx)*x(State::speed)*cos(x(State::Pitch))*sin(x(State::Roll)));
    F(State::Z, State::accel) = (dt*dt)*sin(x(State::Pitch))*(-1.0/2.0);
    F(State::Z, State::Roll) = ((dt*dt)*(x(State::speed)*x(State::speed))*cos(x(State::Pitch))*(x(State::dyaw_dx)*cos(x(State::Roll))+x(State::dpitch_dx)*sin(x(State::Roll))))/2.0;
    F(State::Z, State::Pitch) = (dt*dt)*(x(State::accel)*cos(x(State::Pitch))-x(State::speed)*(x(State::dpitch_dx)*x(State::speed)*cos(x(State::Roll))*sin(x(State::Pitch))-x(State::dyaw_dx)*x(State::speed)*sin(x(State::Pitch))*sin(x(State::Roll))))*(-1.0/2.0)-dt*x(State::speed)*cos(x(State::Pitch));
    F(State::Z, State::dpitch_dx) = (dt*dt)*(x(State::speed)*x(State::speed))*cos(x(State::Pitch))*cos(x(State::Roll))*(-1.0/2.0);
    F(State::Z, State::dyaw_dx) = ((dt*dt)*(x(State::speed)*x(State::speed))*cos(x(State::Pitch))*sin(x(State::Roll)))/2.0;
    F(State::speed, State::speed) = 1.0;
    F(State::speed, State::accel) = dt;
    F(State::accel, State::accel) = 1.0;
    F(State::Roll, State::speed) = (dt*(x(State::droll_dx)*cos(x(State::Pitch))+x(State::dyaw_dx)*cos(x(State::Roll))*sin(x(State::Pitch))+x(State::dpitch_dx)*sin(x(State::Pitch))*sin(x(State::Roll))))/cos(x(State::Pitch));
    F(State::Roll, State::Roll) = (cos(x(State::Pitch))+x(State::dpitch_dx)*dt*x(State::speed)*cos(x(State::Roll))*sin(x(State::Pitch))-dt*x(State::dyaw_dx)*x(State::speed)*sin(x(State::Pitch))*sin(x(State::Roll)))/cos(x(State::Pitch));
    F(State::Roll, State::Pitch) = dt*x(State::speed)*1.0/pow(cos(x(State::Pitch)),2.0)*(x(State::dyaw_dx)*cos(x(State::Roll))+x(State::dpitch_dx)*sin(x(State::Roll)));
    F(State::Roll, State::droll_dx) = dt*x(State::speed);
    F(State::Roll, State::dpitch_dx) = (dt*x(State::speed)*sin(x(State::Pitch))*sin(x(State::Roll)))/cos(x(State::Pitch));
    F(State::Roll, State::dyaw_dx) = (dt*x(State::speed)*cos(x(State::Roll))*sin(x(State::Pitch)))/cos(x(State::Pitch));
    F(State::Pitch, State::speed) = dt*(x(State::dpitch_dx)*cos(x(State::Roll))-x(State::dyaw_dx)*sin(x(State::Roll)));
    F(State::Pitch, State::Roll) = -dt*x(State::speed)*(x(State::dyaw_dx)*cos(x(State::Roll))+x(State::dpitch_dx)*sin(x(State::Roll)));
    F(State::Pitch, State::Pitch) = 1.0;
    F(State::Pitch, State::dpitch_dx) = dt*x(State::speed)*cos(x(State::Roll));
    F(State::Pitch, State::dyaw_dx) = -dt*x(State::speed)*sin(x(State::Roll));
    F(State::Yaw, State::speed) = (dt*(x(State::dyaw_dx)*cos(x(State::Roll))+x(State::dpitch_dx)*sin(x(State::Roll))))/cos(x(State::Pitch));
    F(State::Yaw, State::Roll) = (dt*x(State::speed)*(x(State::dpitch_dx)*cos(x(State::Roll))-x(State::dyaw_dx)*sin(x(State::Roll))))/cos(x(State::Pitch));
    F(State::Yaw, State::Pitch) = dt*x(State::speed)*1.0/pow(cos(x(State::Pitch)),2.0)*sin(x(State::Pitch))*(x(State::dyaw_dx)*cos(x(State::Roll))+x(State::dpitch_dx)*sin(x(State::Roll)));
    F(State::Yaw, State::Yaw) = 1.0;
    F(State::Yaw, State::dpitch_dx) = (dt*x(State::speed)*sin(x(State::Roll)))/cos(x(State::Pitch));
    F(State::Yaw, State::dyaw_dx) = (dt*x(State::speed)*cos(x(State::Roll)))/cos(x(State::Pitch));
    F(State::droll_dx, State::droll_dx) = 1.0;
    F(State::dpitch_dx, State::dpitch_dx) = 1.0;
    F(State::dyaw_dx, State::dyaw_dx) = 1.0;

    // clang-format on

    // Kalman prediction equations according to wikipeadia on EKF
    x = f;
    constrain_state();
    P = F * P * F.transpose() + dt * Q;
}

void AckermannEkf::correct(const Measurement &measurement) {
    Eigen::VectorXd h(MEASUREMENT_SIZE);
    Eigen::MatrixXd H(MEASUREMENT_SIZE, STATE_SIZE);
    h.setZero();
    H.setZero();

    // clang-format off

    // The code here has been autogenerated by matlab and manually pasted in.
    // Do not edit directly!

    h(Measurement::X) = x(State::X)-measurement.sensor_position(1)*(cos(x(State::Roll))*sin(x(State::Yaw))-cos(x(State::Yaw))*sin(x(State::Pitch))*sin(x(State::Roll)))+measurement.sensor_position(2)*(sin(x(State::Roll))*sin(x(State::Yaw))+cos(x(State::Roll))*cos(x(State::Yaw))*sin(x(State::Pitch)))+measurement.sensor_position(0)*cos(x(State::Pitch))*cos(x(State::Yaw));
    h(Measurement::Y) = x(State::Y)+measurement.sensor_position(1)*(cos(x(State::Roll))*cos(x(State::Yaw))+sin(x(State::Pitch))*sin(x(State::Roll))*sin(x(State::Yaw)))-measurement.sensor_position(2)*(cos(x(State::Yaw))*sin(x(State::Roll))-cos(x(State::Roll))*sin(x(State::Pitch))*sin(x(State::Yaw)))+measurement.sensor_position(0)*cos(x(State::Pitch))*sin(x(State::Yaw));
    h(Measurement::Z) = x(State::Z)-measurement.sensor_position(0)*sin(x(State::Pitch))+measurement.sensor_position(2)*cos(x(State::Pitch))*cos(x(State::Roll))+measurement.sensor_position(1)*cos(x(State::Pitch))*sin(x(State::Roll));
    h(Measurement::dx_dt) = x(State::speed)*(x(State::dpitch_dx)*measurement.sensor_position(2)-x(State::dyaw_dx)*measurement.sensor_position(1)+1.0);
    h(Measurement::dy_dt) = -x(State::speed)*(x(State::droll_dx)*measurement.sensor_position(2)-x(State::dyaw_dx)*measurement.sensor_position(0));
    h(Measurement::dz_dt) = -x(State::speed)*(x(State::dpitch_dx)*measurement.sensor_position(0)-x(State::droll_dx)*measurement.sensor_position(1));
    h(Measurement::d2x_dt2) = x(State::accel)-measurement.gravity*sin(x(State::Pitch))+x(State::accel)*x(State::dpitch_dx)*measurement.sensor_position(2)-x(State::accel)*x(State::dyaw_dx)*measurement.sensor_position(1)-x(State::dpitch_dx)*x(State::speed)*(x(State::dpitch_dx)*measurement.sensor_position(0)*x(State::speed)-x(State::droll_dx)*measurement.sensor_position(1)*x(State::speed))+x(State::dyaw_dx)*x(State::speed)*(x(State::droll_dx)*measurement.sensor_position(2)*x(State::speed)-x(State::dyaw_dx)*measurement.sensor_position(0)*x(State::speed));
    h(Measurement::d2y_dt2) = x(State::dyaw_dx)*(x(State::speed)*x(State::speed))+measurement.gravity*cos(x(State::Pitch))*sin(x(State::Roll))-x(State::accel)*x(State::droll_dx)*measurement.sensor_position(2)+x(State::accel)*x(State::dyaw_dx)*measurement.sensor_position(0)+x(State::droll_dx)*x(State::speed)*(x(State::dpitch_dx)*measurement.sensor_position(0)*x(State::speed)-x(State::droll_dx)*measurement.sensor_position(1)*x(State::speed))+x(State::dyaw_dx)*x(State::speed)*(x(State::dpitch_dx)*measurement.sensor_position(2)*x(State::speed)-x(State::dyaw_dx)*measurement.sensor_position(1)*x(State::speed));
    h(Measurement::d2z_dt2) = -x(State::dpitch_dx)*(x(State::speed)*x(State::speed))+measurement.gravity*cos(x(State::Pitch))*cos(x(State::Roll))-x(State::accel)*x(State::dpitch_dx)*measurement.sensor_position(0)+x(State::accel)*x(State::droll_dx)*measurement.sensor_position(1)-x(State::dpitch_dx)*x(State::speed)*(x(State::dpitch_dx)*measurement.sensor_position(2)*x(State::speed)-x(State::dyaw_dx)*measurement.sensor_position(1)*x(State::speed))-x(State::droll_dx)*x(State::speed)*(x(State::droll_dx)*measurement.sensor_position(2)*x(State::speed)-x(State::dyaw_dx)*measurement.sensor_position(0)*x(State::speed));
    h(Measurement::Roll) = x(State::Roll);
    h(Measurement::Pitch) = x(State::Pitch);
    h(Measurement::Yaw) = x(State::Yaw);
    h(Measurement::droll_dt) = x(State::droll_dx)*x(State::speed);
    h(Measurement::dpitch_dt) = x(State::dpitch_dx)*x(State::speed);
    h(Measurement::dyaw_dt) = x(State::dyaw_dx)*x(State::speed);

    H(Measurement::X, State::X) = 1.0;
    H(Measurement::X, State::Roll) = measurement.sensor_position(1)*(sin(x(State::Roll))*sin(x(State::Yaw))+cos(x(State::Roll))*cos(x(State::Yaw))*sin(x(State::Pitch)))+measurement.sensor_position(2)*(cos(x(State::Roll))*sin(x(State::Yaw))-cos(x(State::Yaw))*sin(x(State::Pitch))*sin(x(State::Roll)));
    H(Measurement::X, State::Pitch) = cos(x(State::Yaw))*(-measurement.sensor_position(0)*sin(x(State::Pitch))+measurement.sensor_position(2)*cos(x(State::Pitch))*cos(x(State::Roll))+measurement.sensor_position(1)*cos(x(State::Pitch))*sin(x(State::Roll)));
    H(Measurement::X, State::Yaw) = -measurement.sensor_position(1)*(cos(x(State::Roll))*cos(x(State::Yaw))+sin(x(State::Pitch))*sin(x(State::Roll))*sin(x(State::Yaw)))+measurement.sensor_position(2)*(cos(x(State::Yaw))*sin(x(State::Roll))-cos(x(State::Roll))*sin(x(State::Pitch))*sin(x(State::Yaw)))-measurement.sensor_position(0)*cos(x(State::Pitch))*sin(x(State::Yaw));
    H(Measurement::Y, State::Y) = 1.0;
    H(Measurement::Y, State::Roll) = -measurement.sensor_position(1)*(cos(x(State::Yaw))*sin(x(State::Roll))-cos(x(State::Roll))*sin(x(State::Pitch))*sin(x(State::Yaw)))-measurement.sensor_position(2)*(cos(x(State::Roll))*cos(x(State::Yaw))+sin(x(State::Pitch))*sin(x(State::Roll))*sin(x(State::Yaw)));
    H(Measurement::Y, State::Pitch) = sin(x(State::Yaw))*(-measurement.sensor_position(0)*sin(x(State::Pitch))+measurement.sensor_position(2)*cos(x(State::Pitch))*cos(x(State::Roll))+measurement.sensor_position(1)*cos(x(State::Pitch))*sin(x(State::Roll)));
    H(Measurement::Y, State::Yaw) = -measurement.sensor_position(1)*(cos(x(State::Roll))*sin(x(State::Yaw))-cos(x(State::Yaw))*sin(x(State::Pitch))*sin(x(State::Roll)))+measurement.sensor_position(2)*(sin(x(State::Roll))*sin(x(State::Yaw))+cos(x(State::Roll))*cos(x(State::Yaw))*sin(x(State::Pitch)))+measurement.sensor_position(0)*cos(x(State::Pitch))*cos(x(State::Yaw));
    H(Measurement::Z, State::Z) = 1.0;
    H(Measurement::Z, State::Roll) = cos(x(State::Pitch))*(measurement.sensor_position(1)*cos(x(State::Roll))-measurement.sensor_position(2)*sin(x(State::Roll)));
    H(Measurement::Z, State::Pitch) = -measurement.sensor_position(0)*cos(x(State::Pitch))-measurement.sensor_position(2)*cos(x(State::Roll))*sin(x(State::Pitch))-measurement.sensor_position(1)*sin(x(State::Pitch))*sin(x(State::Roll));
    H(Measurement::dx_dt, State::speed) = x(State::dpitch_dx)*measurement.sensor_position(2)-x(State::dyaw_dx)*measurement.sensor_position(1)+1.0;
    H(Measurement::dx_dt, State::dpitch_dx) = measurement.sensor_position(2)*x(State::speed);
    H(Measurement::dx_dt, State::dyaw_dx) = -measurement.sensor_position(1)*x(State::speed);
    H(Measurement::dy_dt, State::speed) = -x(State::droll_dx)*measurement.sensor_position(2)+x(State::dyaw_dx)*measurement.sensor_position(0);
    H(Measurement::dy_dt, State::droll_dx) = -measurement.sensor_position(2)*x(State::speed);
    H(Measurement::dy_dt, State::dyaw_dx) = measurement.sensor_position(0)*x(State::speed);
    H(Measurement::dz_dt, State::speed) = -x(State::dpitch_dx)*measurement.sensor_position(0)+x(State::droll_dx)*measurement.sensor_position(1);
    H(Measurement::dz_dt, State::droll_dx) = measurement.sensor_position(1)*x(State::speed);
    H(Measurement::dz_dt, State::dpitch_dx) = -measurement.sensor_position(0)*x(State::speed);
    H(Measurement::d2x_dt2, State::speed) = x(State::speed)*((x(State::dpitch_dx)*x(State::dpitch_dx))*measurement.sensor_position(0)+(x(State::dyaw_dx)*x(State::dyaw_dx))*measurement.sensor_position(0)-x(State::dpitch_dx)*x(State::droll_dx)*measurement.sensor_position(1)-x(State::droll_dx)*x(State::dyaw_dx)*measurement.sensor_position(2))*-2.0;
    H(Measurement::d2x_dt2, State::accel) = x(State::dpitch_dx)*measurement.sensor_position(2)-x(State::dyaw_dx)*measurement.sensor_position(1)+1.0;
    H(Measurement::d2x_dt2, State::Pitch) = -measurement.gravity*cos(x(State::Pitch));
    H(Measurement::d2x_dt2, State::droll_dx) = (x(State::speed)*x(State::speed))*(x(State::dpitch_dx)*measurement.sensor_position(1)+x(State::dyaw_dx)*measurement.sensor_position(2));
    H(Measurement::d2x_dt2, State::dpitch_dx) = x(State::accel)*measurement.sensor_position(2)-x(State::dpitch_dx)*measurement.sensor_position(0)*(x(State::speed)*x(State::speed))*2.0+x(State::droll_dx)*measurement.sensor_position(1)*(x(State::speed)*x(State::speed));
    H(Measurement::d2x_dt2, State::dyaw_dx) = -x(State::accel)*measurement.sensor_position(1)+x(State::droll_dx)*measurement.sensor_position(2)*(x(State::speed)*x(State::speed))-x(State::dyaw_dx)*measurement.sensor_position(0)*(x(State::speed)*x(State::speed))*2.0;
    H(Measurement::d2y_dt2, State::speed) = x(State::speed)*(x(State::dyaw_dx)-(x(State::droll_dx)*x(State::droll_dx))*measurement.sensor_position(1)-(x(State::dyaw_dx)*x(State::dyaw_dx))*measurement.sensor_position(1)+x(State::dpitch_dx)*x(State::droll_dx)*measurement.sensor_position(0)+x(State::dpitch_dx)*x(State::dyaw_dx)*measurement.sensor_position(2))*2.0;
    H(Measurement::d2y_dt2, State::accel) = -x(State::droll_dx)*measurement.sensor_position(2)+x(State::dyaw_dx)*measurement.sensor_position(0);
    H(Measurement::d2y_dt2, State::Roll) = measurement.gravity*cos(x(State::Pitch))*cos(x(State::Roll));
    H(Measurement::d2y_dt2, State::Pitch) = -measurement.gravity*sin(x(State::Pitch))*sin(x(State::Roll));
    H(Measurement::d2y_dt2, State::droll_dx) = -x(State::accel)*measurement.sensor_position(2)+x(State::dpitch_dx)*measurement.sensor_position(0)*(x(State::speed)*x(State::speed))-x(State::droll_dx)*measurement.sensor_position(1)*(x(State::speed)*x(State::speed))*2.0;
    H(Measurement::d2y_dt2, State::dpitch_dx) = (x(State::speed)*x(State::speed))*(x(State::droll_dx)*measurement.sensor_position(0)+x(State::dyaw_dx)*measurement.sensor_position(2));
    H(Measurement::d2y_dt2, State::dyaw_dx) = x(State::accel)*measurement.sensor_position(0)+x(State::speed)*x(State::speed)+x(State::dpitch_dx)*measurement.sensor_position(2)*(x(State::speed)*x(State::speed))-x(State::dyaw_dx)*measurement.sensor_position(1)*(x(State::speed)*x(State::speed))*2.0;
    H(Measurement::d2z_dt2, State::speed) = x(State::speed)*(x(State::dpitch_dx)+(x(State::dpitch_dx)*x(State::dpitch_dx))*measurement.sensor_position(2)+(x(State::droll_dx)*x(State::droll_dx))*measurement.sensor_position(2)-x(State::dpitch_dx)*x(State::dyaw_dx)*measurement.sensor_position(1)-x(State::droll_dx)*x(State::dyaw_dx)*measurement.sensor_position(0))*-2.0;
    H(Measurement::d2z_dt2, State::accel) = -x(State::dpitch_dx)*measurement.sensor_position(0)+x(State::droll_dx)*measurement.sensor_position(1);
    H(Measurement::d2z_dt2, State::Roll) = -measurement.gravity*cos(x(State::Pitch))*sin(x(State::Roll));
    H(Measurement::d2z_dt2, State::Pitch) = -measurement.gravity*cos(x(State::Roll))*sin(x(State::Pitch));
    H(Measurement::d2z_dt2, State::droll_dx) = x(State::accel)*measurement.sensor_position(1)-x(State::droll_dx)*measurement.sensor_position(2)*(x(State::speed)*x(State::speed))*2.0+x(State::dyaw_dx)*measurement.sensor_position(0)*(x(State::speed)*x(State::speed));
    H(Measurement::d2z_dt2, State::dpitch_dx) = -x(State::accel)*measurement.sensor_position(0)-x(State::speed)*x(State::speed)-x(State::dpitch_dx)*measurement.sensor_position(2)*(x(State::speed)*x(State::speed))*2.0+x(State::dyaw_dx)*measurement.sensor_position(1)*(x(State::speed)*x(State::speed));
    H(Measurement::d2z_dt2, State::dyaw_dx) = (x(State::speed)*x(State::speed))*(x(State::dpitch_dx)*measurement.sensor_position(1)+x(State::droll_dx)*measurement.sensor_position(0));
    H(Measurement::Roll, State::Roll) = 1.0;
    H(Measurement::Pitch, State::Pitch) = 1.0;
    H(Measurement::Yaw, State::Yaw) = 1.0;
    H(Measurement::droll_dt, State::speed) = x(State::droll_dx);
    H(Measurement::droll_dt, State::droll_dx) = x(State::speed);
    H(Measurement::dpitch_dt, State::speed) = x(State::dpitch_dx);
    H(Measurement::dpitch_dt, State::dpitch_dx) = x(State::speed);
    H(Measurement::dyaw_dt, State::speed) = x(State::dyaw_dx);
    H(Measurement::dyaw_dt, State::dyaw_dx) = x(State::speed);

    // clang-format on

    // Find the indicies of values to include in correction step
    std::vector<int> measurement_indices;
    for (int i = 0; i < MEASUREMENT_SIZE; i++) {
        if (measurement.mask[i]) {
            measurement_indices.push_back(i);
        }
    }

    // Create subsets of z, R, h and H from the indicies of given by the
    // measurement mask. There is no need to work with e.g. the full h vector
    // if only two out of MEASUREMENT_SIZE components are actually used
    // By creating this subset we e.g. in this example only have to invert a 2x2
    // matrix as opposed to a MEASUREMENT_SIZExMEASUREMENT_SIZE one
    Eigen::MatrixXd R(measurement_indices.size(), measurement_indices.size());
    Eigen::VectorXd y(measurement_indices.size());
    Eigen::MatrixXd H_subset(measurement_indices.size(), STATE_SIZE);
    for (int i_ = 0; i_ < measurement_indices.size(); i_++) {
        int i = measurement_indices[i_];

        // Subset h and z (and y)
        y(i_) = measurement.z(i) - h(i);

        // Wrap Yaw error (a == 2*pi*n + a)
        if (i_ == Measurement::Yaw) {
            y(i_) -= round(y(i_) / (2 * M_PI)) * 2 * M_PI;
        }

        // Subset R
        for (int j_ = 0; j_ < measurement_indices.size(); j_++) {
            int j = measurement_indices[j_];

            R(i_, j_) = measurement.R(i, j);
        }

        // Subset H
        for (int j = 0; j < STATE_SIZE; j++) { H_subset(i_, j) = H(i, j); }
    }

    // Kalman correction equations according to wikipeadia on EKF
    Eigen::MatrixXd K = P * H_subset.transpose() *
                        (H_subset * P * H_subset.transpose() + R).inverse();
    x.noalias() += K * y;
    constrain_state();
    P = (Eigen::MatrixXd::Identity(STATE_SIZE, STATE_SIZE) - K * H_subset) * P;
}

void AckermannEkf::constrain_state() {
    x.noalias() = x.cwiseMax(x_min_).cwiseMin(x_max_);
}

} // namespace ackermann_ekf


//==============================================================================
//         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_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_CORRECT_FMA_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_CORRECT_FMA_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/correct_fma.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/two_prod.hpp>
#include <boost/simd/include/functions/simd/two_add.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::correct_fma_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))((simd_<arithmetic_<A0>,X>))((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(3)
    {
      return a0*a1+a2;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::correct_fma_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                              ((simd_<floating_<A0>,X>))
                              ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(3)
    {
      A0 p, rp, s, rs;
      two_prod(a0, a1, p, rp);
      two_add(p, a2, s, rs);
      return s+(rp+rs);
    }
  };
} } }

#endif


// Copyright (c) 2019 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include "pyinterp/detail/gsl/accelerator.hpp"
#include "pyinterp/detail/broadcast.hpp"
#include <Eigen/Core>
#include <gsl/gsl_interp.h>
#include <memory>

namespace pyinterp {
namespace detail {
namespace gsl {

/// Interpolate a 1-D function
class Interpolate1D {
 public:
  /// Interpolate a 1-D function
  ///
  /// @param type fitting model
  /// @param xa 1-D array of real values.
  /// @param ya 1-D array of real values. The length of ya along the
  /// interpolation axis must be equal to the length of xa.
  /// @param acc
  Interpolate1D(const gsl_interp_type* type,
                const Eigen::Ref<const Eigen::VectorXd>& xa,
                const Eigen::Ref<const Eigen::VectorXd>& ya,
                Accelerator acc = Accelerator())
      : xa_(xa),
        ya_(ya),
        workspace_(std::shared_ptr<gsl_interp>(
            gsl_interp_alloc(type, xa.size()),
            [](gsl_interp* ptr) { gsl_interp_free(ptr); })),
        acc_(std::move(acc)) {
    check_container_size("xa", xa_, "ya", ya_);
    gsl_interp_init(static_cast<gsl_interp*>(*this), xa_.data(), ya_.data(),
                    xa_.size());
  }

  /// Returns the name of the interpolation type used
  inline std::string name() const noexcept {
    return gsl_interp_name(static_cast<const gsl_interp*>(*this));
  }

  /// Return the minimum number of points required by the interpolation
  inline size_t min_size() const noexcept {
    return gsl_interp_min_size(static_cast<const gsl_interp*>(*this));
  }

  /// Return the interpolated value of y for a given point x
  inline double interpolate(const double x) const {
    return gsl_interp_eval(static_cast<const gsl_interp*>(*this), xa_.data(),
                           ya_.data(), x, acc_);
  }

  /// Return the derivative d of an interpolated function for a given point x
  inline double derivative(const double x) const {
    return gsl_interp_eval_deriv(static_cast<const gsl_interp*>(*this),
                                 xa_.data(), ya_.data(), x, acc_);
  }

  /// Return the second derivative d of an interpolated function for a given
  /// point x
  inline double second_derivative(const double x) const {
    return gsl_interp_eval_deriv2(static_cast<const gsl_interp*>(*this),
                                  xa_.data(), ya_.data(), x, acc_);
  }

  /// Return the numerical integral result of an interpolated function over the
  /// range [a, b],
  inline double integral(const double a, const double b) const {
    return gsl_interp_eval_integ(static_cast<const gsl_interp*>(*this),
                                 xa_.data(), ya_.data(), a, b, acc_);
  }

 private:
  const Eigen::Ref<const Eigen::VectorXd> xa_;
  const Eigen::Ref<const Eigen::VectorXd> ya_;
  std::shared_ptr<gsl_interp> workspace_;
  Accelerator acc_;

  /// Gets the GSL pointer
  inline explicit operator gsl_interp*() noexcept { return workspace_.get(); }

  /// Gets the GSL const pointer
  inline explicit operator const gsl_interp*() const noexcept {
    return workspace_.get();
  }
};

}  // namespace gsl
}  // namespace detail
}  // namespace pyinterp


#pragma once

#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index_container.hpp>

#include <map>
#include <set>
#include <vector>

namespace bmi = boost::multi_index;

template<class T_Contact>
struct contact_actions
{
    using index_type = size_t;
    using distance_type = typename T_Contact::distance_type;

    static distance_type zero() { return distance_type{}; }
    static distance_type one() { return ++zero(); }
    static distance_type two() { return ++one(); }

    static distance_type distance(T_Contact const& a, T_Contact const& b)
    {
        return a.distance_from(b);
    }

    static bool is_same(T_Contact const& a, T_Contact const& b)
    {
        return distance(a, b) == zero();
    }

    static index_type index_from_distance(distance_type distance)
    {
        index_type cnt{};
        for( ; distance > zero(); distance /= two(), ++cnt);
        return --cnt;
    }

    static index_type index(const T_Contact& a, const T_Contact& b)
    {
        return index_from_distance(distance(a, b));
    }
};

template <class T_Contact, int K = 20>
class KBucket
{
    using actions = contact_actions<T_Contact>;
    using index_type = typename actions::index_type;
    using distance_type = typename actions::distance_type;

    struct by_index {};
    struct by_distance {};

    struct rel_index
    {
        rel_index(T_Contact const& origin) : _origin{ origin } {}

        bool operator ()(T_Contact const& a, T_Contact const& b) const
        {
            auto const& index_a = actions::index(_origin, a);
            auto const& index_b = actions::index(_origin, b);

            return index_a < index_b;
        }

    private:
        T_Contact _origin;
    };

    struct rel_distance
    {
        rel_distance(T_Contact const& origin): _origin{ origin } {}

        bool operator()(T_Contact const& a, T_Contact const& b) const
        {
            auto const& dist_a = actions::distance(_origin, a);
            auto const& dist_b = actions::distance(_origin, b);

            return dist_a < dist_b;
        }

    private:
        T_Contact _origin;
    };

    using ContactIndex = bmi::multi_index_container<T_Contact, bmi::indexed_by<
        bmi::ordered_unique<bmi::tag<by_distance>, bmi::identity<T_Contact>, rel_distance >,
        bmi::ordered_non_unique<bmi::tag<by_index>, bmi::identity<T_Contact>, rel_index > > >;

public:
    using iterator = typename ContactIndex::iterator;
    using const_iterator = typename ContactIndex::const_iterator;

    iterator end() const { return contacts.end(); }
    iterator begin() const { return contacts.begin(); }

    const_iterator cend() const { return contacts.cend(); }
    const_iterator cbegin() const { return contacts.cbegin(); }

    KBucket(T_Contact const& origin_ = {})
        : origin{ origin_ }
        , contacts{boost::make_tuple(
                   boost::make_tuple(bmi::identity<T_Contact>{}, rel_distance{ origin }),
                   boost::make_tuple(bmi::identity<T_Contact>{}, rel_index{ origin })
                   )}
    {}

    std::pair<iterator, bool> insert(T_Contact const& contact);

    iterator erase(iterator const& pos);
    iterator erase(T_Contact const& contact);
    iterator erase(iterator const& first, iterator const& last);
//    KBucket<T_Contact, K> rebase(T_Contact const& new_origin, bool include_origin = true) const;

    void clear()
    {
        ContactIndex{boost::make_tuple(
                        boost::make_tuple(bmi::identity<T_Contact>{}, rel_distance{ origin }),
                        boost::make_tuple(bmi::identity<T_Contact>{}, rel_index{ origin })
                        )}.swap(contacts);
    }

//    bool replace(T_Contact const& contact);
    iterator find(T_Contact const& contact) const;
//    std::vector<T_Contact> list_nearests_to(T_Contact const& contact, bool prefer_same_index = false) const;

    std::vector<std::string> get_nodeids() const;

    void print_list(std::ostream& os);
    void print_count(std::ostream& os);

private:
    T_Contact origin;
    ContactIndex contacts;
};

//template <class T_Contact, int K>
//KBucket<T_Contact, K> KBucket<T_Contact, K>::rebase(T_Contact const& new_origin, bool include_origin) const
//{
//    if (actions::is_same(new_origin, this->origin))
//        return *this;
//
//    KBucket<T_Contact, K> result(new_origin);
//
//    if (include_origin)
//        result.insert(this->origin);
//
//    for (auto const& it : contacts)
//    {
//        if (!actions::is_same(it, new_origin))
//            result.insert(it);
//    }
//
//    return result;
//}

template <class T_Contact, int K>
std::pair<typename KBucket<T_Contact, K>::iterator, bool> KBucket<T_Contact, K>::insert(const T_Contact& contact)
{
    auto distance = actions::distance(origin, contact);
    if (distance == actions::zero())
        throw(std::logic_error{"Trying to insert contact with 0 distance from the origin, i.e. self"});

    auto index = actions::index_from_distance(distance);

    auto eq_index = [&](const T_Contact& c)
    {
        return actions::index(origin, c) == index;
    };

    auto const &contacts_by_index  = contacts.template get<by_index>();
    auto index_count = std::count_if(std::begin(contacts_by_index), std::end(contacts_by_index), eq_index);

    if (index_count >= K)
        return {contacts.template project<by_distance>(std::find_if(std::begin(contacts_by_index), std::end(contacts_by_index), eq_index)), false};

    return contacts.insert(contact);
}

template <class T_Contact, int K>
typename KBucket<T_Contact, K>::iterator KBucket<T_Contact, K>::erase(iterator const& pos)
{
    return contacts.erase(pos);
}

template <class T_Contact, int K>
typename KBucket<T_Contact, K>::iterator KBucket<T_Contact, K>::erase(iterator const& first, iterator const& last)
{
    return contacts.erase(first, last);
}

template <class T_Contact, int K>
typename KBucket<T_Contact, K>::iterator KBucket<T_Contact, K>::erase(T_Contact const& contact)
{
    auto it = find(contact);

    if (it != end())
        return contacts.erase(it);

    return it;
}

//template< class T_SrcIt, class T_DstIt >
//T_DstIt loop_copy(T_SrcIt pivot, T_SrcIt first, T_SrcIt last, T_DstIt d_first, T_DstIt d_last)
//{
//    for (auto it = pivot; it != last && d_first != d_last; ++it)
//        *d_first++ = *it;
//
//    for (auto it = first; it != pivot && d_first != d_last; ++it)
//        *d_first++ = *it;
//
//    return d_first;
//}

//template <class T_Contact, int K>
//std::vector<T_Contact> KBucket<T_Contact, K>::list_nearests_to(const T_Contact& contact, bool prefer_same_index) const
//{
//    std::vector<T_Contact> result{ 10 * K };
//
//    auto& contacts_by_distance = contacts.template get<by_distance>();
//
//    auto it = std::begin(contacts_by_distance);
//    if (prefer_same_index)
//    {
//        auto index_ = actions::index(origin, contact);
//        it = std::find_if(std::begin(contacts_by_distance), std::end(contacts_by_distance),
//                         [&](const T_Contact &a){ return index_ == actions::index(origin, a); } );
//    }
//
//    auto result_end = loop_copy(it,
//                                std::begin(contacts_by_distance),
//                                std::end(contacts_by_distance),
//                                std::begin(result),
//                                std::end(result));
//
//    result.erase(result_end, std::end(result));
//
//    return result;
//}

template <class T_Contact, int K>
std::vector<std::string> KBucket<T_Contact, K>::get_nodeids() const
{
    std::vector<std::string> result;

    auto& contacts_by_distance = contacts.template get<by_distance>();

    auto it = std::begin(contacts_by_distance);
    for (; it != std::end(contacts_by_distance); ++it)
        result.push_back(it->to_string());

    return result;
}

template <class T_Contact, int K>
void KBucket<T_Contact, K>::print_list(std::ostream& os)
{
    auto & contacts_by_distance = contacts.template get<by_distance>();
    for(auto& it : contacts_by_distance)
    {
        auto const& distance = actions::distance(origin, it);
        auto const& index = actions::index_from_distance(distance);
        os<<"index: "<<index<<", id: "<<static_cast<std::string>(it).substr(0, 8) << std::endl;
    };
}

template <class T_Contact, int K>
void KBucket<T_Contact, K>::print_count(std::ostream& os)
{
    std::map<size_t, std::set<std::string>> map_by_distance;

    auto & contacts_by_distance = contacts.template get<by_distance>();
    for(auto &it : contacts_by_distance)
    {
        auto const& distance = actions::distance(origin, it);
        auto const& index = actions::index_from_distance(distance);

        map_by_distance[index].insert(it);
    };

    for (auto const& it : map_by_distance)
        os << "\nslot " << it.first << " : count " << it.second.size() << std::endl;
}

template <class T_Contact, int K>
typename KBucket<T_Contact, K>::iterator KBucket<T_Contact, K>::find(T_Contact const& contact) const
{
    return std::find_if(begin(), end(), [&contact](T_Contact const& c){ return actions::distance(contact, c) == actions::zero(); });
}

// template <class T_Contact, int K>
// bool KBucket<T_Contact, K>::replace(T_Contact const& contact)
// {
//     auto const& it = find(contact);

//     if (it == end())
//         return false;

//     return contacts.replace(it, contact);
// }


/**
 * @date Tue May 31 09:22:33 2011 +0200
 * @author Andre Anjos <andre.anjos@idiap.ch>
 *
 * @brief Implements a Machine
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <cmath>
#include <boost/make_shared.hpp>
#include <boost/format.hpp>

#include <bob.core/array_copy.h>
#include <bob.math/linear.h>

#include <bob.learn.linear/machine.h>

namespace bob { namespace learn { namespace linear {

  Machine::Machine(const blitz::Array<double,2>& weight)
    : m_input_sub(weight.extent(0)),
    m_input_div(weight.extent(0)),
    m_bias(weight.extent(1)),
    m_activation(boost::make_shared<bob::learn::activation::IdentityActivation>()),
    m_buffer(weight.extent(0))
  {
    m_input_sub = 0.0;
    m_input_div = 1.0;
    m_bias = 0.0;
    m_weight.reference(bob::core::array::ccopy(weight));
  }

  Machine::Machine():
    m_input_sub(0),
    m_input_div(0),
    m_weight(0, 0),
    m_bias(0),
    m_activation(boost::make_shared<bob::learn::activation::IdentityActivation>()),
    m_buffer(0)
  {
  }

  Machine::Machine(size_t n_input, size_t n_output):
    m_input_sub(n_input),
    m_input_div(n_input),
    m_weight(n_input, n_output),
    m_bias(n_output),
    m_activation(boost::make_shared<bob::learn::activation::IdentityActivation>()),
    m_buffer(n_input)
  {
    m_input_sub = 0.0;
    m_input_div = 1.0;
    m_weight = 0.0;
    m_bias = 0.0;
  }

  Machine::Machine(const Machine& other):
    m_input_sub(bob::core::array::ccopy(other.m_input_sub)),
    m_input_div(bob::core::array::ccopy(other.m_input_div)),
    m_weight(bob::core::array::ccopy(other.m_weight)),
    m_bias(bob::core::array::ccopy(other.m_bias)),
    m_activation(other.m_activation),
    m_buffer(m_input_sub.shape())
  {
  }

  Machine::Machine (bob::io::base::HDF5File& config) {
    load(config);
  }

  Machine::~Machine() {}

  Machine& Machine::operator=
    (const Machine& other) {
      if(this != &other)
      {
        m_input_sub.reference(bob::core::array::ccopy(other.m_input_sub));
        m_input_div.reference(bob::core::array::ccopy(other.m_input_div));
        m_weight.reference(bob::core::array::ccopy(other.m_weight));
        m_bias.reference(bob::core::array::ccopy(other.m_bias));
        m_activation = other.m_activation;
        m_buffer.resize(m_input_sub.shape());
      }
      return *this;
    }

  bool Machine::operator==(const Machine& b) const {
    return (bob::core::array::isEqual(m_input_sub, b.m_input_sub) &&
        bob::core::array::isEqual(m_input_div, b.m_input_div) &&
        bob::core::array::isEqual(m_weight, b.m_weight) &&
        bob::core::array::isEqual(m_bias, b.m_bias) &&
        m_activation->str() == b.m_activation->str());
  }

  bool Machine::operator!=(const Machine& b) const {
    return !(this->operator==(b));
  }

  bool Machine::is_similar_to(const Machine& b, const double r_epsilon, const double a_epsilon) const {

    return (bob::core::array::isClose(m_input_sub, b.m_input_sub, r_epsilon, a_epsilon) &&
        bob::core::array::isClose(m_input_div, b.m_input_div, r_epsilon, a_epsilon) &&
        bob::core::array::isClose(m_weight, b.m_weight, r_epsilon, a_epsilon) &&
        bob::core::array::isClose(m_bias, b.m_bias, r_epsilon, a_epsilon) &&
        m_activation->str() == b.m_activation->str());
  }

  void Machine::load (bob::io::base::HDF5File& config) {

    //reads all data directly into the member variables
    m_input_sub.reference(config.readArray<double,1>("input_sub"));
    m_input_div.reference(config.readArray<double,1>("input_div"));
    m_weight.reference(config.readArray<double,2>("weights"));
    m_bias.reference(config.readArray<double,1>("biases"));
    m_buffer.resize(m_input_sub.extent(0));

    //switch between different versions - support for version 1
    if (config.hasAttribute(".", "version")) { //new version
      config.cd("activation");
      m_activation = bob::learn::activation::load_activation(config);
      config.cd("..");
    }
    else { //old version
      uint32_t act = config.read<uint32_t>("activation");
      m_activation = bob::learn::activation::make_deprecated_activation(act);
    }

  }

  void Machine::resize (size_t input, size_t output) {

    m_input_sub.resizeAndPreserve(input);
    m_input_div.resizeAndPreserve(input);
    m_buffer.resizeAndPreserve(input);
    m_weight.resizeAndPreserve(input, output);
    m_bias.resizeAndPreserve(output);

  }

  void Machine::save (bob::io::base::HDF5File& config) const {

    config.setAttribute(".", "version", 1);
    config.setArray("input_sub", m_input_sub);
    config.setArray("input_div", m_input_div);
    config.setArray("weights", m_weight);
    config.setArray("biases", m_bias);
    config.createGroup("activation");
    config.cd("activation");
    m_activation->save(config);
    config.cd("..");

  }

  void Machine::forward_ (const blitz::Array<double,1>& input, blitz::Array<double,1>& output) const {

    m_buffer = (input - m_input_sub) / m_input_div;
    bob::math::prod_(m_buffer, m_weight, output);
    for (int i=0; i<m_weight.extent(1); ++i)
      output(i) = m_activation->f(output(i) + m_bias(i));

  }

  void Machine::forward (const blitz::Array<double,1>& input, blitz::Array<double,1>& output) const {

    if (m_weight.extent(0) != input.extent(0)) { //checks input dimension
      boost::format m("mismatch on the input dimension: expected a vector of size %d, but you input one with size = %d instead");
      m % m_weight.extent(0) % input.extent(0);
      throw std::runtime_error(m.str());
    }
    if (m_weight.extent(1) != output.extent(0)) { //checks output dimension
      boost::format m("mismatch on the output dimension: expected a vector of size %d, but you input one with size = %d instead");
      m % m_weight.extent(1) % output.extent(0);
      throw std::runtime_error(m.str());
    }
    forward_(input, output);

  }

  void Machine::setWeights (const blitz::Array<double,2>& weight) {

    if (weight.extent(0) != m_input_sub.extent(0)) { //checks 1st dimension
      boost::format m("mismatch on the weight shape (number of rows): expected a weight matrix with %d row(s), but you input one with %d row(s) instead");
      m % m_input_sub.extent(0) % weight.extent(0);
      throw std::runtime_error(m.str());
    }
    if (weight.extent(1) != m_bias.extent(0)) { //checks 2nd dimension
      boost::format m("mismatch on the weight shape (number of columns): expected a weight matrix with %d column(s), but you input one with %d column(s) instead");
      m % m_bias.extent(0) % weight.extent(1);
      throw std::runtime_error(m.str());
    }
    m_weight.reference(bob::core::array::ccopy(weight));

  }

  void Machine::setBiases (const blitz::Array<double,1>& bias) {

    if (m_weight.extent(1) != bias.extent(0)) {
      boost::format m("mismatch on the bias shape: expected a vector of size %d, but you input one with size = %d instead");
      m % m_weight.extent(1) % bias.extent(0);
      throw std::runtime_error(m.str());
    }
    m_bias.reference(bob::core::array::ccopy(bias));

  }

  void Machine::setInputSubtraction (const blitz::Array<double,1>& v) {

    if (m_weight.extent(0) != v.extent(0)) {
      boost::format m("mismatch on the input subtraction shape: expected a vector of size %d, but you input one with size = %d instead");
      m % m_weight.extent(0) % v.extent(0);
      throw std::runtime_error(m.str());
    }
    m_input_sub.reference(bob::core::array::ccopy(v));

  }

  void Machine::setInputDivision (const blitz::Array<double,1>& v) {

    if (m_weight.extent(0) != v.extent(0)) {
      boost::format m("mismatch on the input division shape: expected a vector of size %d, but you input one with size = %d instead");
      m % m_weight.extent(0) % v.extent(0);
      throw std::runtime_error(m.str());
    }
    m_input_div.reference(bob::core::array::ccopy(v));

  }

  void Machine::setActivation (boost::shared_ptr<bob::learn::activation::Activation> a) {
    m_activation = a;
  }

}}}