// Copyright (C) 2015, Pawel Tomulik <ptomulik@meil.pw.edu.pl>
//
// 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)
#define BOOST_TEST_MODULE test_txpl_vm_eval_binary_mod

#include <txpl/test_config.hpp>
#include <boost/test/unit_test.hpp>

#ifndef TXPL_TEST_SKIP_VM_EVAL_BINARY_MOD

#include <txpl/vm/eval_binary_op.hpp>
#include <txpl/vm/basic_types.hpp>
#include <txpl/vm/value.hpp>
#include <boost/variant/apply_visitor.hpp>
#include <boost/variant/get.hpp>

using namespace txpl::vm;
typedef basic_types<>::char_type   char_type;
typedef basic_types<>::int_type    int_type;
typedef basic_types<>::bool_type   bool_type;
typedef basic_types<>::real_type   real_type;
typedef basic_types<>::string_type string_type;
typedef basic_types<>::regex_type  regex_type;
typedef basic_types<>::blank_type  blank_type;
typedef array<value<> >            array_type;
typedef object<value<> >           object_type;

BOOST_AUTO_TEST_CASE(char__mod__char)
{
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = char_type{'\2'};
    const value<> v2 = char_type{'\4'};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (char_type{'\2'} % char_type{'\4'}));
  }
}
BOOST_AUTO_TEST_CASE(char__mod__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = char_type{'\2'};
    const value<> v2 = int_type{4};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (char_type{'\2'} % int_type{4}));
  }
}
BOOST_AUTO_TEST_CASE(char__mod__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = char_type{'\3'};
    const value<> v2 = bool_type{true};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (char_type{'\3'} % bool_type{true}));
  }
}
BOOST_AUTO_TEST_CASE(char__mod__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = char_type{'\2'};
    const value<> v2 = real_type{2.1};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(char__mod__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = char_type{'a'};
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(char__mod__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = char_type{'a'};
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(char__mod__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = char_type{'a'};
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(char__mod__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = char_type{'a'};
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(int__mod__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = int_type{2};
    const value<> v2 = char_type{'\3'};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (int_type{2} % char_type{'\3'}));
  }
}
BOOST_AUTO_TEST_CASE(int__mod__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = int_type{2};
    const value<> v2 = int_type{3};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (int_type{2} % int_type{3}));
  }
}
BOOST_AUTO_TEST_CASE(int__mod__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = int_type{3};
    const value<> v2 = bool_type{true};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (int_type{3} % bool_type{true}));
  }
}
BOOST_AUTO_TEST_CASE(int__mod__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = int_type{12};
    const value<> v2 = real_type{3.21};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(int__mod__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(int__mod__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(int__mod__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(int__mod__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(bool__mod__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = bool_type{true};
    const value<> v2 = char_type{'\3'};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (bool_type{true} % char_type{'\3'}));
  }
}
BOOST_AUTO_TEST_CASE(bool__mod__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = bool_type{true};
    const value<> v2 = int_type{3};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (bool_type{true} % int_type{3}));
  }
}
BOOST_AUTO_TEST_CASE(bool__mod__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = bool_type{true};
    const value<> v2 = bool_type{true};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (bool_type{true} % bool_type{true}));
  }
}
BOOST_AUTO_TEST_CASE(bool__mod__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = bool_type{true};
    const value<> v2 = real_type{2.12};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(bool__mod__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(bool__mod__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(bool__mod__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(bool__mod__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(real__mod__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = real_type{1.2};
    const value<> v2 = char_type{'\3'};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__mod__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = real_type{1.2};
    const value<> v2 = int_type{3};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__mod__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = real_type{1.2};
    const value<> v2 = bool_type{true};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__mod__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = real_type{1.2};
    const value<> v2 = real_type{3.4};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__mod__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = real_type{0.0};
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__mod__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = real_type{0.0};
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__mod__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = real_type{0.0};
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__mod__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = real_type{0.0};
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(string__mod__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = char_type{'a'};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__mod__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = int_type{0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__mod__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = bool_type{false};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__mod__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = real_type{0.0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__mod__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = string_type("asd");
    const value<> v2 = string_type{"qwer"};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__mod__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__mod__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__mod__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(regex__mod__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = char_type{'\0'};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__mod__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = int_type{0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__mod__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = bool_type{false};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__mod__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = real_type{0.0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__mod__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__mod__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__mod__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__mod__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(array__mod__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = char_type{'\0'};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__mod__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = int_type{0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__mod__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = bool_type{false};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__mod__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = real_type{0.0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__mod__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__mod__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__mod__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<array_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__mod__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(object__mod__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = char_type{'\0'};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__mod__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = int_type{0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__mod__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = bool_type{false};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__mod__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = real_type{0.0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__mod__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__mod__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__mod__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__mod__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::mod_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

#else
BOOST_AUTO_TEST_CASE(dummy)
{
  BOOST_CHECK(true);
}
#endif


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

#include <fstream>
#include <boost/lexical_cast.hpp>
#include <boost/regex.hpp>

#include "luxrays/core/color/spds/irregular.h"
#include "slg/textures/fresnel/fresnelluxpop.h"
#include "slg/textures/fresnel/fresnelconst.h"

using namespace std;
using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
// Fresnel LuxPop texture
//------------------------------------------------------------------------------

FresnelTexture *slg::AllocFresnelLuxPopTex(const Properties &props, const string &propName) {
	const string fileName = props.Get(Property(propName + ".file")("luxpop.nk")).Get<string>();

	ifstream fs;
	fs.open(fileName.c_str());
	vector<float> wl;
	vector<float> n;
	vector<float> k;

	string line;

	boost::smatch m;

	// Read nk data
	boost::regex sample_expr("(\\d*\\.?\\d+(?:[eE]-?\\d+)?)\\s+(\\d*\\.?\\d+(?:[eE]-?\\d+)?)\\s+(\\d*\\.?\\d+(?:[eE]-?\\d+)?)");

	// We want lambda to go from low to high
	while (!getline(fs, line).eof()) {
		if (fs.bad())
			break;

		// Skip comments
		if (!line.empty() && line[0] == ';')
			continue;

		if (!boost::regex_search(line, m, sample_expr))
			throw runtime_error("Unparseable luxpop data in: " + fileName);

		// Wavelength in data file is in Angstroms, we want nm
		wl.push_back(boost::lexical_cast<float>(m[1]) * .1f);
		n.push_back(boost::lexical_cast<float>(m[2]));
		k.push_back(boost::lexical_cast<float>(m[3]));
	}

	if (!fs.eof())
		throw runtime_error("Junk in luxpop file: " + fileName);

	IrregularSPD N(&wl[0], &n[0], wl.size());
	IrregularSPD K(&wl[0], &k[0], wl.size());

	ColorSystem colorSpace;
	const RGBColor Nrgb = colorSpace.ToRGBConstrained(N.ToNormalizedXYZ());
	const RGBColor Krgb = colorSpace.ToRGBConstrained(K.ToNormalizedXYZ());

	return new FresnelConstTexture(Nrgb, Krgb);
}


/*
 * ----------------- BEGIN LICENSE BLOCK ---------------------------------
 *
 * Copyright (C) 2019-2021 Intel Corporation
 *
 * SPDX-License-Identifier: MIT
 *
 * ----------------- END LICENSE BLOCK -----------------------------------
 */

#include "opendrive/geometry/CenterLine.hpp"
#include <boost/array.hpp>
#include <boost/math/tools/rational.hpp>
#include <cmath>
#include <iostream>
#include <spdlog/spdlog.h>
#include "opendrive/geometry/GeometryGenerator.hpp"
#include "opendrive/geometry/LaneUtils.hpp"

namespace opendrive {
namespace geometry {

double laneHeight(ElevationProfileSet const &elevationSet, double s)
{
  return evalPoly3(elevationSet, s);
}

geometry::DirectedPoint CenterLine::eval(double s, bool applyLateralOffset) const
{
  if (s < 0.)
  {
    // as the curve is only defined betwen the range [0 , length], we return the line evaluated at s = 0
    return eval(0.);
  }

  for (auto it = geometry.rbegin(); it != geometry.rend(); it++)
  {
    if (s >= (*it)->GetStartOffset())
    {
      auto directedPoint = (*it)->PosFromDist(s - (*it)->GetStartOffset());

      // This is parametrized as the landmarks positions do not take into account the lateral offset
      if (applyLateralOffset)
      {
        directedPoint.ApplyLateralOffset(calculateOffset(s));
      }

      directedPoint.location.z = laneHeight(elevation, s);
      return directedPoint;
    }
  }

  auto &lastGeometry = geometry.back();
  auto directed_point = lastGeometry->PosFromDist(s - lastGeometry->GetStartOffset());
  directed_point.location.z = laneHeight(elevation, s);
  return directed_point;
}

std::list<double> CenterLine::samplingPoints() const
{
  std::list<double> points;

  points.push_back(length);

  for (auto it = geometry.rbegin(); it != geometry.rend(); it++)
  {
    if ((*it)->GetType() == ::opendrive::GeometryType::ARC)
    {
      auto arc = static_cast<::opendrive::geometry::GeometryArc *>((*it).get());
      double theta = fabs(arc->GetCurvature()) * (*it)->GetLength();

      if (theta < 1e-2)
      {
        points.insert(points.begin(), (*it)->GetStartOffset() + 0.5 * (*it)->GetLength());
      }
      else
      {
        double c = fabs(arc->GetCurvature());
        double dsmax = 2.0 / c * acos(1.0 - c * Geometry::cMaxSamplingError);
        int segments = static_cast<int>((*it)->GetLength() / dsmax);
        for (auto k = segments - 1; k > 0; --k)
        {
          double r = static_cast<double>(k) / static_cast<double>(segments);
          points.insert(points.begin(), (*it)->GetStartOffset() + r * (*it)->GetLength());
        }
      }
    }
    else
    {
      // only start and end point for now
    }

    points.insert(points.begin(), (*it)->GetStartOffset());
  }

  auto const maxErrorSquared = Geometry::cMaxSamplingError * Geometry::cMaxSamplingError;
  // now refine sampling points according to actual points influenced by lateralOffset and elevation
  auto iter = points.begin();
  while (iter != points.end())
  {
    auto nextIter = iter;
    nextIter++;
    if (nextIter == points.end())
    {
      break;
    }
    auto const startPosition = *iter;
    auto const endPosition = *nextIter;
    if (endPosition - startPosition < Geometry::cMaxSamplingError)
    {
      // sampling below the maxError longitudinal distance makes no sense
      ++iter;
      continue;
    }
    auto const centerPosition = 0.5 * startPosition + 0.5 * endPosition;

    auto const start = eval(startPosition).location;
    auto const end = eval(endPosition).location;
    auto const centerLinearInterpolated = 0.5 * start + 0.5 * end;
    auto const centerActual = eval(centerPosition).location;
    auto errorSquared = (centerActual - centerLinearInterpolated).normSquared();
    if (errorSquared > maxErrorSquared)
    {
      points.insert(nextIter, centerPosition);
    }
    else
    {
      iter++;
    }
  }

  return points;
}

double CenterLine::calculateOffset(double s) const
{
  return evalPoly3(laneOffsetSet, s);
}

bool generateCenterLine(RoadInformation &roadInfo, CenterLine &centerLine)
{
  bool ok = true;

  centerLine.geometry.clear();
  centerLine.length = roadInfo.attributes.length;
  centerLine.laneOffsetSet = roadInfo.lanes.lane_offset;
  centerLine.elevation = roadInfo.road_profiles.elevation_profile;

  // Add geometry information
  for (auto &geometry_attribute : roadInfo.geometry_attributes)
  {
    Point start(
      geometry_attribute->start_position_x, geometry_attribute->start_position_y, geometry_attribute->start_position_z);

    try
    {
      switch (geometry_attribute->type)
      {
        case ::opendrive::GeometryType::ARC:
        {
          auto arc = static_cast<GeometryAttributesArc *>(geometry_attribute.get());
          centerLine.geometry.emplace_back(std::make_unique<geometry::GeometryArc>(
            arc->start_position, arc->length, arc->heading, start, arc->curvature));
          break;
        }
        case ::opendrive::GeometryType::LINE:
        {
          auto line = static_cast<GeometryAttributesLine *>(geometry_attribute.get());
          centerLine.geometry.emplace_back(
            std::make_unique<geometry::GeometryLine>(line->start_position, line->length, line->heading, start));
          break;
        }
        break;
        case ::opendrive::GeometryType::SPIRAL:
        {
          // currently not yet supported
          spdlog::error("generateCenterLine() spirals are currently not supported yet");
          ok = false;
          break;
        }
        break;
        case ::opendrive::GeometryType::POLY3:
        {
          auto poly3 = static_cast<GeometryAttributesPoly3 *>(geometry_attribute.get());
          centerLine.geometry.emplace_back(std::make_unique<geometry::GeometryPoly3>(
            poly3->start_position, poly3->length, poly3->heading, start, poly3->a, poly3->b, poly3->c, poly3->d));
          break;
        }
        break;
        case ::opendrive::GeometryType::PARAMPOLY3:
        {
          auto paramPoly3 = static_cast<GeometryAttributesParamPoly3 *>(geometry_attribute.get());
          centerLine.geometry.emplace_back(
            std::make_unique<geometry::GeometryParamPoly3>(paramPoly3->start_position,
                                                           paramPoly3->length,
                                                           paramPoly3->heading,
                                                           start,
                                                           paramPoly3->aU,
                                                           paramPoly3->bU,
                                                           paramPoly3->cU,
                                                           paramPoly3->dU,
                                                           paramPoly3->aV,
                                                           paramPoly3->bV,
                                                           paramPoly3->cV,
                                                           paramPoly3->dV,
                                                           paramPoly3->p_range == "normalized"));
          break;
        }
        break;
        default:
          break;
      }
    }
    catch (...)
    {
      spdlog::error("generateCenterLine() Invalid geometry definition");
      ok = false;
    }
  }

  return ok;
}
} // namespace geometry
} // namespace opendrive


/*=========================================================================

Library:   TubeTK

Copyright 2010 Kitware Inc. 28 Corporate Drive,
Clifton Park, NY, 12065, USA.

All rights reserved.

Licensed under the Apache License, Version 2.0 ( the "License" );
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

=========================================================================*/

#include "tubeMessage.h"

#include <itkDanielssonDistanceMapImageFilter.h>
#include <itkImageFileReader.h>
#include <itkImageFileWriter.h>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>

#include "ComputeRegionSignaturesCLP.h"

// STL includes

typedef enum
{
  SIGNATURE_CVT_CTR = 0,
  SIGNATURE_CVT_MIN = 1
} SignatureType;

template< class TPixel, unsigned int VImageDimension >
int DoIt( int argc, char * argv[] );

#include "../CLI/tubeCLIHelperFunctions.h"

int main( int argc, char **argv )
{
  PARSE_ARGS;
  return tube::ParseArgsAndCallDoIt( argSegImageFileName, argc, argv );
}

/** Check for equality of VNL vectors.
 *
 * \param v 1st VNL vector
 * \param g 2nd VNL vector
 * \param tol Tolerance value
 * \return true in case of equality, false else
 *
 */
template< typename T >
bool CheckVNLVectorEquality( const vnl_vector< T > &v,
  const vnl_vector< T > &g, double tol = 1.0e-6 )
{
  if( v.size() != g.size() )
    {
    return false;
    }
  for( unsigned i=0; i<v.size(); ++i )
    {
    if( std::fabs( g.get( i ) - v.get( i ) ) > tol )
      {
      return false;
      }
    }
  return true;
}

/** Check for equality of VNL matrices.
 *
 * \param V 1st VNL matrix
 * \param G 2nd VNL matrix
 * \param tol Tolerance value
 * \return true in case of equality, false otherwise
 *
 */
template< typename T >
bool CheckVNLMatrixEquality( const vnl_matrix< T > &V,
  const vnl_matrix< T > &G, double tol = 1.0e-6 )
{
  if( V.rows() != G.rows() || V.cols() != G.cols() )
    {
    return false;
    }
  for( unsigned int r=0; r<V.rows(); ++r )
    {
    for( unsigned int c=0; c<V.cols(); ++c )
      {
      if( std::fabs( V.get( r, c ) - G.get( r, c ) ) > tol )
        {
        return false;
        }
      }
    }
  return true;
}

/** Check compatibility of imgages in terms of size, origin, spacing, direction.
 *
 *  \param imageA 1st input image of type ImageType
 *  \param imageB 2nd input image of type ImageType
 *  \returns true if image have equal spacing and size, false otherwise
 *
 */
template< typename ImageT >
bool CheckCompatibility( typename ImageT::Pointer imageA,
  typename ImageT::Pointer imageB )
{
  // Spacing tolerance is imageA's 1st coord. spacing * 1e-6
  const double spacingTol = imageA->GetSpacing()[0] * 1.0e-6;

  if( !CheckVNLVectorEquality( imageA->GetOrigin().GetVnlVector(),
    imageB->GetOrigin().GetVnlVector() ) )
    {
    tube::ErrorMessage( "Origin mismatch between input images" );
    return false;
    }
  if( !CheckVNLVectorEquality( imageA->GetSpacing().GetVnlVector(),
    imageB->GetSpacing().GetVnlVector(), spacingTol ) )
    {
    tube::ErrorMessage( "Spacing mismatch between input images" );
    return false;
    }
  if( !( imageA->GetLargestPossibleRegion().GetSize() ==
         imageB->GetLargestPossibleRegion().GetSize() ) )
    {
    tube::ErrorMessage( "Size mismatch between input images" );
    return false;
    }
  if( !CheckVNLMatrixEquality(
    imageA->GetDirection().GetVnlMatrix().as_ref(),
    imageB->GetDirection().GetVnlMatrix().as_ref() ) )
    {
    tube::ErrorMessage( "Directions mismatch between input images" );
    return false;
    }
  return true;
}

/** Create an empty image.
 *
 *  \param outImage Image that is about to be created ( has to exist )
 *  \param targetSize Desired size of the image
 *
 */
template< class ImageT >
void CreateEmptyImage( typename ImageT::Pointer & outImage,
  typename ImageT::SizeType targetSize )
{
  typename ImageT::IndexType start;
  start.Fill( 0 );
  typename ImageT::SizeType outImageSize = targetSize;

  typename ImageT::RegionType region( start, outImageSize );
  outImage->SetRegions( region );
  outImage->Allocate();
  outImage->FillBuffer( 0 );
}

/** Check if image contains only discrete values.
 *
 * \param fileName Image file name
 * \return 'true' if image, given by 'fileName' has a discrete-value type
 *
 */
bool IsDiscrete( const std::string & fileName )
{
  typedef itk::ImageIOBase::IOComponentType ScalarPixelType;

  itk::ImageIOBase::Pointer imageIO = itk::ImageIOFactory::CreateImageIO(
    fileName.c_str(), itk::ImageIOFactory::ReadMode );

  imageIO->SetFileName( fileName.c_str() );
  imageIO->ReadImageInformation();
  const ScalarPixelType pixelType = imageIO->GetComponentType();

  if( pixelType == itk::ImageIOBase::FLOAT ||
      pixelType == itk::ImageIOBase::DOUBLE )
    {
    return false;
    }
  return true;
}

/** Map all S elements in a set to {0,...S-1}.
 *
 *  \param in STL set of values to be renumbered
 *  \param map STL map that is filled up
 */
template< typename TPixel >
void CreateRenumberingMap( const std::set< TPixel > & in,
  std::map< TPixel, unsigned int > & map )
{
  unsigned int cnt = 0;
  typename std::set< TPixel >::const_iterator it = in.begin();
  while( it != in.end() )
    {
    map[( *it )] = cnt++;
    ++it;
    }
}

template< class TPixel, unsigned int VImageDimension >
int DoIt( int argc, char **argv )
{
  PARSE_ARGS;

  // Commonly-used types
  typedef TPixel                                          InputPixelType;
  typedef itk::Image< InputPixelType, VImageDimension >   InputImageType;

  typedef InputPixelType                                  OutputPixelType;
  typedef itk::Image< OutputPixelType, VImageDimension >  OutputImageType;

  typedef itk::ImageFileReader< InputImageType >          InputReaderType;
  typedef itk::ImageFileWriter< OutputImageType >         OutputWriterType;

  typedef itk::ImageRegionIteratorWithIndex< InputImageType >
    ImageIteratorType;

  typedef std::vector< typename InputImageType::IndexType >
    InputIndexVectorType;

  typedef std::map< TPixel, InputIndexVectorType >    IdToIndexVectorType;

  // Read segmenation image and image of Central-Voronoi-Tesellation ( CVT )
  // cells
  typename InputImageType::Pointer  segImage, cvtImage;
  typename InputReaderType::Pointer segImageReader = InputReaderType::New();
  typename InputReaderType::Pointer cvtImageReader = InputReaderType::New();

  segImageReader->SetFileName( argSegImageFileName.c_str() );
  cvtImageReader->SetFileName( argCVTImageFileName.c_str() );

  try
    {
    segImageReader->Update();
    cvtImageReader->Update();
    }
  catch( itk::ExceptionObject & ex )
    {
    tube::ErrorMessage( ex.what() );
    return EXIT_FAILURE;
    }
  segImage = segImageReader->GetOutput();
  cvtImage = cvtImageReader->GetOutput();

  if( !IsDiscrete( argSegImageFileName ) ||
      !IsDiscrete( argCVTImageFileName ) )
    {
    tube::ErrorMessage( "Input images contain non-discrete values" );
    return EXIT_FAILURE;
    }

  // Read the CVT center file
  typename std::vector< typename InputImageType::IndexType > cvtCentersIdx;

  std::ifstream cvtCenterFile( argCVTCenterFileName.c_str() );
  if( cvtCenterFile.is_open() )
    {
    double c0 = 0.0;
    double c1 = 0.0;
    double c2 = 0.0;
    int nCenters;

    cvtCenterFile >> nCenters;
    cvtCentersIdx.resize( nCenters );

    tube::FmtInfoMessage( "Reading %d CVT centers ...",
                          nCenters );

    unsigned int centerCounter = 0;
    while( cvtCenterFile >> c0 >> c1 >> c2 )
      {
      itkAssertOrThrowMacro( VImageDimension == 3,
                             "No support for 2-D CVT center file" );

      itk::Point< float, VImageDimension > p;
      p[0] = c0;
      p[1] = c1;
      p[2] = c2;

      typename InputImageType::IndexType targetIndex;
      bool isInside = cvtImage->TransformPhysicalPointToIndex( p,
        targetIndex );
      if( !isInside )
        {
        tube::ErrorMessage( "CVT cell center outside image" );
        return EXIT_FAILURE;
        }

      cvtCentersIdx[centerCounter] = targetIndex;
      ++centerCounter;
      }
    cvtCenterFile.close();
    itkAssertOrThrowMacro( centerCounter = nCenters,
                           "#CVT centers mismatch" );
    }
  else
    {
    tube::FmtErrorMessage( "Could not read %s",
                           argCVTCenterFileName.c_str() );
    return EXIT_FAILURE;
    }

  // Ensure compatibility of CVT and segmenation image
  if( !CheckCompatibility< InputImageType >( segImage, cvtImage ) )
    {
    tube::FmtErrorMessage( "%s and %s are incompatible",
      argSegImageFileName.c_str(), argCVTImageFileName.c_str() );
    return EXIT_FAILURE;
    }

  // Determine the number of unqique segmentation regions
  typename itk::ImageRegionIteratorWithIndex< InputImageType > segImageIt(
    segImage, segImage->GetLargestPossibleRegion() );

  // Holds the IDs of the segmentation regions
  std::set< TPixel > segmentationRegions;

  segImageIt.GoToBegin();
  while( !segImageIt.IsAtEnd() )
    {
    segmentationRegions.insert( segImageIt.Get() );
    ++segImageIt;
    }

  tube::FmtInfoMessage( "Found %d distinct segmentation IDs",
    segmentationRegions.size() );

  // Map segmentation IDs to the ( artificial ) segmentation IDs {0, ..., S-1}
  std::map< TPixel, unsigned int > fwdSegMapper;
  std::map< unsigned int, TPixel > invSegMapper;

  unsigned int segmentationRegionCounter = 0;
  typename std::set< TPixel>::const_iterator regionIt =
    segmentationRegions.begin();

  while( regionIt != segmentationRegions.end() )
    {
    fwdSegMapper[ *regionIt ] = segmentationRegionCounter;
    invSegMapper[ segmentationRegionCounter++ ] = *regionIt;
    ++regionIt;
    }

  itkAssertOrThrowMacro( segmentationRegions.size() ==
    segmentationRegionCounter,
    "Size mismatch after seg. region renumbering" );

  // Create a map of ( artificial ) segmentation IDs -> Vector of voxel
  // indices
  IdToIndexVectorType segToIndexVector;

  segImageIt.GoToBegin();
  while( !segImageIt.IsAtEnd() )
    {
    const typename InputImageType::IndexType &index =
      segImageIt.GetIndex();
    segToIndexVector[ fwdSegMapper[ segImageIt.Get() ] ].push_back( index );
    ++segImageIt;
    }

  itkAssertOrThrowMacro( segToIndexVector.size() ==
    segmentationRegions.size(),
    "Size mismatch after region -> index vector mapping" );

  // Map CVT cell IDs to ( artificial ) CVT cell IDs {0, ..., C-1}
  ImageIteratorType cvtImageIt( cvtImage,
    cvtImage->GetLargestPossibleRegion() );

  std::set< TPixel > cvtCellSet;
  cvtImageIt.GoToBegin();
  while( !cvtImageIt.IsAtEnd() )
    {
    cvtCellSet.insert( cvtImageIt.Get() );
    ++cvtImageIt;
    }

  std::map< TPixel, unsigned int > fwdCVTMapper;
  CreateRenumberingMap< TPixel >( cvtCellSet, fwdCVTMapper );

  itkAssertOrThrowMacro( fwdCVTMapper.size() == cvtCellSet.size(),
    "Size mismatch after renumbering CVT cells" );

  // Create a map of ( artificial ) CVT cell IDs -> Vector of voxel indices
  IdToIndexVectorType cvtToIndexVector;

  cvtImageIt.GoToBegin();
  while( !cvtImageIt.IsAtEnd() )
    {
    TPixel id = cvtImageIt.Get();
    cvtToIndexVector[ fwdCVTMapper[id] ].push_back( cvtImageIt.GetIndex() );
    ++cvtImageIt;
    }

  TPixel largestKey = cvtToIndexVector.rbegin()->first;
  itkAssertOrThrowMacro( ( largestKey + 1 ) ==
    static_cast< TPixel >( cvtToIndexVector.size() ),
    "Size mismatch after CVT to index vector mapping" );

  // Build index vector for exclusion regions
  boost::dynamic_bitset<> excludeRegions( segmentationRegions.size() );
  for( unsigned int e = 0; e < argExcludeRegions.size(); ++e )
    {
    unsigned int mappedRegionID = fwdCVTMapper[ argExcludeRegions[e] ];
    itkAssertOrThrowMacro( mappedRegionID < segmentationRegions.size(),
      "Excluded region ID out of range" );

    excludeRegions[ mappedRegionID ] = 1;
    }

  // Create numberOfSegmentedRegions-dimensional distance signature vectors
  unsigned int numberOfCVTCells = cvtToIndexVector.size();
  unsigned int numberOfSegmentedRegions = segmentationRegions.size();

  vnl_matrix< TPixel > signatureMatrix ( numberOfCVTCells,
    numberOfSegmentedRegions );

  typename IdToIndexVectorType::iterator mapIt = segToIndexVector.begin();
  while( mapIt != segToIndexVector.end() )
    {
    TPixel id = ( *mapIt ).first;

    if( excludeRegions[id] )
      {
      tube::FmtInfoMessage( "Exclude segmentation region %d ( Orig: %d )",
        id, invSegMapper[id] );

      ++mapIt;
      continue;
      }

    std::stringstream mapFileName, prtFileName;
    mapFileName << boost::format( "%04i-map.mha" ) % invSegMapper[id];
    prtFileName << boost::format( "%04i-prt.mha" ) % invSegMapper[id];

    boost::filesystem::path dir( argOutputDirectory.c_str() );
    boost::filesystem::path mapFilePath ( mapFileName.str().c_str() );
    boost::filesystem::path prtFilePath ( prtFileName.str().c_str() );
    boost::filesystem::path fullMapFilePath = dir / mapFilePath;
    boost::filesystem::path fullPrtFilePath = dir / prtFilePath;

    typename OutputImageType::Pointer prtImage, mapImage;
    if( boost::filesystem::exists( fullPrtFilePath.string().c_str() ) &&
        boost::filesystem::exists( fullMapFilePath.string().c_str() ) )
      // Try to load existing data ...
      {
      tube::FmtInfoMessage( "Found %s and %s - Loading ...",
        fullPrtFilePath.string().c_str(),
        fullMapFilePath.string().c_str() );

      typename InputReaderType::Pointer singlePrtReader =
        InputReaderType::New();
      typename InputReaderType::Pointer singleMapReader =
        InputReaderType::New();
      singlePrtReader->SetFileName( fullPrtFilePath.string().c_str() );
      singleMapReader->SetFileName( fullMapFilePath.string().c_str() );
      try
        {
        singlePrtReader->Update();
        singleMapReader->Update();
        }
      catch( itk::ExceptionObject & ex )
        {
        tube::ErrorMessage( ex.what() );
        return EXIT_FAILURE;
        }
      mapImage = singleMapReader->GetOutput();
      prtImage = singlePrtReader->GetOutput();
      }
    else
      // Otherwise, recompute distance maps ...
      {
      tube::FmtInfoMessage(
        "Recomputing distance map for ( original ) seg. %d",
        invSegMapper[id] );

      typename OutputImageType::Pointer outImage = OutputImageType::New();

      CreateEmptyImage< OutputImageType >( outImage,
        segImage->GetLargestPossibleRegion().GetSize() );

      itkAssertOrThrowMacro( segImage->GetLargestPossibleRegion().GetSize()
        == outImage->GetLargestPossibleRegion().GetSize(),
        "Newly created output image differs in size" );

      const InputIndexVectorType &indices = ( *mapIt ).second;
      for( unsigned int v=0; v<indices.size(); ++v )
        {
        outImage->SetPixel( indices[v], 1 );
        }

      typedef typename itk::DanielssonDistanceMapImageFilter<
        InputImageType,
        OutputImageType > DistanceMapType;

      typename DistanceMapType::Pointer distanceMap =
        DistanceMapType::New();
      distanceMap->SetInputIsBinary( true );
      distanceMap->SetUseImageSpacing( true );
      distanceMap->SetInput( outImage );
      distanceMap->Update();

      mapImage = distanceMap->GetDistanceMap();
      prtImage = outImage;

      typename OutputWriterType::Pointer prtWriter =
        OutputWriterType::New();
      prtWriter->SetFileName( fullPrtFilePath.string().c_str() );
      prtWriter->SetInput( prtImage );
      prtWriter->SetUseCompression( true );

      typename OutputWriterType::Pointer mapWriter =
        OutputWriterType::New();
      mapWriter->SetFileName( fullMapFilePath.string().c_str() );
      mapWriter->SetInput( mapImage );
      mapWriter->SetUseCompression( true );

      try
        {
        mapWriter->Update();
        prtWriter->Update();
        }
      catch( itk::ExceptionObject & ex )
        {
        tube::ErrorMessage( ex.what() );
        return EXIT_FAILURE;
        }
      }

      switch( argSignatureSelector )
      {
      case SIGNATURE_CVT_CTR:
        {
        for( unsigned int c=0; c<numberOfCVTCells; ++c )
          {
          // Distance to c-th ( i.e., c-th CVT cell ) center
          TPixel dist = mapImage->GetPixel( cvtCentersIdx[c] );
          signatureMatrix.put( c, id, dist );
          }
        break;
        }
      case SIGNATURE_CVT_MIN:
        {
        namespace acc = boost::accumulators;
        typedef acc::stats< acc::tag::min >               StatsType;
        typedef acc::accumulator_set< TPixel, StatsType > AccumulatorType;

        typename IdToIndexVectorType::iterator cvtToIndexVectorIt =
          cvtToIndexVector.begin();

        while( cvtToIndexVectorIt != cvtToIndexVector.end() )
          {
          AccumulatorType cellAcc;

          // Artificial CVT cell ID
          TPixel cvtCell = ( *cvtToIndexVectorIt ).first;

          const std::vector< typename InputImageType::IndexType > &
            voxelIndices = ( *cvtToIndexVectorIt ).second;

          // Add distances to accumulator to keep track of the minimum
          for( unsigned int i=0; i<voxelIndices.size(); ++i )
            {
            TPixel dist = mapImage->GetPixel( voxelIndices[i] );
            cellAcc( dist );
            }

          // Get the minimum distance to the current segment
          TPixel minDist = acc::min( cellAcc );

          // We need to map the original CVT cell ID to the artificial one
          signatureMatrix.put( cvtCell, id, minDist );
          ++cvtToIndexVectorIt;
          }
        break;
        }
      }
      ++mapIt;
    }

  std::ofstream outSignatureFile( argDistanceSignatureFileName.c_str() );
  if( outSignatureFile.is_open() )
    {
    for( unsigned int r=0; r<signatureMatrix.rows(); ++r )
      {
      for( unsigned int c=0; c<signatureMatrix.columns()-1; ++c )
        {
        outSignatureFile << signatureMatrix.get( r, c ) << ",";
        }
      outSignatureFile << signatureMatrix.get( r,
        signatureMatrix.columns() - 1 ) << std::endl;
      }
    }
  else
    {
    tube::FmtErrorMessage( "Could not open %s for writing",
      argDistanceSignatureFileName.c_str() );
    return EXIT_FAILURE;
    }
  outSignatureFile.close();
  return EXIT_SUCCESS;
}


#include "Debug.h"
#include "SolverEigen.h"
#include "SolverTime.h"
#include "ComputerTime.h"
#include "util.h"

//#include <Eigen/SuperLUSupport>
#include <Eigen/SparseExtra>
#include <Eigen/IterativeSolvers>

extern SolverTime      solverTime;
extern ComputerTime    computerTime;

using namespace std;
using namespace Eigen;

void SolverEigen::computeConditionNumber()
{

  return;
}

void SolverEigen::setupMatricesAndVectors()
{
    return;
}

void SolverEigen::solverSchurCG()
{
  return;
}

void SolverEigen::solverSchurBiCGSTAB()
{
  return;
}

void SolverEigen::solverSchurGMRES()
{
  return;
}

void SolverEigen::solverUzawaType1()
{
  return;
}

//
void SolverEigen::solverUzawaType2()
{
  return;
}
//

int  SolverEigen::myBiCGSTAB()
{
  return 1;
}

void  SolverEigen::updatePreconditioner()
{
  //////////////////////
  //
  //  preconditioner
  //
  //////////////////////

  //precond1.setDroptol(1.0e-3);
  //precond1.setFillfactor(2);
  //precond1.compute(mtx);

    time_t tstart, tend;
    tstart = time(0);

  //precond.free();

  //precond.setDroptol(1.0e-3);
  //precond.setFillfactor(3);

  //precond.compute(mtx);

  tend = time(0);
  printf("ILUT preconditioner took %8.4f second(s) \n ", difftime(tend, tstart) );

  //mtx.makeCompressed();

  //DiagonalPreconditioner<double>  precond(mtx);

  update_precond++;

  return;

}

int  SolverEigen::solverSchurExplicitCG()
{
    return 0;
}

int  SolverEigen::solverSchurExplicitDirect()
{

    return 0;
}


/*
   Copyright 2018 Simon Vogl <svogl@voxel.at>
                  Angel Merino-Sastre <amerino@voxel.at>

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/
#include <string>
#include <iostream>
#include <fstream>
#include <limits.h>

#include <pcl/range_image/range_image_planar.h>
#include <pcl/common/transforms.h>

#include <boost/log/trivial.hpp>
#include <boost/foreach.hpp>
#include <boost/property_tree/json_parser.hpp>

#include "toffy/3d/transform.hpp"

using namespace toffy;
using namespace toffy::filters::f3d;
using namespace cv;
using namespace std;

#ifdef CM_DEBUG
const bool dbg=true;
#else
const bool dbg=false;
#endif

std::size_t Transform::_filter_counter = 1;
const std::string Transform::id_name = "transform";

Transform::Transform(): Filter(Transform::id_name,_filter_counter),
    _in_cloud("cloud"), _out_cloud(_in_cloud)
{
    _filter_counter++;
}

/*
int Transform::loadConfig(const boost::property_tree::ptree& pt) {
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__;
    const boost::property_tree::ptree& tree = pt.get_child(this->type());

    loadGlobals(tree);

    updateConfig(tree);

    return true;
}
*/

bool Transform::filter(const Frame &in, Frame& out) const {
	BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ << " " << id();
	pcl::RangeImagePlanar::Ptr planar;
	try {
	    planar = boost::any_cast<pcl::RangeImagePlanar::Ptr>(in.getData(_in_cloud));
	} catch(const boost::bad_any_cast &) {
	    BOOST_LOG_TRIVIAL(warning) <<
		    "Could not cast input " << _in_cloud <<
		    ", filter  " << id() <<" not applied.";
	    return false;
	}
	Eigen::Affine3f tr;
	tr.setIdentity();
	for (size_t i = 0; i < _actions.size(); i++) {
		if (_operations[i] == Transform::rotation) {
			Eigen::Matrix3f m;
			m = Eigen::AngleAxisf(_actions[i][0], Eigen::Vector3f::UnitX())
			  * Eigen::AngleAxisf(_actions[i][1],  Eigen::Vector3f::UnitY())
			  * Eigen::AngleAxisf(_actions[i][2], Eigen::Vector3f::UnitZ());
			tr = m * tr;
		} else if (_operations[i] == Transform::translation) {
			Eigen::Translation3f t(_actions[i][0],
					_actions[i][1],_actions[i][2]);
			tr = t *tr;
		} else if (_operations[i] == Transform::scaling) {
			//Eigen::Affine3f m = ;
			tr = Eigen::Scaling((float)_actions[i][0],(float)_actions[i][1],(float)_actions[i][2]) *tr;
		} else
			BOOST_LOG_TRIVIAL(warning) <<
				"Operation unknown.";
	}

	pcl::transformPointCloud(*planar, *planar, tr);
	out.addData(_out_cloud, planar);
	return true;
}

boost::property_tree::ptree Transform::getConfig() const
{
    boost::property_tree::ptree pt, actions;
    pt = Filter::getConfig();

    for (size_t i=0; i < _actions.size();i++) {
	boost::property_tree::ptree action;
	if (_operations[i] == Transform::rotation) {
	    action.put("rotation",_actions[i]);
	} else if (_operations[i] == Transform::translation) {
	    action.put("translation",_actions[i]);

	} else if (_operations[i] == Transform::scaling) {
	    action.put("scaling",_actions[i]);
	}
	// quirk to create array elements:
	actions.push_back(std::make_pair("", action));
    }

    pt.add_child("actions", actions);

    pt.put("inputs.cloud", _in_cloud);

    pt.put("outputs.cloud", _out_cloud);

    return pt;
}

void Transform::updateConfig(const boost::property_tree::ptree &pt)
{
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ <<  " " << id();

    using namespace boost::property_tree;

    Filter::updateConfig(pt);

    _actions.clear();
    _operations.clear();
    try {
	BOOST_FOREACH(const ptree::value_type &v, pt.get_child("actions")) {
	    BOOST_FOREACH(const ptree::value_type &w, v.second) {

		std::vector<double> vec;
		BOOST_LOG_TRIVIAL(debug) << "w.first " << w.first;
		BOOST_LOG_TRIVIAL(debug) << w.second.data();
		std::stringstream ss(w.second.data());
		boost::property_tree::ptree jvec;
		boost::property_tree::json_parser::read_json(ss,jvec);
		BOOST_LOG_TRIVIAL(debug) << "jvec.size() " << jvec.size();
		BOOST_FOREACH(const ptree::value_type &x, jvec) {
		    BOOST_LOG_TRIVIAL(debug) << "x.second.data()" << x.second.data();
		    vec.push_back(x.second.get<double>(""));
		}
		BOOST_LOG_TRIVIAL(debug) << "vec.size()" << vec.size();
		if (vec.size() == 3) {
		    _actions.push_back(cv::Vec3d(vec[0],vec[1],vec[2]));
		    if(w.first == "rotation")
			_operations.push_back(Transform::rotation);
		    else if(w.first == "translation")
			_operations.push_back(Transform::translation);
		    else if(w.first == "scaling")
			_operations.push_back(Transform::scaling);
		}
	    }
	}
    } catch (const std::exception& ex) {
	BOOST_LOG_TRIVIAL(debug) << "Transform values are wrong. Ex:" << ex.what();
    }

    //_minAmpl = pt.get("options.minAmpl", _minAmpl);
    //_maxAmpl =  pt.get("options.maxAmpl", _maxAmpl);

    _in_cloud = pt.get("inputs.cloud", _in_cloud);

    _out_cloud = pt.get("outputs.cloud", _out_cloud);

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_OPERATOR_FUNCTIONS_SIMD_COMMON_IF_ELSE_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_OPERATOR_FUNCTIONS_SIMD_COMMON_IF_ELSE_HPP_INCLUDED
#include <boost/simd/toolbox/operator/functions/if_else.hpp>
#include <boost/simd/include/functions/simd/bitwise_select.hpp>
#include <boost/simd/include/functions/simd/is_nez.hpp>
#include <boost/simd/include/functions/simd/genmask.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::if_else_, tag::cpu_, (A0)(A1)(X)
                            , ((simd_< logical_<A0>, X >))
                              ((simd_< arithmetic_<A1>, X >))
                              ((simd_< arithmetic_<A1>, X >))
                            )
  {
    typedef A1 result_type;
    inline result_type operator()(const A0& a0, const A1& a1, const A1&a2) const
    {
      return bitwise_select(genmask(a0), a1, a2);
    }
  };
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::if_else_, tag::cpu_, (A0)(A1)(X)
                            , ((simd_< arithmetic_<A0>, X >))
                              ((simd_< arithmetic_<A1>, X >))
                              ((simd_< arithmetic_<A1>, X >))
                            )
  {
    typedef A1 result_type;
    inline result_type operator()(const A0& a0, const A1& a1, const A1&a2) const
    {
      return if_else( is_nez(a0), a1, a2 );
    }
  };
} } }

#endif


#define BOOST_TEST_DYN_LINK

#include <iostream>
#include <boost/test/unit_test.hpp>
#include <fstream>
#include <string>
#include "vm_context.hpp"
#include "std.hpp"
#include "std/real.cpp"

using namespace std;
using namespace cuttle::vm;

struct context_fixture {
    context_t context;

    void setup() {
        populate(context);
    }
};

BOOST_FIXTURE_TEST_SUITE(real_plus_suite, context_fixture)

    BOOST_AUTO_TEST_CASE(case1) {
		value_t val1 = { { type_id::real },{ context.gc.add(new real_t{ 1.0 }) } };
		value_t val2 = { {type_id::real}, {context.gc.add(new real_t{ 2.0 })} };
		value_t expect = { {type_id::real}, {context.gc.add(new real_t{ 3.0 })} };
		value_t ret;

		real_plus_func(context, { val1, val2 }, ret);

		BOOST_CHECK(ret == expect);
	}

    BOOST_AUTO_TEST_CASE(case2) {
		value_t val1 = { { type_id::real },{ context.gc.add(new real_t{ 100.01 }) } };
		value_t val2 = { { type_id::real },{ context.gc.add(new real_t{ 3.002 }) } };
		value_t expect = { { type_id::real },{ context.gc.add(new real_t{ 103.012 }) } };
		value_t ret;

		real_plus_func(context, { val1, val2 }, ret);

		BOOST_CHECK(ret == expect);
	}

BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(real_minus_suite, context_fixture)

    BOOST_AUTO_TEST_CASE(case1) {
		value_t val1 = { { type_id::real },{ context.gc.add(new real_t{ 1.0 }) } };
		value_t val2 = { { type_id::real },{ context.gc.add(new real_t{ 2.0 }) } };
		value_t expect = { { type_id::real },{ context.gc.add(new real_t{ -1.0 }) } };
		value_t ret;

		real_minus_func(context, { val1, val2 }, ret);

		BOOST_CHECK(ret == expect);
	}

    BOOST_AUTO_TEST_CASE(case2) {
		value_t val1 = { { type_id::real },{ context.gc.add(new real_t{ 100.02 }) } };
		value_t val2 = { { type_id::real },{ context.gc.add(new real_t{ 3.01 }) } };
		value_t expect = { { type_id::real },{ context.gc.add(new real_t{ 97.01 }) } };
		value_t ret;

		real_minus_func(context, { val1, val2 }, ret);

		BOOST_CHECK(ret == expect);
	}

BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(real_multiply_suite, context_fixture)

    BOOST_AUTO_TEST_CASE(case1) {
		value_t val1 = { { type_id::real },{ context.gc.add(new real_t{ 1.0 }) } };
		value_t val2 = { { type_id::real },{ context.gc.add(new real_t{ 2.1 }) } };
		value_t expect = { { type_id::real },{ context.gc.add(new real_t{ 2.1 }) } };
		value_t ret;

		real_multiply_func(context, { val1, val2 }, ret);

		BOOST_CHECK(ret == expect);
	}

    BOOST_AUTO_TEST_CASE(case2) {
		value_t val1 = { { type_id::real },{ context.gc.add(new real_t{ 100.02 }) } };
		value_t val2 = { { type_id::real },{ context.gc.add(new real_t{ 0.03 }) } };
		value_t expect = { { type_id::real },{ context.gc.add(new real_t{ 3.0006 }) } };
		value_t ret;

		real_multiply_func(context, { val1, val2 }, ret);

		BOOST_CHECK(ret == expect);
	}

BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(real_divide_suite, context_fixture)

    BOOST_AUTO_TEST_CASE(case1) {
		value_t val1 = { { type_id::real },{ context.gc.add(new real_t{ 1.0 }) } };
		value_t val2 = { { type_id::real },{ context.gc.add(new real_t{ 2.0 }) } };
		value_t expect = { { type_id::real },{ context.gc.add(new real_t{ 0.5 }) } };
		value_t ret;

		real_divide_func(context, { val1, val2 }, ret);

		BOOST_CHECK(ret == expect);
	}

    BOOST_AUTO_TEST_CASE(case2) {
		value_t val1 = { { type_id::real },{ context.gc.add(new real_t{ 29.0 }) } };
		value_t val2 = { { type_id::real },{ context.gc.add(new real_t{ 5.0 }) } };
		value_t expect = { { type_id::real },{ context.gc.add(new real_t{ 5.8 }) } };
		value_t ret;

		real_divide_func(context, { val1, val2 }, ret);

		BOOST_CHECK(ret == expect);
	}

BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(string_to_real_suite, context_fixture)

    BOOST_AUTO_TEST_CASE(case1) {
		value_t val = { { type_id::string } };
		val.data.string = context.gc.add_r(new std::string{ "3.10" });
		value_t expect = { { type_id::real },{ context.gc.add(new real_t{ 3.1 }) } };
		value_t ret;

		string_to_real_func(context, { val }, ret);

		BOOST_CHECK(ret == expect);
	}

    BOOST_AUTO_TEST_CASE(case2) {
		value_t val = { { type_id::string } };
		val.data.string = context.gc.add_r(new std::string{ "3" });
		value_t expect = { { type_id::real },{ context.gc.add(new real_t{ 3.0 }) } };
		value_t ret;

		string_to_real_func(context, { val }, ret);

		BOOST_CHECK(ret == expect);
	}

    BOOST_AUTO_TEST_CASE(case3) {
		value_t val = { { type_id::string } };
		val.data.string = context.gc.add_r(new std::string{ "ads" });
		value_t ret;

		BOOST_CHECK_THROW(string_to_real_func(context, { val }, ret), std::invalid_argument);
	}

BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(integral_to_real_suite, context_fixture)

    BOOST_AUTO_TEST_CASE(case1) {
		value_t val = { { type_id::integral } };
		val.data.integral = context.gc.add_r(new integral_t{ 3 });
		value_t expect = { { type_id::real },{ context.gc.add(new real_t{ 3.0 }) } };
		value_t ret;

		integral_to_real_func(context, { val }, ret);

		BOOST_CHECK(ret == expect);
	}

BOOST_AUTO_TEST_SUITE_END()


//
// Created by qfeng on 2019-01-08.
//

#include <feature/log_op_ctx.h>

#include "feature/log_op_ctx.h"
#include "featpb/cont_feat_grp.pb.h"
#include "feature/log_op.h"
#include <string>
#include <optional>
#include <absl/strings/str_split.h>
#include <boost/algorithm/string.hpp>

using namespace std;
using namespace boost::algorithm;

namespace algocomp {
    bool LogOpCtx::parse_procs_and_params(const string &proc_desc, const string &param_desc) {
        try {
            // Check the lengths of inputs (necessary inputs must not be empty)
            if (proc_desc == "") {
                _logger->warn(
                        "Empty procs for log feature operation context (in_gname={}, in_fname={}).",
                        _in_gname, _in_fname);
                return false;
            }

            // Parse processes together with their parameters
            std::vector<std::string> proc_splits = absl::StrSplit(proc_desc, ';');
            std::vector<std::string> param_splits = absl::StrSplit(param_desc, ';');

            // The processes and parameters are corresponding one by one. The belows are to check the correctness
            if (proc_splits.size() != param_splits.size()) {
                _logger->error("Parsed inconsistent sizes of procs {} and params {} (in_gname={}, in_fname={}).",
                               proc_desc, param_desc, _in_gname, _in_fname);
                return false;
            }

            for (int i = 0; i < proc_splits.size(); ++i) {
                auto proc_name = trim_copy(proc_splits[i]);

                // Parse processes and their corresponding parameters
                if (proc_name == "log10") {
                    _log_op_type = LogOpType::LOG10;
                } else if (proc_name == "log") {
                    _log_op_type = LogOpType::LOG;
                } else if (proc_name == "log2") {
                    _log_op_type = LogOpType::LOG2;
                } else {
                    _logger->error("Parsed unknown log proc {} (in_gname={}, in_fname={})", proc_splits[i], _in_gname,
                                   _in_fname);
                    return false;
                }
            }

            return true;

        } catch (const std::exception &ex) {
            _logger->error("Parsed wrong log procs {} and params {} (in_gname={}, in_fname={}).", proc_desc, param_desc,
                           _in_gname, _in_fname);
        }

        return false;
    }

    optional<pair<string, ContFeatGrp>>
    LogOpCtx::apply(const vector<::google::protobuf::Map<string, ContFeatGrp>> &inp_feats,
                    google::protobuf::Arena &arena) const {
        try {
            ContFeatGrp *outp_feats = google::protobuf::Arena::CreateMessage<ContFeatGrp>(&arena);

            for (auto &inp_feats_: inp_feats) {
                // Match input feature
                auto g_found = inp_feats_.find(_in_gname);
                if (g_found != inp_feats_.end()) {
                    auto &feats = (*g_found).second.feats();

                    if (this->op_scope() == OP_SCOPE::GNAME) {
                        for (auto &feat: feats) {
                            // Apply logarithm transform on every feature of matched feature group name
                            auto res = this->_op->apply(feat.second, *this, arena);
                            if (res.has_value()) {
                                auto &res_feat = res.value();
                                (*outp_feats->mutable_feats())[res_feat.fname()] = res_feat;
                            } else {
                                _logger->warn("Logarithm (gname scope) fail on feature {} - {}", _in_gname, feat.first);
                            }
                        }
                    } else if (this->op_scope() == OP_SCOPE::FNAME) {
                        auto f_found = feats.find(_in_fname);
                        if (f_found != feats.end()) {
                            auto &feat = (*f_found).second;
                            // Apply logarithm transform on matched feature name
                            auto res = this->_op->apply(feat, *this, arena);

                            if (res.has_value()) {
                                auto &res_feat = res.value();
                                (*outp_feats->mutable_feats())[res_feat.fname()] = res_feat;
                            } else {
                                _logger->warn("Logarithm (fname scope) fail on feature {} - {}", _in_gname, feat.fname());
                            }
                        }
                    }
                }
            }

            if (outp_feats->feats_size() > 0) {
                auto outp_gname = outp_feats->feats().begin()->second.gname();
                return make_pair(outp_gname, *outp_feats);
            } else return std::nullopt;

        } catch (const exception &ex) {
            _logger->error("Log wised fail on features: {}", ex.what());
        }

        return std::nullopt;
    }
}


#include "xcb_inference.hpp"

#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include <Eigen/LU>
#include <fstream>
#include <iostream>
#include <queue>
#include <vector>
#include <math.h>
#include <random>

typedef Eigen::SparseMatrix<float> eigen_colmajor;
typedef Eigen::SparseMatrix<float, Eigen::RowMajor> eigen_rowmajor;
typedef std::size_t size_t;
typedef std::pair<size_t, float> score;
typedef std::pair<int, size_t> node;
typedef std::pair<node, size_t> chunk;

float combine(float x, float y, std::string combiner) {
  if (combiner.compare("noop") == 0) {
    return y;
  }
  if (combiner.compare("add") == 0) {
    return x + y;
  }
  if (combiner.compare("multiply") == 0) {
    return x * y;
  }
  throw "combiner has not been implemented!";
  return 0;
}

float transform(float x, std::string post_processor) {
  if (post_processor.compare("noop") == 0) {
    return x;
  }
  if (post_processor.compare("sigmoid") == 0) {
    return 1.0 / (1.0 + std::exp(-x));
  }
  if (post_processor.compare("l3-hinge") == 0) {
    float z = std::max(0.0, 1.0 - x);
    return std::exp(-std::pow(z, 3));
  }
  throw "post_processor has not been implemented!";
}

/*!  Adds elements to the next level of the model_chain.
    It takes in weight matrix and cluster matrix, and pushes them at the end of
    the weights and clusters.

*/
void ModelChain::add_elements(eigen_colmajor weight, eigen_colmajor cluster,
                              eigen_colmajor regression_weight) {
  this->weights.push_back(weight);
  this->clusters.push_back(cluster);
  this->regression_weights.push_back(regression_weight);
  this->size = weights.size();
  this->num_labels = weight.cols();
}

/*!  Perform xcb beam search

    in_vector: input sparse vector
    beam_size: beam_size to be used
    topk: number of labels to fetch
    num_explore: number of slots allowed for exploration
    multiplier: falcon parameter

    returns:
    prediction and score of singleton arms
    mapping of singleton arms to chunks if required

*/
std::pair<std::vector<score>, std::vector<chunk>> ModelChain::beam_search(
    eigen_rowmajor &in_vector, int beam_size, int topk, int num_explore,
    float multiplier, std::string post_processor, std::string combiner,
    bool explore_in_routing, std::string explore_strategy, float alpha) {
  std::vector<score> codes;
  score cp(0, 1.0);
  codes.push_back(cp);
  std::vector<score> top_scores;
  std::vector<size_t> labels;
  std::vector<chunk> selected_chunks;
  std::vector<node> rejected_nodes;
  for (int d = 0; d < this->size; ++d) {
    std::vector<score> labels = this->get_labels(codes, this->clusters[d]);
    int get_size;
    if (d == this->size - 1) {
      get_size = topk;
    } else {
      get_size = beam_size;
    }
    top_scores = this->get_top_labels(
        in_vector, d, labels, get_size, num_explore, multiplier, post_processor,
        combiner, explore_in_routing, explore_strategy, rejected_nodes,
        selected_chunks, alpha);
    // get top scores for level d in the model chain
    codes.clear();
    codes = top_scores;
    // only do the beam search starting from these top-score labels in the next
    // round.
  }
  std::pair<std::vector<score>, std::vector<chunk>> result(top_scores,
                                                           selected_chunks);
  return result;
}

/*!  Get all labels that connect with the given codes on a given level.
    It takes in the codes as a vector and cluster matrix.

    codes: the input codes for the level
    cluster_mat: the clustering matrix from this->clusters[level] is used to
   infer the connections.

*/
std::vector<score> ModelChain::get_labels(std::vector<score> &codes,
                                          eigen_colmajor &cluster_mat) {
  std::vector<score> labels;
  for (int i = 0; i < codes.size(); i++) {
    size_t k = codes[i].first;
    float val = codes[i].second;
    if ((k >= cluster_mat.cols()) || (k < 0)) {
      throw "column does not exist in cluster matrix!";
      continue;
    }
    for (eigen_colmajor::InnerIterator it(cluster_mat, k); it; ++it) {
      score cp(it.index(), val);
      labels.push_back(cp);  // inner index, here it is equal to it.row()
    }
  }
  return labels;
}

/*!  Get a random arm from the subtree.

*/

size_t ModelChain::get_random_arm(node &current_node) {
  auto d = current_node.first;
  std::vector<score> codes;
  score cp(current_node.second, 1.0);
  codes.push_back(cp);
  for (int i = d + 1; i < this->size; i++) {
    std::vector<score> labels = this->get_labels(codes, this->clusters[i]);
    int index = std::rand() % labels.size();
    codes.clear();
    codes.push_back(labels[index]);
  }
  return codes[0].first;
}

/*  Get all top labels from a level given input std::vector and a list of
   allowed labels.

    in_vector: input vector
    weights: weight matrix for a given level
    labels: allowed set of labels
    topk: number of top labels to withdraw

*/
std::vector<score> ModelChain::get_top_labels(
    eigen_rowmajor &in_vector, int level, std::vector<score> &labels, int topk,
    int num_explore, float multiplier, std::string post_processor,
    std::string combiner, bool explore_in_routing, std::string explore_strategy,
    std::vector<node> &rejected_nodes, std::vector<chunk> &selected_chunks,
    float alpha) {
  eigen_colmajor &weights = this->weights[level];
  if (level == this->size - 1) {
    weights = this->regression_weights[level];
  }  // singleton arms need regression weights
  std::vector<std::pair<float, size_t>> score_list;
  for (int i = 0; i < labels.size(); i++) {
    size_t k = labels[i].first;
    float prev_score = labels[i].second;
    if ((k >= weights.cols()) || (k < 0)) {
      throw "column does not exist in weight matrix!";
      continue;
    }
    float sum = 0;
    for (eigen_rowmajor::InnerIterator it(in_vector, 0); it; ++it) {
      sum += it.value() * weights.coeffGet(it.index(), k);
    }
    if (level < this->size - 1) {
      sum = transform(sum, post_processor);  // using post processing
      sum = combine(prev_score, sum, combiner);
    }
    std::pair<float, size_t> cp(sum, k);
    score_list.push_back(cp);  // pushing f(x; l) to the score list
  }
  // add rejected nodes to scores.
  if (level == this->size - 1) {
    for (int i = 0; i < rejected_nodes.size(); i++) {
      node n = rejected_nodes[i];
      weights = this->regression_weights[n.first];
      size_t k = n.second;
      if ((k >= weights.cols()) || (k < 0)) {
        throw "column does not exist in weight matrix!";
        continue;
      }
      float sum = 0;
      for (eigen_rowmajor::InnerIterator it(in_vector, 0); it; ++it) {
        sum += it.value() * weights.coeffGet(it.index(), k);
      }
      std::pair<float, size_t> cp(sum, 2 * this->num_labels + i);
      score_list.push_back(cp);
    }
  }
  auto top_scores =
      this->get_top_scores(score_list, topk, level, num_explore, multiplier,
                           explore_in_routing, explore_strategy, alpha);
  // adding to rejected nodes
  if (level < this->size - 1) {
    std::map<size_t, int> accepted;
    for (int i = 0; i < top_scores.size(); i++) {
      accepted[top_scores[i].first] = 1;
    }
    for (int i = 0; i < labels.size(); i++) {
      auto elem = labels[i].first;
      if (accepted.find(elem) == accepted.end()) {
        node cp(level, elem);
        rejected_nodes.push_back(cp);
      }
    }
    return top_scores;
  } else {
    std::vector<score> actual_scores;  // mapping from chunks
    for (int i = 0; i < top_scores.size(); i++) {
      auto elem = top_scores[i];
      if (elem.first < this->num_labels) {
        actual_scores.push_back(elem);
      } else if (elem.first - 2 * this->num_labels >= 0) {
        node current_node = rejected_nodes[elem.first - 2 * this->num_labels];
        auto arm = this->get_random_arm(current_node);
        score cp(arm, elem.second);
        actual_scores.push_back(cp);
        chunk selected(current_node, arm);
        selected_chunks.push_back(selected);
      }
    }
    return actual_scores;
  }
}

/*! get top scores given a score list along with exploration.

*/

std::vector<score> ModelChain::get_top_scores(
    std::vector<std::pair<float, size_t>> &score_list, int topk, int level,
    int num_explore, float multiplier, bool explore_in_routing,
    std::string explore_strategy, float alpha) {
  std::vector<score> top_scores;
  int num_exp;
  float mult = multiplier;
  if (level < this->size - 1) {
    num_exp = 1;
    if (!explore_in_routing) {
      mult = -1.0;
    }
  } else {
    num_exp = num_explore;
  }
  if ((mult < 0) || (score_list.size() < topk)) {
    // exploit only mode
    std::priority_queue<std::pair<float, size_t>> score_q;
    for (int i = 0; i < score_list.size(); i++) {
      std::pair<float, size_t> cp(score_list[i].first, score_list[i].second);
      score_q.push(cp);
    }

    for (int k = 0; k < topk; ++k) {
      if (score_q.empty()) {
        break;
      }
      auto top_elem = score_q.top();
      score cp(top_elem.second, top_elem.first);
      top_scores.push_back(cp);
      score_q.pop();
    }

  } else {
    // exploration is allowed
    int count = 0;
    int num_exploit = topk - num_exp;
    while (count < topk) {
      auto cp = this->get_sample(score_list, multiplier, level, num_exploit,
                                 explore_strategy, alpha);
      top_scores.push_back(cp);
      count++;
    }
  }
  return top_scores;
}

/* Get sample from Falcon distribution
    score_list: list for scores and corresponding indices
    multiplier: falcon parameter
    level: level in the tree
    num_exploit: top num_exploit slots are always fetched and then exploration
   begins.
*/
score ModelChain::get_sample(std::vector<std::pair<float, size_t>> &score_list,
                             float multiplier, int level, int &num_exploit,
                             std::string explore_strategy, float alpha) {
  int k = score_list.size();
  float maxima = -std::numeric_limits<float>::infinity();
  int max_index = -1;
  int ind;
  std::random_device rd;
  std::mt19937 gen(rd());
  for (int i = 0; i < k; i++) {
    if (score_list[i].first > maxima) {
      maxima = score_list[i].first;
      max_index = i;
    }
  }
  if (num_exploit > 0) {
    ind = max_index;
    num_exploit--;
  } else {
    std::vector<float> weights;
    float psum = 0;
    float eta = std::pow(float(k) * multiplier, alpha);
    // creating distributions
    if (explore_strategy.compare("falcon") == 0) {
      for (int i = 0; i < k; i++) {
        if (i != max_index) {
          float p = 1.0 / (float(k) + eta * (maxima - score_list[i].first));
          weights.push_back(p);
          psum = psum + p;
        } else {
          weights.push_back(0.0);
        }
      }
      weights[max_index] = 1.0 - psum;
    } else if (explore_strategy.compare("e-greedy") == 0) {
      for (int i = 0; i < k; i++) {
        if (i != max_index) {
          float p = multiplier / float(k);
          weights.push_back(p);
          psum = psum + p;
        } else {
          weights.push_back(0.0);
        }
      }
      weights[max_index] = 1.0 - psum;
    } else if (explore_strategy.compare("boltzmann") == 0) {
      for (int i = 0; i < k; i++) {
        float p = std::exp(multiplier * score_list[i].first);
        weights.push_back(p);
        psum = psum + p;
      }
      for (int i = 0; i < k; i++) {
        weights[i] /= psum;
      }
    } else {
      throw "explore strategy not implemented!";
    }
    std::discrete_distribution<> d(weights.begin(), weights.end());
    ind = d(gen);
  }
  auto elem = score_list[ind];
  score cp(elem.second, elem.first);
  score_list[ind] = score_list[score_list.size() - 1];
  score_list.pop_back();
  // cannot select the same index twice, so it is pushed out.
  return cp;
}


#include <chrono>
#include <cstdlib>
#include <iostream>
#include <string>
#include <thread>
#include <boost/asio.hpp>
#include <asio-zmq.hpp>

static std::string const ep = "inproc://lat_test";
static boost::asio::io_service ios;
static boost::asio::zmq::context ctx;

void requester(int rc, int msize)
{
    boost::asio::zmq::socket req(ios, ctx, ZMQ_REQ);
    req.connect(ep);

    while (--rc >= 0) {
        req.write_frame(boost::asio::zmq::frame(msize));
        req.read_frame();
    }
}

int main(int argc, char* argv[])
{
    if (argc != 3) {
        std::cerr << "usage: inproc_lat <message-size> <roundtrip-count>\n";
        return 1;
    }

    int message_size = std::atoi(argv[1]);
    int roundtrip_count = std::atoi(argv[2]);

    std::cout << "message size: " << message_size << " [B]\n";
    std::cout << "roundtrip count: " << roundtrip_count << "\n";

    boost::asio::zmq::socket rep(ios, ctx, ZMQ_REP);
    rep.bind(ep);

    std::thread worker(std::bind(requester, roundtrip_count, message_size));

    auto watch = std::chrono::system_clock::now();

    for (int i = 0; i < roundtrip_count; ++i) rep.write_frame(rep.read_frame());

    auto elapsed = std::chrono::duration_cast<std::chrono::microseconds>(
                       std::chrono::system_clock::now() - watch).count();
    double latency = static_cast<double>(elapsed) / (roundtrip_count * 2);

    std::cout << "average latency: " << latency << " [us]\n";

    worker.join();
}


/*
 * patSimulation.cc
 *
 *  Created on: May 25, 2012
 *      Author: jchen
 */

#include "patSimulation.h"
#include "patKMLPathWriter.h"
#include "patObservation.h"
#include "patUtilityFunction.h"
#include <vector>
#include <map>
#include "patMultiModalPath.h"
#include "patOd.h"
#include <fstream>
#include "patObservation.h"
#include "patReadChoiceSetFromKML.h"
#include "patNetworkEnvironment.h"
#include "patNetworkElements.h"
#include "patComputePathSize.h"
#include <boost/lexical_cast.hpp>
#include "patSampleDiscreteDistribution.h"
#include "patSimulateProbabilisticPaths.h"
#include "patRouter.h"
#include <boost/lexical_cast.hpp>
patSimulation::patSimulation(const patNetworkEnvironment* network_environment,
		patUtilityFunction* utility_function, int rng) :
		m_network_environment(network_environment), m_utility_function(
				utility_function), m_rnd(rng) {
}
void patSimulation::run(string folder, const patRouter* router) {
	patObservation observation;

	patReadChoiceSetFromKML rc(&m_network_environment->getNetworkElements());

	string choiceset_file = folder + "universal_choice_set.kml";
	if (!ifstream(choiceset_file.c_str())) {
		return;
	}

	map<patOd, patChoiceSet> od_choice_set = rc.read(choiceset_file, m_rnd);
//	DEBUG_MESSAGE(od_choice_set.size());
	patOd od = od_choice_set.begin()->first;
	patChoiceSet choice_set = od_choice_set.begin()->second;
	set<patMultiModalPath> paths_set = choice_set.getChoiceSet();
//	DEBUG_MESSAGE(paths_set.size());
	vector<patMultiModalPath> paths_vector;
	vector<double> utility_vector;

	//Normalize utility vector

	vector<int> sampled_observations;
	vector<double> sampled_empirical_proba;
	vector<double> sampled_count;

	DEBUG_MESSAGE("universal path size"<<paths_set.size());
	patComputePathSize ps_computer;

	map<const patMultiModalPath, double> ps = ps_computer.computePS(choice_set);
	DEBUG_MESSAGE("ps calculated"<<ps.size());
	for (set<patMultiModalPath>::iterator path_iter = paths_set.begin();
			path_iter != paths_set.end(); ++path_iter) {
		paths_vector.push_back(*path_iter);

		map<const patMultiModalPath, double>::const_iterator find_ps = ps.find(
				*path_iter);
		if (find_ps == ps.end()) {
			throw RuntimeException("no ps found");
		}

		DEBUG_MESSAGE(
				m_utility_function->getCost( *path_iter)<<"+"<< m_utility_function->getLinkCostScale()<<"*"<<m_utility_function->getPathSizeCoefficient()<<"*"<<log(find_ps->second));

		double utility = m_utility_function->getCostWithPathSize(*path_iter,
				find_ps->second);
		utility_vector.push_back(exp(utility));
		DEBUG_MESSAGE(utility<<","<<utility_vector.back());
		sampled_empirical_proba.push_back(0.0);
		sampled_count.push_back(0);
	}
	double sum = 0.0;
	for (vector<double>::const_iterator u_iter = utility_vector.begin();
			u_iter != utility_vector.end(); ++u_iter) {
		sum += *u_iter;
	}
	for (vector<double>::size_type i = 0; i < utility_vector.size(); ++i) {
		utility_vector[i] /= sum;
	}
//	return;
	DEBUG_MESSAGE(paths_vector.size()<<" alternatives");

	unsigned int total_sampled_count = 1000;
	ofstream sampled_file((folder + "sampled.txt").c_str());
	for (unsigned int i = 0; i < total_sampled_count; ++i) {
//		if (i % 10 == 0) {
//			DEBUG_MESSAGE(i);
//		}
		string i_str = boost::lexical_cast<string>(i);
		patKMLPathWriter kml_writer(folder + "observations/" + i_str + ".kml");
		int sampled = patSampleDiscreteDistribution()(utility_vector, m_rnd);
		sampled_file << sampled << endl;
		sampled_observations.push_back(sampled);
		sampled_empirical_proba[sampled] += 1.0 / (double) total_sampled_count;
		sampled_count[sampled]++;

		map<string, string> attrs_true;
		attrs_true["true"] = boost::lexical_cast<string>(
				utility_vector[sampled]);
		attrs_true["id"] = boost::lexical_cast<string>(sampled);
		attrs_true["proba"] = boost::lexical_cast<string>(
				1.0 - patNBParameters::the()->errorInSimulatedObservations);
		kml_writer.writePath(paths_vector[sampled], attrs_true);

		//Deprecated and replaced by patSimulationProbabilisicObs
//		if (patNBParameters::the()->errorInSimulatedObservations > 0.0
//				&& patNBParameters::the()->nbrOfSimulatedErrorPaths > 0) {
//			patSimulateProbabilisticPaths simulate_prob_paths(
//					paths_vector[sampled], router);
//			map<patMultiModalPath, double> simulated_paths =
//					simulate_prob_paths.run(
//							patNBParameters::the()->nbrOfSimulatedErrorPaths,
//							patNBParameters::the()->errorInSimulatedObservations);
//			for (map<patMultiModalPath, double>::iterator path_iter =
//					simulated_paths.begin(); path_iter != simulated_paths.end();
//					++path_iter) {
//				map<string, string> attrs;
//				attrs["true"] = boost::lexical_cast<string>(
//						utility_vector[sampled]);
//				attrs["id"] = boost::lexical_cast<string>(sampled);
//				attrs["proba"] = boost::lexical_cast<string>(path_iter->second);
//				kml_writer.writePath(path_iter->first, attrs);
//
//			}
//		}
		kml_writer.close();
	}
	sampled_file.close();
	double chi2 = 0.0;
	for (unsigned int i = 0; i < utility_vector.size(); ++i) {
		double empirical_proba = (double) sampled_count[i]
				/ total_sampled_count;
		double theo_proba = utility_vector[i];
		chi2 += total_sampled_count * (theo_proba - empirical_proba)
				* (theo_proba - empirical_proba) / theo_proba;
		DEBUG_MESSAGE(
				utility_vector[i]<<","<<sampled_count[i]/(double)total_sampled_count);
	}DEBUG_MESSAGE("chi2: "<<chi2);
}

patSimulation::~patSimulation() {
	// TODO Auto-generated destructor stub
}


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_TYPE_TRAITS_HAS_INVERSE_HPP_JOFA_090205
#define BOOST_ICL_TYPE_TRAITS_HAS_INVERSE_HPP_JOFA_090205

#include <boost/type_traits/is_signed.hpp>
#include <boost/type_traits/is_floating_point.hpp>

namespace boost{ namespace icl
{

    template <class Type> struct has_inverse
    {
        typedef has_inverse<Type> type;
        BOOST_STATIC_CONSTANT(bool,
            value = (type_traits::ice_or<boost::is_signed<Type>::value,
                                         is_floating_point<Type>::value>::value));
    };

}} // namespace boost icl

#endif // BOOST_ICL_TYPE_TRAITS_HAS_INVERSE_HPP_JOFA_090205


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file SteamCATrajVar.hpp
///
/// \author Tim Tang, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_CA_TRAJECTORY_VAR_HPP
#define STEAM_CA_TRAJECTORY_VAR_HPP

#include <Eigen/Core>

#include <steam/common/Time.hpp>
#include <steam/evaluator/blockauto/transform/TransformEvaluator.hpp>
#include <steam/state/VectorSpaceStateVar.hpp>

#include <steam/trajectory/SteamTrajVar.hpp>

namespace steam {
namespace se3 {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief This class wraps a pose and velocity evaluator to act as a discrete-time trajectory
///        state variable for continuous-time trajectory estimation.
//////////////////////////////////////////////////////////////////////////////////////////////
class SteamCATrajVar : public SteamTrajVar
{
 public:

  /// Shared pointer typedefs for readability
  typedef boost::shared_ptr<SteamCATrajVar> Ptr;
  typedef boost::shared_ptr<const SteamCATrajVar> ConstPtr;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  SteamCATrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0,
                 const VectorSpaceStateVar::Ptr& velocity,
                 const VectorSpaceStateVar::Ptr& acceleration);

  SteamCATrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0,
                 const VectorSpaceStateVar::Ptr& velocity,
                 const VectorSpaceStateVar::Ptr& acceleration,
                 const Eigen::Matrix<double,18,18> cov);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get velocity state variable
  //////////////////////////////////////////////////////////////////////////////////////////////
  const VectorSpaceStateVar::Ptr& getAcceleration() const;

 private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Generalized 6D acceleration state variable
  //////////////////////////////////////////////////////////////////////////////////////////////
  VectorSpaceStateVar::Ptr acceleration_;
};

} // se3
} // steam

#endif // STEAM_CA_TRAJECTORY_VAR_HPP