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

// Copyright (c) 2010-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2019-2021.
// Modifications copyright (c) 2019-2021, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#include <iostream>
#include <iomanip>
#include <string>

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/algorithms/union.hpp>
#include <boost/geometry/algorithms/difference.hpp>
#include <boost/geometry/algorithms/intersects.hpp>
#include <boost/geometry/algorithms/within.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <boost/geometry/io/wkt/read.hpp>
#include <boost/geometry/io/wkt/write.hpp>

#include <boost/geometry/policies/robustness/get_rescale_policy.hpp>

#include <boost/geometry/strategies/strategies.hpp>

#if defined(TEST_WITH_SVG)
#  include <boost/geometry/io/svg/svg_mapper.hpp>
#endif

template <typename Geometry>
inline void test_assemble(std::string const& id, Geometry const& p, Geometry const& q, char operation = 'i')
{
    std::vector<Geometry> u, i, d1, d2;
    bg::detail::union_::union_insert<Geometry>(p, q, std::back_inserter(u));
    bg::detail::intersection::intersection_insert<Geometry>(p, q, std::back_inserter(i));
    bg::detail::difference::difference_insert<Geometry>(p, q, std::back_inserter(d1));
    bg::detail::difference::difference_insert<Geometry>(q, p, std::back_inserter(d2));

    if (operation == 'i')
    {
        typedef typename bg::default_area_result<Geometry>::type type;
        type area_p = bg::area(p);
        type area_q = bg::area(q);

        type area_i = 0, area_u = 0, area_d1 = 0, area_d2 = 0;

        for (Geometry const& g : u)
        {
            area_u += bg::area(g);
        }
        for (Geometry const& g : i)
        {
            area_i += bg::area(g);
        }
        for (Geometry const& g : d1)
        {
            area_d1 += bg::area(g);
        }
        for (Geometry const& g : d2)
        {
            area_d2 += bg::area(g);
        }

        type diff = (area_p + area_q) - area_u - area_i;
        type diff_d1 = (area_u - area_q) - area_d1;
        type diff_d2 = (area_u - area_p) - area_d2;

        bool ok = bg::math::abs(diff) < 0.001
            && bg::math::abs(diff_d1) < 0.001
            && bg::math::abs(diff_d2) < 0.001;

        BOOST_CHECK_MESSAGE(ok,
            id << " diff:  "
                << diff << " d1: "
                << diff_d1 << " d2: "
                << diff_d2);
    }

#if defined(TEST_WITH_SVG)
    {
        std::ostringstream filename;
        filename << "assemble_" << id << "_" << operation << ".svg";
        std::ofstream svg(filename.str().c_str());

        bg::svg_mapper<typename bg::point_type<Geometry>::type> mapper(svg, 500, 500);
        mapper.add(p);
        mapper.add(q);
        mapper.map(p, "fill-opacity:0.3;fill:rgb(51,51,153);stroke:rgb(51,51,153);stroke-width:3");
        mapper.map(q, "fill-opacity:0.5;fill:rgb(153,204,0);stroke:rgb(153,204,0);stroke-width:3");
        std::string linestyle = "opacity:0.7;fill:none;stroke-opacity:1;stroke-miterlimit:4;";

        std::vector<Geometry> const& v = operation == 'i' ? i
            : operation == 'u' ? u
            : operation == 'd' ? d1
            : d2
            ;

        for (Geometry const& geometry : v)
        {
            mapper.map(geometry,
                linestyle + "stroke-width:3;stroke-linejoin:round;stroke-linecap:square;stroke-dasharray:12,12;stroke:rgb(255,0,0);");
        }
    }
#endif
}

template <typename Polygon>
inline bool int_ok(Polygon const& poly)
{
    typename bg::point_type<Polygon>::type const& pi =
        bg::interior_rings(poly)[0].front();

    return bg::within(pi, bg::exterior_ring(poly));
}

template <typename T>
void generate()
{

    static std::string exteriors[4] = {
            "(0 0,0 10,10 10,10 0,0 0)",
            "(1 1,1 9,8 9,8 1,1 1)",
            "(2 0.5, 0.5 2,0.5 8,2 9.5,6 9.5,8.5 8,8.5 2,7 0.5,2 0.5)",
            "(3 3,3 7,6 7,6 3,3 3)"
    };
    static std::string interiors[4] = {
            "(2 2,2 8,7 8,7 2,2 2)",
            "(8.5 1,8.5 2,9.5 2,9.5 1,8.5 1)",
            "(4 4,4 5,5 5,5 4,4 4)",
            "(6 4,6 5,9 5,9 4,6 4)"
    };
    for (int pe = 0; pe < 4; pe++)
    {
        for (int qe = 0; qe < 4; qe++)
        {
            for (int pi = 0; pi < 4; pi++)
            {
                for (int qi = 0; qi < 4; qi++)
                {
                    std::string ps = "POLYGON(" + exteriors[pe] + "," + interiors[pi] + ")";
                    std::string qs = "POLYGON(" + exteriors[qe] + "," + interiors[qi] + ")";

                    typedef bg::model::d2::point_xy<T> point_type;
                    bg::model::polygon<point_type> p, q;
                    bg::read_wkt(ps, p);
                    bg::read_wkt(qs, q);
                    bg::correct(p);
                    bg::correct(q);
                    if (! bg::intersects(p)
                        && ! bg::intersects(q)
                        && int_ok(p)
                        && int_ok(q)
                        )
                    {
                        std::ostringstream out;
                        out << pe << qe << pi << qi;
                        test_assemble(out.str(), p, q);

#if defined(TEST_WITH_SVG)
                        test_assemble(out.str(), p, q, 'u');
                        test_assemble(out.str(), p, q, 'd');
                        test_assemble(out.str(), p, q, 'r');
#endif
                    }
                }
            }
        }
    }
}

#if ! defined(GEOMETRY_TEST_MULTI)
int test_main(int, char* [])
{
    generate<double>();
    return 0;
}
#endif


// __BEGIN_LICENSE__
// Copyright (C) 2006-2010 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <vw/FileIO.h>
#include <vw/Image.h>
#include <vw/Math.h>
#include <vw/Cartography.h>
using namespace vw;

#include <boost/program_options.hpp>
#include <boost/type_traits.hpp>
#include <boost/utility/enable_if.hpp>
namespace po = boost::program_options;

// Function for highlighting spots of data
template<class PixelT>
class NotNoDataFunctor {
  typedef typename CompoundChannelType<PixelT>::type channel_type;
  channel_type m_nodata;
  typedef ChannelRange<channel_type> range_type;
public:
  NotNoDataFunctor( channel_type nodata ) : m_nodata(nodata) {}

  template <class Args> struct result {
    typedef channel_type type;
  };

  inline channel_type operator()( channel_type const& val ) const {
    return val != m_nodata ? range_type::max() : range_type::min();
  }
};

template <class ImageT, class NoDataT>
UnaryPerPixelView<ImageT,UnaryCompoundFunctor<NotNoDataFunctor<typename ImageT::pixel_type>, typename ImageT::pixel_type>  >
inline notnodata( ImageViewBase<ImageT> const& image, NoDataT nodata ) {
  typedef UnaryCompoundFunctor<NotNoDataFunctor<typename ImageT::pixel_type>, typename ImageT::pixel_type> func_type;
  func_type func( nodata );
  return UnaryPerPixelView<ImageT,func_type>( image.impl(), func );
}

// Linear Transfer Function
class LinearTransFunc : public vw::UnaryReturnSameType {
public:
  LinearTransFunc() {}

  template <class ArgT>
  ArgT operator()( ArgT const& value ) const { return value; }
};

// Cosine Transfer Function (this tracks 180 degrees with level at high and low)
template <class PixelT>
class CosineTransFunc : public vw::UnaryReturnSameType {
  typedef typename CompoundChannelType<PixelT>::type channel_type;
  typedef ChannelRange<channel_type> range_type;
public:
  CosineTransFunc() {}

  template <class ArgT>
  inline typename boost::enable_if<typename boost::is_floating_point<ArgT>,ArgT>::type
  operator()( ArgT const& value ) const {
    return range_type::max()*((1.0-cos(value/float(range_type::max())*M_PI))/2.0);
  }

  template <class ArgT>
  inline typename boost::disable_if<typename boost::is_floating_point<ArgT>,ArgT>::type
  operator()( ArgT const& value ) const {
    ArgT result = ArgT(range_type::max()*((1.0-cos(float(value)/float(range_type::max())*M_PI))/2.0));
    if ( result == 0 && value != 0 )
      result = 1;
    return result;
  }
};

// 90 degree Cosine transfer function ( high slope at beginning and low slope at end )
template <class PixelT>
class Cosine90TransFunc : public vw::UnaryReturnSameType {
  typedef typename CompoundChannelType<PixelT>::type channel_type;
  typedef ChannelRange<channel_type> range_type;
public:
  Cosine90TransFunc() {}

  template <class ArgT>
  inline typename boost::enable_if<typename boost::is_floating_point<ArgT>,ArgT>::type
  operator()( ArgT const& value ) const {
    return range_type::max()*(-cos(value/float(range_type::max())*(M_PI/2.0) + M_PI/2.0));
  }

  template <class ArgT>
  inline typename boost::disable_if<typename boost::is_floating_point<ArgT>,ArgT>::type
  operator()( ArgT const& value ) const {
    ArgT result = ArgT(range_type::max()*(-cos(float(value)/float(range_type::max())*(M_PI/2.0) + M_PI/2.0)));
    if ( result == 0 && value != 0 )
      result = 1;
    return result;
  }
};

// Function to zip 2 images into content and alpha image
template <class Arg1T, class Arg2T>
struct CreateAlphaFunc: public ReturnFixedType<typename PixelWithAlpha<Arg1T>::type > {
  typedef typename PixelWithAlpha<Arg1T>::type result_type;
  inline result_type operator()(Arg1T const& arg1,
                                Arg2T const& arg2 ) const {
    result_type t(arg1);
    t.a() = arg2;
    return t;
  }
};

template <class Image1T, class Image2T>
inline BinaryPerPixelView<Image1T, Image2T, CreateAlphaFunc<typename Image1T::pixel_type, typename Image2T::pixel_type> >
create_alpha( ImageViewBase<Image1T> const& image1,
              ImageViewBase<Image2T> const& image2 ) {
  typedef CreateAlphaFunc<typename Image1T::pixel_type, typename Image2T::pixel_type> func_type;
  return BinaryPerPixelView<Image1T, Image2T, func_type >(image1.impl(), image2.impl(), func_type());
}

struct Options {
  Options() : nodata(-1), feather_min(0), feather_max(0) {}
  // Input
  std::vector<std::string> input_files;

  // Settings
  double nodata;
  float feather_min;
  float feather_max;
  std::string filter;
  std::string output_filename;
  bool force_float;
  float blur_sigma;
};

// Operation code for data that uses nodata
template <class PixelT>
void grassfire_nodata( Options& opt,
                       std::string input,
                       std::string output ) {
  typedef typename CompoundChannelType<PixelT>::type inter_type;
  typedef ChannelRange<inter_type> range_type;

  cartography::GeoReference georef;
  cartography::read_georeference(georef, input);
  DiskImageView<PixelT> input_image(input);
  ImageView<int32> distance =
    grassfire(notnodata(input_image,
                        inter_type(opt.nodata)));

  // Check to see if the user has specified a feather length.  If not,
  // then we send the feather_max to the max pixel value (which
  // results in a full grassfire blend all the way to the center of the image.)
  if (opt.feather_max < 1)
    opt.feather_max = max_pixel_value( distance );
  vw_out() << "\t--> Distance range: [ " << opt.feather_min << " " << opt.feather_max << " ]\n";

  ImageViewRef<inter_type> norm_dist;
  norm_dist = pixel_cast<inter_type>(range_type::max() / (opt.feather_max - opt.feather_min) *
                                     clamp(pixel_cast<float>(distance) - opt.feather_min,
                                           0.0, opt.feather_max - opt.feather_min));

  // The user may wish to blur the grassfire result before applying
  // the transfer function.  This makes for an even smoother blend.
  if (opt.blur_sigma > 0)
    norm_dist = gaussian_filter(pixel_cast<inter_type>(range_type::max() / (opt.feather_max - opt.feather_min) *
                                                       clamp(pixel_cast<float>(distance) - opt.feather_min,
                                                             0.0, opt.feather_max - opt.feather_min)), opt.blur_sigma);

  ImageViewRef<typename PixelWithAlpha<PixelT>::type> result;
  if ( opt.filter == "linear" ) {
    result = create_alpha(input_image,per_pixel_filter(norm_dist,LinearTransFunc()));
  } else if ( opt.filter == "cosine" ) {
    result = create_alpha(input_image,per_pixel_filter(norm_dist,CosineTransFunc<inter_type>()));
  } else if ( opt.filter == "cosine90" ) {
    result = create_alpha(input_image,per_pixel_filter(norm_dist,Cosine90TransFunc<inter_type>()));
  } else {
    vw_throw( ArgumentErr() << "Unknown transfer function " << opt.filter );
  }

  cartography::write_georeferenced_image(output, result, georef,
                                         TerminalProgressCallback("tools.grassfirealpha","Writing:"));
}

// Same as above but modified for alpha input
template <class PixelT>
void grassfire_alpha( Options& opt,
                      std::string input,
                      std::string output ) {
  cartography::GeoReference georef;
  cartography::read_georeference(georef, input);
  DiskImageView<PixelT> input_image(input);
  ImageView<int32> distance = grassfire(apply_mask(invert_mask(alpha_to_mask(input_image)),1));

  // Check to see if the user has specified a feather length.  If not,
  // then we send the feather_max to the max pixel value (which
  // results in a full grassfire blend all the way to the center of the image.)
  if (opt.feather_max < 1)
    opt.feather_max = max_pixel_value( distance );
  vw_out() << "\t--> Distance range: [ " << opt.feather_min << " " << opt.feather_max << " ]\n";

  typedef typename CompoundChannelType<PixelT>::type inter_type;
  typedef ChannelRange<typename CompoundChannelType<PixelT>::type> range_type;

  ImageViewRef<inter_type> norm_dist;
  norm_dist = pixel_cast<inter_type>(range_type::max() / (opt.feather_max - opt.feather_min) *
                                     clamp(pixel_cast<float>(distance) - opt.feather_min,
                                           0.0, opt.feather_max - opt.feather_min));

  ImageViewRef<PixelT> result;
  if ( opt.filter == "linear" ) {
    result = create_alpha(input_image,per_pixel_filter(norm_dist,LinearTransFunc()));
  } else if ( opt.filter == "cosine" ) {
    result = create_alpha(input_image,per_pixel_filter(norm_dist,CosineTransFunc<inter_type>()));
  } else if ( opt.filter == "cosine90" ) {
    result = create_alpha(input_image,per_pixel_filter(norm_dist,Cosine90TransFunc<inter_type>()));
  } else {
    vw_throw( ArgumentErr() << "Unknown transfer function " << opt.filter );
  }

  cartography::write_georeferenced_image(output, result, georef,
                                         TerminalProgressCallback("tools.grassfirealpha","Writing:"));
}

// Handling input
void handle_arguments( int argc, char *argv[], Options& opt ) {
  unsigned cache_size;

  po::options_description general_options("");
  general_options.add_options()
    ("nodata-value", po::value(&opt.nodata), "Value that is nodata in the input image. Not used if input has alpha.")
    ("feather-min", po::value(&opt.feather_min)->default_value(0), "Length in pixels to feather from an edge. Default size of zero is to feather to maximum distance in image.")
    ("feather-max,f", po::value(&opt.feather_max)->default_value(0), "Length in pixels to feather from an edge. Default size of zero is to feather to maximum distance in image.")
    ("transfer-func,t", po::value(&opt.filter)->default_value("cosine"), "Transfer function to used for alpha. [linear, cosine, cosine90]")
    ("output-filename,o", po::value(&opt.output_filename), "Filename to use for output files.")
    ("cache", po::value<unsigned>(&cache_size)->default_value(1024), "Source data cache size, in megabytes")
    ("blur-sigma", po::value<float>(&opt.blur_sigma)->default_value(0), "Blur the grassfire result before appyling the tranfer function to create an even smoother blend.")
    ("force-float", "Force the data to be read in as a float.  This option also turns off auto-rescaling.  Useful for reading 16-bit integer DEMs as though they were full of floats.")
    ("help,h", "Display this help message");

  po::options_description positional("");
  positional.add_options()
    ("input-files", po::value<std::vector<std::string> >(&opt.input_files));

  po::positional_options_description positional_desc;
  positional_desc.add("input-files", -1);

  po::options_description all_options;
  all_options.add(general_options).add(positional);

  po::variables_map vm;
  try {
    po::store( po::command_line_parser( argc, argv ).options(all_options).positional(positional_desc).run(), vm );
    po::notify( vm );
  } catch (po::error &e) {
    vw_throw( ArgumentErr() << "Error parsing input:\n\t"
              << e.what() << general_options );
  }

  std::ostringstream usage;
  usage << "Usage: " << argv[0] << " [options] <image-files>\n";
  boost::to_lower( opt.filter );

  if ( vm.count("help") )
    vw_throw( ArgumentErr() << usage.str() << general_options );
  if ( opt.input_files.empty() )
    vw_throw( ArgumentErr() << "Missing input files!\n"
              << usage.str() << general_options );

  if ( vm.count("force-float") )
    opt.force_float = true;
  else
    opt.force_float = false;

  // Set the system cache size
  vw_settings().set_system_cache_size( cache_size*1024*1024 );

}

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

  Options opt;
  try {
    handle_arguments( argc, argv, opt );

    BOOST_FOREACH( const std::string& input, opt.input_files ) {

      // Determining the format of the input
      SrcImageResource *rsrc = DiskImageResource::open(input);
      ChannelTypeEnum channel_type = rsrc->channel_type();
      PixelFormatEnum pixel_format = rsrc->pixel_format();

      // The user can elect to
      if (opt.force_float) {
        std::cout << "\t--> Overriding input channel type to 32-bit float. "
                  << "Disabling image auto-rescaling.\n";
        channel_type = VW_CHANNEL_FLOAT32;

        // Turn off automatic rescaling when reading in images
        DiskImageResource::set_default_rescale(false);
      }

      // Check for nodata value in the file
      if ( rsrc->has_nodata_read() ) {
        opt.nodata = rsrc->nodata_read();
        std::cout << "\t--> Extracted nodata value from file: " << opt.nodata << ".\n";
      }
      delete rsrc;

      vw_out() << "Loading: " << input << "\n";
      size_t pt_idx = input.rfind(".");
      std::string output;
      if (opt.output_filename.size() != 0)
        output = opt.output_filename;
      else {
        output = input.substr(0,pt_idx)+"_grass";
        output += input.substr(pt_idx,input.size()-pt_idx);
      }

      switch (pixel_format) {
      case VW_PIXEL_GRAY:
        switch (channel_type) {
        case VW_CHANNEL_UINT8:
          grassfire_nodata<PixelGray<uint8> >(opt,input,output); break;
        case VW_CHANNEL_INT16:
          grassfire_nodata<PixelGray<int16> >(opt,input,output); break;
        case VW_CHANNEL_UINT16:
          grassfire_nodata<PixelGray<uint16> >(opt,input,output); break;
        default:
          grassfire_nodata<PixelGray<float32> >(opt,input,output); break;
        }
        break;
      case VW_PIXEL_GRAYA:
        switch (channel_type) {
        case VW_CHANNEL_UINT8:
          grassfire_alpha<PixelGrayA<uint8> >(opt,input,output); break;
        case VW_CHANNEL_INT16:
          grassfire_alpha<PixelGrayA<int16> >(opt,input,output); break;
        case VW_CHANNEL_UINT16:
          grassfire_alpha<PixelGrayA<uint16> >(opt,input,output); break;
        default:
          grassfire_alpha<PixelGrayA<float32> >(opt,input,output); break;
        }
        break;
      case VW_PIXEL_RGB:
        switch (channel_type) {
        case VW_CHANNEL_UINT8:
          grassfire_nodata<PixelRGB<uint8> >(opt,input,output); break;
        case VW_CHANNEL_INT16:
          grassfire_nodata<PixelRGB<int16> >(opt,input,output); break;
        case VW_CHANNEL_UINT16:
          grassfire_nodata<PixelRGB<uint16> >(opt,input,output); break;
        default:
          grassfire_nodata<PixelRGB<float32> >(opt,input,output); break;
        }
        break;
      default:
        switch (channel_type) {
        case VW_CHANNEL_UINT8:
          grassfire_alpha<PixelRGBA<uint8> >(opt,input,output); break;
        case VW_CHANNEL_INT16:
          grassfire_alpha<PixelRGBA<int16> >(opt,input,output); break;
        case VW_CHANNEL_UINT16:
          grassfire_alpha<PixelRGBA<uint16> >(opt,input,output); break;
        default:
          grassfire_alpha<PixelRGBA<float32> >(opt,input,output); break;
        }
        break;
      }
    } // end FOREACH
  } catch ( const ArgumentErr& e ) {
    vw_out() << e.what() << std::endl;
    return 1;
  } catch ( const Exception& e ) {
    std::cerr << "Error: " << e.what() << std::endl;
    return 1;
  }
}


#include "builders/terrain/LineGridSplitter.hpp"

#include <boost/test/unit_test.hpp>

using namespace ClipperLib;
using namespace utymap::builders;
using namespace utymap::math;

namespace {
typedef std::vector<Vector2> DoublePoints;
typedef std::vector<IntPoint> IntPoints;
const double Precision = 0.1e-9;
}

BOOST_AUTO_TEST_SUITE(Builders_Terrain_LineGridSplitter)

BOOST_AUTO_TEST_CASE(GivenHorizontal_WhenSplitWithIntStep_ThenCanSplit) {
  IntPoint start(0, 0);
  IntPoint end(10, 0);
  LineGridSplitter splitter;
  DoublePoints result;

  splitter.split(start, end, result);

  for (int i = 0; i <= 10; ++i) {
    BOOST_CHECK_CLOSE(i, result[i].x, Precision);
    BOOST_CHECK_CLOSE(0, result[i].y, Precision);
  }
}

BOOST_AUTO_TEST_CASE(GivenVertical_WhenSplitWithIntStep_ThenCanSplit) {
  IntPoint start(0, 0);
  IntPoint end(0, 10);
  LineGridSplitter splitter;
  DoublePoints result;

  splitter.split(start, end, result);

  for (int i = 0; i <= 10; ++i) {
    BOOST_CHECK_CLOSE(0, result[i].x, Precision);
    BOOST_CHECK_CLOSE(i, result[i].y, Precision);
  }
}

BOOST_AUTO_TEST_CASE(Given45Angle_WhenSplitWithIntStep_ThenCanSplit) {
  IntPoint start(0, 0);
  IntPoint end(-10, 10);
  LineGridSplitter splitter;
  DoublePoints result;

  splitter.split(start, end, result);

  for (int i = 0; i <= 10; ++i) {
    BOOST_CHECK_CLOSE(-i, result[i].x, Precision);
    BOOST_CHECK_CLOSE(i, result[i].y, Precision);
  }
}

BOOST_AUTO_TEST_CASE(Given45Angle_WhenSplitWithHighLoD_ThenCanSplit) {
  LineGridSplitter splitter;
  IntPoint start(0, 0);
  IntPoint end(10, 10);
  DoublePoints result;

  splitter.split(start, end, result);

  for (int i = 0; i <= 10; ++i) {
    BOOST_CHECK_CLOSE(i, result[i].x, Precision);
    BOOST_CHECK_CLOSE(i, result[i].y, Precision);
  }
}

// These tests are for some bugs observed for real data
BOOST_AUTO_TEST_CASE(GivenSpecificCase1_WhenSplit_ThenCanSplit) {
  LineGridSplitter splitter;
  splitter.setParams(1E8, 3);
  IntPoint start(5286462881, -916628251);
  IntPoint end(5388136261, -968852454);
  DoublePoints result;

  splitter.split(start, end, result);

  for (int i = 0; i < result.size(); ++i) {
    BOOST_CHECK_LT(std::abs(result[i].x), 54);
    BOOST_CHECK_LT(std::abs(result[i].y), 10);
  }
}

// These tests are for some bugs observed for real data
BOOST_AUTO_TEST_CASE(GivenSpecificCase2_WhenSplit_ThenCanSplit) {
  LineGridSplitter splitter;
  splitter.setParams(1E7, 3);
  IntPoint start(-428193799, 626823300);
  IntPoint end(-411886999, 634824599);
  DoublePoints result;

  splitter.split(start, end, result);

  BOOST_CHECK_EQUAL(result.size(), 4);
}

BOOST_AUTO_TEST_CASE(GivenSpecificCase3_WhenSplit_ThenDoNotInflateDuplicates) {
  LineGridSplitter splitter;
  splitter.setParams(10000000, 0.0006103515625);
  IntPoints points = {
      {133691881, 525218163},
      {133693424, 525219786},
      {133692010, 525218310}
  };
  DoublePoints result;

  auto lastItemIndex = points.size() - 1;
  for (int i = 0; i <= lastItemIndex; i++)
    splitter.split(points[i], points[i==lastItemIndex ? 0 : i + 1], result);

  // TODO better to check duplicates directly
  BOOST_CHECK_EQUAL(result.size(), 6);
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/config.hpp>

namespace project {

#ifdef BOOST_HAS_INT128
typedef boost::int128_type int_t;
typedef boost::uint128_type uint_t;

inline int_t mul(int_t v1, int_t v2, int_t v3) {
    return v1 * v2 * v3;
}

#else // #ifdef BOOST_HAS_INT128

#ifdef BOOST_NO_LONG_LONG
#error "This code requires at least int64_t support"
#endif

struct int_t { boost::long_long_type hi, lo; };
struct uint_t { boost::ulong_long_type hi, lo; };

inline int_t mul(int_t v1, int_t v2, int_t v3) {
    // Some hand written math.
    // ...
    return int_t();
}

#endif // #ifdef BOOST_HAS_INT128

} // namespace project

int main() {
    using namespace project;

    mul(int_t(), int_t(), int_t());
    return 0;
}


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

#include <gnss_conversion_nodes/gnss_conversion_node.hpp>

#include <common/types.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>

#include <tf2/buffer_core.h>
#include <tf2_eigen/tf2_eigen.h>

#include <Eigen/Core>

#include <limits>
#include <memory>
#include <string>
#include <vector>

namespace
{
static constexpr auto kDefaultHistorySize = 10;
static constexpr auto kHistorySizeTag = "history_size";
static constexpr auto kOutputTopic = "gnss_position";
static constexpr auto kGnssFixInputTopic = "wgs84_position";
static constexpr auto kFrameIdTag = "output_frame_id";
static constexpr auto kChildFrameIdTag = "child_frame_id";
static constexpr auto kDefaultChildFrameIdTag = "base_link";
static constexpr auto kOverrideCovarianceTag = "override_variances";
static constexpr auto kDefaultFrameId = "earth";
static constexpr auto kDefaultLoggingInterval = 1000;  // Milliseconds.

using autoware_auto_geometry_msgs::msg::RelativePositionWithCovarianceStamped;

template<typename ArrayT>
Eigen::Matrix3d to_eigen(const ArrayT & covariance)
{
  if (covariance.size() < 9UL) {
    throw std::domain_error(
            "The array representing the covariance matrix does not have enough elements. "
            "Expected " + std::to_string(9UL) + " but has " + std::to_string(covariance.size()));
  }
  return Eigen::Matrix3d{Eigen::Map<const Eigen::Matrix3d>{&covariance[0]}};
}

template<typename ArrayT>
void fill_covariance_from_eigen(ArrayT & covariance, const Eigen::Matrix3d & matrix)
{
  if (covariance.size() < 9UL) {
    throw std::domain_error(
            "The array representing the covariance matrix does not have enough elements. "
            "Expected " + std::to_string(9UL) + " but has " + std::to_string(covariance.size()));
  }
  Eigen::Map<Eigen::Matrix3d>{&covariance[0]} = matrix;
}

Eigen::Matrix3d rotate_covariance(
  const Eigen::Isometry3d & tf__new__old,
  const Eigen::Matrix3d & covariance)
{
  return tf__new__old.rotation() * covariance * tf__new__old.rotation().transpose();
}

geometry_msgs::msg::Point to_point(const Eigen::Vector3d & v)
{
  return geometry_msgs::build<geometry_msgs::msg::Point>().x(v[0]).y(v[1]).z(v[2]);
}

void switch_frames_if_needed(
  autoware_auto_geometry_msgs::msg::RelativePositionWithCovarianceStamped & msg,
  const std_msgs::msg::Header::_frame_id_type & new_frame_id,
  const tf2::BufferCore & buffer)
{
  if (new_frame_id == msg.header.frame_id) {return;}
  const Eigen::Isometry3d tf__new_frame_id__msg_frame_id = tf2::transformToEigen(
    buffer.lookupTransform(
      new_frame_id, msg.header.frame_id, tf2_ros::fromMsg(msg.header.stamp)));
  Eigen::Vector3d position;
  tf2::fromMsg(msg.position, position);
  msg.position = to_point(tf__new_frame_id__msg_frame_id * position);

  // Update position covariance
  fill_covariance_from_eigen(
    msg.covariance, rotate_covariance(tf__new_frame_id__msg_frame_id, to_eigen(msg.covariance)));
  msg.header.frame_id = new_frame_id;
}

}  // namespace

namespace autoware
{
namespace gnss_conversion_nodes
{

GnssConversionNode::GnssConversionNode(const rclcpp::NodeOptions & options)
:  Node("gnss_conversion_nodes", options),
  m_wgs84_to_ecef_convertor{
    GeographicLib::Constants::WGS84_a(),
    GeographicLib::Constants::WGS84_f()},
  m_tf_listener(m_tf_buffer, std::shared_ptr<rclcpp::Node>(this, [](auto) {}), false)
{
  const auto history_size{declare_parameter(kHistorySizeTag, kDefaultHistorySize)};
  if (history_size <= 0) {throw std::domain_error("History size must be positive.");}
  m_frame_id = declare_parameter(kFrameIdTag, kDefaultFrameId);
  m_child_frame_id = declare_parameter(kChildFrameIdTag, kDefaultChildFrameIdTag);
  const auto qos = rclcpp::QoS{static_cast<std::size_t>(history_size)};

  m_publisher = create_publisher<RelativePositionWithCovarianceStamped>(kOutputTopic, qos);
  m_gnss_nav_fix_subscription = create_subscription<sensor_msgs::msg::NavSatFix>(
    kGnssFixInputTopic, qos,
    std::bind(&GnssConversionNode::nav_sat_fix_callback, this, std::placeholders::_1));
  m_override_variances_diagonal =
    declare_parameter(kOverrideCovarianceTag, std::vector<common::types::float64_t>{});
  if (m_override_variances_diagonal.size() != 3UL) {
    throw std::runtime_error("Override covariance must have exactly 3 entries.");
  }
}

void GnssConversionNode::nav_sat_fix_callback(
  const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) const
{
  if (msg->status.status == msg->status.STATUS_NO_FIX) {
    RCLCPP_WARN_THROTTLE(
      get_logger(),
      m_steady_clock,
      kDefaultLoggingInterval,
      "GNSS has no fix, so nothing is published.");
    return;
  }
  RelativePositionWithCovarianceStamped out_msg{};
  out_msg.header.stamp = msg->header.stamp;
  out_msg.header.frame_id = kDefaultFrameId;
  out_msg.child_frame_id = m_child_frame_id;
  m_wgs84_to_ecef_convertor.Forward(
    msg->latitude, msg->longitude, msg->altitude,
    out_msg.position.x, out_msg.position.y, out_msg.position.z);
  try {
    switch_frames_if_needed(out_msg, m_frame_id, m_tf_buffer);
  } catch (const tf2::LookupException & exception) {
    RCLCPP_WARN_THROTTLE(
      get_logger(),
      m_steady_clock,
      kDefaultLoggingInterval,
      "Skipping publishing of a GNSS pose message.\n"
      "Could not look up transformation between " +
      out_msg.header.frame_id + " and " +
      m_frame_id + " with the exception: " + exception.what());
    return;
  }
  if (!m_override_variances_diagonal.empty()) {
    const Eigen::Vector3d variances_diagonal{
      Eigen::Map<const Eigen::Vector3d>{m_override_variances_diagonal.data()}};
    const auto override_covariance = variances_diagonal.array().square().matrix().asDiagonal();
    auto pose_covariance = Eigen::Map<Eigen::Matrix3d>{&out_msg.covariance.front()};
    pose_covariance.topLeftCorner(
      override_covariance.rows(), override_covariance.cols()) = override_covariance;
  }
  m_publisher->publish(out_msg);
}

}  // namespace gnss_conversion_nodes
}  // namespace autoware

RCLCPP_COMPONENTS_REGISTER_NODE(autoware::gnss_conversion_nodes::GnssConversionNode)


#pragma once

#include <polyfem/Problem.hpp>
#include <polyfem/InterpolatedFunction.hpp>
#include <polyfem/RBFInterpolation.hpp>

#include <vector>
#include <Eigen/Dense>

namespace polyfem
{
	class NodeValues
	{
	public:

		NodeValues();

		void load(const std::string &path);
		void init(const Mesh &mesh);

		double dirichlet_interpolate(const int p_id, const Eigen::MatrixXd &uv) const
		{
			return interpolate(p_id, uv, true);
		}
		double neumann_interpolate(const int p_id, const Eigen::MatrixXd &uv) const
		{
			return interpolate(p_id, uv, false);
		}
	private:
		double interpolate(const int p_id, const Eigen::MatrixXd &uv, bool is_dirichelt) const;

		std::vector<int> raw_ids_;
		std::vector<std::vector<double>> raw_data_;
		std::vector<bool> raw_dirichelt_;

		std::vector<Eigen::VectorXd> data_;
		std::vector<bool> dirichelt_;
	};

	class NodeProblem: public Problem
	{
	public:
		NodeProblem(const std::string &name);
		void init(const Mesh &mesh) override;

		void rhs(const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		bool is_rhs_zero() const override { return abs(rhs_) < 1e-10; }

		void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;
		void neumann_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const override;

		bool has_exact_sol() const override { return false; }
		bool is_scalar() const override { return true; }

		void set_parameters(const json &params) override;

		bool is_dimention_dirichet(const int tag, const int dim) const override;
		bool all_dimentions_dirichelt() const override { return all_dimentions_dirichelt_; }
	private:
		bool all_dimentions_dirichelt_ = true;
		std::vector<Eigen::Matrix<bool, 1, 3, Eigen::RowMajor>> dirichelt_dimentions_;
		double rhs_;
		NodeValues values_;
		bool is_all_;
	};
}


#ifndef PYTHONIC_INCLUDE_NUMPY_COSH_HPP
#define PYTHONIC_INCLUDE_NUMPY_COSH_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/cosh.hpp>

namespace pythonic
{

  namespace numpy
  {
#define NUMPY_NARY_FUNC_NAME cosh
#define NUMPY_NARY_FUNC_SYM boost::simd::cosh
#include "pythonic/include/types/numpy_nary_expr.hpp"
  }
}

#endif


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the deep learning tools from the dlib C++
    Library.  I'm assuming you have already read the dnn_introduction_ex.cpp, the
    dnn_introduction2_ex.cpp and the dnn_introduction3_ex.cpp examples.  In this example
    program we are going to show how one can train a neural network using an unsupervised
    loss function.  In particular, we will train the ResNet50 model from the paper
    "Deep Residual Learning for Image Recognition" by Kaiming He, Xiangyu Zhang, Shaoqing
    Ren, Jian Sun.

    To train the unsupervised loss, we will use the self-supervised learning (SSL) method
    called Barlow Twins, introduced in this paper:
    "Barlow Twins: Self-Supervised Learning via Redundancy Reduction" by Jure Zbontar,
    Li Jing, Ishan Misra, Yann LeCun, Stéphane Deny.

    The paper contains a good explanation on how and why this works, but the main idea
    behind the Barlow Twins method is:
        - generate two distorted views of a batch of images: YA, YB
        - feed them to a deep neural network and obtain their representations and
          and batch normalize them: ZA, ZB
        - compute the empirical cross-correlation matrix between both feature
          representations as: C = trans(ZA) * ZB.
        - make C as close as possible to the identity matrix.

    This removes the redundancy of the feature representations, by maximizing the
    encoded information about the images themselves, while minimizing the information
    about the transforms and data augmentations used to obtain the representations.

    The original Barlow Twins paper uses the ImageNet dataset, but in this example we
    are using CIFAR-10, so we will follow the recommendations of this paper, instead:
    "A Note on Connecting Barlow Twins with Negative-Sample-Free Contrastive Learning"
    by Yao-Hung Hubert Tsai, Shaojie Bai, Louis-Philippe Morency, Ruslan Salakhutdinov,
    in which they experiment with Barlow Twins on CIFAR-10 and Tiny ImageNet.  Since
    the CIFAR-10 contains relatively small images, we will define a ResNet50 architecture
    that doesn't downsample the input in the first convolutional layer, and doesn't have
    a max pooling layer afterwards, like the paper does.
*/

#include <dlib/cmd_line_parser.h>
#include <dlib/data_io.h>
#include <dlib/dnn.h>
#include <dlib/global_optimization.h>
#include <dlib/gui_widgets.h>
#include <dlib/svm_threaded.h>

using namespace std;
using namespace dlib;

// A custom definition of ResNet50 with a downsampling factor of 8 instead of 32.
// It is essentially the original ResNet50, but without the max pooling and a
// convolutional layer with a stride of 1 instead of 2 at the input.
namespace resnet50
{
    using namespace dlib;
    template <template <typename> class BN>
    struct def
    {
        template <long N, int K, int S, typename SUBNET>
        using conv = add_layer<con_<N, K, K, S, S, K / 2, K / 2>, SUBNET>;

        template<long N, int S, typename SUBNET>
        using bottleneck = BN<conv<4 * N, 1, 1, relu<BN<conv<N, 3, S, relu<BN<conv<N, 1, 1, SUBNET>>>>>>>>;

        template <long N,  typename SUBNET>
        using residual = add_prev1<bottleneck<N, 1, tag1<SUBNET>>>;

        template <typename SUBNET> using res_512 = relu<residual<512, SUBNET>>;
        template <typename SUBNET> using res_256 = relu<residual<256, SUBNET>>;
        template <typename SUBNET> using res_128 = relu<residual<128, SUBNET>>;
        template <typename SUBNET> using res_64  = relu<residual<64, SUBNET>>;

        template <long N, int S, typename SUBNET>
        using transition = add_prev2<BN<conv<4 * N, 1, S, skip1<tag2<bottleneck<N, S, tag1<SUBNET>>>>>>>;

        template <typename INPUT>
        using backbone = avg_pool_everything<
            repeat<2, res_512, transition<512, 2,
            repeat<5, res_256, transition<256, 2,
            repeat<3, res_128, transition<128, 2,
            repeat<2, res_64,  transition<64, 1,
            relu<BN<conv<64, 3, 1,INPUT>>>>>>>>>>>>;
    };
};

// This model namespace contains the definitions for:
// - SSL model using the Barlow Twins loss, a projector head and an input_rgb_image_pair.
// - A feature extractor model using the loss_metric (to get the outputs) and an input_rgb_image.
namespace model
{
    template <typename SUBNET> using projector = fc<128, relu<bn_fc<fc<512, SUBNET>>>>;
    using train = loss_barlow_twins<projector<resnet50::def<bn_con>::backbone<input_rgb_image_pair>>>;
    using feats = loss_metric<resnet50::def<affine>::backbone<input_rgb_image>>;
}

rectangle make_random_cropping_rect(
    const matrix<rgb_pixel>& image,
    dlib::rand& rnd
)
{
    const double mins = 7. / 15.;
    const double maxs = 7. / 8.;
    const auto scale = rnd.get_double_in_range(mins, maxs);
    const auto size = scale * std::min(image.nr(), image.nc());
    const rectangle rect(size, size);
    const point offset(rnd.get_random_32bit_number() % (image.nc() - rect.width()),
                       rnd.get_random_32bit_number() % (image.nr() - rect.height()));
    return move_rect(rect, offset);
}

matrix<rgb_pixel> augment(
    const matrix<rgb_pixel>& image,
    const bool prime,
    dlib::rand& rnd
)
{
    matrix<rgb_pixel> crop;
    // blur
    matrix<rgb_pixel> blurred;
    const double sigma = rnd.get_double_in_range(0.1, 1.1);
    if (!prime || (prime && rnd.get_random_double() < 0.1))
    {
        const auto rect = gaussian_blur(image, blurred, sigma);
        extract_image_chip(blurred, rect, crop);
        blurred = crop;
    }
    else
    {
        blurred = image;
    }

    // randomly crop
    const auto rect = make_random_cropping_rect(image, rnd);
    extract_image_chip(blurred, chip_details(rect, chip_dims(32, 32)), crop);

    // image left-right flip
    if (rnd.get_random_double() < 0.5)
        flip_image_left_right(crop);

    // color augmentation
    if (rnd.get_random_double() < 0.8)
        disturb_colors(crop, rnd, 0.5, 0.5);

    // grayscale
    if (rnd.get_random_double() < 0.2)
    {
        matrix<unsigned char> gray;
        assign_image(gray, crop);
        assign_image(crop, gray);
    }

    // solarize
    if (prime && rnd.get_random_double() < 0.2)
    {
        for (auto& p : crop)
        {
            if (p.red > 128)
                p.red = 255 - p.red;
            if (p.green > 128)
                p.green = 255 - p.green;
            if (p.blue > 128)
                p.blue = 255 - p.blue;
        }
    }
    return crop;
}

int main(const int argc, const char** argv)
try
{
    // The default settings are fine for the example already.
    command_line_parser parser;
    parser.add_option("batch", "set the mini batch size per GPU (default: 64)", 1);
    parser.add_option("dims", "set the projector dimensions (default: 128)", 1);
    parser.add_option("lambda", "penalize off-diagonal terms (default: 1/dims)", 1);
    parser.add_option("learning-rate", "set the initial learning rate (default: 1e-3)", 1);
    parser.add_option("min-learning-rate", "set the min learning rate (default: 1e-5)", 1);
    parser.add_option("num-gpus", "number of GPUs (default: 1)", 1);
    parser.set_group_name("Help Options");
    parser.add_option("h", "alias for --help");
    parser.add_option("help", "display this message and exit");
    parser.parse(argc, argv);

    if (parser.number_of_arguments() < 1 || parser.option("h") || parser.option("help"))
    {
        cout << "This example needs the CIFAR-10 dataset to run." << endl;
        cout << "You can get CIFAR-10 from https://www.cs.toronto.edu/~kriz/cifar.html" << endl;
        cout << "Download the binary version the dataset, decompress it, and put the 6" << endl;
        cout << "bin files in a folder.  Then give that folder as input to this program." << endl;
        parser.print_options();
        return EXIT_SUCCESS;
    }

    const size_t num_gpus = get_option(parser, "num-gpus", 1);
    const size_t batch_size = get_option(parser, "batch", 64) * num_gpus;
    const long dims = get_option(parser, "dims", 128);
    const double lambda = get_option(parser, "lambda", 1.0 / dims);
    const double learning_rate = get_option(parser, "learning-rate", 1e-3);
    const double min_learning_rate = get_option(parser, "min-learning-rate", 1e-5);

    // Load the CIFAR-10 dataset into memory.
    std::vector<matrix<rgb_pixel>> training_images, testing_images;
    std::vector<unsigned long> training_labels, testing_labels;
    load_cifar_10_dataset(parser[0], training_images, training_labels, testing_images, testing_labels);

    // Initialize the model with the specified projector dimensions and lambda.  According to the
    // second paper, lambda = 1/dims works well on CIFAR-10.
    model::train net((loss_barlow_twins_(lambda)));
    layer<1>(net).layer_details().set_num_outputs(dims);
    disable_duplicative_biases(net);
    dlib::rand rnd;
    std::vector<int> gpus(num_gpus);
    std::iota(gpus.begin(), gpus.end(), 0);

    // Train the feature extractor using the Barlow Twins method
    {
        dnn_trainer<model::train, adam> trainer(net, adam(1e-6, 0.9, 0.999), gpus);
        trainer.set_mini_batch_size(batch_size);
        trainer.set_learning_rate(learning_rate);
        trainer.set_min_learning_rate(min_learning_rate);
        trainer.set_iterations_without_progress_threshold(10000);
        trainer.set_synchronization_file("barlow_twins_sync");
        trainer.be_verbose();
        cout << trainer << endl;

        // During the training, we will compute the empirical cross-correlation matrix
        // between the features of both versions of the augmented images.  This matrix
        // should be getting close to the identity matrix as the training progresses.
        // Note that this step is already done in the loss layer, and it's not necessary
        // to do it here for the example to work.  However, it provides a nice
        // visualization of the training progress: the closer to the identity matrix,
        // the better.
        resizable_tensor eccm;
        eccm.set_size(dims, dims);
        // Some tensors needed to perform batch normalization
        resizable_tensor za_norm, zb_norm, means, invstds, rms, rvs, gamma, beta;
        const double eps = DEFAULT_BATCH_NORM_EPS;
        gamma.set_size(1, dims);
        beta.set_size(1, dims);
        image_window win;

        std::vector<std::pair<matrix<rgb_pixel>, matrix<rgb_pixel>>> batch;
        while (trainer.get_learning_rate() >= trainer.get_min_learning_rate())
        {
            batch.clear();
            while (batch.size() < trainer.get_mini_batch_size())
            {
                const auto idx = rnd.get_random_32bit_number() % training_images.size();
                auto image = training_images[idx];
                batch.emplace_back(augment(image, false, rnd), augment(image, true, rnd));
            }
            trainer.train_one_step(batch);

            // Compute the empirical cross-correlation matrix every 100 steps. Again,
            // this is not needed for the training to work, but it's nice to visualize.
            if (trainer.get_train_one_step_calls() % 100 == 0)
            {
                // Wait for threaded processing to stop in the trainer.
                trainer.get_net(force_flush_to_disk::no);
                // Get the output from the last fc layer
                const auto& out = net.subnet().get_output();
                // The trainer might have synchronized its state to the disk and cleaned
                // the network state. If that happens, the output will be empty, in
                // which case, we just skip the empirical cross-correlation matrix
                // computation.
                if (out.size() == 0)
                    continue;
                // Separate both augmented versions of the images
                alias_tensor split(out.num_samples() / 2, dims);
                auto za = split(out);
                auto zb = split(out, split.size());
                gamma = 1;
                beta = 0;
                // Perform batch normalization on each feature representation, independently.
                tt::batch_normalize(eps, za_norm, means, invstds, 1, rms, rvs, za, gamma, beta);
                tt::batch_normalize(eps, zb_norm, means, invstds, 1, rms, rvs, za, gamma, beta);
                // Compute the empirical cross-correlation matrix between the features and
                // visualize it.
                tt::gemm(0, eccm, 1, za_norm, true, zb_norm, false);
                eccm /= batch_size;
                win.set_image(round(abs(mat(eccm)) * 255));
                win.set_title("Barlow Twins step#: " + to_string(trainer.get_train_one_step_calls()));
            }
        }
        trainer.get_net();
        net.clean();
        // After training, we can discard the projector head and just keep the backone
        // to train it or finetune it on other downstream tasks.
        serialize("resnet50_self_supervised_cifar_10.net") << layer<5>(net);
    }

    // Now, we initialize the feature extractor model with the backbone we have just learned.
    model::feats fnet(layer<5>(net));
    // And we will generate all the features for the training set to train a multiclass SVM
    // classifier.
    std::vector<matrix<float, 0, 1>> features;
    cout << "Extracting features for linear classifier..." << endl;
    features = fnet(training_images, 4 * batch_size);
    vector_normalizer<matrix<float, 0, 1>> normalizer;
    normalizer.train(features);
    for (auto& feature : features)
        feature = normalizer(feature);

    // Find the most appropriate C setting using find_max_global.
    auto cross_validation_score = [&](const double c)
    {
        svm_multiclass_linear_trainer<linear_kernel<matrix<float, 0, 1>>, unsigned long> trainer;
        trainer.set_c(c);
        trainer.set_epsilon(0.01);
        trainer.set_max_iterations(100);
        trainer.set_num_threads(std::thread::hardware_concurrency());
        cout << "C: " << c << endl;
        const auto cm = cross_validate_multiclass_trainer(trainer, features, training_labels, 3);
        const double accuracy = sum(diag(cm)) / sum(cm);
        cout << "cross validation accuracy: " << accuracy << endl;
        cout << "confusion matrix:\n " << cm << endl;
        return accuracy;
    };
    const auto result = find_max_global(cross_validation_score, 1e-3, 1000, max_function_calls(50));
    cout << "Best C: " << result.x(0) << endl;

    // Proceed to train the SVM classifier with the best C.
    svm_multiclass_linear_trainer<linear_kernel<matrix<float, 0, 1>>, unsigned long> trainer;
    trainer.set_num_threads(std::thread::hardware_concurrency());
    trainer.set_c(result.x(0));
    cout << "Training Multiclass SVM..." << endl;
    const auto df = trainer.train(features, training_labels);
    serialize("multiclass_svm_cifar_10.dat") << df;

    // Finally, we can compute the accuracy of the model on the CIFAR-10 train and test images.
    auto compute_accuracy = [&fnet, &df, batch_size](
        const std::vector<matrix<float, 0, 1>>& samples,
        const std::vector<unsigned long>& labels
    )
    {
        size_t num_right = 0;
        size_t num_wrong = 0;
        for (size_t i = 0; i < labels.size(); ++i)
        {
            if (labels[i] == df(samples[i]))
                ++num_right;
            else
                ++num_wrong;
        }
        cout << "  num right:  " << num_right << endl;
        cout << "  num wrong:  " << num_wrong << endl;
        cout << "  accuracy:   " << num_right / static_cast<double>(num_right + num_wrong) << endl;
        cout << "  error rate: " << num_wrong / static_cast<double>(num_right + num_wrong) << endl;
    };

    // We should get a training accuracy of around 93% and a testing accuracy of around 89%.
    cout << "\ntraining accuracy" << endl;
    compute_accuracy(features, training_labels);
    cout << "\ntesting accuracy" << endl;
    features = fnet(testing_images, 4 * batch_size);
    for (auto& feature : features)
        feature = normalizer(feature);
    compute_accuracy(features, testing_labels);
    return EXIT_SUCCESS;
}
catch (const exception& e)
{
    cout << e.what() << endl;
    return EXIT_FAILURE;
}


// Catch assumes that std::nullptr_t is available on GCC 5 without including
// the corresponding header.
#include <cstddef>

#include <websocketpp/server.hpp>
#include <websocketpp/config/asio_no_tls.hpp>
#include <boost/network/protocol/http.hpp>
#define CATCH_CONFIG_MAIN // This tells Catch to provide a main() - only do this in one cpp file
#include "catch.hpp"
#include <boost/filesystem/operations.hpp>
#include "sqlite3.h"
#include <iostream>
#include "rapidxml.hpp"
#include "rapidjson/document.h"

unsigned int Factorial(unsigned int number)
{
	return number <= 1 ? number : Factorial(number - 1) * number;
}

TEST_CASE("Factorials are computed", "[factorial]")
{
	REQUIRE(Factorial(1) == 1);
	REQUIRE(Factorial(2) == 2);
	REQUIRE(Factorial(3) == 6);
	REQUIRE(Factorial(10) == 3628800);

	websocketpp::server<websocketpp::config::asio> s;

	sqlite3 *db;
	REQUIRE(!sqlite3_open(":memory:", &db));
	sqlite3_close(db);

	rapidxml::xml_document<> doc;
	char source[] = "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"yes\"?><root></root>";
	doc.parse<0>(source);

	rapidjson::Document document;

	std::cout << boost::filesystem::current_path() << '\n';

	boost::network::http::client::request request_("http://google.com/");
	request_ << boost::network::header("Connection", "close");
	boost::network::http::client client_;
	boost::network::http::client::response response_ = client_.get(request_);
	std::string body_ = body(response_);
	std::cerr << body_ << '\n';
}


#ifndef PYTHONIC_SCIPY_SPECIAL_JV_HPP
#define PYTHONIC_SCIPY_SPECIAL_JV_HPP

#include "pythonic/include/scipy/special/jv.hpp"

#include "pythonic/types/ndarray.hpp"
#include "pythonic/utils/functor.hpp"
#include "pythonic/utils/numpy_traits.hpp"

#define BOOST_MATH_THREAD_LOCAL thread_local
#include <boost/math/special_functions/bessel.hpp>

PYTHONIC_NS_BEGIN

namespace scipy
{
  namespace special
  {
    namespace details
    {
      template <class T0, class T1>
      double jv(T0 x, T1 y)
      {
        using namespace boost::math::policies;
        return boost::math::cyl_bessel_j(x, y,
                                         make_policy(promote_double<true>()));
      }
    }

#define NUMPY_NARY_FUNC_NAME jv
#define NUMPY_NARY_FUNC_SYM details::jv
#include "pythonic/types/numpy_nary_expr.hpp"
  }
}
PYTHONIC_NS_END

#endif


/*
 * SPDX-FileCopyrightText: © 2018 Ambrosys GmbH
 *
 * SPDX-FileContributor: Fabian Sandoval Saldias <fabianvss@gmail.com>
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <AppComponents/Common/Filter/SamplingPointFinder.h>

#include <Core/Common/Geometry/Helper.h>

#include <amblog/global.h>

#include <boost/iterator/function_output_iterator.hpp>
#include <boost/geometry/index/rtree.hpp>

#include <cassert>
#include <unordered_map>

namespace {

using StreetIndexGeoindexGeometry = boost::geometry::model::box<Core::Common::Geometry::Point>;
using StreetIndexGeoindexValue = std::pair<StreetIndexGeoindexGeometry, std::pair<size_t, size_t>>;
using StreetIndexGeoindexAlgorithm = boost::geometry::index::quadratic<16>;
using StreetIndexGeoindex = boost::geometry::index::rtree<StreetIndexGeoindexValue, StreetIndexGeoindexAlgorithm>;

/**
 * Add street index and all segment indices to spatial index.
 */
void addStreetIndex(StreetIndexGeoindex & geoindex, size_t const index, Core::Common::Geometry::LineString const & lineString, double const searchRadius)
{
    for (size_t i = 0; i < lineString.size() - 1; ++i)
    {
        auto box = Core::Common::Geometry::buffer(
            boost::geometry::return_envelope<StreetIndexGeoindexGeometry>(Core::Common::Geometry::Segment{lineString[i], lineString[i + 1]}), searchRadius);

        geoindex.insert(StreetIndexGeoindexValue{box, {index, i}});
    }
}

/**
 * @return vector of { streetIndex, streetSegmentIndex }
 */
std::vector<std::pair<size_t, size_t>> getStreetIndices(StreetIndexGeoindex & geoindex, Core::Common::Geometry::Point point)
{
    std::vector<std::pair<size_t, size_t>> indices;
    auto inserter = [&](StreetIndexGeoindexValue const & value) { indices.push_back(value.second); };

    geoindex.query(boost::geometry::index::contains(point), boost::make_function_output_iterator(inserter));
    return indices;
}

/**
 *
 * @param trackHeading Heading of the track point.
 * @param segmentHeading Heading in forwards-direction of the segment. The reversed `segmentHeading` may be considered for the difference-calculation.
 * @param travelDirection If `TravelDirection::both`, the direction nearest to the `trackHeading` is considered. If `TravelDirection::backwards` the `segmentHeading` is considered reversed.
 * @return { headingDifference, trackTravelDirection } `trackTravelDirection` is `TravelDirection::both` if `travelDirection` is also `TravelDirection::both` and the heading difference is nearly 90 degrees.
 */
std::pair<double, AppComponents::Common::Types::Street::TravelDirection>
headingDifference(double const trackHeading, double segmentHeading, AppComponents::Common::Types::Street::TravelDirection const travelDirection)
{
    // TODO: make `67.5` below configurable
    using namespace Core::Common;
    using AppComponents::Common::Types::Street::TravelDirection;
    switch (travelDirection)
    {
        case TravelDirection::both:
        {
            double a = Geometry::absHeadingDiff(trackHeading, segmentHeading);
            double b = Geometry::absHeadingDiff(trackHeading, Geometry::reversedHeading(segmentHeading));
            if (a <= b)
                return {a, a <= 67.5 ? TravelDirection::forwards : TravelDirection::both};
            else
                return {b, b <= 67.5 ? TravelDirection::backwards : TravelDirection::both};
        }
        case TravelDirection::forwards: return {Geometry::absHeadingDiff(trackHeading, segmentHeading), TravelDirection::forwards};
        case TravelDirection::backwards: return {Geometry::absHeadingDiff(trackHeading, Geometry::reversedHeading(segmentHeading)), TravelDirection::backwards};
    }
    assert(false);
    return {};
}

}  // namespace

namespace AppComponents::Common::Filter {

SamplingPointFinder::SamplingPointFinder(SelectionStrategy selectionStrategy, double const searchRadius, double const maxHeadingDifference)
  : Filter("SamplingPointFinder"), selectionStrategy_(selectionStrategy), searchRadius_(searchRadius), maxHeadingDifference_(maxHeadingDifference)
{
    setRequirements({"PointList", "SegmentList", "TravelDirectionList"});
    setOptionals({"HeadingList", "PartialHeadingList"});
    setFulfillments({"SamplingPointList"});
}

bool SamplingPointFinder::operator()(
    Types::Track::PointList const & pointList,
    Types::Track::HeadingList const & headingList,
    Types::Street::SegmentList const & segmentList,
    Types::Street::TravelDirectionList const & travelDirectionList,
    Types::Routing::SamplingPointList & samplingPointList)
{
    assert(pointList.size() == headingList.size() || headingList.empty());
    assert(segmentList.size() == travelDirectionList.size());

    StreetIndexGeoindex geoindex;
    for (size_t i = 0; i < segmentList.size(); ++i)
        addStreetIndex(geoindex, i, segmentList[i].geometry, searchRadius_);

    for (size_t trackIndex = 0; trackIndex < pointList.size(); ++trackIndex)
    {
        std::vector<std::pair<size_t, size_t>> streetIndices = getStreetIndices(geoindex, pointList[trackIndex]);
        if (streetIndices.empty())
            continue;

        // `samplingPoints` are ordered by using the following comparator.
        auto samplingPointCandidateComparator = [&](Types::Routing::SamplingPointCandidate const & a, Types::Routing::SamplingPointCandidate const & b)
        {
            if (a.streetSegmentDistance < b.streetSegmentDistance)
                return true;
            if (a.streetSegmentDistance == b.streetSegmentDistance)
            {
                if (a.streetSegmentHeadingDifference < b.streetSegmentHeadingDifference)
                    return true;
                if (a.streetSegmentHeadingDifference == b.streetSegmentHeadingDifference)
                {
                    if (segmentList[a.streetIndex].originId < segmentList[b.streetIndex].originId)
                        return true;
                    if (segmentList[a.streetIndex].originId == segmentList[b.streetIndex].originId)
                    {
                        if (segmentList[a.streetIndex].originOffset + a.streetSegmentIndex < segmentList[b.streetIndex].originOffset + b.streetSegmentIndex)
                            return true;
                        if (segmentList[a.streetIndex].originOffset + a.streetSegmentIndex == segmentList[b.streetIndex].originOffset + b.streetSegmentIndex)
                        {
                            // Note: originId may not be unique, so it is possible to get to this point.
                            return a.streetIndex < b.streetIndex;
                        }
                    }
                }
            }
            return false;
        };
        auto candidates = std::set<Types::Routing::SamplingPointCandidate, decltype(samplingPointCandidateComparator)>{samplingPointCandidateComparator};

        for (auto [streetIndex, streetSegmentIndex] : streetIndices)
        {
            auto const & segmentGeometry = segmentList[streetIndex].geometry;
            auto segment = Core::Common::Geometry::Segment{segmentGeometry[streetSegmentIndex], segmentGeometry[streetSegmentIndex + 1]};
            auto [streetSegmentDistance, streetSegmentProjectedPoint, streetSegmentProjectedPointNormLength] = Core::Common::Geometry::geoDistance(pointList[trackIndex], segment);

            if (streetSegmentDistance > searchRadius_)
                continue;

            double streetSegmentHeading = Core::Common::Geometry::heading(segment);

            double streetSegmentHeadingDifference;
            Types::Street::TravelDirection streetSegmentTravelDirection;
            if (!headingList.empty())
                std::tie(streetSegmentHeadingDifference, streetSegmentTravelDirection)
                    = headingDifference(headingList[trackIndex], streetSegmentHeading, travelDirectionList[streetIndex]);
            else
            {
                streetSegmentHeadingDifference = 0.0;
                streetSegmentTravelDirection = travelDirectionList[streetIndex];
            }

            if (streetSegmentHeadingDifference > maxHeadingDifference_)
                continue;

            auto candidate = Types::Routing::SamplingPointCandidate{};
            candidate.streetIndex = streetIndex;
            candidate.streetSegmentDistance = streetSegmentDistance;
            candidate.streetSegmentIndex = streetSegmentIndex;
            candidate.streetSegmentProjectedPoint = streetSegmentProjectedPoint;
            candidate.streetSegmentProjectedPointNormLength = streetSegmentProjectedPointNormLength;
            candidate.streetSegmentHeading = streetSegmentHeading;
            candidate.streetSegmentHeadingDifference = streetSegmentHeadingDifference;
            candidate.streetSegmentTravelDirection = streetSegmentTravelDirection;

            candidates.insert(candidate);
        }

        switch (selectionStrategy_)
        {
            case SelectionStrategy::all:
                // Add all candidates.
                if (!candidates.empty())
                    samplingPointList.push_back({trackIndex, {candidates.begin(), candidates.end()}});
                break;
            case SelectionStrategy::best:
                // Only add the best candidate.
                if (!candidates.empty())
                    samplingPointList.push_back({trackIndex, {*candidates.begin()}});
                break;
            case SelectionStrategy::singles:
                // Only add a candidate if it is the only one.
                if (candidates.size() == 1)
                    samplingPointList.push_back({trackIndex, {*candidates.begin()}});
                break;
        }
    }

    APP_LOG(noise) << samplingPointList.size() << " samplingPoints found";

    return true;
}

}  // namespace AppComponents::Common::Filter


#include <iostream>		// cout
#include <fstream>    // std::ifstream std::ofstream
#include <stdint.h>       	// int8_t, uint8_t, uint16_t, ...

#include <vector>
#include <array>			// letzte sensorwerte
#include <thread>			// multithreading

#include <boost/circular_buffer.hpp>			// speichern der letzten n werte
#include <opencv2/opencv.hpp>				// opencv für bildauswertung
#include <math.h>					// abs, cos, sin, ...

#include "Logger.h"			// Logger class
#include "gruen.h"			// alles mit grünpunkt
#include "line.h"				// alles mit linie
#include "util.h"				// sonstige funktionen
#include "config.h"			// defines
#include "config_file.h"	// config datei
#include "drive.h"

#include "CameraCapture.h"		// thread zum kamera einlesen
#include "VideoServer.h"			// thread für Videoausgabe über IP
#include "KamelDevices.h"					// kommunikation mit Arduino und Servosteuerung

namespace lvl = spdlog::level;

using namespace std;
using namespace cv;

Logger debug_lg("debug");			// logger class for log file 'debug.log'

configuration::data configdata;

void image_processing() {

	Mat img_rgb;	// input image
	Mat hsv;			// input Bild im hsv Format
	Mat bin_sw;		// binäres bild schwarz / weiß erkennung; schwarze linie auf bild weiß, alles andere schwarz
	Mat bin_gr;		// binäres bild grünerkennung; grüner punkt weiß, alles andere schwarz

	debug_lg << " > init camera" << lvl::debug;
	CameraCapture cam(0);		// Video eingabe der Kamera '0'; 0, wenn nur eine Kamera angeschlossen ist
	VideoServer srv;				// Klasse für den VideoServer

	cam.set(cv::CAP_PROP_FPS, 30);			// Kamera Framerate auf 30 fps

	// Does not work with raspbian buster on the Raspberry 4B
	//cam.set(cv::CAP_PROP_FRAME_WIDTH, 640);			// Bildauflösung auf 640 x 480px
	//cam.set(cv::CAP_PROP_FRAME_HEIGHT, 480);

	//Füge Fenster zum VideoServer hinzu
	srv.namedWindow("Mask Green");
	srv.namedWindow("Mask SW");
	srv.namedWindow("HSV");
	srv.namedWindow("Input");
#ifdef PLOT_HSV_HISTOGRAM
	srv.namedWindow("H-S Histogram");
#endif

	int64 tloop;

	while(1) {

		tloop = getTickCount();			// Tickcount for whole loop

		while(!cam.read(img_rgb)){}		// warten bis das aktuelle bild einlesbar ist, dann in img_rbg einlesen

		// Filter image and convert to hsv
		GaussianBlur(img_rgb, img_rgb, Size(5,5),2,2);		// Gaussian blur to normalize image
		cvtColor(img_rgb, hsv, COLOR_BGR2HSV);			// Convert to HSV and save in Mat hsv

#ifdef PLOT_HSV_HISTOGRAM

		// Histogramm der HSV Matrix ausgeben (wenn PLOT_HSV_HISTOGRAM definiert ist)

		vector<Mat> bgr_planes;
		split( hsv, bgr_planes );
		int histSize = 256;
		float range[] = { 0, 256 }; //the upper boundary is exclusive
		const float* histRange = { range };
		bool uniform = true, accumulate = false;
		Mat b_hist, g_hist, r_hist;
		calcHist( &bgr_planes[0], 1, 0, Mat(), b_hist, 1, &histSize, &histRange, uniform, accumulate );
		calcHist( &bgr_planes[1], 1, 0, Mat(), g_hist, 1, &histSize, &histRange, uniform, accumulate );
		calcHist( &bgr_planes[2], 1, 0, Mat(), r_hist, 1, &histSize, &histRange, uniform, accumulate );
		int hist_w = 512, hist_h = 400;
		int bin_w = cvRound( (double) hist_w/histSize );
		Mat histImage( hist_h, hist_w, CV_8UC3, Scalar( 255,255,255) );
		normalize(b_hist, b_hist, 0, histImage.rows, NORM_MINMAX, -1, Mat() );
		normalize(g_hist, g_hist, 0, histImage.rows, NORM_MINMAX, -1, Mat() );
		normalize(r_hist, r_hist, 0, histImage.rows, NORM_MINMAX, -1, Mat() );
		for( int i = 1; i < histSize; i++ )
		{
				line( histImage, Point( bin_w*(i-1), hist_h - cvRound(b_hist.at<float>(i-1)) ),
							Point( bin_w*(i), hist_h - cvRound(b_hist.at<float>(i)) ),
							Scalar( 255, 0, 0), 2, 8, 0  );
				line( histImage, Point( bin_w*(i-1), hist_h - cvRound(g_hist.at<float>(i-1)) ),
							Point( bin_w*(i), hist_h - cvRound(g_hist.at<float>(i)) ),
							Scalar( 0, 255, 0), 2, 8, 0  );
				line( histImage, Point( bin_w*(i-1), hist_h - cvRound(r_hist.at<float>(i-1)) ),
							Point( bin_w*(i), hist_h - cvRound(r_hist.at<float>(i)) ),
							Scalar( 0, 0, 255), 2, 8, 0  );
		}
		srv.imshow("H-S Histogram", histImage );
#endif

		separate_gruen(hsv, bin_gr);  // grün auf dem Bild erkennen und in bin_gr als weiß schreiben, muss vor line_calc stehen, wenn der grünpunkt nicht als Linie erkannt werden soll

		line_calc(img_rgb, hsv, bin_sw, bin_gr);		// linienpunkte berechnen, in m_line_points schreiben

		gruen_calc(img_rgb, hsv, bin_sw, bin_gr);		// grstate und grcenter berechnen

		// Alle Bilder an VideoServer übergeben

		srv.imshow("Input", img_rgb);
		srv.imshow("HSV", hsv);
		srv.imshow("Mask SW", bin_sw);
		srv.imshow("Mask Green", bin_gr);
		srv.update();			// VideoServer updaten

		std::cout << "Processing took: " << (getTickCount() - tloop) / getTickFrequency() * 1000.0 << " ms; FPS: " <<  cv::getTickFrequency() / (cv::getTickCount() - tloop) << std::endl;

	}
}

// MAIN FUNCTION TO CALL THREADS

int main() {

	debug_lg.set_level(lvl::debug);

	debug_lg << "read config file" << lvl::debug;
	std::ifstream ifs("/home/pi/projects/KamelPi/src/config.info", std::ifstream::in);			// Config datei einlesen
	ifs >> configdata;			// Config laden
	ifs.close();						// Datei wieder schließen
	debug_lg << "> COMPLETE" << lvl::debug;

	debug_lg << "set config variables" << lvl::debug;
	debug_lg << "THRESH_BLACK" << lvl::debug;
	set_thresh_black(configdata.getintvalue("THRESH_BLACK"));				// Schwarz threshold setzen
	debug_lg << "LOWER_GREEN, UPPER_GREEN" << lvl::debug;
	set_gruen_range(configdata.getscalarvalue("LOWER_GREEN"),configdata.getscalarvalue("UPPER_GREEN"));		// Gruen Threshold setzen

	debug_lg << "FLASCHE_FAHREN" << lvl::debug;
	set_flasche_fahren(configdata.getintvalue("FLASCHE_FAHREN"));
	debug_lg << "FLASCHE_LINKS" << lvl::debug;
	set_flasche_links(configdata.getboolvalue("FLASCHE_LINKS"));
	debug_lg << "KAMERA_WINKEL" << lvl::debug;
	set_kamera_winkel(configdata.getintvalue("KAMERA_WINKEL"));
	debug_lg << "> COMPLETE" << lvl::debug;

	debug_lg << "init sensoren arduino" << lvl::debug;
	if(sen::init_sensoren(0x09) == -1) {
		debug_lg << "> FAILED - error opening sensor_arduino" << lvl::off;
		return -1;
	}
	debug_lg << "> COMPLETE" << lvl::debug;

	debug_lg << "starting threads" << lvl::debug;
	thread drive_t (m_drive);			// thread starten; ruft void m_drive auf
	thread imu_t (sen::m_imu);					// thread startet; void m_imu in neuem thread
	image_processing();						// start void image_processing

	debug_lg << "stopping all threads" << lvl::warn;
	drive_t.detach();							// drive_t anhalten
	imu_t.detach();							// imu_t anhalten
	debug_lg << "> COMPLETE - all threads closed" << lvl::warn;

	return -1;						// fehler
}


/*******************************************************************
Author: David Ge (dge893@gmail.com, aka Wei Ge)
Last modified: 11/16/2020
Allrights reserved by David Ge

field source
********************************************************************/
#include "FieldSourceTss.h"
#include "TimeTssBase.h"

#include <boost/multiprecision/cpp_dec_float.hpp>

#define ih(i, h) ((h)*eMax+(i))

FieldSourceTss::FieldSourceTss()
{
	f0 = g0 = w0 = u0 = NULL;
	_initialized = false;
}

FieldSourceTss::~FieldSourceTss()
{
	cleanup();
}

void FieldSourceTss::cleanup()
{
	if (f0 != NULL)
	{
		free(f0); f0 = NULL;
	}
	if (g0 != NULL)
	{
		free(g0); g0 = NULL;
	}
	if (w0 != NULL)
	{
		free(w0); w0 = NULL;
	}
	if (u0 != NULL)
	{
		free(u0); u0 = NULL;
	}
}

int FieldSourceTss::initialize(SimStruct *params)
{
	int ret = ERR_OK;
	//size_t emMax2;
	size_t srcDim2;
	cleanup();
	pams = params;
	nx1 = pams->nx + 1; ny1 = pams->ny + 1; nz1 = pams->nz + 1;
	emMax = 2 * pams->kmax + 3;
	//emMax2 = emMax * emMax;
	srcDim = 2 * pams->kmax + 1;
	srcDim2 = srcDim * srcDim;
	f0 = (double *)malloc(srcDim2*sizeof(double));
	g0 = (double *)malloc(srcDim2*sizeof(double));
	w0 = (double *)malloc(srcDim2*sizeof(double));
	u0 = (double *)malloc(srcDim2*sizeof(double));
	if (f0 == NULL || g0 == NULL || w0 == NULL || u0 == NULL)
	{
		ret = ERR_OUTOFMEMORY;
	}
	else
	{
		for (unsigned int k = 0; k < srcDim2; k++)
		{
			f0[k] = g0[k] = w0[k] = u0[k] = 0.0;
		}

	}
	_initialized = true;
	return ret;
}

/*
	calculate field fource coefficients
*/
int FieldSourceTss::onInitialized(void *e0, void *m0)
{
	using namespace boost::multiprecision;
	int ret = ERR_OK;
	size_t srcDim2 = srcDim * srcDim;
	cpp_dec_float_100 *f  = (cpp_dec_float_100 *)malloc(srcDim2*sizeof(cpp_dec_float_100));
	cpp_dec_float_100 *g  = (cpp_dec_float_100 *)malloc(srcDim2*sizeof(cpp_dec_float_100));
	cpp_dec_float_100 *w  = (cpp_dec_float_100 *)malloc(srcDim2*sizeof(cpp_dec_float_100));
	cpp_dec_float_100 *u  = (cpp_dec_float_100 *)malloc(srcDim2*sizeof(cpp_dec_float_100));
	cpp_dec_float_100 *f2 = (cpp_dec_float_100 *)malloc(srcDim2*sizeof(cpp_dec_float_100));
	cpp_dec_float_100 *g2 = (cpp_dec_float_100 *)malloc(srcDim2*sizeof(cpp_dec_float_100));
	cpp_dec_float_100 *w2 = (cpp_dec_float_100 *)malloc(srcDim2*sizeof(cpp_dec_float_100));
	cpp_dec_float_100 *u2 = (cpp_dec_float_100 *)malloc(srcDim2*sizeof(cpp_dec_float_100));
	cpp_dec_float_100 *f1 = (cpp_dec_float_100 *)malloc(srcDim2*sizeof(cpp_dec_float_100));
	cpp_dec_float_100 *g1 = (cpp_dec_float_100 *)malloc(srcDim2*sizeof(cpp_dec_float_100));
	cpp_dec_float_100 *w1 = (cpp_dec_float_100 *)malloc(srcDim2*sizeof(cpp_dec_float_100));
	cpp_dec_float_100 *u1 = (cpp_dec_float_100 *)malloc(srcDim2*sizeof(cpp_dec_float_100));
	//
	cpp_dec_float_100 *sr = (cpp_dec_float_100 *)malloc((emMax + 2)*sizeof(cpp_dec_float_100));
	if (sr == NULL || f == NULL || g == NULL || w == NULL || u == NULL
		|| f1 == NULL || g1 == NULL || w1 == NULL || u1 == NULL
		|| f2 == NULL || g2 == NULL || w2 == NULL || u2 == NULL)
		ret = ERR_OUTOFMEMORY;
	else
	{
		cpp_dec_float_100 *e = (cpp_dec_float_100 *)e0;
		cpp_dec_float_100 *m = (cpp_dec_float_100 *)m0;
		for (unsigned int k = 0; k < srcDim2; k++)
		{
			f[k]  = 0.0; g[k]  = 0.0; w[k]  = 0.0; u[k]  = 0.0;
		}
		cpp_dec_float_100 XN = 1.0;
		cpp_dec_float_100 X = 0.0;
		cpp_dec_float_100 dtN = 1.0;
		//sr[k] = (∆_t^k)/ k!
		for (unsigned int k = 0; k < emMax + 2; k++)
		{
			sr[k] = dtN / XN;
			X += 1.0;
			XN = XN * X;
			dtN = dtN * pams->dt;
		}
		/*
		S_m (2k+1)=∑_(h=0)^k▒〖∇^{2(k-h)} ×∑_(i=0)^2h▒〖m_i^{2(k-h)}   (d^(2h-i) J_m)/(dt^(2h-i) )〗〗
			      +∑_(h=0)^(k-1)▒〖∇^{2(k-h)-1} ×∑_(i=0)^(2h+1)▒〖e_i^{2(k-h)-1}   (d^(2h-i+1) J_e)/(dt^(2h-i+1) )〗〗
		S_e (2k+1)=∑_(h=0)^k▒〖∇^{2(k-h)} ×∑_(i=0)^2h▒〖e_i^{2(k-h)}   (d^(2h-i) J_e)/(dt^(2h-i) )〗〗
			      +∑_(h=0)^(k-1)▒〖∇^{2(k-h)-1} ×∑_(i=0)^(2h+1)▒〖m_i^{2(k-h)-1}   (d^(2h-i+1) J_m)/(dt^(2h-i+1) )〗〗
		S_m (2k)  =∑_(h=0)^(k-1)▒(∇^{2(k-1-h)} ×∑_(i=0)^(2h+1)▒〖m_i^{2(k-1-h)}   (d^(2h+1-i) J_m)/(dt^(2h+1-i) )〗
				  +∇^{2(k-h)-1} ×∑_(i=0)^2h▒〖e_i^{2(k-h)-1}   (d^(2h-i) J_e)/(dt^(2h-i) )〗)
		S_e (2k)  =∑_(h=0)^(k-1)▒(∇^{2(k-1-h)} ×∑_(i=0)^(2h+1)▒〖e_i^{2(k-1-h)}   (d^(2h+1-i) J_e)/(dt^(2h+1-i) )〗
				  +∇^{2(k-h)-1} ×∑_(i=0)^2h▒〖m_i^{2(k-h)-1}   (d^(2h-i) J_m)/(dt^(2h-i) )〗)

		S^h (k,∆_t)=(∆_t^2k)/(2k)! S_m (2k) + (∆_t^(2k+1))/(2k+1)! S_m (2k+1)
		S^e (k,∆_t)=(∆_t^2k)/(2k)! S_e (2k) + (∆_t^(2k+1))/(2k+1)! S_e (2k+1)

		SH=∑_(k=0)^(k_max)▒〖S^h (k,∆_t ) 〗-> get f and g -> C.F.dJm + C.G.dJe
		SE=∑_(k=0)^(k_max)▒〖S^e (k,∆_t ) 〗-> get u and w -> C.U.dJe + C.W.dJm

		S_m (2k)   -> f2, g2;  S_m (2k+1) -> f1, g1;
		S_e (2k)   -> u2, w2;  S_e (2k+1) -> u1, w1
		f = sr[2k]*f2 + sr[2k+1]*f1; g = sr[2k]*g2 + sr[2k+1]*g1
		u = sr[2k]*u2 + sr[2k+1]*u1; w = sr[2k]*w2 + sr[2k+1]*w1
		*/
		size_t t,t1=0;
		unsigned int kmax = pams->kmax;
		t = 0;
		for (unsigned int k = 0; k <= kmax; k++)
		{
			//temporary summation holders
			for (t = 0; t < srcDim2; t++)
			{
				f1[t] = 0.0; g1[t] = 0.0; w1[t] = 0.0; u1[t] = 0.0;
				f2[t] = 0.0; g2[t] = 0.0; w2[t] = 0.0; u2[t] = 0.0;
			}
			//H = C.F.dJm + C.G.dJe + ...
			//E = C.U.dJe + C.W.dJm + ...
			for (unsigned int h = 0; h <= k; h++)
			{
				//first part of S_m (2k+1)->f1:Jm, S_e (2k+1)->u1:Je
				//∇^{2(k-h)} -> row number 0...2kmax
				for (unsigned int i = 0; i <= 2 * h; i++)
				{
					//d^(2h-i), column number 2 * h - i
					t = Sr_ih(2 * h - i, 2 * (k - h));  //0...2kmax, 0...,2kmax
					t1 = I_ih(i, 2 * (k - h)); //m_i^{2(k-h)}, e_i^{2(k-h)}
					f1[t] += m[t1];
					u1[t] += e[t1];
				}
			}
			for (unsigned int h = 0; h < k; h++)
			{
				for (unsigned int i = 0; i <= 2 * h + 1; i++)
				{
					//second part of S_m (2k+1)->g1:Je, S_e (2k+1)->w1:Jm
					//∇^{2(k-h)-1} ->row number
					//column number 2h-i+1
					t = Sr_ih(2 * h - i + 1, 2 * (k - h) - 1);
					t1 = I_ih(i, 2 * (k - h) - 1); //e_i^{2(k-h)-1}, m_i^{2(k-h)-1}
					g1[t] += e[t1];
					w1[t] += m[t1];
					//first part of S_m (2k)->f2:Jm, S_e (2k)->u2:Je
					//∇^{2(k-1-h)} -> row number; column number d^(2h+1-i)
					t = Sr_ih(2 * h + 1 - i, 2 * (k - 1 - h));
					t1 = I_ih(i, 2 * (k - 1 - h)); //m_i^{2(k-1-h)}, e_i^{2(k-1-h)}
					f2[t] += m[t1];
					u2[t] += e[t1];
				}
				//second part of  S_m (2k)->g2:Je, S_e (2k)->w2:Jm
				//∇^{2(k-h)-1} -> row number; column number d^(2h-i)
				for (unsigned int i = 0; i <= 2 * h; i++)
				{
					t = Sr_ih(2 * h - i, 2 * (k - h) - 1);
					t1 = I_ih(i, 2 * (k - h) - 1); //e_i^{2(k-h)-1}, m_i^{2(k-h)-1}
					g2[t] += e[t1];
					w2[t] += m[t1];
				}
			}
			//add to the results
			for (t = 0; t < srcDim2; t++)
			{
				f[t] += f2[t] * sr[2 * k] + f1[t] * sr[2 * k + 1];
				g[t] += g2[t] * sr[2 * k] + g1[t] * sr[2 * k + 1];
				u[t] += u2[t] * sr[2 * k] + u1[t] * sr[2 * k + 1];
				w[t] += w2[t] * sr[2 * k] + w1[t] * sr[2 * k + 1];
			}
		}
		//convert to double
		for (t = 0; t < srcDim2; t++)
		{
			f0[t] = f[t].convert_to<double>();
			g0[t] = g[t].convert_to<double>();
			w0[t] = w[t].convert_to<double>();
			u0[t] = u[t].convert_to<double>();
		}
	}
	if(f != NULL) free(f);
	if(g != NULL) free(g);
	if(w != NULL) free(w);
	if(u != NULL) free(u);
	//
	if (f1 != NULL) free(f1);
	if (g1 != NULL) free(g1);
	if (w1 != NULL) free(w1);
	if (u1 != NULL) free(u1);
	//
	if (f2 != NULL) free(f2);
	if (g2 != NULL) free(g2);
	if (w2 != NULL) free(w2);
	if (u2 != NULL) free(u2);
	//
	if(sr != NULL) free(sr);
	return ret;
}

int FieldSourceTss::applySourceToFields(TimeTssBase *timeModule)
{
	if (timeModule->FieldType() == Field_type_3D)
	{
		return applySources(timeModule->GetTimeValue(), timeModule->GetTimeIndex(), timeModule->GetFieldE(), timeModule->GetFieldH());
	}
	else if (timeModule->FieldType() == Field_type_z_rotateSymmetry)
	{
		return applyToZrotateSymmetry(timeModule->GetTimeValue(), timeModule->GetTimeIndex(), timeModule->GetFieldZrotateSymmetryE(), timeModule->GetFieldZrotateSymmetryH());
	}
	else
	{
		return ERR_SOURCE_NOT_SUPPORT;
	}
}


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

// Copyright (c) 2007-2013 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_STRATEGIES_BUFFER_JOIN_MITER_HPP
#define BOOST_GEOMETRY_EXTENSIONS_STRATEGIES_BUFFER_JOIN_MITER_HPP

// Buffer strategies

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/strategies/tags.hpp>
#include <boost/geometry/strategies/side.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

#include <boost/geometry/extensions/strategies/buffer_side.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace buffer
{

// TODO: condider merging/baseclassing join strategies to avoid duplicate code

template
<
    typename PointIn,
    typename PointOut
>
struct join_miter
{
    typedef typename strategy::side::services::default_strategy<typename cs_tag<PointIn>::type>::type side;
    typedef typename coordinate_type<PointIn>::type coordinate_type;

    // Constructor compatible with other join strategies:
    inline join_miter(int = 0)
    {}

    template <typename RangeOut>
    inline void apply(PointIn const& ip, PointIn const& vertex,
                PointIn const& perp1, PointIn const& perp2,
                coordinate_type const& buffer_distance,
                RangeOut& range_out) const
    {
        coordinate_type zero = 0;
        int signum = buffer_distance > zero ? 1
                   : buffer_distance < zero ? -1
                   : 0;

        if (side::apply(perp1, ip, perp2) == signum)
        {
        }
        else
        {
            PointIn p = ip;

            // Normalize it and give it X*dist.
            coordinate_type dx = get<0>(ip) - get<0>(vertex);
            coordinate_type dy = get<1>(ip) - get<1>(vertex);

            coordinate_type length = sqrt(dx * dx + dy * dy);

            // TODO: make max-mitre-limit flexible
            coordinate_type ten = 10.0;
            coordinate_type zero_seven = 0.7;

            coordinate_type max = ten * geometry::math::abs(buffer_distance);

            if (length > max)
            {

                coordinate_type prop = zero_seven * buffer_distance;
                prop /= length;
                set<0>(p, get<0>(vertex) + dx * prop);
                set<1>(p, get<1>(vertex) + dy * prop);

#ifdef BOOST_GEOMETRY_DEBUG_BUFFER
                std::cout << length << std::endl;
#endif
            }

            range_out.push_back(perp1);
            range_out.push_back(p);
            range_out.push_back(perp2);
        }

    }
};

}} // namespace strategy::buffer

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_STRATEGIES_BUFFER_JOIN_MITER_HPP


#include <vector>
#include <iostream>
#include <string>
#include <nuSQuIDS/nuSQuIDS.h>
#include <nuSQuIDS/marray.h>
#include <nuSQuIDS/tools.h>
#include <boost/tokenizer.hpp>
#include "muSQuIDS.h"
#include "loadFlux.h"

using namespace nusquids;
using namespace musquids;

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

  squids::Const units;

  nusquids::marray<double,1> e_nodes({1});
  marray<double,2> muon_flux({1,2});
  marray<double,3> neutrino_state({1,2,3});

  std::string FileName;
  if (argc>1)
    FileName=std::string(argv[1]);
  else
    {
      std::cout<<"No file name supplied, using standard sample"<<std::endl;
      FileName="data/sample.txt";
    }

  LoadData(e_nodes,muon_flux,neutrino_state,FileName);

  muSQUIDS musq(e_nodes);

  std::shared_ptr<Vacuum> vacuum = std::make_shared<Vacuum>();
  std::shared_ptr<Vacuum::Track> vacuum_track = std::make_shared<Vacuum::Track>(500.*units.meter);
  musq.Set_Body(vacuum);
  musq.Set_Track(vacuum_track);

  musq.Set_initial_state(muon_flux,neutrino_state,flavor);
  musq.Set_ProgressBar(true);

  musq.EvolveState();

  for(size_t ie=0; ie<e_nodes.size(); ie++){
    std::cout << e_nodes[ie]/units.GeV << " " << musq.GetMuonFlux(ie,0) << " " << musq.GetMuonFlux(ie,1) << std::endl;
  }

  return 0;
}


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef SNPTEST_CASE_CONTROL_LOG_LIKELIHOOD_HPP
#define SNPTEST_CASE_CONTROL_LOG_LIKELIHOOD_HPP

#include <vector>
#include <boost/noncopyable.hpp>
#include "Eigen/Core"
#include "snptest/FinitelySupportedFunctionSet.hpp"

namespace snptest {
	namespace case_control {
		struct LogLikelihood: public boost::noncopyable
		{
		public:
			typedef Eigen::VectorXd Point ;
			typedef Eigen::VectorXd Vector ;
			typedef Eigen::RowVectorXd RowVector ;
			typedef Eigen::MatrixXd Matrix ;

			LogLikelihood() ;

			LogLikelihood& set_phenotypes( Vector const& phenotypes ) ;
			LogLikelihood& set_predictor_probs( Matrix const& genotypes, Vector const& levels ) ;
			LogLikelihood& set_covariates( Matrix const& covariates ) ;

			void evaluate_at( Point const& parameters ) ;
			double get_value_of_function() const ;
			Vector get_value_of_first_derivative() const ;
			Matrix get_value_of_second_derivative() const ;

		private:
			Vector m_phenotypes ;
			Matrix m_covariates ;
			Matrix m_genotype_call_probabilities ;
			Vector m_genotype_levels ;
			std::vector< int > m_exclusions ;

			Matrix m_design_matrix ;
			Matrix m_design_matrix_row_tensor_squares ;
			Matrix m_outcome_probabilities ;
			Matrix m_V ;

			double m_value_of_function ;
			Vector m_value_of_first_derivative ;
			Matrix m_value_of_second_derivative ;

		private:
			void add_exclusions( Vector const& v ) ;
			void add_exclusions( Matrix const& v ) ;
			Matrix calculate_design_matrix( Matrix const& covariates ) const ;
			void compute_tensor_squares_of_rows( Matrix& design_matrix, Matrix& result ) ;
			// Calculate the probability of outcome given the genotype, parameters, and covariates.
			Vector evaluate_mean_function( Vector const& linear_combinations, Vector const& outcomes ) const ;
			// Calculate matrix of probabilities of outcome per genotype, given the parameters.
			void calculate_outcome_probabilities( Vector const& parameters, Vector const& phenotypes, Matrix& design_matrix, Matrix* result ) const ;
			void compute_value_of_function( Matrix const& V ) ;
			void compute_value_of_first_derivative( Matrix& V ) ;
			void compute_value_of_second_derivative( Matrix const& V ) ;
		} ;
	}
}

#endif


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

#include <boost/property_tree/ptree.hpp>
#include <boost/random/poisson_distribution.hpp>

#include "clotho/mutation/mutation_rate_parameter.hpp"

namespace clotho {
namespace genetics {

template < class RNG, class IntType = size_t >
class mutation_allocator {
public:
    typedef RNG                                     random_engine_type;
    typedef IntType                                 int_type;
    typedef double                                  real_type;
    typedef mutation_rate_parameter< real_type >    rate_type;

    typedef boost::random::poisson_distribution< int_type, real_type > distribution_type;

    mutation_allocator( random_engine_type * rng, boost::property_tree::ptree & config ) :
        m_rand( rng )
        , m_mut_rate( config )
    {}

    int_type allocate( size_t N ) {
        typedef typename distribution_type::param_type param_type;

        m_dist.param( param_type( m_mut_rate.m_mu * N ) );

        return m_dist( *m_rand );
    }

    virtual ~mutation_allocator() {}

protected:
    random_engine_type  * m_rand;
    rate_type           m_mut_rate;
    distribution_type   m_dist;
};

}   // namespace genetics
}   // namespace clotho

#endif  // CLOTHO_MUTATION_ALLOCATOR_HPP_


#include "OccupancyGrid/gtsam_adaptor.hpp"
#include "OccupancyGrid/sumproduct.hpp"
#include "OccupancyGrid/cvmat_serialization.h"
#include "OccupancyGrid/visualiser.h"
#include "OccupancyGrid/loadData.h"
#include "OccupancyGrid/OccupancyGrid.h"
#include "OccupancyGrid/BPLaserFactor.hpp"
#include "OccupancyGrid/TwoAssumptionAlgorithm.h"
#include "OccupancyGrid/loadOccupancyGrid.h"
#include <boost/foreach.hpp>
#include <boost/typeof/typeof.hpp>

#include <opencv2/opencv.hpp>
#include <boost/program_options.hpp>
namespace po = boost::program_options;

using namespace std;
using namespace gtsam;
using namespace occgrid;

typedef G<OccupancyGrid, logdouble, size_t, gtsam::Index> OccupancyGridGraph;
namespace occgrid {
  namespace detail {
      // full template specialization for overriding the sumproduct update
      // mechanism
      template <> logdouble
      belief
      < OccupancyGridGraph,
        typename OccupancyGridGraph::MessageValues,
        typename OccupancyGridGraph::SampleSpaceMap,
        typename OccupancyGridGraph::FactorMap>
        (
         typename boost::graph_traits<OccupancyGridGraph>::edge_descriptor e,
         const OccupancyGridGraph  &fg,
         typename OccupancyGridGraph::MessageValues &msgs,
         typename OccupancyGridGraph::SampleSpaceMap &cdmap,
         typename OccupancyGridGraph::FactorMap& fmap,
         //const typename Assignment::value_type &xv
         const typename OccupancyGridGraph::SampleSpaceMap::value_type::first_type::value_type &xv,
         f2v_edge_tag
        ) {
          BOOST_AUTO_TPL(factor, source(e, fg));
          BOOST_AUTO_TPL(cell, target(e, fg));
          return fg.compute_belief(factor, cell, xv, msgs);
        }
  }
}

Visualiser global_vis_;

template <typename FactorGraph, typename MessageValues>
struct display_peridically_visitor
: public boost::base_visitor<display_peridically_visitor<FactorGraph, MessageValues> >
{
  typedef boost::on_examine_edge event_filter;
  display_peridically_visitor(const MessageValues& msgs) : msgs_(msgs), count_(0), clock_start_(clock()) {}
  void operator()(typename boost::graph_traits<FactorGraph>::edge_descriptor e,
      const FactorGraph& fg) {
    if ((count_ % 500000 == 0)) {
      std::cout << "Iterations:" << count_ << std::endl;
      display(fg);
    }
    ++count_;
  }
  void display(const FactorGraph& fg) {
    /// Very very slow
    typedef typename boost::graph_traits<FactorGraph>::vertex_descriptor vertex_descriptor;
    typedef typename boost::property_traits<MessageValues>::value_type value_type;
    std::vector<value_type> margs(num_variables(fg));
    gtsam::Assignment<gtsam::Index> best_assign;
    size_t count = 0;
    BOOST_FOREACH(vertex_descriptor u, variables(fg)) {
      value_type prod_0(marginal(fg, msgs_, u, 0));
      value_type prod_1(marginal(fg, msgs_, u, 1));
      value_type denom = prod_0 + prod_1;
      if (denom == value_type(0))
        margs[count] = value_type(0);
      else
        margs[count] = prod_1 / denom;
      best_assign[count] = (margs[count] < value_type(0.5)) ? 0 : 1;
      ++count;
    }
    fg.display(global_vis_, margs);
    double energy = fg.g_(best_assign);
    clock_t et = clock();
    std::cout << "<Energy>\t" << ((float)(et - clock_start_)) / CLOCKS_PER_SEC << "\t" << energy << std::endl;
  }
  private:
  const MessageValues& msgs_;
  int count_;
  clock_t clock_start_;
};

int main(int argc, const char *argv[])
{
  global_vis_.enable_show();
  cv::namedWindow("c", cv::WINDOW_NORMAL);
  // parse arguments ///////////////////////////////////////////
  // Declare the supported options.
  po::options_description desc("Run dual decomposition");
  desc.add_options()
    ("help", "produce help message")
    ("resolution", po::value<double>()->required(), "Size of square cell in the map")
    ("width", po::value<double>(), "Width")
    ("height", po::value<double>(), "Height")
    ("dir", po::value<std::string>()->default_value("Data/player_sim_with_reflectance"), "Data directory")
    ("clock", po::value<double>()->default_value(400), "Max clock")
;

  po::positional_options_description pos;
  pos.add("resolution", 1);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(desc).positional(pos).run(), vm);
  po::notify(vm);

  std::string directory = vm["dir"].as<std::string>();
  double max_clock = CLOCKS_PER_SEC * vm["clock"].as<double>();
  // end of parse arguments ////////////////////////////////////

  // Create the occupancy grid data structure
  OccupancyGrid occupancyGrid = loadOccupancyGrid(vm);
  global_vis_.init(occupancyGrid.height(), occupancyGrid.width());
  global_vis_.enable_show();

  typename OccupancyGridGraph::MessageTypes::base_type msg_values(logdouble(0.5));
  typename OccupancyGridGraph::MessageValues msgs(msg_values);
  OccupancyGridGraph ogg(occupancyGrid);
  using occgrid::get;
  OccupancyGridGraph::SampleSpaceMap cdmap = get(sample_space_t(), ogg);
  OccupancyGridGraph::FactorMap fmap = get(factor_map_t(), ogg);
  OccupancyGridGraph::IsFactorMap is_factor = get(is_factor_t(), ogg);

  display_peridically_visitor<OccupancyGridGraph,
    typename OccupancyGridGraph::MessageValues > display_vis(msgs);

  // initialize belief propagation by two assumption algorithm
  typedef typename boost::graph_traits<OccupancyGridGraph>::vertex_descriptor vertex_descriptor;
  typedef typename boost::property_traits<OccupancyGridGraph::MessageValues>::value_type msg_value_type;
  LaserFactor::Occupancy occupancy = occupancyGrid.emptyOccupancy();
  std::vector<double> energy(occupancyGrid.cellCount());
  two_assumption_algorithm(occupancyGrid, occupancy, energy);
  using boost::adjacency_iterator;
  BOOST_FOREACH(vertex_descriptor x, variables(ogg)) {
    BOOST_FOREACH(vertex_descriptor f, adjacent_vertices(x, ogg)) {
      gtsam::Index value =  occupancy[x];
      msgs[typename OccupancyGridGraph::MessageValues::key_type(x, f, value)] = msg_value_type(0.9);
      msgs[typename OccupancyGridGraph::MessageValues::key_type(x, f, 1 - value)] = msg_value_type(0.1);
    }
  }

  sumproduct_visitor<
    OccupancyGridGraph,
    OccupancyGridGraph::MessageValues,
    OccupancyGridGraph::SampleSpaceMap,
    OccupancyGridGraph::FactorMap,
    OccupancyGridGraph::IsFactorMap>
      spvis (msgs, cdmap, fmap, is_factor);

  BOOST_AUTO(vistor_list, std::make_pair(spvis, display_vis));
  // getting num_edges(ogg)
  BOOST_AUTO(n_iter, num_edges(ogg) * 1.0);
  std::cout << "Number of iterations:" << n_iter << std::endl;
  random_edge_traversal(ogg, vistor_list, n_iter, max_clock);
  display_vis.display(ogg);
  std::stringstream ss;
  ss << directory << "/run_belief_propagation.png";
  global_vis_.save(ss.str());
}


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

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>

//! [main]
#include <boost/hana/set.hpp>
using namespace boost::hana;

int main() {
    BOOST_HANA_CONSTEXPR_LAMBDA auto xs = set(1, 2.2, 'x');
    BOOST_HANA_CONSTEXPR_ASSERT(elem(xs, 'x'));
}
//! [main]


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

#ifndef BOOST_MPL11_DETAIL_STD_INDEX_SEQUENCE_HPP
#define BOOST_MPL11_DETAIL_STD_INDEX_SEQUENCE_HPP

#include <boost/mpl11/detail/std_size_t.hpp>

namespace boost { namespace mpl11 { namespace detail {
    /*!
     * @ingroup details
     *
     * A simple sequence of integers.
     */
    template <std_size_t ...I>
    struct std_index_sequence;

    namespace index_sequence_detail {
        template <typename L, typename R>
        struct concat;

        template <std_size_t ...I, std_size_t ...J>
        struct concat<std_index_sequence<I...>, std_index_sequence<J...>> {
            using type = std_index_sequence<I..., (sizeof...(I) + J)...>;
        };
    } // end namespace index_sequence_detail

    /*!
     * @ingroup details
     *
     * Create an `std_index_sequence` with indices from `0` to `n`.
     */
    template <std_size_t n>
    struct make_std_index_sequence {
        using type = typename index_sequence_detail::concat<
            typename make_std_index_sequence<n / 2>::type,
            typename make_std_index_sequence<n - n / 2>::type
        >::type;
    };

    template <>
    struct make_std_index_sequence<0> {
        using type = std_index_sequence<>;
    };

    template <>
    struct make_std_index_sequence<1> {
        using type = std_index_sequence<0>;
    };
}}} // end namespace boost::mpl11::detail

#endif // !BOOST_MPL11_DETAIL_STD_INDEX_SEQUENCE_HPP


#include <boost/test/unit_test.hpp>
#include "algorithms/search/number_of_closed_islands.hpp"

BOOST_AUTO_TEST_SUITE(TestNumberOfClosedIslands)

BOOST_AUTO_TEST_CASE(empty_matrix)
{
    const std::vector<std::vector<int>> matrix = {};
    BOOST_CHECK(0 == Algo::Search::NumberOfClosedIslands::calc_recursive(matrix));
}

BOOST_AUTO_TEST_CASE(matrix_one_elem_one_island)
{
    const std::vector<std::vector<int>> matrix = { { 0 } };
    BOOST_CHECK(0 == Algo::Search::NumberOfClosedIslands::calc_recursive(matrix));
}

BOOST_AUTO_TEST_CASE(matrix_one_elem_no_islands)
{
    const std::vector<std::vector<int>> matrix = { { 1 } };
    BOOST_CHECK(0 == Algo::Search::NumberOfClosedIslands::calc_recursive(matrix));
}

BOOST_AUTO_TEST_CASE(valid_matrix_no_islands)
{
    const std::vector<std::vector<int>> matrix =
        { {1, 1, 1},
          {1, 1, 1},
          {1, 1, 1} };

    BOOST_CHECK(0 == Algo::Search::NumberOfClosedIslands::calc_recursive(matrix));
}

BOOST_AUTO_TEST_CASE(valid_matrix_2x4_island_on_border)
{
    const std::vector<std::vector<int>> matrix =
        { {1, 1, 1, 0},
          {1, 1, 1, 0} };

    BOOST_CHECK(0 == Algo::Search::NumberOfClosedIslands::calc_recursive(matrix));
}

BOOST_AUTO_TEST_CASE(valid_matrix_4x4_have_islands)
{
    const std::vector<std::vector<int>> matrix =
        { {0, 0, 1, 1},
          {0, 0, 1, 1},
          {1, 1, 0, 1},
          {1, 1, 1, 0},};

    BOOST_CHECK(1 == Algo::Search::NumberOfClosedIslands::calc_recursive(matrix));
}

BOOST_AUTO_TEST_CASE(valid_matrix_8x5_have_two_islands)
{
    const std::vector<std::vector<int>> matrix =
        {{1,1,1,1,1,1,1,0},
         {1,0,0,0,0,1,1,0},
         {1,0,1,0,1,1,1,0},
         {1,0,0,0,0,1,0,1},
         {1,1,1,1,1,1,1,0}};

    BOOST_CHECK(2 == Algo::Search::NumberOfClosedIslands::calc_recursive(matrix));
}

BOOST_AUTO_TEST_CASE(valid_matrix_4x3_have_island)
{
    const std::vector<std::vector<int>> matrix =
        {{0,0,1,0,0},
         {0,1,0,1,0},
         {0,1,1,1,0}};

    BOOST_CHECK(1 == Algo::Search::NumberOfClosedIslands::calc_recursive(matrix));
}

BOOST_AUTO_TEST_CASE(valid_matrix_7x7_have_two_islands)
{
    const std::vector<std::vector<int>> matrix =
        {{1,1,1,1,1,1,1},
         {1,0,0,0,0,0,1},
         {1,0,1,1,1,0,1},
         {1,0,1,0,1,0,1},
         {1,0,1,1,1,0,1},
         {1,0,0,0,0,0,1},
         {1,1,1,1,1,1,1}};

    BOOST_CHECK(2 == Algo::Search::NumberOfClosedIslands::calc_recursive(matrix));
}

BOOST_AUTO_TEST_SUITE_END()