#include <boost/format.hpp>
#include <iostream>
#include <sstream>

#include "biggles/detail/observation_reservoir.hpp"
#include "biggles/sampling/simple.hpp"
#include "biggles/tools/clutter_transform.hpp"
#include "biggles/tools/track_coll_transform.hpp"
#include "biggles/tools/max_creation.hpp"
#include "biggles/clutter.hpp"
#include "biggles/track_collection.hpp"
#include "biggles/partition.hpp"
#include "biggles/mh_moves/mh_moves.hpp"

// include this last to stop pre-processor macros breaking things
extern "C" {
#include <ccan/tap/tap.h>
}

using namespace biggles;

bool observation_in_track_collection(const observation& o, const track_collection& tc) {
    for (track_collection::const_iterator tcit = tc.begin(); tcit not_eq tc.end(); ++tcit) {
        if ((*tcit)->observations().contains(o))
            return true;
    }
    return false;
}

void test_proc1() {
    typedef boost::shared_ptr<ObservationReservoir> observation_reservoir_var_ptr;
    observation_reservoir_var_ptr reservoir(new ObservationReservoir);
    for (time_stamp t = 0; t < 10; ++t) {
        size_t num = sampling::uniform_int(0, 10);
        for (size_t i = 0; i < num; ++i) {
            reservoir->push(sampling::uniform_real(0.f, 10.f), sampling::uniform_real(0.f, 10.f), t);
        }
    }
    std::deque<size_t> idx_clutter;
    for (size_t i = 0; i < reservoir->size(); ++i) {
        idx_clutter.push_back(i);
    }
    clutter_ptr clutter(new clutter_t());
    partition::track_collection_ptr tracks(new track_collection());

    // convert index clutter to observation clutter
    clutter_index_to_observation(reservoir, idx_clutter, *clutter);

    ok(idx_clutter.size() == clutter->size(), "clutter size invariant");

    // statr partition only has clutter
    partition_ptr_t start_partition( new partition(reservoir, tracks, clutter));

    // create some tracks
    partition_ptr_t half;
    max_creation(start_partition, half);
    ok(start_partition->volume() == half->volume(), "volume invariant");
    ok(half->observation_count() == reservoir->size(), "obs count invariant");
    ok(2*half->clutter().size() <= half->observation_count(), "at least half the obs are in tracks");

    ok(reservoir == half->pool(), "reservoir pointers identical");

    // converting partition "half" into indexed stuff
    clutter_index_t indexed_clutter;
    track_coll_index_t indexed_tracks;
    clutter_observation_to_index(half->pool(), half->clutter(), indexed_clutter);
    track_coll_observation_to_index(half->pool(), half->tracks(), indexed_tracks);

    // compare indexed and non-indexed stuff
    ok(half->tracks().size() == indexed_tracks.size(), "num tracks invariant");
    ok(half->clutter().size() == indexed_clutter.size(), "clutter size invariant");

    // convert indexed "half" back to observed collections
    clutter_ptr clutter2(new clutter_t());
    boost::shared_ptr<track_collection> tracks2(new track_collection());
    clutter_index_to_observation(reservoir, indexed_clutter, *clutter2);
    track_coll_index_to_observation(reservoir, indexed_tracks, *tracks2);
    // do tests
    ok(tracks2->size() == half->tracks().size(), "num tracks recovered");
    ok(clutter2->size() == half->clutter().size(), "clutter size recovered");
    int failcount = 0;
    for (clutter_t::const_iterator it = clutter2->begin(); it != clutter2->end(); ++it) {
        for (clutter_t::observation_container::const_iterator oit = it->second.begin(); oit != it->second.end(); ++oit) {
            failcount += half->clutter().contains(*oit) ? 0 : 1;
        }
    }
    ok(failcount == 0, "clutter observations recovered");
    failcount = 0;
    BOOST_FOREACH(const shared_const_track_ptr& tp, *tracks2) {
        BOOST_FOREACH (const observation& o, tp->observations()) {
            failcount += observation_in_track_collection(o, half->tracks()) ? 0 : 1;
        }
    }
    ok(failcount == 0, "track observations recovered");

    ObservationReservoir::const_iterator oit1 = reservoir->begin();
    ObservationReservoir::const_iterator oit2 = half->pool()->begin();
    failcount = 0;
    while (oit1 != reservoir->end()) {
        failcount += oit1++ == oit2++ ? 0 : 1;
    }
    ok(failcount == 0, "observations order is preserved");
    ok(oit2 == half->pool()->end(), "second observation pool was exhausted");

    // do some MH moves
    mh_moves::weighted_move_sampler move_sampler;
    move_sampler.add(mh_moves::BIRTH).add(mh_moves::DEATH).add(mh_moves::EXTEND).add(mh_moves::REDUCE)
                .add(mh_moves::MERGE).add(mh_moves::SPLIT).add(mh_moves::UPDATE).add(mh_moves::TRANSFER)
                .add(mh_moves::CROSS_OVER);
    partition_ptr_t to_part;
    partition_ptr_t from_part(half);
    capability_recorder_ptr cap_rec_ptr(new_cap_recorder(*from_part));
    const size_t nloops = 1000;
    float pdr = 0.f;
    for (size_t i = 0 ; i < nloops; ++i) {
        from_part->set_capability_recorder(cap_rec_ptr);
        bool success = mh_moves::propose(move_sampler(), from_part, to_part, pdr);
        if (success) {
            from_part = to_part;
            cap_rec_ptr->commit_changes(from_part->tracks());
        }
    }

    oit1 = from_part->pool()->begin();
    oit2 = half->pool()->begin();
    failcount = 0;
    while (oit1 != from_part->pool()->end()) {
        failcount += oit1++ == oit2++ ? 0 : 1;
    }
    ok(failcount == 0, "observations order is preserved");
    ok(from_part->volume() == half->volume(), "volume is preserved");
}

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

    plan_tests(15);
    //plan_no_plan();

    test_proc1();

    return exit_status();
}


#ifndef MEMORYRANGE_HH
#define MEMORYRANGE_HH

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <vector>
#include <string>

#include "memory.hh"
#include "dna.pb.h"

class MemoryRange {
private:
  Memory _lower, _upper;

  mutable std::vector< boost::accumulators::accumulator_set< Memory::DataType,
							     boost::accumulators::stats<
							       boost::accumulators::tag::median > > > _acc;
  mutable unsigned int _count;

public:
  MemoryRange( const Memory & s_lower, const Memory & s_upper )
    : _lower( s_lower ), _upper( s_upper ), _acc( Memory::datasize ), _count( 0 )
  {}

  std::vector< MemoryRange > bisect( void ) const;
  Memory range_median( void ) const;

  bool contains( const Memory & query ) const;

  void use( void ) const { _count++; }
  unsigned int count( void ) const { return _count; }
  void reset_count( void ) const { _count = 0; }

  void track( const Memory & query ) const;

  bool operator==( const MemoryRange & other ) const;

  std::string str( void ) const;

  RemyBuffers::MemoryRange DNA( void ) const;
  MemoryRange( const RemyBuffers::MemoryRange & dna );

  friend size_t hash_value( const MemoryRange & mr );
};

#endif


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

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

#ifndef TXPL_TEST_SKIP_VM_EVAL_BINARY_BITAND

#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__bitand__char)
{
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = char_type{'\0'};
    const value<> v2 = char_type{'\0'};
    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{'\0'} & char_type{'\0'}));
  }
}
BOOST_AUTO_TEST_CASE(char__bitand__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = char_type{'\0'};
    const value<> v2 = int_type{0};
    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{'\0'} & int_type{0}));
  }
}
BOOST_AUTO_TEST_CASE(char__bitand__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = char_type{'\0'};
    const value<> v2 = bool_type{false};
    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{'\0'} & bool_type{false}));
  }
}
BOOST_AUTO_TEST_CASE(char__bitand__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = char_type{'\0'};
    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(char__bitand__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = char_type{'\0'};
    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{0} & char_type{'\0'}));
  }
}
BOOST_AUTO_TEST_CASE(int__bitand__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = int_type{0x1700};
    const value<> v2 = int_type{0x0134};
    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{0x1700} & int_type{0x0134}));
  }
}
BOOST_AUTO_TEST_CASE(int__bitand__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = bool_type{false};
    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{0} & bool_type{false}));
  }
}
BOOST_AUTO_TEST_CASE(int__bitand__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = int_type{0};
    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(int__bitand__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = char_type{'\0'};
    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{false} & bool_type{'\0'}));
  }
}
BOOST_AUTO_TEST_CASE(bool__bitand__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = int_type{0};
    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{false} & int_type{0}));
  }
}
BOOST_AUTO_TEST_CASE(bool__bitand__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = bool_type{false};
    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{false} & bool_type{false}));
  }
}
BOOST_AUTO_TEST_CASE(bool__bitand__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = bool_type{false};
    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(bool__bitand__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = real_type{0.0};
    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(real__bitand__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = real_type{0.0};
    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(real__bitand__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = real_type{0.0};
    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(real__bitand__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = real_type{0.0};
    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(real__bitand__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = string_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(string__bitand__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(r);
  {
    const value<> v1 = string_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(string__bitand__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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__bitand__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitand_>(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


#ifndef PYP_TOPICS_HH
#define PYP_TOPICS_HH

#include <vector>
#include <iostream>
#include <boost/ptr_container/ptr_vector.hpp>

#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/mersenne_twister.hpp>

#include "pyp.hh"
#include "corpus.hh"
#include "workers.hh"

class PYPTopics {
public:
  typedef std::vector<int> DocumentTopics;
  typedef std::vector<DocumentTopics> CorpusTopics;
  typedef long double F;

public:
  PYPTopics(int num_topics, bool use_topic_pyp=false, unsigned long seed = 0,
        int max_threads = 1, int num_jobs = 1)
    : m_num_topics(num_topics), m_word_pyps(1),
    m_topic_pyp(0.5,1.0,seed), m_use_topic_pyp(use_topic_pyp),
    m_seed(seed),
    uni_dist(0,1), rng(seed == 0 ? (unsigned long)this : seed),
    rnd(rng, uni_dist), max_threads(max_threads), num_jobs(num_jobs) {}

  void sample_corpus(const Corpus& corpus, int samples,
                     int freq_cutoff_start=0, int freq_cutoff_end=0,
                     int freq_cutoff_interval=0,
                     int max_contexts_per_document=0,
                     F temp_start=1.0, F temp_end=1.0);

  int sample(const DocumentId& doc, const Term& term, F inv_temp=1.0);
  std::pair<int,F> max(const DocumentId& doc, const Term& term) const;
  std::pair<int,F> max(const DocumentId& doc) const;
  int max_topic() const;

  void set_backoff(const std::string& filename) {
    m_backoff.reset(new TermBackoff);
    m_backoff->read(filename);
    m_word_pyps.clear();
    m_word_pyps.resize(m_backoff->order(), PYPs());
  }
  void set_backoff(TermBackoffPtr backoff) {
    m_backoff = backoff;
    m_word_pyps.clear();
    m_word_pyps.resize(m_backoff->order(), PYPs());
  }

  F prob(const Term& term, int topic, int level=0) const;
  void decrement(const Term& term, int topic, int level=0);
  void increment(const Term& term, int topic, int level=0);

  F log_likelihood() const;

  std::ostream& print_document_topics(std::ostream& out) const;
  std::ostream& print_topic_terms(std::ostream& out) const;

private:
  F word_pyps_p0(const Term& term, int topic, int level) const;

  int m_num_topics;
  F m_term_p0, m_topic_p0, m_backoff_p0;

  CorpusTopics m_corpus_topics;
  typedef boost::ptr_vector< PYP<int> > PYPs;
  PYPs m_document_pyps;
  std::vector<PYPs> m_word_pyps;
  PYP<int> m_topic_pyp;
  bool m_use_topic_pyp;

  unsigned long m_seed;

  typedef boost::mt19937 base_generator_type;
  typedef boost::uniform_real<> uni_dist_type;
  typedef boost::variate_generator<base_generator_type&, uni_dist_type> gen_type;

  uni_dist_type uni_dist;
  base_generator_type rng; //this gets the seed
  gen_type rnd; //instantiate: rnd(rng, uni_dist)
                //call: rnd() generates uniform on [0,1)

  typedef boost::function<F()> JobReturnsF;

  F hresample_docs(int start, int end); //does i in [start, end)

  F hresample_topics();

  int max_threads;
  int num_jobs;
  TermBackoffPtr m_backoff;
};

#endif // PYP_TOPICS_HH


#pragma once
/** @file KDTree
 * @brief A binary tree constructed by splitting each box by a median.
 */

#include <vector>
#include <algorithm>
#include <type_traits>
#include <iterator>

#include <iostream>
#include <iomanip>

#include <boost/range.hpp>
#include <boost/iterator/permutation_iterator.hpp>

#include "fmmtl/tree/util/CountedProxyIterator.hpp"

#include "fmmtl/util/Logger.hpp"
#include "fmmtl/numeric/Vec.hpp"
#include "fmmtl/tree/BoundingSphere.hpp"

#include "fmmtl/numeric/bits.hpp"

namespace fmmtl {
using boost::has_range_iterator;

//! Class for tree structure
template <unsigned DIM>
class BallTree {
  // Predeclarations
  struct Box;
  struct Body;
  struct BoxData;

  // The type of this tree
  typedef BallTree<DIM> tree_type;

 public:
  //! The type of indices and integers in this tree
  typedef unsigned size_type;

  //! The spacial point type used for centers and extents
  typedef Vec<DIM,double> point_type;

  //! Public type declarations
  typedef Box           box_type;
  typedef Body          body_type;
  using box_iterator  = CountedProxyIterator<Box,  const BallTree, size_type>;
  using body_iterator = CountedProxyIterator<Body, const BallTree, size_type>;

 private:
  // Tree representation

  // Permutation: permute_[i] is the current idx of originally ith point
  std::vector<size_type> permute_;
  // Vector of data describing a box
  std::vector<BoxData> box_data_;

  struct BoxData {
    typedef BoundingSphere<point_type> bounding_sphere_type;

    // Index of the first body in this box
    size_type body_begin_;
    // Index of one-past-last body in this box
    size_type body_end_;
    // Bounding sphere
    bounding_sphere_type bounding_sphere_;

    // Constructor
    BoxData(size_type bb, size_type be, const bounding_sphere_type& bs)
        : body_begin_(bb), body_end_(be), bounding_sphere_(bs) {}
  };

  struct Body {
    /** Construct an invalid Body */
    Body() {}
    //! The original order this body was seen
    size_type number() const {
      return tree_->permute_[idx_];
    }
    //! The current order of this body
    size_type index() const {
      return idx_;
    }
   private:
    size_type idx_;
    tree_type* tree_;
    friend body_iterator;
    Body(size_type idx, const tree_type* tree)
        : idx_(idx), tree_(const_cast<tree_type*>(tree)) {
      FMMTL_ASSERT(idx_ < tree_->size());
    }
    friend class BallTree;
  };

  // A box of the tree
  struct Box {
    typedef typename tree_type::box_iterator  box_iterator;
    typedef typename tree_type::body_iterator body_iterator;

    //! Construct an invalid Box
    Box() {}
    //! The index of this box
    size_type index() const {
      return idx_;
    }
    //! The level of this box (root level is 0)
    size_type level() const {
      return std::log2(idx_+1);  // XXX: Slow? do this with bits
    }

    //! The dimension of each side of this box
    point_type extents() const {
      point_type e;
      e.fill(2*std::sqrt(radius_sq()));
      return e;
    }
    //! The squared radius of this box
    double radius_sq() const {
      return data().bounding_sphere_.radius_sq();
    }
    //! The center of this box
    point_type center() const {
      return data().bounding_sphere_.center();
    }

    //! The parent box of this box
    Box parent() const {
      FMMTL_ASSERT(!(*this == tree_->root()));
      return Box((idx_-1)/2, tree_);
    }

    //! True if this box is a leaf and has no children
    bool is_leaf() const {
      return 2*idx_+1 >= tree_->box_data_.size();
    }
    //! The begin iterator to the child boxes contained in this box
    box_iterator child_begin() const {
      FMMTL_ASSERT(!is_leaf());
      return box_iterator(2*idx_+1, tree_);
    }
    //! The end iterator to the child boxes contained in this box
    box_iterator child_end() const {
      FMMTL_ASSERT(!is_leaf());
      return box_iterator(2*idx_+3, tree_);
    }
    //! The number of children this box has
    static constexpr size_type num_children() {
      return 2;
    }

    //! The begin iterator to the bodies contained in this box
    body_iterator body_begin() const {
      return body_iterator(data().body_begin_, tree_);
    }
    //! The end iterator to the bodies contained in this box
    body_iterator body_end() const {
      return body_iterator(data().body_end_, tree_);
    }
    //! The number of bodies this box contains
    size_type num_bodies() const {
      return std::distance(body_begin(), body_end());
    }

    //! Equality comparison operator
    bool operator==(const Box& b) const {
      FMMTL_ASSERT(tree_ == b.tree_);
      return idx_ == b.idx_;
    }
    //! Comparison operator for std:: containers and algorithms
    bool operator<(const Box& b) const {
      FMMTL_ASSERT(tree_ == b.tree_);
      return idx_ < b.idx_;
    }

    //! Write a Box to an output stream
    friend std::ostream& operator<<(std::ostream& s, const box_type& b) {
      size_type num_bodies = b.num_bodies();
      size_type first_body = b.body_begin()->index();
      size_type last_body = first_body + num_bodies - 1;
      size_type parent_idx = b.index()==0 ? 0 : b.parent().index();

      return s << "Box " << b.index()
               << " (L" << b.level() << ", P" << parent_idx
               << ", " << num_bodies << (num_bodies == 1 ? " body" : " bodies")
               << " " << first_body << "-" << last_body
               << "): Center: " << b.center()
               << "; Radius: " << std::sqrt(b.radius_sq());
    }

   private:
    size_type idx_;
    tree_type* tree_;
    friend box_iterator;
    Box(size_type idx, const tree_type* tree)
        : idx_(idx), tree_(const_cast<tree_type*>(tree)) {
      FMMTL_ASSERT(idx_ < tree_->boxes());
    }
    inline BoxData& data() const {
      return tree_->box_data_[idx_];
    }
    friend class BallTree;
  };

 public:

  /** Construct a tree encompassing a bounding box
   * and insert a range of points */
  template <typename Range>
  BallTree(const Range& rng, size_type n_crit = 256,
           typename std::enable_if<has_range_iterator<Range>::value>::type* = 0)
      : BallTree(rng.begin(), rng.end(), n_crit) {
  }

  /** Construct an tree encompassing a bounding box
   * and insert a range of points */
  template <typename PointIter>
  BallTree(PointIter first, PointIter last, size_type n_crit = 256) {
    insert(first, last, n_crit);
  }

  /** Return the Bounding Box that this BallTree encompasses */
  BoundingSphere<point_type> bounding_sphere() const {
    return box_data_[0].bounding_sphere_;
  }

  /** Return the center of this BallTree */
  point_type center() const {
    return root().center();
  }

  /** The number of bodies contained in this tree */
  inline size_type size() const {
    return permute_.size();
  }
  /** The number of bodies contained in this tree */
  inline size_type bodies() const {
    return size();
  }

  /** The number of boxes contained in this tree */
  inline size_type boxes() const {
    return box_data_.size();
  }

  /** The number of boxes contained in level L of this tree */
  inline size_type boxes(size_type L) const {
    return (1 << L);
  }

  /** The maximum level of any box in this tree */
  inline size_type levels() const {
    return std::log2(boxes());  // XXX: Slow?
  }

  /** Returns true if the box is contained in this tree, false otherwise */
  inline bool contains(const box_type& box) const {
    return this == box.tree_;
  }
  /** Returns true if the body is contained in this tree, false otherwise */
  inline bool contains(const body_type& body) const {
    return this == body.tree_;
  }

  /** Return the root box of this tree */
  box_type root() const {
    return Box(0, this);
  }
  /** Return a box given its index */
  box_type box(const size_type idx) const {
    FMMTL_ASSERT(idx < box_data_.size());
    return Box(idx, this);
  }
  /** Return a body given its index */
  body_type body(const size_type idx) const {
    FMMTL_ASSERT(idx < size());
    return Body(idx, this);
  }
  /** Return an iterator to the first body in this tree */
  body_iterator body_begin() const {
    return body_iterator(0, this);
  }
  /** Return an iterator one past the last body in this tree */
  body_iterator body_end() const {
    return body_iterator(bodies(), this);
  }
  /** Return an iterator to the first box in this tree */
  box_iterator box_begin() const {
    return box_iterator(0, this);
  }
  /** Return an iterator one past the last box in this tree */
  box_iterator box_end() const {
    return box_iterator(boxes(), this);
  }
  /** Return an iterator to the first box at level L in this tree
   * @pre L < levels()
   */
  box_iterator box_begin(size_type L) const {
    FMMTL_ASSERT(L < levels());
    return box_iterator((1 << L) - 1, this);
  }
  /** Return an iterator one past the last box at level L in this tree
   * @pre L < levels()
   */
  box_iterator box_end(size_type L) const {
    FMMTL_ASSERT(L < levels());
    return box_iterator((1 << (L+1)) - 1, this);
  }

  template <typename RandomAccessIter>
  struct body_permuted_iterator {
    typedef typename std::vector<size_type>::const_iterator permute_iter;
    typedef boost::permutation_iterator<RandomAccessIter, permute_iter> type;
  };

  /** Tranform (permute) an iterator so its traversal follows the same order as
   * the bodies contained in this tree
   */
  template <typename RandomAccessIter>
  typename body_permuted_iterator<RandomAccessIter>::type
  body_permute(RandomAccessIter it, const body_iterator& bi) const {
    return boost::make_permutation_iterator(it, permute_.cbegin() + bi.index());
  }

  /** Tranform (permute) an iterator so its traversal follows the same order as
   * the bodies contained in this tree
   *
   * Specialized for bi = body_begin().
   */
  template <typename RandomAccessIter>
  typename body_permuted_iterator<RandomAccessIter>::type
  body_permute(RandomAccessIter it) const {
    return body_permute(it, body_begin());
  }

  /** Write a BallTree to an output stream */
  friend std::ostream& operator<<(std::ostream& s, const BallTree& t) {
    struct {
      std::ostream& print(std::ostream& s, const box_type& box) {
        s << std::string(2*box.level(), ' ') << box;
        if (!box.is_leaf())
          for (auto ci = box.child_begin(); ci != box.child_end(); ++ci)
            print(s << "\n", *ci);
        return s;
      }
    } recursive_box;

    return recursive_box.print(s, t.root());
  }

 private:
  //! TODO: Make dynamic and public?
  template <typename PointIter>
  void insert(PointIter p_first, PointIter p_last, size_type NCRIT) {
    FMMTL_LOG("BallTree Insert");

    assert(p_first != p_last);

    // Create a point-idx pair vector
    typedef typename std::iterator_traits<PointIter>::value_type point_i_type;

    // XXX: Generalize?
    static_assert(std::is_same<point_i_type, point_type>::value,
                  "PointIter value_type must be point_type");

    typedef std::pair<point_type, unsigned> point_t;

    std::vector<point_t> point;
    // If iterators are random access, we can reserve space efficiently
    if (std::is_same<typename std::iterator_traits<PointIter>::iterator_category,
        std::random_access_iterator_tag>::value)
      point.reserve(std::distance(p_first, p_last));

    // Copy to point-idx pair
    unsigned idx = 0;
    for (PointIter pi = p_first; pi != p_last; ++pi, ++idx) {
      point.emplace_back(*pi, idx);
    }
    permute_.reserve(point.size());

    // Helper std::pair projection operator
    struct pair2point {
      point_type& operator()(point_t& p) const { return p.first; }
    };
    using proj = boost::transform_iterator<pair2point, decltype(point.begin())>;

    // The number of leaf boxes that will be created
    // (Smallest power of two greater than or equal to ceil(N/NCRIT))
    unsigned leaves = ceil_pow_2((point.size() + NCRIT - 1) / NCRIT);
    unsigned levels = std::log2(leaves);

    // Reserve the number of boxes that will be added
    box_data_.reserve(2*leaves - 1);

    // Push the root sphere which contains all points
    box_data_.emplace_back(0, size_type(point.size()),
                           approx_bounding_sphere(proj(point.begin()),
                                                  proj(point.end())));

    // For every box that is created
    unsigned end_k = (1 << levels) - 1;
    for (unsigned k = 0; k < end_k; ++k) {
      // The range of points in this box
      auto p_begin = point.begin() + box_data_[k].body_begin_;
      auto p_end   = point.begin() + box_data_[k].body_end_;

      // Invariant of helper functions
      assert(p_begin != p_end);

      // Compute the furthest point from the center
      const point_type& p1 = furthest_point_from(proj(p_begin), proj(p_end),
                                                 box(k).center());
      // Compute the furthest point from point1
      const point_type& p2 = furthest_point_from(proj(p_begin), proj(p_end),
                                                 p1);

      // Partition on the median: balanced tree, but worse spheres
      auto p_mid = p_begin + (p_end - p_begin) / 2;
      point_type r = p2 - p1;
      std::nth_element(p_begin, p_mid, p_end,
                       [&](const point_t& a, const point_t& b) {
           return inner_prod(a.first-p1, r) < inner_prod(b.first-p1, r);
        });

      /*
      // Partition on proximity: better spheres, potentially unbalanced
      // Would need to account for the possibility of empty boxes
      auto p_mid = std::partition(p_begin, p_end, [&] (const point_t& a) {
          return norm_2_sq(a.first-p1) < norm_2_sq(a.first-p2);
        });
      */

      // Record the child boxes
      size_type mid = p_mid - point.begin();
      box_data_.emplace_back(box_data_[k].body_begin_, mid,
                             approx_bounding_sphere(proj(p_begin), proj(p_mid)));

      box_data_.emplace_back(mid, box_data_[k].body_end_,
                             approx_bounding_sphere(proj(p_mid), proj(p_end)));
    }

    // Assert no re-allocation
    assert(box_data_.size() <= 2*leaves-1);

    // Extract the permutation idx
    for (const point_t& p : point)
      permute_.push_back(p.second);
  }

  /** Calculate the approximate bounding sphere of a set of points
   * using the Points Closest to Furthest Pair approach.
   */
  template <typename PointIter>
  BoundingSphere<point_type> approx_bounding_sphere(PointIter first,
                                                    PointIter last) {
    auto n = std::distance(first, last);
    point_type center = std::accumulate(first, last, point_type{}) / n;

    double r_sq = std::accumulate(first, last, double{},
                                  [&] (double r_sq, point_type& p) {
                                    return std::max(r_sq, norm_2_sq(center-p));
                                  });

    return {center, r_sq};
  }

  /** Find the point in a set of points that is furthest away from a target point
   */
  template <typename PointIter>
  const point_type& furthest_point_from(PointIter first,
                                        PointIter last,
                                        const point_type& target) {
    using pair = std::pair<double, const point_type*>;
    return *std::accumulate(first, last, pair{},
                            [&] (const pair& r, const point_type& p) {
                              return std::max(r, pair{norm_2_sq(target-p), &p});
                            }).second;
  }

  // Just making sure for now
  BallTree(const BallTree&) {};
  void operator=(const BallTree&) {};
};

} // end namespace fmmtl


// Copyright 2009-2010 Green Code LLC
//
// 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.
//
// CHANGELOG:
//
// 05/09/19: Modified by George E. Hilley to use OpenMP for performance enhancements.
//	        This required eliminating use of BOOST_FOREACH in interpolation loop.

#include <cmath>
#include <iostream>
#include <stdlib.h>

#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <omp.h>

#include "DisjointRegions.h"
#include "IInterpolationRegion.h"
#include "IPointVector.h"
#include "IRegionGenerator.h"
#include "LineIndent.h"
#include "Point.h"
#include "ProgressBar.h"
#include "RasterSurface.h"
#include "RegularizedSpline.h"
#include "SplineExceptions.h"
#include "SurfaceInterpolation.h"
#include "XYCoordinates.h"

namespace mcc
{

  int pointIncrement = 0;
  int lastPointSelected = 0;

  // A point selector that selects every point.
  bool useEveryPoint(const IPoint & point)
  {
    return true;
  }

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

  SurfaceInterpolation::SurfaceInterpolation() :
    prevCellResolution_(0)
  {
  }

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

  boost::shared_ptr<IRasterSurface> SurfaceInterpolation::operator()(const IPointVector & points,
                                                                     double               cellResolution,
                                                                     double               tension)
  {
      std::cout << "Using all points." << std::endl;
      return this->operator()(points, &useEveryPoint, cellResolution, tension);
  }

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

  boost::shared_ptr<IRasterSurface> SurfaceInterpolation::operator()(const IPointVector & points,
                                                                     PointSelector        pointSelector,
                                                                     double               cellResolution,
                                                                     double               tension)
  {
    // If the cell resolution is different from the previous call, then
    // create a new raster based on the new cell size.
    if (cellResolution != prevCellResolution_) {
      // Determine the desired raster dimensions by adding a 1/2 cell-wide
      // margin around the boundaries of the point cloud read by the program.
      double margin = cellResolution / 2;
      double desiredWidth  = (inputExtent_.maxX - inputExtent_.minX) + 2 * margin; // left & right margins
      double desiredHeight = (inputExtent_.maxY - inputExtent_.minY) + 2 * margin; // top & bottom margins

      // Compute the numer of rows & columns needed to minimally cover the
      // desired raster dimensions.
      unsigned int cols = int(std::ceil(desiredWidth  / cellResolution));
      unsigned int rows = int(std::ceil(desiredHeight / cellResolution));

      // Determine the lower-left corner of the raster by centering its actual
      // area around the original input extent.
      double actualWidth = cols * cellResolution;
      double horizontalMargin = (actualWidth - desiredWidth) / 2;
      Coordinate x0 = Coordinate(inputExtent_.minX - horizontalMargin);

      double actualHeight = rows * cellResolution;
      double verticalMargin = (actualHeight - desiredHeight) / 2;
      Coordinate y0 = Coordinate(inputExtent_.minY - verticalMargin);

      XYCoordinates lowerLeft(x0, y0);

      rasterSurface_ = boost::make_shared<RasterSurface>(rows, cols, lowerLeft, Coordinate(cellResolution));
      prevCellResolution_ = cellResolution;
    }

    // Determine where splines will be interpolated for the points and the
    // raster.
    boost::shared_ptr<IRegionGenerator> regions = boost::make_shared<DisjointRegions>();
    int nRegions = regions->subdivide(points, pointSelector, *rasterSurface_);

    rasterSurface_->setNoDataValue(-9999);

    LineIndent indent("  ");

    // For each region, compute a spline for its points and then interpolate
    // heights for its cells.
    std::cout << indent << "Computing splines for regions and cell heights for raster surface:" << std::endl
              << indent << "  ";
    //ProgressBar progressBar(std::cout, nRegions);
    int nSplinesComputed = 0;

    int sj, sstop, tn, tj;

    sj = -1;     // shared loop counter
    sstop = 0;   // shared stopping condition
    const IInterpolationRegion *region; // Region, declared as private below

    #pragma omp parallel private(tn,tj,region)
    {
      tn = omp_get_thread_num();
      while (!sstop)
      {
        /* Threads update the shared counter by turns */
        #pragma omp critical
        {
          sj++;      // increment the shared loop counter...
          tj = sj;   // ...and keep a private copy of it
        }

        const Cell *cell = regions->getNextCell();
        if(!cell) {
          sstop = 1;
          #pragma omp flush(sstop)
        } else {
          region = regions->getRegionForCell(cell);
          bool splineComputed = false;
          while (! splineComputed) {
            try {
              std::vector<const IPoint *> points;
              std::vector<Cell> cells;
              regions->getPointsAndCellsForCell(cell, 0, points, cells);
              if(points.size() >= 3) {
                RegularizedSpline spline(points, 0.0);
                splineComputed = true;

            	  for(std::vector<Cell>::size_type i = 0; i < cells.size(); i++) {
            	    (*rasterSurface_)[cells[i]] = spline.interpolateHeight(cells[i].x(), cells[i].y());
            	  }
              } else {
                std::cout << "did not compute. Fewer than 3 points." << std::endl;
              }
            }
            catch (SingularMatrixException) {
              std::cout << indent << "Caught singular matrix for spline" << std::endl;
              // Add another neighboring point and try the spline calculation again.

              // Add neighboring points needs to be written in terms of grid location.
              std::vector<const IPoint *> points;
              std::vector<Cell> cells;
              regions->getPointsAndCellsForCell(cell, 1, points, cells);

              // A safety check to prevent an endless loop from consuming all the
              // point cloud.
              if (points.size() >= 300)
                throw;  // Bail
            }
          }
          nSplinesComputed++;
          //std::cout << "updating progress bar" << std::endl;
          //progressBar.update(nSplinesComputed);
          //std::cout << "updated progress bar" << std::endl;
        }
        /* When sstop=1, most threads continue to this statment */
      }
    }

    std::cout << std::endl;

    std::cout << "returning from interpolation." << std::endl;

    return rasterSurface_;
  }
}


//
// Created by roy_shilkrot on 8/27/18.
//

#include "opencv2/face.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/objdetect.hpp"
#include "opencv2/videoio.hpp"
#include <boost/format.hpp>
#include <fstream>
#include <iostream>
#include <istream>
#include <opencv2/calib3d.hpp>
#include <string>
#include <vector>

using namespace std;
using namespace cv;
using namespace cv::face;
using $ = boost::format;

void faceDetector(const Mat& image, std::vector<Rect>& faces, CascadeClassifier& face_cascade)
{
    Mat gray;

    // The cascade classifier works best on grayscale images
    if (image.channels() > 1) {
        cvtColor(image, gray, COLOR_BGR2GRAY);
    } else {
        gray = image.clone();
    }

    // Histogram equalization generally aids in face detection
    equalizeHist(gray, gray);

    faces.clear();

    // Run the cascade classifier
    face_cascade.detectMultiScale(
        gray, faces, 1.4, 3, CASCADE_SCALE_IMAGE + CASCADE_FIND_BIGGEST_OBJECT);
}

// Read the annotation file for a frame in the 300-VW dataset: 68 facial landmarks
vector<Point2f> readAnnotationFile(const string& file)
{
    ifstream in(file);
    string line;
    for (int i = 0; i < 3; ++i) { // Roll 3 lines for the header
        getline(in, line);
    }

    vector<Point2f> points;
    while (getline(in, line)) {
        stringstream l(line);
        Point2f p;
        l >> p.x >> p.y; // Each line contains a single point
        if (p.x != 0.0 and p.y != 0.0) {
            points.push_back(p);
        }
    }
    return points;
}

float MeanEuclideanDistance(const vector<Point2f>& A, const vector<Point2f>& B)
{
    float med = 0.0f;
    for (int i = 0; i < A.size(); ++i) {
        med += cv::norm(A[i] - B[i]);
    }
    return med / (float)A.size();
}

vector<Point3f> objectPoints {
    { 8.27412, 1.33849, 10.63490 }, // left eye corner
    { -8.27412, 1.33849, 10.63490 }, // right eye corner
    { 0, -4.47894, 17.73010 }, // nose tip
    { -4.61960, -10.14360, 12.27940 }, // right mouth corner
    { 4.61960, -10.14360, 12.27940 }, // left mouth corner
};
vector<Point3f> objectPointsForReprojection {
    objectPoints[2], // nose
    objectPoints[2] + Point3f(0, 0, 15), // nose and Z-axis
    objectPoints[2] + Point3f(0, 15, 0), // nose and Y-axis
    objectPoints[2] + Point3f(15, 0, 0) // nose and X-axis
};

// Facial landmarks IDs (from the 68) to match the 3D points
vector<int> landmarksIDsFor3DPoints { 45, 36, 30, 48, 54 }; // 0-index

int main(int argc, char** argv)
{
    // Give the path to the directory containing all the files containing data
    CommandLineParser parser(argc, argv,
        "{ help h usage ?    |      | give the following arguments in following format }"
        "{ model_filename f  |      | (required) path to binary file storing the trained model "
        "which is to be loaded [example - /data/file.dat]}"
        "{ v vid_base        |      | (required) path to directory with video and facemark "
        "annotations.[example - /data/vid1/] }"
        "{ face_cascade c    |      | Path to the face cascade xml file which you want to use as a "
        "detector}");
    // Read in the input arguments
    if (parser.has("help")) {
        parser.printMessage();
        cerr << "TIP: Use absolute paths to avoid any problems with the software!" << endl;
        return 0;
    }
    string filename(parser.get<string>("model_filename"));
    if (filename.empty()) {
        parser.printMessage();
        cerr << "The name  of  the model file to be loaded for detecting landmarks is not found"
             << endl;
        return -1;
    }
    string vid_base(parser.get<string>("vid_base"));
    if (vid_base.empty()) {
        parser.printMessage();
        cerr << "The video base dir name is not found" << endl;
        return -1;
    }
    string cascade_name(parser.get<string>("face_cascade"));
    if (cascade_name.empty()) {
        parser.printMessage();
        cerr << "The name of the cascade classifier to be loaded to detect faces is not found"
             << endl;
        return -1;
    }

    Mat img_orig;
    VideoCapture v(vid_base + "/vid.avi");

    if (not v.isOpened()) {
        cerr << "Cannot open video" << endl;
        return -1;
    }

    v >> img_orig;
    if (img_orig.empty()) {
        cerr << "Cannot read video" << endl;
        return -1;
    }

    cout << "original image size " << img_orig.size() << endl;

    // pass the face cascade xml file which you want to pass as a detector
    CascadeClassifier face_cascade;
    if (not face_cascade.load(cascade_name)) {
        cerr << "Cannot load cascade classifier: " << cascade_name << endl;
        return -1;
    }

    Ptr<Facemark> facemark = createFacemarkLBF();
    facemark->loadModel(filename);
    cout << "Loaded facemark LBF model" << endl;

    Size small_size(700, 700 * (float)img_orig.rows / (float)img_orig.cols);
    const float scaleFactor = 700.0f / img_orig.cols;

    const float w = small_size.width, h = small_size.height;
    Matx33f K { w, 0, w / 2.0f, 0, w, h / 2.0f, 0, 0, 1.0f };

    Mat img, img_out, img_out_dir;
    Mat rvec = Mat::zeros(3, 1, CV_64FC1);
    Mat tvec = Mat::zeros(3, 1, CV_64FC1);
    Mat R = Mat::eye(3, 3, CV_64FC1);
    Rodrigues(R, rvec);

    for (;;) {
        v >> img_orig;
        if (img_orig.empty()) {
            break;
        }

        const uint32_t frameId = v.get(CAP_PROP_POS_FRAMES);
        const string filename = str($(vid_base + "/annot/%06d.pts") % frameId);
        const vector<Point2f> ground_truth = readAnnotationFile(filename);
        Mat(ground_truth) *= scaleFactor;

        resize(img_orig, img, small_size, 0, 0, INTER_LINEAR_EXACT);
        img.copyTo(img_out);
        img.copyTo(img_out_dir);

        // Draw the ground truth landmarks
        drawFacemarks(img_out, ground_truth, cv::Scalar(0, 255));

        vector<Rect> faces;
        faceDetector(img, faces, face_cascade);

        // Check if faces detected or not
        if (faces.size() != 0) {
            // We assume a single face so we look at the first only
            cv::rectangle(img_out, faces[0], Scalar(255, 0, 0), 2);

            vector<vector<Point2f>> shapes;

            if (facemark->fit(img, faces, shapes)) {
                // Draw the detected landmarks
                drawFacemarks(img_out, shapes[0], cv::Scalar(0, 0, 255));

                putText(img_out,
                    str($("MED: %.3f") % MeanEuclideanDistance(shapes[0], ground_truth)),
                    { 10, 30 }, FONT_HERSHEY_COMPLEX, 0.75, Scalar(0, 255, 0), 2);

                // Collect 2D landmarks for 2D-3D correspondence
                vector<Point2f> points2d;
                for (int pId : landmarksIDsFor3DPoints) {
                    points2d.push_back(shapes[0][pId] / scaleFactor);
                }

                // Find object/camera transform
                solvePnP(objectPoints, points2d, K, Mat(), rvec, tvec, true);

                // Reproject the axes back onto the image
                vector<Point2f> projectionOutput(objectPointsForReprojection.size());
                projectPoints(objectPointsForReprojection, rvec, tvec, K, Mat(), projectionOutput);
                Mat(projectionOutput) *= scaleFactor;

                // Draw head direction axes
                arrowedLine(img_out_dir, projectionOutput[0], projectionOutput[1],
                    Scalar(255, 255, 0), 2, 8, 0, 0.3);
                arrowedLine(img_out_dir, projectionOutput[0], projectionOutput[2],
                    Scalar(0, 255, 255), 2, 8, 0, 0.3);
                arrowedLine(img_out_dir, projectionOutput[0], projectionOutput[3],
                    Scalar(255, 0, 255), 2, 8, 0, 0.3);
            }
        } else {
            cout << "Faces not detected." << endl;
        }

        if (frameId % 10 == 0) {
            imwrite(($("orig%03d.jpg") % frameId).str(), img_orig);
            imwrite(($("out%03d.jpg") % frameId).str(), img_out);
            imwrite(($("dir%03d.jpg") % frameId).str(), img_out_dir);
        }

        imshow("input", img_out);
        if (waitKey(30) == 27) {
            break;
        }
    }

    v.release();

    return 0;
}


#include <string>
#include <sstream>
#include <algorithm>
#include <iterator>
#include <string.h>
#include <stdlib.h>
#include <iostream>
#include <map>
#include <set>
#include <bitset>
#include <pthread.h>
#include <fstream>
#include <climits>
#include <random>
#include <algorithm>
#include <unordered_set>
#include <unordered_map>
#include <boost/threadpool.hpp>
#include <cstring>
#include <unistd.h>
#include <stdio.h>
#include <sys/types.h>

#define NUM_NODES 100000//0
#define BUFFERSIZE 512
#define THREADS 32

#define NUMBER_WALKS 100
#define LENGTH_WALKS 20

using namespace std;
using namespace boost::threadpool;

struct Edge {
  unsigned int edge ;
  unsigned int node ;
} ;

unordered_map<unsigned int, vector<Edge>> graph ;

random_device rd;
mt19937 rng(rd());
uniform_int_distribution<int> uni(0,INT_MAX);

ofstream fout;
boost::mutex mtx;

int num_nodes = NUM_NODES;
int threads = THREADS;
int number_walks = NUMBER_WALKS;
int length_walks = LENGTH_WALKS;

void build_graph(string fname) {
  char buffer[BUFFERSIZE];
  graph.reserve(NUM_NODES) ;
  ifstream in(fname);
  cout << "ifstreaming";
  while(in) {
    in.getline(buffer, BUFFERSIZE);
    if(in) {
      Edge e ;
      unsigned int source = atoi(strtok(buffer, " "));
      e.node = atoi(strtok(NULL, " ")) ;
      e.edge = atoi(strtok(NULL, " ")) ;
      graph[source].push_back(e) ;
    }
  }
}

void walk(unsigned int source) {
  vector<vector<unsigned int>> walks(number_walks) ;
  if (graph[source].size()>0) { // if there are outgoing edges at all
    for (int i = 0 ; i < number_walks ; i++) {
      int count = length_walks ;
      int current = source ;
      walks[i].push_back(source) ;
      while (count > 0) {
	if (graph[current].size() > 0 ) { // if there are outgoing edges
	  unsigned int r = uni(rng) % graph[current].size();
	  Edge next = graph[current][r] ;
	  int target = next.node ;
	  int edge = next.edge ;
	  walks[i].push_back(edge) ;
	  walks[i].push_back(target) ;
	  current = target ;
	} else {
	  int edge = INT_MAX ; // null edge
	  current = source ;
	  walks[i].push_back(edge) ;
	  walks[i].push_back(current) ;
	}
	count-- ;
      }
    }
  }
  stringstream ss;
  for(vector<vector<unsigned int>>::iterator it = walks.begin(); it != walks.end(); ++it) {
    for(size_t i = 0; i < (*it).size(); ++i) {
      if(i != 0) {
	ss << " ";
      }
      ss << (*it)[i];
    }
    ss << "\n" ;
  }
  mtx.lock() ;
  fout << ss.str() ;
  fout.flush() ;
  mtx.unlock() ;
}

void generate_corpus() {
  pool tp(threads);
  for ( auto it = graph.begin(); it != graph.end(); ++it ) {
    unsigned int source = it -> first ;
    tp.schedule(boost::bind(&walk, source ) ) ;
  }
  cout << tp.pending() << " tasks pending." << "\n" ;
  tp.wait() ;
}

int main (int argc, char *argv[]) {
  cout << "Building graph from " << argv[1] << "\n";
  build_graph(argv[1]);
  num_nodes = graph.size();
  cout << "Number of nodes in graph: " << num_nodes << "\n" ;
  cout << "Writing walks to " << argv[2] << "\n" ;

  if (argc > 3) {
    number_walks = strtol(argv[3], nullptr, 0);
  }
  if (argc > 4) {
    length_walks = strtol(argv[4], nullptr, 0);
  }
  if (argc > 5) {
    threads = strtol(argv[5], nullptr, 0);
  }

  cout << "Number of walks to " << number_walks << "\n" ;
  cout << "Walk length to " << length_walks << "\n" ;
  cout << "Threads to " << threads << "\n" ;
  fout.open(argv[2]) ;
  generate_corpus() ;
  fout.close() ;
}


#include "board.h"
#include "count_reader.h"
#include <fstream>
#include <chrono>
#include <vector>
#include <boost/program_options.hpp>
namespace po = boost::program_options;
typedef std::vector<int64_t> vL;
typedef std::vector<vL> vvL;

/*
  0 - unknown
  1 - (black turn) black cannot move (brown win)
  2 - (brown turn) brown has move to state 1
 2*n - (brown turn) brown has move to state 2*n - 1
 2*n + 1 - (black turn) all black move lead to state 2*m (m <= n)
 */

template<int SIZE, int capture_type=0>
class CountSolve {
public:
  vvL ret;
  vvL zero_turn;
  const size_t BLOCK_SIZE = 0x10000;
  CountSolve() :ret(SIZE, vL(100)), zero_turn(SIZE, vL(2)) {}
  void count() {
    uint64_t all = 0ull;
    CountReader<SIZE, 0> cr;
    for (size_t i = 0; i < CountReader<SIZE, capture_type>::table_size(); i += BLOCK_SIZE) {
      if (i % 0x100000000ull == 0) {
	std::cerr << "i=" << i << std::endl;
      }
      char buffer[BLOCK_SIZE];
      size_t read_size = std::min(BLOCK_SIZE, CountReader<SIZE, capture_type>::table_size() - i);
      cr.ifs.read(&buffer[0], read_size);
      for (size_t j = 0; j < read_size; j++) {
	Board<SIZE> b(i + j);
	bool debug_flag = b.browns_size() == 24;
	Board<SIZE> b1 = b.normalize();
	if (debug_flag) {
	  std:: cerr << "b=" << b << ",b1=" << b1 << std::endl;
	  if (b1.v < i + j) {
	    std::cerr << "prune" << std::endl;
	  }
	}
	if (b1.v < i + j) {
	  // std:: cerr << "i+j) = " << (i + j) << ",b=" << b << ",b1=" << b1 << std::endl;
	  continue;
	}
	int c = b.browns_size();
#if 0
	if (c == 0 && j == 100) {
	  std:: cerr << "i+j) = " << (i + j) << ",b=" << b << ",b.browns_size() = " << b.browns_size() << std::endl;
	}
#endif
	int t = buffer[j];
	ret[c][t]++;
	all++;
	if (t == 0) {
	  int turn = b.turn();
	  zero_turn[c][turn]++;
	}
      }
    }
    std::cout << "all=" << all << std::endl;
    for (int i = 0; i < SIZE; i++) {
      std::cout << i << "," << zero_turn[i][0] << "," << zero_turn[i][1];
      for (size_t j = 1; j < 70; j++)
	std::cout << "," << ret[i][j];
      std::cout << std::endl;
    }
  }
};

int main(int ac, char **ag) {
  int board_size;
  int capture_type;

  po::options_description options("all_options");
  options.add_options()
    ("board-size,n",
     po::value<int>(&board_size)->default_value(25),
     "board size (25, 31, 33)")
    ("capture-type,t",
     po::value<int>(&capture_type)->default_value(0),
     "capture type : 0 (ALL), 1(any corner), 2(one corner), 3(1,0), 4(2,0)")
    ;
  po::variables_map vm;
  try
    {
      po::store(po::parse_command_line(ac, ag, options), vm);
      po::notify(vm);
    }
  catch (std::exception& e)
    {
      std::cerr << "error in parsing options" << std::endl
		<< e.what() << std::endl;
      std::cerr << options << std::endl;
      return 1;
    }
  if (vm.count("help")) {
    std::cerr << options << std::endl;
    return 0;
  }
  if (board_size == 25) {
    if (capture_type == 0)
      CountSolve<25, 0>().count();
    else if (capture_type == 1)
      CountSolve<25, 1>().count();
    else if (capture_type == 2)
      CountSolve<25, 2>().count();
    else if (capture_type == 3)
      CountSolve<25, 3>().count();
    else if (capture_type == 4)
      CountSolve<25, 4>().count();
    else {
      std::cerr << options << std::endl;
      return 0;
    }
  }
  else if (board_size == 31) {
    if (capture_type == 0)
      CountSolve<31, 0>().count();
  }
  else if (board_size == 33) {
    if (capture_type == 0)
      CountSolve<33, 0>().count();
  }
  else {
    std::cerr << options << std::endl;
    return 0;
  }
}


#include "prediction_bhmip.h"
#include "scene_elements/person_bhmip.h"
#include <math.h>
#include <algorithm>
#include <stdio.h>
#include <Eigen/Dense>
#include <iostream>

Cprediction_bhmip::Cprediction_bhmip( Cperson_abstract::filtering_method filter,
		Cperson_abstract::update_person_method update_method ) :
		Cscene_abstract(filter, update_method, Cperson_abstract::Spherical)
{
	//read destinations from txt
	//this->set_destinations(dest);
}

Cprediction_bhmip::~Cprediction_bhmip()
{
	for( std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit != person_list_.end() ; iit++ )
	{
		delete (*iit);
	}
	delete robot_;
}

void Cprediction_bhmip::update_scene(const std::vector<SdetectionObservation>& observation )
{
	// the filter bool sets if we want to filter velocities or use the provided in the update.
	//associate current targets with tracking observations
	Cperson_abstract* found_person;
	SdetectionObservation  point;
	for(unsigned int i=0 ; i< observation.size() ; ++i)
	{
		//std::cout << "iteration" << i << std::endl;
		if( find_person( observation[i].id , &found_person ) )
		{
			//std::cout << "updating target after observation " << found_person->is_observation_updated() << std::endl;
			point = observation[i];
			found_person->add_pointV( point, filtering_method_);
			now_ = observation[i].time_stamp;
		}
		//new targets
		else
		{
			found_person = add_person( observation[i].id );
			//std::cout << "new person :" << found_person->get_id() << std::endl;
			point = observation[i];
			found_person->add_pointV( point , filtering_method_);
			now_ = observation[i].time_stamp;
		}
	}

	//Remove persons
	std::vector<unsigned int> removed_targets;
	for(std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit!=person_list_.end() ; iit++)
	{
		//std::cout << "analyzing target " << (*iit)->is_observation_updated() << std::endl;
		if ( (*iit)->is_observation_updated() )
		{
			(*iit)->set_observation_update( false );//resets the observation flag until next update
			//std::cout << " updating target " << (*iit)->is_observation_updated() << std::endl;
		}
		else
		{
			removed_targets.push_back((*iit)->get_id());
			//std::cout << "trying to remove target " << (*iit)->get_id() << std::endl;
		}
	}
	for ( unsigned int i = 0; i < removed_targets.size() ; ++i)
	{
		if( update_method_ == Cperson_abstract::Autoremove )
		{
			remove_person(removed_targets[i]);
		}
		else
		{
			if( find_person(removed_targets[i], &found_person ) )
				found_person->refresh_person(now_);
		}
	}
	//else remove MUST be set manually outside the update function, each time the user wants to remove a person

}

void Cprediction_bhmip::rotate_and_traslate_scene(unsigned int id, double R, double thetaZ, double linear_vx, double linear_vy, double v_rot_x, double v_rot_y, std::vector<double> vect_odom_eigen_tf, bool debug)
{ 	// Function for local tracking. This function change the frame of the Current_pose and the window of poses for a concrete person (id), tacking into account the actual position of the robot.
	//  id ->person id ; R -> Robot Translation ; thetaZ -> Robot rotation

	Cperson_abstract* found_person;
	if( find_person( id , &found_person ) )
	{
		found_person->rotate_and_translate_trajectory(R,thetaZ,linear_vx,linear_vy,v_rot_x,v_rot_y, vect_odom_eigen_tf,debug);
	}
}

Cperson_abstract* Cprediction_bhmip::add_person_container( unsigned int id,
		std::list<Cperson_abstract*>::iterator iit )
{
	Cperson_bhmip* person = new Cperson_bhmip(id,Cperson_abstract::Person, scene_force_type_, filtering_time_window_);

	person_list_.insert( iit , person );
	person->set_destinations(destinations_);

	return (Cperson_abstract*) person;
}

void Cprediction_bhmip::scene_intentionality_prediction_bhmip()
{
	for( std::list<Cperson_abstract*>::iterator iit = person_list_.begin() ; iit != person_list_.end() ;  iit++ )
	{
		(*iit)->prediction( min_v_to_predict_);
	}

}

void Cprediction_bhmip::clear_scene()
{

}


#include "ann/iann_controller.h"
#include "sync_controller.h"
#include "async_controller.h"
#include "shuffled_range.h"
#include <boost/range/algorithm/copy.hpp>
#include "utils.h"

namespace ann {

IANNControllerPtr createController(ControllerType t)
{
    std::unique_ptr<IANNController> result;
    switch (t)
    {
    case ControllerType::SYNC:
        result.reset(new sync::SyncController());
        break;

    case ControllerType::ASYNC:
        result.reset(new async::AsyncController());
        break;
    }

    return result;
}

math::MatrixF getImageDataBasedOnRange(const db::DBInterface &db, const ShuffledRange::IndexedRange &range)
{
    const auto rowCount = db.getPixelCount();
    auto result = math::make_matrix<float>(rowCount, range.size());

    for (size_t i = 0; i < range.size(); ++i)
    {
        auto image = db.getImageMatrix(range[i]);
        boost::copy(image, result.columnBegin(i));
    }
    return result;
}

math::MatrixF getImageClassBasedOnRange(const db::DBInterface &db, const ShuffledRange::IndexedRange &range)
{
    const auto rowCount = 10;
    auto result = math::make_matrix<float>(rowCount, range.size());

    for (size_t i = 0; i < range.size(); ++i)
    {
        auto label = db.getImageLabel(range[i]);
        boost::copy(label, result.columnBegin(i));
    }
    return result;
}

}


// integrators/adaptive.cpp*
#include "adaptive.h"
#include "camera.h"
#include "filters/box.h"
#include "paramset.h"
#include "progressreporter.h"
#include "stats.h"
#include <boost/math/special_functions/erf.hpp>

namespace pbrt {

STAT_COUNTER("Integrator/Camera rays traced", nCameraRays);
STAT_COUNTER("Integrator/Render time in milliseconds", nElapsedMilliseconds);

Statistics::Statistics(const ParamSet &paramSet, const Film *originalFilm,
                       const Sampler &sampler)
    : mode([](const std::string &modeString) {
          return modeString == "time"  ? Mode::TIME
               : modeString == "error" ? Mode::ERROR
               :                         Mode::NORMAL;
      }(paramSet.FindOneString("mode", "normal"))),
      maxSamples(sampler.samplesPerPixel),
      minSamples(paramSet.FindOneInt("minsamples", 64)),
      errorThreshold(paramSet.FindOneFloat("errorthreshold", 0.01)),
      errorHeuristic(paramSet.FindOneString("errorheuristic", "relative")),
      batchSize(paramSet.FindOneInt("batchsize", 16)),
      targetSeconds(paramSet.FindOneInt("targetseconds", 60)),
      originalFilm(originalFilm),
      pixelBounds(originalFilm->croppedPixelBounds),
      alpha(paramSet.FindOneFloat("alpha", 0.99)),
      zconstant(ZConstant()),
      pixels(new Pixel[pixelBounds.Area()]) {}

void Statistics::RenderBegin() {
    startTime = Clock::now();
}

void Statistics::RenderEnd() const {
    nElapsedMilliseconds = ElapsedMilliseconds();
}

void Statistics::WriteImages() const {
    CHECK(originalFilm);
    Film samplesFilm(*originalFilm, StatImagesFilter(), "_samples");
    Film varianceFilm(*originalFilm, StatImagesFilter(), "_variance");
    Film errorFilm(*originalFilm, StatImagesFilter(), "_error");

    for (Point2i pixel : pixelBounds) {
        Point2f floatPixel(static_cast<Float>(pixel.x),
                           static_cast<Float>(pixel.y));
        const Pixel &statsPixel = GetPixel(pixel);
        samplesFilm.AddSplat(floatPixel, Sampling(statsPixel));
        varianceFilm.AddSplat(floatPixel, Variance(statsPixel));
        errorFilm.AddSplat(floatPixel, Error(statsPixel));
    }

    samplesFilm.WriteImage();
    varianceFilm.WriteImage();
    errorFilm.WriteImage();
}

bool Statistics::StartNextBatch(int index) {
    switch (mode) {
    case Mode::TIME:
        if(ElapsedMilliseconds() > targetSeconds*1000 || (index+1) * BatchSize() > maxSamples)
            return false;
        return true;
        // Is there enough time remaining to start the next batch?
        // Would the batch index + 1 respect the _maxSamples_ budget?
    default:
        // Only process one batch
        if (batchOnce) {
            batchOnce = false;
            return true;
        } else
            return false;
    }
}

long Statistics::BatchSize() const {
    return batchSize;
}

void Statistics::SamplingLoop(Point2i pixel, const SamplingFunctor &sampleOnce){
    auto loop = [&]() { UpdateStats(pixel, sampleOnce()); };

    switch (mode) {

    case Mode::TIME:
        for( long i = 0; i < BatchSize(); i++) //bootstrap
            loop();
        break;
    case Mode::NORMAL:
        for (long i = 0; i < maxSamples; ++i)
            loop();
        break;
    default:
        //ERROR
        // Divide render between bootstrap and adaptive phases
        for( long i = 0; i < minSamples; i++) //bootstrap
            loop();
        while( !StopCriterion(pixel))
            loop();
        break;

    }
}

void Statistics::UpdateStats(Point2i pixel, Spectrum &&L) {
    if (!InsideExclusive(pixel, pixelBounds)) return;

    Pixel &statsPixel = GetPixel(pixel);
    long &samples = statsPixel.samples;
    Spectrum &mean = statsPixel.mean;
    Spectrum &moment2 = statsPixel.moment2;
    samples += 1;
    auto delta = L - mean;
    mean += delta / samples;
    auto delta2 = L - mean;
    moment2 += delta * delta2;

    // Update the statistics above using Welford's online algorithm
}

Float Statistics::Sampling(const Pixel &statsPixel) const {
    switch (mode) {
        case Mode::ERROR:
            return (static_cast<Float>(statsPixel.samples)/static_cast<Float>(maxSamples));
        default:
            return static_cast<Float>(statsPixel.samples);
    }

}

Spectrum Statistics::Variance(const Pixel &statsPixel) const {
    return statsPixel.moment2 / ( statsPixel.samples - 1 );
}

Spectrum Statistics::Error(const Pixel &statsPixel) const {
  auto variance = Variance(statsPixel);
  auto squared = Sqrt( variance / statsPixel.samples );
  if(errorHeuristic == "standart")
      return squared;
  if(errorHeuristic == "relative")
      return squared / Sqrt( statsPixel.mean * statsPixel.mean + Spectrum(1e-4) * Spectrum(1e-4) );
  return (2 * (zconstant * squared)) / Sqrt( statsPixel.mean * statsPixel.mean + Spectrum(1e-4) * Spectrum(1e-4) );

}

float Statistics::ZConstant() const {
    return boost::math::erf_inv((1 + alpha)/2);
}

bool Statistics::StopCriterion(Point2i pixel) const {
    if (!InsideExclusive(pixel, pixelBounds)) return true;
    const Pixel &statsPixel = GetPixel(pixel);
    return statsPixel.samples >= maxSamples || Error(statsPixel).MaxComponentValue() <= errorThreshold;
}

long Statistics::ElapsedMilliseconds() const {
    using namespace std::chrono;
    return duration_cast<milliseconds>(Clock::now() - startTime).count();
}

std::string Statistics::WorkTitle() const {
    std::string type = mode == Mode::TIME  ? "time"
                     : mode == Mode::ERROR? "error"
                     :                       "sampling";
    return "Rendering (equal " + type + ')';
}

long Statistics::UpdateWork() const {
    return mode == Mode::TIME ? 0 : 1;
}

Statistics::Pixel &Statistics::GetPixel(Point2i pixel) {
    const auto *that = this;
    return const_cast<Pixel &>(that->GetPixel(pixel));
}

const Statistics::Pixel &Statistics::GetPixel(Point2i pixel) const {
    CHECK(InsideExclusive(pixel, pixelBounds));
    int width = pixelBounds.pMax.x - pixelBounds.pMin.x;
    int offset = (pixel.x - pixelBounds.pMin.x)
               + (pixel.y - pixelBounds.pMin.y) * width;
    return pixels[offset];
}

std::unique_ptr<Filter> Statistics::StatImagesFilter() {
    return std::unique_ptr<Filter>(new BoxFilter({0, 0}));
}

    VolPathAdaptive::VolPathAdaptive(Statistics stats, int maxDepth,
                                 std::shared_ptr<const Camera> camera,
                                 std::shared_ptr<Sampler> sampler,
                                 const Bounds2i &pixelBounds, Float rrThreshold,
                                 const std::string &lightSampleStrategy)
    : VolPathIntegrator(maxDepth, std::move(camera), sampler, pixelBounds,
                        rrThreshold, lightSampleStrategy),
      stats(std::move(stats)),
      sampler(std::move(sampler)),
      pixelBounds(pixelBounds) {}

void VolPathAdaptive::Render(const Scene &scene) {
    Preprocess(scene, *sampler);
    // Render image tiles in parallel

    // Compute number of tiles, _nTiles_, to use for parallel rendering
    Bounds2i sampleBounds = camera->film->GetSampleBounds();
    Vector2i sampleExtent = sampleBounds.Diagonal();
    const int tileSize = 16;
    Point2i nTiles((sampleExtent.x + tileSize - 1) / tileSize,
                   (sampleExtent.y + tileSize - 1) / tileSize);

    stats.RenderBegin(); //Modified start time
    ProgressReporter reporter(nTiles.x * nTiles.y, stats.WorkTitle()); //Modified
    for (int batch = 0; stats.StartNextBatch(batch); ++batch) {
        ParallelFor2D([&](Point2i tile) {
            // Render section of image corresponding to _tile_

            // Allocate _MemoryArena_ for tile
            MemoryArena arena;

            // Get sampler instance for tile
            int seed = nTiles.x * nTiles.y * batch + nTiles.x * tile.y + tile.x;
            std::unique_ptr<Sampler> tileSampler = sampler->Clone(seed);

            // Compute sample bounds for tile
            int x0 = sampleBounds.pMin.x + tile.x * tileSize;
            int x1 = std::min(x0 + tileSize, sampleBounds.pMax.x);
            int y0 = sampleBounds.pMin.y + tile.y * tileSize;
            int y1 = std::min(y0 + tileSize, sampleBounds.pMax.y);
            Bounds2i tileBounds(Point2i(x0, y0), Point2i(x1, y1));
            LOG(INFO) << "Starting image tile " << tileBounds;

            // Get _FilmTile_ for tile
            auto filmTile = camera->film->GetFilmTile(tileBounds);

            // Loop over pixels in tile to render them
            for (Point2i pixel : tileBounds) {
                {
                    ProfilePhase pp(Prof::StartPixel);
                    tileSampler->StartPixel(pixel);
                    tileSampler->SetSampleNumber(batch * stats.BatchSize()); //Modified
                }

                // Do this check after the StartPixel() call; this keeps
                // the usage of RNG values from (most) Samplers that use
                // RNGs consistent, which improves reproducibility /
                // debugging.
                if (!InsideExclusive(pixel, pixelBounds))
                    continue;

                stats.SamplingLoop(pixel, [&]() { //Modified
                    // Initialize _CameraSample_ for current sample
                    CameraSample cameraSample =
                        tileSampler->GetCameraSample(pixel);

                    // Generate camera ray for current sample
                    RayDifferential ray;
                    Float rayWeight =
                        camera->GenerateRayDifferential(cameraSample, &ray);
                    ray.ScaleDifferentials(
                        1 / std::sqrt((Float)tileSampler->samplesPerPixel));
                    ++nCameraRays;

                    // Evaluate radiance along camera ray
                    Spectrum L(0.f);

                    if (rayWeight > 0)
                        L = Li(ray, scene, *tileSampler, arena, 0);

                    // Add camera ray's contribution to image
                    filmTile->AddSample(cameraSample.pFilm, L, rayWeight);

                    // Free _MemoryArena_ memory from computing image sample
                    // value
                    arena.Reset();

                    tileSampler->StartNextSample();
                    return L;
                });
            }
            LOG(INFO) << "Finished image tile " << tileBounds;

            // Merge image tile into _Film_
            camera->film->MergeFilmTile(std::move(filmTile));
            reporter.Update(stats.UpdateWork());
        }, nTiles);
    }
    reporter.Done();
    LOG(INFO) << "Rendering finished";
    stats.RenderEnd();

    // Save final image after rendering
    camera->film->WriteImage();
    stats.WriteImages();
}

VolPathAdaptive *CreateVolPathAdaptive(const ParamSet &params,
                                       std::shared_ptr<Sampler> sampler,
                                       std::shared_ptr<const Camera> camera) {
    int maxDepth = params.FindOneInt("maxdepth", 5);
    int np;
    const int *pb = params.FindInt("pixelbounds", &np);
    Bounds2i pixelBounds = camera->film->GetSampleBounds();
    if (pb) {
        if (np != 4)
            Error("Expected four values for \"pixelbounds\" parameter. Got %d.",
                  np);
        else {
            pixelBounds = Intersect(pixelBounds,
                                    Bounds2i{{pb[0], pb[2]}, {pb[1], pb[3]}});
            if (pixelBounds.Area() == 0)
                Error("Degenerate \"pixelbounds\" specified.");
        }
    }
    Float rrThreshold = params.FindOneFloat("rrthreshold", 1.);
    std::string lightStrategy =
        params.FindOneString("lightsamplestrategy", "spatial");

    return new VolPathAdaptive({params, camera->film, *sampler}, maxDepth,
                               camera, sampler, pixelBounds, rrThreshold,
                               lightStrategy);
}

}  // namespace pbrt


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

#ifndef PARTICLE_HPP
#define PARTICLE_HPP
#include <type_traits>
#include "JointPolicy.h"
#include "ObservationModel.hpp"
#include "PRNG.h"
#include "ParticleUtilities.h"
#include "PolicyGraphTraversal.h"
#include "RewardModel.hpp"
#include "StateTransitionModel.hpp"
#include "JointPolicyHistories.h"
#include "common.hpp"
#include <boost/random/discrete_distribution.hpp>
#include "ros/ros.h"
#include <ros/console.h>

namespace pgi {

template <typename State>
struct ParticleSet {
  std::vector<State> states_;
  std::vector<JointPolicy::joint_vertex_t> nodes_;
  std::vector<double> weights_;
};

template <typename State, typename StateTransitionModel,
          typename ObservationModel>
ParticleSet<State> step_forward(ParticleSet<State> particles,
                                const JointPolicy& jp,
                                const StateTransitionModel& t,
                                const ObservationModel& o,
                                const JointActionSpace& jas,
                                const JointObservationSpace& jos, PRNG& rng) {
  static_assert(std::is_base_of<pgi::StateTransitionModel<State>,
                                StateTransitionModel>::value,
                "StateTransitionModel must inherit from "
                "pgi::StateTransitionModel<State>");
  static_assert(
      std::is_base_of<pgi::ObservationModel<State>, ObservationModel>::value,
      "ObservationModel must inherit from "
      "pgi::ObservationModel<State>");

  for (std::size_t i = 0; i < particles.weights_.size(); ++i) {
    JointPolicyGraphTraversal tv(particles.nodes_[i], jp);
    const std::size_t j_act = tv.current_action(jas);
    const std::size_t j_obs =
        simulate_step(particles.states_[i], j_act, t, o, rng);
    // no weight update needed as observation is also sampled

    if (tv.can_traverse()) {
      tv.traverse(j_obs, jos);
      particles.nodes_[i] = tv.current_vertex();
    } else {
      particles.nodes_[i] = -1;  // TODO: required? better way?
    }
  }
  normalize(particles.weights_);

  return particles;
}

template <typename State, typename StateTransitionModel,
          typename ObservationModel>
std::size_t simulate_step(State& s, std::size_t j_act,
                          const StateTransitionModel& t,
                          const ObservationModel& o, PRNG& rng) {
  s = t.sample_next_state(s, j_act, rng);
  return o.sample_observation(s, j_act, rng);
}

template <typename State, typename StateTransitionModel,
          typename ObservationModel>
std::pair<History, bool> sample_random_history(
    State s, JointPolicy::joint_vertex_t q_start, const JointPolicy& jp,
    const StateTransitionModel& t, const ObservationModel& o,
    const JointActionSpace& jas, PRNG& rng) {
  unsigned int hist_length = jp.max_steps() - jp.steps_remaining(q_start);

  if (hist_length == 0) return std::make_pair(History(), true);

  History h;
  h.reserve(hist_length);
  boost::random::uniform_int_distribution<std::size_t> act_dist(
      0, jas.num_joint_indices() - 1);
  for (unsigned int i = 0; i < hist_length; ++i) {
    const std::size_t j_act = rng(act_dist);
    s = t.sample_next_state(s, j_act, rng);
    std::size_t j_obs = o.sample_observation(s, j_act, rng);
    h.emplace_back(ActionObservation{j_act, j_obs});
  }
  return std::make_pair(h, true);
}

template <typename State, typename StateTransitionModel,
          typename ObservationModel, typename RewardModel>
double estimate_value(int num_rollouts, const std::vector<State>& states,
                      const std::vector<double>& weights, const JointPolicy& jp,
                      JointPolicy::joint_vertex_t q_start,
                      const StateTransitionModel& t, const ObservationModel& o,
                      const RewardModel& r, const JointActionSpace& jas,
                      const JointObservationSpace& jos, PRNG& rng) {
  double avg_value = 0.0;
  for (int n = 0; n < num_rollouts; ++n) {
    const double value =
        rollout(states, weights, jp, q_start, t, o, r, jas, jos, rng);
    avg_value = (value + static_cast<double>(n) * avg_value) /
                static_cast<double>(n + 1);
  }
  return avg_value;
}

template <typename State, typename StateTransitionModel,
          typename ObservationModel, typename RewardModel>
double rollout(std::vector<State> states,
               std::vector<double> weights, const JointPolicy& jp,
               JointPolicy::joint_vertex_t q_start,
               const StateTransitionModel& t, const ObservationModel& o,
               const RewardModel& r, const JointActionSpace& jas,
               const JointObservationSpace& jos, PRNG& rng) {
  double v = 0.0;
  JointPolicyGraphTraversal tv(q_start, jp);
  for (unsigned int s = jp.steps_remaining(q_start); s > 0; --s)
  {
    const std::size_t j_act = tv.current_action(jas);
    v += r.get(states, weights, j_act);
    // sample an observation according to one of the states
    boost::random::discrete_distribution<> dist(weights.begin(), weights.end());
    State s_next = t.sample_next_state(states[rng(dist)], j_act, rng);
    const std::size_t j_obs = o.sample_observation(s_next, j_act, rng);
    if (tv.can_traverse())
    {
      tv.traverse(j_obs, jos);
    }
    // update particles
    SIR_step(states, weights, j_act, j_obs, t, o,
             0.1 * static_cast<double>(weights.size()), rng);
  }

  v += r.final_reward(states, weights);
  return v;
}

template <typename State, typename StateTransitionModel,
          typename ObservationModel, typename RewardModel>
double expected_sum_of_rewards(const std::vector<State>& states,
                               const std::vector<double>& weights,
                               const JointPolicy& jp,
                               JointPolicy::joint_vertex_t q_start,
                               const StateTransitionModel& t,
                               const ObservationModel& o, const RewardModel& r,
                               const JointActionSpace& jas,
                               const JointObservationSpace& jos, PRNG& rng) {
  if (q_start == 1234567890)
  {
    return r.final_reward(states, weights);
  }

  JointPolicyGraphTraversal tv(q_start, jp);
  const std::size_t j_act = tv.current_action(jas);

  // init with immediate reward
  double v = r.get(states, weights, j_act);

  // std::cout << "loop start:\n";
  std::vector<double> v_obs(jos.num_joint_indices(), 0.0);
  std::vector<double> p_obs(jos.num_joint_indices(), 0.0);
  for (std::size_t j_obs = 0; j_obs < jos.num_joint_indices(); ++j_obs)
  {
    std::vector<State> states_obs(states);
    std::vector<double> weights_obs(weights);

    p_obs[j_obs] = SIR_step(states_obs, weights_obs, j_act, j_obs, t, o,
             0.1 * static_cast<double>(weights.size()), rng);

    JointPolicyGraphTraversal tv(q_start, jp);
    std::size_t q_next = 1234567890;
    if (jp.steps_remaining(q_start) > 1)
    {
      tv.traverse(j_obs, jos);
      q_next = tv.current_vertex();
    }
    v_obs[j_obs] = expected_sum_of_rewards(states_obs, weights_obs, jp, q_next, t, o,
                                      r, jas, jos, rng);
  }
  normalize(p_obs);
  double v_future = std::inner_product(p_obs.begin(), p_obs.end(), v_obs.begin(), 0.0);

  return v + v_future;
}

template <typename State>
ParticleSet<State> get_particles_at_node(const ParticleSet<State>& particles,
                                         JointPolicy::joint_vertex_t q) {
  ParticleSet<State> qp;
  for (std::size_t i = 0; i < particles.weights_.size(); ++i) {
    if (particles.nodes_[i] == q) {
      qp.states_.push_back(particles.states_[i]);
      qp.nodes_.push_back(particles.nodes_[i]);
      qp.weights_.push_back(particles.weights_[i]);
    }
  }
  return qp;
}
}  // namespace pgi
#endif  // PARTICLE_HPP