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

// Copyright (c) 2014-2015, Oracle and/or its affiliates.

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_disjoint_coverage
#endif

// unit test to test disjoint for all geometry combinations

#include <iostream>

#include <boost/test/included/unit_test.hpp>

#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

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

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

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

#include <boost/geometry/algorithms/disjoint.hpp>

#include <from_wkt.hpp>

namespace bg = ::boost::geometry;

//============================================================================

struct test_disjoint
{
    template <typename Geometry1, typename Geometry2>
    static inline void apply(std::string const& case_id,
                             Geometry1 const& geometry1,
                             Geometry2 const& geometry2,
                             bool expected_result)
    {
        bool result = bg::disjoint(geometry1, geometry2);
        BOOST_CHECK_MESSAGE(result == expected_result,
            "case ID: " << case_id << ", G1: " << bg::wkt(geometry1)
            << ", G2: " << bg::wkt(geometry2) << " -> Expected: "
            << expected_result << ", detected: " << result);

        result = bg::disjoint(geometry2, geometry1);
        BOOST_CHECK_MESSAGE(result == expected_result,
            "case ID: " << case_id << ", G1: " << bg::wkt(geometry2)
            << ", G2: " << bg::wkt(geometry1) << " -> Expected: "
            << expected_result << ", detected: " << result);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
        std::cout << "case ID: " << case_id << "; G1 - G2: ";
        std::cout << bg::wkt(geometry1) << " - ";
        std::cout << bg::wkt(geometry2) << std::endl;
        std::cout << std::boolalpha;
        std::cout << "expected/computed result: "
                  << expected_result << " / " << result << std::endl;
        std::cout << std::endl;
        std::cout << std::noboolalpha;
#endif
    }
};

//============================================================================

// areal-areal geometries
template <typename P>
inline void test_box_box()
{
    typedef bg::model::box<P> B;

    typedef test_disjoint tester;

    tester::apply("b-b-01",
                  from_wkt<B>("BOX(2 2,3 3)"),
                  from_wkt<B>("BOX(0 0,2 2)"),
                  false);

    tester::apply("b-b-02",
                  from_wkt<B>("BOX(1 1,3 3)"),
                  from_wkt<B>("BOX(0 0,2 2)"),
                  false);

    tester::apply("b-b-03",
                  from_wkt<B>("BOX(3 3,4 4)"),
                  from_wkt<B>("BOX(0 0,2 2)"),
                  true);
}

template <typename P>
inline void test_ring_box()
{
    typedef bg::model::box<P> B;
    typedef bg::model::ring<P, false, false> R; // ccw, open

    typedef test_disjoint tester;

    tester::apply("r-b-01",
                  from_wkt<B>("BOX(2 2,3 3)"),
                  from_wkt<R>("POLYGON((0 0,2 0,2 2,0 2))"),
                  false);

    tester::apply("r-b-02",
                  from_wkt<B>("BOX(1 1,3 3)"),
                  from_wkt<R>("POLYGON((0 0,2 0,2 2,0 2))"),
                  false);

    tester::apply("r-b-03",
                  from_wkt<B>("BOX(3 3,4 4)"),
                  from_wkt<R>("POLYGON((0 0,2 0,2 2,0 2))"),
                  true);
}

template <typename P>
inline void test_polygon_box()
{
    typedef bg::model::box<P> B;
    typedef bg::model::polygon<P, false, false> PL; // ccw, open

    typedef test_disjoint tester;

    tester::apply("pg-b-01",
                  from_wkt<B>("BOX(2 2,3 3)"),
                  from_wkt<PL>("POLYGON((0 0,2 0,2 2,0 2))"),
                  false);

    tester::apply("pg-b-02",
                  from_wkt<B>("BOX(1 1,3 3)"),
                  from_wkt<PL>("POLYGON((0 0,2 0,2 2,0 2))"),
                  false);

    tester::apply("pg-b-03",
                  from_wkt<B>("BOX(3 3,4 4)"),
                  from_wkt<PL>("POLYGON((0 0,2 0,2 2,0 2))"),
                  true);
}

template <typename P>
inline void test_multipolygon_box()
{
    typedef bg::model::box<P> B;
    typedef bg::model::polygon<P, false, false> PL; // ccw, open
    typedef bg::model::multi_polygon<PL> MPL;

    typedef test_disjoint tester;

    tester::apply("mpg-b-01",
                  from_wkt<B>("BOX(2 2,3 3)"),
                  from_wkt<MPL>("MULTIPOLYGON(((0 0,2 0,2 2,0 2)))"),
                  false);

    tester::apply("mpg-b-02",
                  from_wkt<B>("BOX(1 1,3 3)"),
                  from_wkt<MPL>("MULTIPOLYGON(((0 0,2 0,2 2,0 2)))"),
                  false);

    tester::apply("mpg-b-03",
                  from_wkt<B>("BOX(3 3,4 4)"),
                  from_wkt<MPL>("MULTIPOLYGON(((0 0,2 0,2 2,0 2)))"),
                  true);
}

template <typename P>
inline void test_ring_ring()
{
    typedef bg::model::ring<P, false, false> R; // ccw, open

    typedef test_disjoint tester;

    tester::apply("r-r-01",
                  from_wkt<R>("POLYGON((2 2,2 3,3 3,3 2))"),
                  from_wkt<R>("POLYGON((0 0,2 0,2 2,0 2))"),
                  false);

    tester::apply("r-r-02",
                  from_wkt<R>("POLYGON((1 1,1 3,3 3,3 1))"),
                  from_wkt<R>("POLYGON((0 0,2 0,2 2,0 2))"),
                  false);

    tester::apply("r-r-03",
                  from_wkt<R>("POLYGON((3 3,3 4,4 4,4 3))"),
                  from_wkt<R>("POLYGON((0 0,2 0,2 2,0 2))"),
                  true);
}

template <typename P>
inline void test_polygon_ring()
{
    typedef bg::model::ring<P, false, false> R; // ccw, open
    typedef bg::model::polygon<P, false, false> PL; // ccw, open

    typedef test_disjoint tester;

    tester::apply("pg-r-01",
                  from_wkt<R>("POLYGON((2 2,2 3,3 3,3 2))"),
                  from_wkt<PL>("POLYGON((0 0,2 0,2 2,0 2))"),
                  false);

    tester::apply("pg-r-02",
                  from_wkt<R>("POLYGON((1 1,1 3,3 3,3 1))"),
                  from_wkt<PL>("POLYGON((0 0,2 0,2 2,0 2))"),
                  false);

    tester::apply("pg-r-03",
                  from_wkt<R>("POLYGON((3 3,3 4,4 4,4 3))"),
                  from_wkt<PL>("POLYGON((0 0,2 0,2 2,0 2))"),
                  true);
}

template <typename P>
inline void test_multipolygon_ring()
{
    typedef bg::model::ring<P, false, false> R; // ccw, open
    typedef bg::model::polygon<P, false, false> PL; // ccw, open
    typedef bg::model::multi_polygon<PL> MPL;

    typedef test_disjoint tester;

    tester::apply("mpg-r-01",
                  from_wkt<R>("POLYGON((2 2,2 3,3 3,3 2))"),
                  from_wkt<MPL>("MULTIPOLYGON(((0 0,2 0,2 2,0 2)))"),
                  false);

    tester::apply("mpg-r-02",
                  from_wkt<R>("POLYGON((1 1,1 3,3 3,3 1))"),
                  from_wkt<MPL>("MULTIPOLYGON(((0 0,2 0,2 2,0 2)))"),
                  false);

    tester::apply("mpg-r-03",
                  from_wkt<R>("POLYGON((3 3,3 4,4 4,4 3))"),
                  from_wkt<MPL>("MULTIPOLYGON(((0 0,2 0,2 2,0 2)))"),
                  true);
}

template <typename P>
inline void test_polygon_polygon()
{
    typedef bg::model::polygon<P, false, false> PL; // ccw, open

    typedef test_disjoint tester;

    tester::apply("pg-pg-01",
                  from_wkt<PL>("POLYGON((2 2,2 3,3 3,3 2))"),
                  from_wkt<PL>("POLYGON((0 0,2 0,2 2,0 2))"),
                  false);

    tester::apply("pg-pg-02",
                  from_wkt<PL>("POLYGON((1 1,1 3,3 3,3 1))"),
                  from_wkt<PL>("POLYGON((0 0,2 0,2 2,0 2))"),
                  false);

    tester::apply("pg-pg-03",
                  from_wkt<PL>("POLYGON((3 3,3 4,4 4,4 3))"),
                  from_wkt<PL>("POLYGON((0 0,2 0,2 2,0 2))"),
                  true);

    tester::apply("pg-pg-04",
                  from_wkt<PL>("POLYGON((0 0,9 0,9 9,0 9))"),
                  from_wkt<PL>("POLYGON((3 3,6 3,6 6,3 6))"),
                  false);
    // polygon with a hole which entirely contains the other polygon
    tester::apply("pg-pg-05",
                  from_wkt<PL>("POLYGON((0 0,9 0,9 9,0 9),(2 2,2 7,7 7,7 2))"),
                  from_wkt<PL>("POLYGON((3 3,6 3,6 6,3 6))"),
                  true);
    // polygon with a hole, but the inner ring intersects the other polygon
    tester::apply("pg-pg-06",
                  from_wkt<PL>("POLYGON((0 0,9 0,9 9,0 9),(3 2,3 7,7 7,7 2))"),
                  from_wkt<PL>("POLYGON((2 3,6 3,6 6,2 6))"),
                  false);
    // polygon with a hole, but the other polygon is entirely contained
    // between the inner and outer rings.
    tester::apply("pg-pg-07",
                  from_wkt<PL>("POLYGON((0 0,9 0,9 9,0 9),(6 2,6 7,7 7,7 2))"),
                  from_wkt<PL>("POLYGON((3 3,5 3,5 6,3 6))"),
                  false);
    // polygon with a hole and the outer ring of the other polygon lies
    // between the inner and outer, but without touching either.
    tester::apply("pg-pg-08",
                  from_wkt<PL>("POLYGON((0 0,9 0,9 9,0 9),(3 3,3 6,6 6,6 3))"),
                  from_wkt<PL>("POLYGON((2 2,7 2,7 7,2 7))"),
                  false);

    {
        typedef bg::model::polygon<P> PL; // cw, closed

        // https://svn.boost.org/trac/boost/ticket/10647
        tester::apply("ticket-10647",
            from_wkt<PL>("POLYGON((0 0, 0 5, 5 5, 5 0, 0 0)(1 1, 4 1, 4 4, 1 4, 1 1))"),
            from_wkt<PL>("POLYGON((2 2, 2 3, 3 3, 3 2, 2 2))"),
            true);
    }
}

template <typename P>
inline void test_polygon_multipolygon()
{
    typedef bg::model::polygon<P, false, false> PL; // ccw, open
    typedef bg::model::multi_polygon<PL> MPL;

    typedef test_disjoint tester;

    tester::apply("pg-mpg-01",
                  from_wkt<PL>("POLYGON((2 2,2 3,3 3,3 2))"),
                  from_wkt<MPL>("MULTIPOLYGON(((0 0,2 0,2 2,0 2)))"),
                  false);

    tester::apply("pg-mpg-02",
                  from_wkt<PL>("POLYGON((1 1,1 3,3 3,3 1))"),
                  from_wkt<MPL>("MULTIPOLYGON(((0 0,2 0,2 2,0 2)))"),
                  false);

    tester::apply("pg-mpg-03",
                  from_wkt<PL>("POLYGON((3 3,3 4,4 4,4 3))"),
                  from_wkt<MPL>("MULTIPOLYGON(((0 0,2 0,2 2,0 2)))"),
                  true);
}

template <typename P>
inline void test_multipolygon_multipolygon()
{
    typedef bg::model::polygon<P, false, false> PL; // ccw, open
    typedef bg::model::multi_polygon<PL> MPL;

    typedef test_disjoint tester;

    tester::apply("mpg-mpg-01",
                  from_wkt<MPL>("MULTIPOLYGON(((2 2,2 3,3 3,3 2)))"),
                  from_wkt<MPL>("MULTIPOLYGON(((0 0,2 0,2 2,0 2)))"),
                  false);

    tester::apply("mpg-mpg-02",
                  from_wkt<MPL>("MULTIPOLYGON(((1 1,1 3,3 3,3 1)))"),
                  from_wkt<MPL>("MULTIPOLYGON(((0 0,2 0,2 2,0 2)))"),
                  false);

    tester::apply("mpg-mpg-03",
                  from_wkt<MPL>("MULTIPOLYGON(((3 3,3 4,4 4,4 3)))"),
                  from_wkt<MPL>("MULTIPOLYGON(((0 0,2 0,2 2,0 2)))"),
                  true);
}

//============================================================================

template <typename CoordinateType>
inline void test_areal_areal()
{
    typedef bg::model::point<CoordinateType, 2, bg::cs::cartesian> point_type;

    test_polygon_polygon<point_type>();
    test_polygon_multipolygon<point_type>();
    test_polygon_ring<point_type>();
    test_polygon_box<point_type>();

    test_multipolygon_multipolygon<point_type>();
    test_multipolygon_ring<point_type>();
    test_multipolygon_box<point_type>();

    test_ring_ring<point_type>();
    test_ring_box<point_type>();

    test_box_box<point_type>();
}

//============================================================================

BOOST_AUTO_TEST_CASE( test_areal_areal_all )
{
    test_areal_areal<double>();
    test_areal_areal<int>();
}


#include "include/GeneralizedRfAlgo.h"
#include <boost/math/special_functions/factorials.hpp>
#include <queue>
// boost logging
#include <FastSplitList.h>
#include <SpiAlgo.h>
#include <boost/log/attributes/constant.hpp>
#include <boost/log/sources/record_ostream.hpp>
#include <future>

std::atomic_size_t GeneralizedRfAlgo::tree_idx = 0;

LogDblFact GeneralizedRfAlgo::factorials = LogDblFact();
GeneralizedRfAlgo::GeneralizedRfAlgo() : pairwise_split_scores(0) {
	logger.add_attribute("Tag", boost::log::attributes::constant<std::string>("generalized_RF"));
}

RfAlgorithmInterface::Scalar GeneralizedRfAlgo::h_info_content(size_t a, size_t b) {
	// no trivial splits allowed here (outer log would return infty, because no information present)
	assert(a >= 2);
	assert(b >= 2);
	// precompute (maybe lazy) all double fac results for a and b separatly
	return -factorials.lg_rooted_dbl_fact_fast(static_cast<long>(a)) -
	       factorials.lg_rooted_dbl_fact_fast(static_cast<long>(b)) +
	       factorials.lg_unrooted_dbl_fact_fast(static_cast<long>(a + b));
}

size_t GeneralizedRfAlgo::bits_too_many(size_t taxa) {
	constexpr size_t bit_amount_split = sizeof(pll_split_base_t) * 8;
	auto bits_too_many =
	    taxa % bit_amount_split == 0 ? 0 : bit_amount_split - (taxa % bit_amount_split);
	assert(bits_too_many < sizeof(pll_split_base_t) * 8);
	return bits_too_many;
}

void GeneralizedRfAlgo::calc_thread(GeneralizedRfAlgo &alg,
                                    const std::vector<FastSplitList> &trees,
                                    size_t pairwise_tree_cnt,
                                    SymmetricMatrix<Scalar> &sim) {
	while (true) {
		size_t index = tree_idx++;
		if (index >= pairwise_tree_cnt) {
			break;
		}
		// Calculates the corresponding row from the index using the quadratic formula
		auto row = static_cast<size_t>(std::sqrt(1 + 8 * index) / 2 - .5);
		size_t col = index - (row * row + row) / 2;
		auto score = alg.calc_tree_score(trees[row], trees[col]);
		sim.raw_set_at(index, score);
	}
}

RfMetricInterface::Results GeneralizedRfAlgo::calculate(std::vector<PllTree> &trees,
                                                        const RfMetricInterface::Params &params) {
	assert(trees.size() >= 2);
	// extract splits. Each tree now identifies by its index in all_splits
	std::vector<PllSplitList> all_splits;
	all_splits.reserve(trees.size());
	for (auto &t : trees) {
		t.alignNodeIndices(*trees.begin());
		all_splits.emplace_back(t);
	}
	PllSplit::split_len = all_splits.back().computeSplitLen();
	taxa = all_splits.back().size() + 3;

	BOOST_LOG_SEV(logger, lg::notification) << "Parsed trees. Starting calculations.";
	factorials.reserve(4 * taxa + 8);
	std::vector<FastSplitList> fast_trees = generateFastList(all_splits);
	assert(PllSplit::split_len != std::numeric_limits<size_t>::max());
	setup_temporary_storage(PllSplit::split_len);
	pairwise_split_scores = calcPairwiseSplitScores();

	BOOST_LOG_SEV(logger, lg::notification)
	    << "Calculated pairwise scores; Calculating pairwise tree scores.";
	RfMetricInterface::Results res(trees.size());

	size_t pairwise_tree_cnt = res.pairwise_similarities.get_num_entries();
	int num_threads = params.threads == -1 ? static_cast<int>(std::thread::hardware_concurrency())
	                                       : params.threads;
	std::vector<std::thread> pool;
	match_solver.init(all_splits.back().size());

	pool.reserve(num_threads);
    for (int i = 0; i < num_threads; ++i) {
		pool.emplace_back(calc_thread,
		                  std::ref(*this),
		                  std::ref(fast_trees),
		                  std::ref(pairwise_tree_cnt),
		                  std::ref(res.pairwise_similarities));
	}

	for (auto &thread : pool) {
		thread.join();
	}

	// calc mean distance between trees
	double total_dst = 0.;
	for (size_t idx_a = 0; idx_a < all_splits.size(); ++idx_a) {
		for (size_t idx_b = 0; idx_b <= idx_a; ++idx_b) {
			total_dst += res.pairwise_similarities.at(idx_a, idx_b);
		}
	}
	res.mean_distance = total_dst / static_cast<Scalar>(trees.size());

	// calculate distances
	calc_pairwise_tree_dist(fast_trees, res);

	factorials.printLog();
	return res;
}
RfAlgorithmInterface::Scalar GeneralizedRfAlgo::calc_tree_score(const SplitList &A,
                                                                const SplitList &B) {
	//	auto scores = calc_pairwise_split_scores(A, B);
	SplitScores scores(A.size());
	Scalar max_val = 0;
	for (size_t row = 0; row < A.size(); ++row) {
		size_t row_idx = A[row].getScoreIndex();
		for (size_t col = 0; col < A.size(); ++col) {
			size_t col_idx = B[col].getScoreIndex();
			auto val = pairwise_split_scores.checked_at(row_idx, col_idx);
			scores.scores.set(row, col, val);
			if (val > max_val) {
				max_val = val;
			}
		}
	}
	scores.max_score = max_val;
	auto total_score = match_solver.solve(scores);
	return total_score;
}

[[maybe_unused]] GeneralizedRfAlgo::SplitScores
GeneralizedRfAlgo::calc_pairwise_split_scores(const SplitList &S1, const SplitList &S2) {
	SplitScores scores(S1.size());
	factorials.reserve(taxa + taxa + 4);
	for (size_t row = 0; row < S1.size(); ++row) {
		for (size_t col = 0; col < S1.size(); ++col) {
			Scalar val;
			// when using fast Split list, the pointers to PllSplit define equality
			if (&S1[row] == &S2[col]) {
				assert(S1[row] == S2[col]);
				val = calc_split_score(S1[row]);
			} else {
				val = calc_split_score(S1[row], S2[col]);
			}

			if (scores.max_score < val) {
				scores.max_score = val;
			}
			scores.scores.set(row, col, val);
		}
	}

	return scores;
}

void GeneralizedRfAlgo::compute_split_comparison(const PllSplit &S1, const PllSplit &S2) {
	// B1 -> &split_buffer[0]
	S1.set_not(PllSplit::split_len, &temporary_split_content[0]);
	// B2 -> &split_buffer[split_len]
	S2.set_not(PllSplit::split_len, &temporary_split_content[PllSplit::split_len]);
	// A1 and A2 -> &split_buffer[2 * split_len]
	S1.intersect(S2, PllSplit::split_len, &temporary_split_content[2 * PllSplit::split_len]);
	// B1 and B2 -> &split_buffer[3 * split_len]
	temporary_splits[0].intersect(temporary_splits[1],
	                              PllSplit::split_len,
	                              &temporary_split_content[3 * PllSplit::split_len]);
	// A1 and B2 -> &split_buffer[4 * split_len]
	S1.intersect(temporary_splits[1],
	             PllSplit::split_len,
	             &temporary_split_content[4 * PllSplit::split_len]);
	// A2 and B1 -> &split_buffer[5 * split_len]
	S2.intersect(temporary_splits[0],
	             PllSplit::split_len,
	             &temporary_split_content[5 * PllSplit::split_len]);
}
RfAlgorithmInterface::Scalar GeneralizedRfAlgo::calc_tree_info_content(const SplitList &S) {
	Scalar sum = 0;
	for (size_t i = 0; i < S.size(); ++i) {
		sum += S[i].getHInfoContent();
	}
	return sum;
}
void GeneralizedRfAlgo::calc_pairwise_tree_dist(const std::vector<FastSplitList> &trees,
                                                RfMetricInterface::Results &res) {
	std::vector<GeneralizedRfAlgo::Scalar> tree_info(trees.size());
	for (size_t i = 0; i < trees.size(); ++i) {
		tree_info[i] = calc_tree_info_content(trees[i]);
	}

	GeneralizedRfAlgo::Scalar summed_dist = 0;
	for (size_t row = 0; row < trees.size(); ++row) {
		for (size_t col = 0; col <= row; ++col) {
			auto score = res.pairwise_similarities.at(row, col);
			auto max = tree_info[row] + tree_info[col];
			res.pairwise_distances.set_at(row, col, (max - score - score));
			summed_dist += max / score;
		}
	}

	res.mean_distance = summed_dist / (static_cast<Scalar>(trees.size() + 1) *
	                                   (static_cast<Scalar>(trees.size()) / 2.));
}
void GeneralizedRfAlgo::setup_temporary_storage(size_t split_len) {
	temporary_split_content.assign(split_len * 6, 0);
	temporary_splits.reserve(6);
	for (size_t i = 0; i < 6; ++i) {
		temporary_splits.emplace_back(&temporary_split_content[i * split_len]);
	}
}
GeneralizedRfAlgo::GeneralizedRfAlgo(size_t split_len) : pairwise_split_scores(0) {
	logger.add_attribute("Tag", boost::log::attributes::constant<std::string>("generalized_RF"));
	setup_temporary_storage(split_len);
}
std::vector<FastSplitList>
GeneralizedRfAlgo::generateFastList(const std::vector<PllSplitList> &slow_split_list) {
	// expect that all elements of slow_split_list[i] are already sorted
	// -> Perform k-way merge to only store non-duplicate PllSplits
	PllSplit::split_len = slow_split_list.front().computeSplitLen();
	BOOST_LOG_SEV(logger, lg::normal)
	    << "Start reducing PllSplit Size, current split_len: " << PllSplit::split_len;

	std::vector<FastSplitList> returnList(slow_split_list.size(),
	                                      FastSplitList(slow_split_list.front().size()));

	// data used inside k-way merge
	std::vector<size_t> currently_inPQ(slow_split_list.size(), 0);
	size_t current_split_offset = 0;
	size_t found_duplicates = 0;
	// PQ which stores a PllSplit (underlying data not touched) and the index from which tree it
	// originates from
	typedef std::pair<PllSplit, size_t> pq_type;
	std::priority_queue<pq_type, std::vector<pq_type>, std::greater<>> pq;
	// initialize by inserting first elements of all slow lists
	for (size_t i = 0; i < slow_split_list.size(); ++i) {
		pq.push(std::make_pair(PllSplit(slow_split_list[i][0]), i));
	}
	auto insert_in_pq = [&](size_t active_slow_idx) {
		// takes the current index
		auto &active_slow_list = slow_split_list[active_slow_idx];
		// increment where the active element is
		size_t list_idx = ++currently_inPQ[active_slow_idx];
		if (list_idx < active_slow_list.size()) {
			PllSplit insert_split(active_slow_list[list_idx]());
			pq.push(std::make_pair(insert_split, active_slow_idx));
		}
	};
	while (!pq.empty()) {
		// no further duplications store the next split in unique_pll_splits (either because of
		// start/or handled all duplicates), increment current_split_offset
		unique_pll_splits.emplace_back(pq.top().first);
		{
			// pre-calc
			unique_pll_splits.back().perform_popcount_precalc(PllSplit::split_len);
			const size_t a = unique_pll_splits.back().getPrecalcPopcnt();
			unique_pll_splits.back().setHInfoContent(h_info_content(a, taxa - a));
		}

		// get current state
		PllSplit curr_split = pq.top().first;
		{
			size_t curr_slow_list_idx = pq.top().second;
			// add current unique split to result
			assert(currently_inPQ[curr_slow_list_idx] < slow_split_list.back().size());
			returnList[curr_slow_list_idx].setOffsetAt(currently_inPQ[curr_slow_list_idx],
			                                           current_split_offset);
			// add from the same list a replacement element
			pq.pop();

			insert_in_pq(curr_slow_list_idx);
		}
		// check if further elements are equal
		while (!pq.empty() && curr_split == pq.top().first) {
			++found_duplicates;
			// duplicate found - use the same offset value
			size_t equal_in_tree_idx = pq.top().second;
			// the found value must have un-inserted elements
			assert(currently_inPQ[equal_in_tree_idx] < slow_split_list[equal_in_tree_idx].size());
			returnList[equal_in_tree_idx].setOffsetAt(currently_inPQ[equal_in_tree_idx],
			                                          current_split_offset);
			pq.pop();
			// insertion of replacement for equal_in_tree element
			insert_in_pq(equal_in_tree_idx);
		}
		++current_split_offset;
	}
	{
		size_t total_splits = slow_split_list.size() * slow_split_list.front().size();
		double duplicate_ratio =
		    static_cast<double>(found_duplicates) / static_cast<double>(total_splits);
		BOOST_LOG_SEV(logger, lg::notification)
		    << "Done construction of FastSplitList. " << found_duplicates << " duplicates of "
		    << total_splits << " possible PllSplits. Duplicate ratio: " << duplicate_ratio
		    << " Number of unique splits: " << unique_pll_splits.size();
	}
	// unique_pll_splits will no longer reallocate -> write base-ptr to Static variable
	FastSplitList::setBasePtr(&unique_pll_splits[0]);
	return returnList;
}

SymmetricMatrix<GeneralizedRfAlgo::Scalar> GeneralizedRfAlgo::calcPairwiseSplitScores() {
	assert(!unique_pll_splits.empty());
	factorials.reserve(taxa + taxa + 4);

	size_t split_num = unique_pll_splits.size();
	SymmetricMatrix<Scalar> resMtx(split_num);
	for (size_t row = 0; row < split_num; ++row) {
		auto &rSplit = unique_pll_splits[row];
		// let the split know, which index it is. -> should happen once per PllSplit
		rSplit.setIntersectionIdx(row);
		for (size_t col = 0; col < row; ++col) {
			auto &cSplit = unique_pll_splits[col];
			resMtx.set_at(row, col, calc_split_score(rSplit, cSplit));
		}
		resMtx.set_at(row, row, calc_split_score(rSplit));
	}
	return resMtx;
}


// Copyright 2016 Robert Maier, Technical University Munich
#ifndef DVO_H
#define DVO_H

#ifndef WIN64
    #define EIGEN_DONT_ALIGN_STATICALLY
#endif
#include <Eigen/Dense>

#include <opencv2/core/core.hpp>

typedef Eigen::Matrix<float, 6, 6> Mat6f;
typedef Eigen::Matrix<float, 6, 1> Vec6f;

class DVO
{
public:
    enum MinimizationAlgo
    {
        GaussNewton = 0,
        GradientDescent = 1,
        LevenbergMarquardt = 2
    };

    DVO();
    ~DVO();

    void init(int w, int h, const Eigen::Matrix3f &K);

    void buildPyramid(const cv::Mat &depth, const cv::Mat &gray, std::vector<cv::Mat> &depthPyramid, std::vector<cv::Mat> &grayPyramid);

    void align(const cv::Mat &depthRef, const cv::Mat &grayRef,
               const cv::Mat &depthCur, const cv::Mat &grayCur,
               Eigen::Matrix4f &pose);

    void align(const std::vector<cv::Mat> &depthRefPyramid, const std::vector<cv::Mat> &grayRefPyramid,
               const std::vector<cv::Mat> &depthCurPyramid, const std::vector<cv::Mat> &grayCurPyramid,
               Eigen::Matrix4f &pose);

private:
    cv::Mat downsampleGray(const cv::Mat &gray);
    cv::Mat downsampleDepth(const cv::Mat &depth);

    void convertSE3ToTf(const Vec6f &xi, Eigen::Matrix3f &rot, Eigen::Vector3f &t);
    void convertSE3ToTf(const Vec6f &xi, Eigen::Matrix4f &pose);
    void convertTfToSE3(const Eigen::Matrix3f &rot, const Eigen::Vector3f &t, Vec6f &xi);

    void computeGradient(const cv::Mat &gray, cv::Mat &gradient, int direction);
    float interpolate(const float* ptrImgIntensity, float x, float y, int w, int h);
    float calculateError(const float* residuals, int n);
    void calculateErrorImage(const float* residuals, int w, int h, cv::Mat &errorImage);
    void calculateError(const cv::Mat &grayRef, const cv::Mat &depthRef,
                        const cv::Mat &grayCur, const cv::Mat &depthCur,
                        const Eigen::VectorXf &xi, const Eigen::Matrix3f &K,
                        float* residuals);
    void convertTfToSE3(const Eigen::Matrix4f &pose, Vec6f &xi);

    void calculateMeanStdDev(const float* residuals, float &mean, float &stdDev, int n);
    void computeWeights(const float* residuals, float* weights, int n);
    void applyWeights(const float* weights, float* residuals, int n);

    void deriveNumeric(const cv::Mat &grayRef, const cv::Mat &depthRef,
                                      const cv::Mat &grayCur, const cv::Mat &depthCur,
                                      const Eigen::VectorXf &xi, const Eigen::Matrix3f &K,
                                      float* residuals, float* J);
    void deriveAnalytic(const cv::Mat &grayRef, const cv::Mat &depthRef,
                       const cv::Mat &grayCur, const cv::Mat &depthCur,
                       const cv::Mat &gradX_, const cv::Mat &gradY_,
                       const Eigen::VectorXf &xi, const Eigen::Matrix3f &K,
                       float* residuals, float* J);

    void compute_JtR(const float* J, const float* residuals, Vec6f &b, int validRows);
    void compute_JtJ(const float* J, Mat6f &A, const float* weights, int validRows, bool useWeights);

    int numPyramidLevels_;
    std::vector<Eigen::Matrix3f> kPyramid_;
    std::vector<cv::Size> sizePyramid_;
    bool useWeights_;
    int numIterations_;

    std::vector<cv::Mat> gradX_;
    std::vector<cv::Mat> gradY_;
    std::vector<float*> J_;
    std::vector<float*> residuals_;
    std::vector<float*> weights_;

    MinimizationAlgo algo_;
};

#endif


#include "SkeletalAnimator.h"
#include <boost/range/algorithm/find_if.hpp>
#include <iostream>

// для перегруженной glm::lerp, принимающей glm::vec3
#include <glm/gtx/compatibility.hpp>
#include <glm/gtc/quaternion.hpp>

using glm::quat;
using glm::vec3;

namespace
{
template <class T>
using Keyframe = CNodeAnimation::Keyframe<T>;

// Выполняет сферическую линейную интерполяцию (slerp) двух кватернионов.
quat Interpolate(const quat &a, const quat &b, float weight)
{
    return glm::slerp(a, b, weight);
}

// Выполняет линейную интерполяцию (смешивание) двух векторов.
vec3 Interpolate(const vec3 &a, const vec3 &b, float weight)
{
    return glm::mix(a, b, weight);
}

// Возвращает значение, интерполированное между двумя ключевыми кадрами.
template <class T>
T GetInterpolatedValue(const std::vector<Keyframe<T>> &keyframes, double phase)
{
    // Обрабатываем вырожденный случай (массив из 1-го элемента).
    if (keyframes.size() <= 1)
    {
        return keyframes.at(0).value;
    }

    // Ищем ближайший ключевой кадр (keyframe),
    //  который анимации предстоит пройти.
    size_t ki = 0;
    for (; ki < keyframes.size(); ++ki)
    {
        if (keyframes[ki].time >= phase)
        {
            break;
        }
    }

    // Если анимация не достигла первого ключевого кадра, используем его.
    if (ki == 0)
    {
        return keyframes.front().value;
    }
    // Если анимация преодолела последний ключевой кадр, используем его.
    if (ki == keyframes.size())
    {
        return keyframes.back().value;
    }

    // Если анимация находится между двумя ключевыми кадрами,
    //  пройденным и предстоящим, и мы интерполируем их значения.
    // Функция Interpolate перегружена для vec3 и для quat.
    const auto &a = keyframes[ki - 1];
    const auto &b = keyframes[ki];
    const double weight = (phase - a.time) / (b.time - a.time);

    // Вес будет в диапазоне [0..1], что даёт усреднённое значение между A и B.
    assert(weight > -0.01f && weight < 1.01f);

    return Interpolate(a.value, b.value, float(weight));
}
}

void CSkeletalAnimator::SetModel(const CSkeletalModel3DPtr &pModel)
{
    m_pModel = pModel;
}

void CSkeletalAnimator::SetAnimationName(const std::string &name)
{
    if (!m_pModel)
    {
        throw std::runtime_error("No model set for animator");
    }

    const auto &animations = GetAnimations();
    auto it = boost::find_if(animations, [&](const CModelAnimation &anim) {
        return anim.m_name == name;
    });
    if (it == animations.end())
    {
        throw std::runtime_error("No animation '" + name + "' in model");
    }

    m_animationNo = unsigned(it - animations.begin());
}

void CSkeletalAnimator::SetAnimationNo(unsigned animationNo)
{
    m_animationNo = animationNo;
}

void CSkeletalAnimator::Update(float deltaSec)
{
    if (!m_pModel)
    {
        return;
    }

    const CModelAnimation &anim = GetAnimations().at(m_animationNo);

    // Используем деление с остатком на период текущей анимации,
    //  чтобы определить фазу анимации
    //  (т.е. число ticks, проигранных с начала анимации).
    const double deltaTicks = double(deltaSec) * anim.m_ticksPerSecond;
    m_animationPhase = fmod(m_animationPhase + deltaTicks, anim.m_duration);

    // Применяем текущую фазу анимации ко всем узлам.
    for (const CNodeAnimation &nodeAnim : anim.m_channels)
    {
        ApplyNodeAnimation(nodeAnim);
    }
}

const std::vector<CModelAnimation> &CSkeletalAnimator::GetAnimations() const
{
    return m_pModel->m_animations;
}

void CSkeletalAnimator::ApplyNodeAnimation(const CNodeAnimation &anim)
{
    CTransform3D &transform = anim.m_pNode->m_transform;
    transform.m_sizeScale = GetInterpolatedValue(anim.m_scaleKeyframes, m_animationPhase);
    transform.m_position = GetInterpolatedValue(anim.m_positionKeyframes, m_animationPhase);
    transform.m_orientation = GetInterpolatedValue(anim.m_rotationKeyframes, m_animationPhase);
}


/**
 Minimal AR, see also VISP vpAROgre

 1) pick named monitor fullscreen
 2) background image
 3) camera from intrinsics
 4) mesh rendered flat
 */
#include <Ogre.h>
#include <Eigen/Dense>
#include "capture.hpp"

using namespace Ogre;

void setVisionCameraProjection(Ogre::Camera *camera, int w, int h, float n, float f, float px, float py, float u0,
                               float v0) {
    Ogre::Real f_m_n, f_p_n;
    f_m_n = (Ogre::Real) (f - n);
    f_p_n = (Ogre::Real) (f + n);
    Ogre::Matrix4 Projection
            = Ogre::Matrix4((Ogre::Real) (2.0 * px / w), 0, (Ogre::Real) (1.0 - 2.0 * (u0 / w)), 0,
                            0, (Ogre::Real) (2.0 * py / h), (Ogre::Real) (-1.0 + 2.0 * (v0 / h)), 0,
                            0, 0, (Ogre::Real) (-1.0 * f_p_n / f_m_n), (Ogre::Real) (-2.0 * f * n / f_m_n),
                            0, 0, -1.0, 0);
    camera->setCustomProjectionMatrix(true, Projection);
}

void setVisionCameraProjection(Ogre::Camera *camera, Eigen::Vector2i size, Eigen::Vector2f nearfar, Eigen::Matrix3f K) {
    setVisionCameraProjection(camera, size.x(), size.y(), nearfar.x(), nearfar.y(),
                              K(0, 0), K(1, 1), K(0, 2), K(1, 2));
}

void setCameraPose(Ogre::Camera *camera, Eigen::Matrix4f cMw) {
    Ogre::Matrix4 ModelView
            = Ogre::Matrix4((Ogre::Real) cMw(0, 0), (Ogre::Real) cMw(0, 1), (Ogre::Real) cMw(0, 2),
                            (Ogre::Real) cMw(0, 3),
                            (Ogre::Real) -cMw(1, 0), (Ogre::Real) -cMw(1, 1), (Ogre::Real) -cMw(1, 2),
                            (Ogre::Real) -cMw(1, 3),
                            (Ogre::Real) -cMw(2, 0), (Ogre::Real) -cMw(2, 1), (Ogre::Real) -cMw(2, 2),
                            (Ogre::Real) -cMw(2, 3),
                            (Ogre::Real) 0, (Ogre::Real) 0, (Ogre::Real) 0, (Ogre::Real) 1);
    camera->setCustomViewMatrix(true, ModelView);

}

class ImageBackground {
public:
    void setup(SceneManager *scene, int w, int h);

    // get pixel buffer / update
protected:
    int mBackgroundWidth, mBackgroundHeight;
    Ogre::Rectangle2D *mBackground;
    Ogre::SceneNode *mBackgroundNode;

    Ogre::HardwarePixelBufferSharedPtr mPixelBuffer;        /** Pointer to the pixel buffer */
};

void ImageBackground::setup(SceneManager *sceneMgr, int w, int h) {
    mBackgroundWidth = w;
    mBackgroundHeight = h;
    // Create a rectangle to show the incoming images from the camera
    mBackground = new Ogre::Rectangle2D(true); // true = textured
    mBackground->setCorners(-1.0, 1.0, 1.0, -1.0); // Spread all over the window
    mBackground->setBoundingBox(Ogre::AxisAlignedBox(-100000.0 * Ogre::Vector3::UNIT_SCALE,
                                                     100000.0 * Ogre::Vector3::UNIT_SCALE)); // To be shown everywhere

    Ogre::MaterialManager::getSingleton().setDefaultTextureFiltering(Ogre::TFO_NONE);
    Ogre::MaterialManager::getSingleton().setDefaultAnisotropy(1);

    Ogre::TextureManager::getSingleton().createManual("BackgroundTexture",
                                                      Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
                                                      Ogre::TEX_TYPE_2D,
                                                      mBackgroundWidth,//width
                                                      mBackgroundHeight,//height
                                                      0,  // num of mip maps
            //Ogre::PF_BYTE_RGBA,
                                                      Ogre::PF_BYTE_BGRA,
                                                      Ogre::TU_DYNAMIC_WRITE_ONLY_DISCARDABLE);

    Ogre::TexturePtr dynTexPtr = Ogre::TextureManager::getSingleton().getByName("BackgroundTexture");
    mPixelBuffer = dynTexPtr->getBuffer();

    Ogre::MaterialPtr Backgroundmaterial = Ogre::MaterialManager::getSingleton().create("BackgroundMaterial",
                                                                                Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
    Ogre::Technique *Backgroundtechnique = Backgroundmaterial->createTechnique();
    Backgroundtechnique->createPass();
    Backgroundmaterial->getTechnique(0)->getPass(0)->setLightingEnabled(false);
    Backgroundmaterial->getTechnique(0)->getPass(0)->setDepthCheckEnabled(false); // Background
    Backgroundmaterial->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false); // Background
    Backgroundmaterial->getTechnique(0)->getPass(0)->createTextureUnitState("BackgroundTexture");
    mBackground->setMaterial("BackgroundMaterial"); // Attach the material to the rectangle
    mBackground->setRenderQueueGroup(Ogre::RENDER_QUEUE_BACKGROUND); // To be rendered in Background

    // Add the background to the Scene Graph so it will be rendered
    mBackgroundNode = sceneMgr->getRootSceneNode()->createChildSceneNode("BackgoundNode");
    mBackgroundNode->attachObject(mBackground);
}

class MinOgre {
public:
    MinOgre();

    ~MinOgre();

    void setCameraPose();

    void setBackground();

    void addMesh(std::string filename, std::string meshname);

    SceneManager *sceneMgr;
    Ogre::Root *root;
    Camera *camera;
protected:
    void initRoot();

    void createCamera();

    RenderWindow *window;

    //OutputCapture capture;
    ImageBackground background;

};

MinOgre::~MinOgre() {
    delete root;
}

void MinOgre::initRoot() {
    root = new Root("plugins.cfg"); // pluginsOGRE_BUILD_SUFFIX.cfg, "ogre.cfg", "Ogre.log");
    bool canInit = true;
    bool mshowConfigDialog = false;
    if (mshowConfigDialog) {
        root->restoreConfig();
        if (!root->showConfigDialog())
            canInit = false;
    } else {
        if (!root->restoreConfig())
            canInit = false;
    }
    if (!root->isInitialised()) {
        if (!canInit) { //We set the default renderer system
            const Ogre::RenderSystemList &lRenderSystemList = root->getAvailableRenderers();
            if (lRenderSystemList.size() == 0) {
                std::cout << "No renderers\n;";
                throw "ConfigDialog aborted"; // Exit the application on cancel
            }

            Ogre::RenderSystem *lRenderSystem = lRenderSystemList.at(0);
            std::cout << "Using " << lRenderSystem->getName() << " as renderer." << std::endl;
            root->setRenderSystem(lRenderSystem);
        }
        root->initialise(false);
    }
    std::cerr << "initialise done\n";

}

MinOgre::MinOgre() {
    // MULTIPLE: Ogre::Root::getSingletonPtr()
    initRoot();

    ResourceGroupManager &resources = ResourceGroupManager::getSingleton();
    resources.addResourceLocation(".", "FileSystem");
    resources.initialiseAllResourceGroups();
    std::cerr << "resource done\n";
    sceneMgr = root->createSceneManager(ST_GENERIC);

    window = root->initialise(true, "Simple Ogre App");
    std::cerr << "window done\n";
    createCamera();
    std::cerr << "camera done\n";

//          Ogre::Viewport* viewPort = window->addViewport(camera);
    Ogre::Viewport *viewPort = camera->getViewport();
    viewPort->setClearEveryFrame(true);
    std::cerr << "viewport done\n";

    std::cerr << "creating scene\n";
    sceneMgr->getRootSceneNode()->createChildSceneNode()->attachObject(camera);
    std::cerr << "creating camera\n";
    std::cerr << "creating background\n";
    // background.setup(sceneMgr,128,128);
    sceneMgr->setAmbientLight(Ogre::ColourValue(0.5, 0.5, 0.5));

/*
  mRoot->addFrameListener(this);

  // Register as a Window listener
  Ogre::WindowEventUtilities::addWindowEventListener(mWindow, this);
  mInputManager = OIS::InputManager::createInputSystem( pl );

  //Create all devices
  // Here we only consider the keyboard input
  mKeyboard = static_cast<OIS::Keyboard*>(mInputManager->createInputObject( OIS::OISKeyboard, bufferedKeys ));
  if ( !bufferedKeys ) mKeyboard->setEventCallback ( this);

    Ogre::WindowEventUtilities::removeWindowEventListener(mWindow, this);
#ifdef VISP_HAVE_OIS
  // Get keyboard input
  mKeyboard->capture();
  if(mKeyboard->isKeyDown(OIS::KC_ESCAPE))
    return false;
#endif

*/

    std::cerr << "attach camera to scene\n";
}

void MinOgre::addMesh(std::string filename, std::string meshname) {
    Ogre::Entity *newEntity = sceneMgr->createEntity(meshname, filename);
    Ogre::SceneNode *newNode = sceneMgr->getRootSceneNode()->createChildSceneNode(meshname);
    newNode->attachObject(newEntity);

}

void MinOgre::setCameraPose() {

}

void MinOgre::setBackground() {
    //mPixelBuffer->lock(Ogre::HardwareBuffer::HBL_DISCARD); // Lock the buffer
    //const Ogre::PixelBox& pixelBox = mPixelBuffer->getCurrentLock();
    // TODO copy to buffer
    //mPixelBuffer->unlock();
}

void MinOgre::createCamera() {
    Camera *cam = sceneMgr->createCamera("SimpleCamera");
    cam->setPosition(Vector3(0.0f, 0.0f, 500.0f));
    cam->lookAt(Vector3(0.0f, 0.0f, 0.0f));
    cam->setNearClipDistance(5.0f);
    cam->setFarClipDistance(5000.0f);

    Viewport *v = window->addViewport(cam);
    v->setBackgroundColour(ColourValue(0.5, 0.5, 0.5));

    cam->setAspectRatio(Real(v->getActualWidth()) / v->getActualHeight());

    camera = cam;
}

#if OGRE_PLATFORM == OGRE_PLATFORM_WIN32
#define WIN32_LEAN_AND_MEAN
#include "windows.h"
INT WINAPI WinMain(HINSTANCE hInst, HINSTANCE, LPSTR strCmdLine, INT){
#else
int main(int argc, char **argv) {
#endif
    MinOgre mo;
    mo.addMesh("rviz_cube.mesh", "test");
    mo.camera->setPosition(0, 10, 50);

    Ogre::Light *light = mo.sceneMgr->createLight();
    light->setDiffuseColour(1, 1, 1); // scaled RGB values
    light->setSpecularColour(1, 1, 1); // scaled RGB values
    light->setPosition(-5, -5, 10);
    light->setType(Ogre::Light::LT_POINT);

    std::cout << "starting loop\n";
    while (true) {
        WindowEventUtilities::messagePump();
        mo.root->renderOneFrame();
    }
    std::cout << "ending loop\n";

    return 0;
}


#include "globimap/counting_globimap.hpp"
#include "globimap_test_config.hpp"
#include <chrono>
#include <fstream>
#include <iostream>
#include <limits>
#include <math.h>
#include <string>

#include <highfive/H5File.hpp>
#include <tqdm.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include "loc.hpp"
#include "rasterizer.hpp"
#include "shapefile.hpp"

#include "archive.h"
#include <H5Cpp.h>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;
namespace bgt = boost::geometry::strategy::transform;
typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
typedef bg::model::box<point_t> box_t;
typedef bg::model::polygon<point_t> polygon_t;

typedef std::vector<polygon_t> poly_collection_t;

#ifndef TUM1_ICAML_ORG
const std::string base_path = "/mnt/G/datasets/atlas/";
const std::string vector_base_path = "/mnt/G/datasets/vector/";
const std::string experiments_path =
    "/home/moritz/workspace/bgdm/globimap/experiments/";
#else
const std::string base_path = "/home/moritz/tf/pointclouds_2d/data/";
const std::string experiments_path = "/home/moritz/tf/globimap/experiments/";
const std::string vector_base_path = "/home/moritz/tf/vector/";
#endif

std::vector<std::string> polygon_sets{
    "tl_2017_us_zcta510/tl_2017_us_zcta510",
    "Global_LSIB_Polygons_Detailed/Global_LSIB_Polygons_Detailed"};

std::vector<polygon_t> get_polygons(const std::string &filename) {
  std::vector<polygon_t> res;
  importSHP(filename, [&](SHPObject *shp, int start, int end) {
    if (shp->nSHPType == SHPT_POLYGON) {
      polygon_t poly;
      for (auto i = start; i < end; ++i) {
        bg::append(poly.outer(), point_t(shp->padfX[i], shp->padfY[i]));
      }
      res.push_back(poly);
    }
  });
  return res;
}
std::vector<uint64_t> rasterize_polygon(const polygon_t &poly) {
  std::vector<uint64_t> res;
  rasterize::Rasterizer<point_t> rasta;

  rasta.rasterize(poly, [&](double x, double y) {
    res.push_back((uint64_t)x);
    res.push_back((uint64_t)y);
  });
  return res;
}

std::vector<polygon_t> get_polygons_trans(const std::string &shapefile,
                                          double width, double height) {
  auto polys = get_polygons(shapefile);

  for (auto &poly : polys) {
    bgt::translate_transformer<double, 2, 2> translate(180, 90);
    bgt::scale_transformer<double, 2, 2> scale(1 / 360.0, 1 / 180.0);
    bgt::scale_transformer<double, 2, 2> scale2(width, height);

    bgt::matrix_transformer<double, 2, 2> translateScaleScale2(
        scale2.matrix() * scale.matrix() * translate.matrix());

    polygon_t p3 = poly;
    bg::transform(p3, poly, translateScaleScale2);
  }

  // TODO transform polys
  double min_x = std::numeric_limits<double>::max(),
         min_y = std::numeric_limits<double>::max(),
         max_x = std::numeric_limits<double>::min(),
         max_y = std::numeric_limits<double>::min();

  for (auto poly : polys) {
    box_t box;
    bg::envelope(poly, box);

    min_x = std::min(box.min_corner().get<0>(), min_x);
    min_y = std::min(box.min_corner().get<1>(), min_y);

    max_x = std::max(box.max_corner().get<0>(), max_x);
    max_y = std::max(box.max_corner().get<1>(), max_y);
  }
  std::cout << " min_x: " << min_x << " min_y: " << min_y << " max_x: " << max_x
            << " max_y: " << max_y << std::endl;
  return polys;
}

void make_rasterized_poly_ds_(const std::vector<polygon_t> &polys,
                              const std::string &filename) {
  H5::H5File file(filename, H5F_ACC_TRUNC);
  const hsize_t n_dims = 1;
  const hsize_t n_rows = polys.size();
  H5::DataSpace dataspace(n_dims, &n_rows);

  // target dtype for the file
  H5::IntType item_type(H5::PredType::INTEL_U64);
  H5::VarLenType file_type(item_type);

  // dtype of the generated data
  H5::IntType mem_item_type(H5::PredType::INTEL_U64);
  H5::VarLenType mem_type(item_type);

  H5::DataSet dataset = file.createDataSet("raster", file_type, dataspace);

  std::vector<std::vector<uint64_t>> data;
  data.reserve(n_rows);

  // this structure stores length of each varlen row and a pointer to
  // the actual data
  std::vector<hvl_t> varlen_spec(n_rows);

  std::cout << "polygons: " << polys.size() << " ..." << std::endl;
  auto P = tq::tqdm(polys);
  P.set_prefix("raster check for polygons ");
  hsize_t idx = 0;
  for (const auto &p : P) {
    auto raster = rasterize_polygon(p);
    auto size = raster.size();
    data.push_back(raster);

    varlen_spec.at(idx).len = size;
    varlen_spec.at(idx).p = (void *)&data.at(idx).front();
    idx++;
  }
  std::cout << "write dataset " << filename << std::endl;
  dataset.write(&varlen_spec.front(), mem_type);
}
void make_rasterized_poly_ds(const std::vector<polygon_t> &polys,
                             const std::string &pathname) {

  const hsize_t n_dims = 1;
  const hsize_t n_rows = polys.size();

  std::vector<std::vector<uint64_t>> data;
  data.reserve(n_rows);

  std::cout << "polygons: " << polys.size() << " ..." << std::endl;
  auto P = tq::tqdm(polys);
  P.set_prefix("rasterize polygons ");

  mkdir((pathname).c_str(), 0777);
  uint idx = 0;
  for (const auto &p : P) {
    auto raster = rasterize_polygon(p);
    std::stringstream ss;
    ss << pathname << "/" << std::setw(8) << std::setfill('0') << idx;
    auto filename = ss.str();
    // std::cout << "\n\nwrite dataset " << filename << std::endl;
    std::ofstream ofile(filename, std::ios::binary);
    Archive<std::ofstream> a(ofile);
    a << raster;
    // std::cout << "close " << filename << std::endl;
    ofile.close();
    idx++;
  }
  std::cout << "end " << pathname << std::endl;
}
void make_rasterized_poly_ds__(const std::vector<polygon_t> &polys,
                               const std::string &filename) {

  const hsize_t n_dims = 1;
  const hsize_t n_rows = polys.size();

  std::vector<std::vector<uint64_t>> data;
  data.reserve(n_rows);

  std::cout << "polygons: " << polys.size() << " ..." << std::endl;
  auto P = tq::tqdm(polys);
  P.set_prefix("raster check for polygons ");
  uint idx = 0;
  for (const auto &p : P) {
    auto raster = rasterize_polygon(p);
    data.push_back(raster);
    idx++;
  }

  std::ofstream ofile(filename, std::ios::binary);
  Archive<std::ofstream> a(ofile);
  a << data;
  std::cout << "close " << filename << std::endl;
  ofile.close();
}

int main() {
  const uint max_i = 6;
  for (auto i = 0; i < max_i; i += 1) {
    uint mul = (uint)powl(2, i);
    uint width = mul * 2048, height = mul * 2048;
    std::cout << i << "/" << max_i << " -> " << mul << " -> " << width << "x"
              << height << std::endl;
    for (auto shp : polygon_sets) {
      std::string shp_short = shp.substr(shp.find("/"));
      shp = vector_base_path + shp;
      std::stringstream ss;
      ss << vector_base_path << shp_short << "-" << width << "x" << height;

      if (file_exists(ss.str())) {
        std::cout << "path already exists: " << ss.str() << std::endl;
      } else {

        std::cout << shp << "... " << std::endl;
        auto polys = get_polygons_trans(shp, width, height);
        std::cout << shp_short << ": " << polys.size() << std::endl;
        // << ".bin";
        make_rasterized_poly_ds(polys, ss.str());
        std::cout << "\n\ndone writing " << ss.str() << std::endl;
      }
    }
  }
};


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

#include <boost/simd/arithmetic/functions/minmod.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/min.hpp>
#include <boost/simd/include/functions/simd/is_gez.hpp>
#include <boost/simd/include/functions/simd/is_nltz.hpp>
#include <boost/simd/include/functions/simd/if_else_zero.hpp>
#include <boost/simd/include/functions/simd/bitwise_xor.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( minmod_, tag::cpu_
                                   , (A0)(X)
                                   , ((simd_<int_<A0>,X>))((simd_<int_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return if_else_zero(is_gez(b_xor(a0, a1)),boost::simd::min(a0, a1));
    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( minmod_, tag::cpu_
                                   , (A0)(X)
                                   , ((simd_<uint_<A0>,X>))((simd_<uint_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return  boost::simd::min(a0,a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( minmod_, tag::cpu_
                                   , (A0)(X)
                                   , ((simd_<floating_<A0>,X>))((simd_<floating_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return if_else_zero(is_nltz((a0*a1)),boost::simd::min(a0, a1));
    }
  };
} } }

#endif


/*****************************************************************************/
/*  Copyright (c) 2016, Alessandro Pieropan                                  */
/*  All rights reserved.                                                     */
/*                                                                           */
/*  Redistribution and use in source and binary forms, with or without       */
/*  modification, are permitted provided that the following conditions       */
/*  are met:                                                                 */
/*                                                                           */
/*  1. Redistributions of source code must retain the above copyright        */
/*  notice, this list of conditions and the following disclaimer.            */
/*                                                                           */
/*  2. Redistributions in binary form must reproduce the above copyright     */
/*  notice, this list of conditions and the following disclaimer in the      */
/*  documentation and/or other materials provided with the distribution.     */
/*                                                                           */
/*  3. Neither the name of the copyright holder nor the names of its         */
/*  contributors may be used to endorse or promote products derived from     */
/*  this software without specific prior written permission.                 */
/*                                                                           */
/*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS      */
/*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT        */
/*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR    */
/*  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT     */
/*  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,   */
/*  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT         */
/*  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,    */
/*  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY    */
/*  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT      */
/*  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE    */
/*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.     */
/*****************************************************************************/

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <fstream>
#include <iomanip>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <random>
#include <cmath>

using namespace std;
using namespace cv;
using namespace Eigen;

double deg2rad(double deg) {
    return deg * M_PI / 180.0;
}

template <typename T>
string toString(Mat& mat) {
  stringstream ss;
  ss << fixed << setprecision(3);
  for (auto i = 0; i < mat.rows; ++i) {
    for (auto j = 0; j < mat.cols; ++j) {
      ss << mat.at<T>(i, j) << " ";
    }
    ss << "\n";
  }

  return ss.str();
}

void printKalman(KalmanFilter& kf) {
  ofstream file("/home/alessandro/debug/pkf.txt");

  Mat prediction = kf.predict();

  file << "transition matrix: " << toString<float>(kf.transitionMatrix) << "\n"
       << "post " << toString<float>(kf.statePost) << "\n"
       << "predict " << toString<float>(prediction) << endl;

  Mat_<float> measurement(6, 1);
  measurement(0) = 0.01;
  measurement(1) = 0;
  measurement(2) = 0;
  measurement(3) = 0;
  measurement(4) = 0;
  measurement(5) = 0;

  Mat estimated = kf.correct(measurement);

  file << "measurements: " << toString<float>(measurement) << "\n"
       << "post " << toString<float>(kf.statePost) << "\n"
       << "estimated " << toString<float>(estimated) << endl;

  file.close();

}

KalmanFilter initKalmanPose() {
  KalmanFilter kalman_pose_pnp_(18, 6);

  Mat A = Mat::eye(18, 18, CV_32FC1);

  double dt = 1;

  for (auto i = 0; i < 9; ++i) {
    auto id_vel = i + 3;
    auto id_acc = i + 6;
    auto id_vel2 = i + 12;
    auto id_acc2 = i + 15;

    if (id_vel < 9) A.at<float>(i, id_vel) = dt;
    if (id_acc < 9) A.at<float>(i, id_acc) = 0.5 * dt * dt;
    if (id_vel2 < 18) A.at<float>(i + 9, id_vel2) = dt;
    if (id_acc2 < 18) A.at<float>(i + 9, id_acc2) = 0.5 * dt * dt;
  }

  kalman_pose_pnp_.transitionMatrix = A.clone();

  kalman_pose_pnp_.measurementMatrix.at<float>(0, 0) = 1;
  kalman_pose_pnp_.measurementMatrix.at<float>(1, 1) = 1;
  kalman_pose_pnp_.measurementMatrix.at<float>(2, 2) = 1;
  kalman_pose_pnp_.measurementMatrix.at<float>(3, 9) = 1;
  kalman_pose_pnp_.measurementMatrix.at<float>(4, 10) = 1;
  kalman_pose_pnp_.measurementMatrix.at<float>(5, 11) = 1;

  setIdentity(kalman_pose_pnp_.processNoiseCov, Scalar::all(1e-1));
  setIdentity(kalman_pose_pnp_.measurementNoiseCov, Scalar::all(1e-2));
  setIdentity(kalman_pose_pnp_.errorCovPost, Scalar::all(.1));

  kalman_pose_pnp_.measurementNoiseCov.at<float>(0, 0) = 5;
  kalman_pose_pnp_.measurementNoiseCov.at<float>(1, 1) = 1;
  kalman_pose_pnp_.measurementNoiseCov.at<float>(2, 2) = 1;
  kalman_pose_pnp_.measurementNoiseCov.at<float>(3, 3) = 1;
  kalman_pose_pnp_.measurementNoiseCov.at<float>(4, 4) = 1;
  kalman_pose_pnp_.measurementNoiseCov.at<float>(5, 5) = 1;

  kalman_pose_pnp_.statePre.at<float>(0) = 0;
  kalman_pose_pnp_.statePre.at<float>(1) = 0;
  kalman_pose_pnp_.statePre.at<float>(2) = 0;
  kalman_pose_pnp_.statePre.at<float>(3) = 0;
  kalman_pose_pnp_.statePre.at<float>(4) = 0;
  kalman_pose_pnp_.statePre.at<float>(5) = 0;
  kalman_pose_pnp_.statePre.at<float>(6) = 0;
  kalman_pose_pnp_.statePre.at<float>(7) = 0;
  kalman_pose_pnp_.statePre.at<float>(8) = 0;
  kalman_pose_pnp_.statePre.at<float>(9) = 0;
  kalman_pose_pnp_.statePre.at<float>(10) = 0;
  kalman_pose_pnp_.statePre.at<float>(11) = 0;
  kalman_pose_pnp_.statePre.at<float>(12) = 0;
  kalman_pose_pnp_.statePre.at<float>(13) = 0;
  kalman_pose_pnp_.statePre.at<float>(14) = 0;
  kalman_pose_pnp_.statePre.at<float>(15) = 0;
  kalman_pose_pnp_.statePre.at<float>(16) = 0;
  kalman_pose_pnp_.statePre.at<float>(17) = 0;

  return kalman_pose_pnp_;
}

void testKalmanPose()
{

    ofstream file("/home/alessandro/debug/pkf.txt");
    random_device rd;
    default_random_engine dre(rd());

    std::uniform_real_distribution<float> dist(0, 1);

    float tx = 1;
    float tx_acc =0;
    float rx_acc = 0;
    float rx = deg2rad(2);

    uniform_real_distribution<float> deg_dist(0, rx/4);

    KalmanFilter kfp = initKalmanPose();

    file << toString<float>(kfp.measurementNoiseCov) << "\n";

    for(auto i = 0; i < 5; ++i)
    {
        float val = dist(dre);

        tx_acc += val + tx;
        rx_acc += rx + deg_dist(dre);

        Mat_<float> measurement(6, 1);
        measurement(0) = tx_acc;
        measurement(1) = 0;
        measurement(2) = 0;
        measurement(3) = 0;
        measurement(4) = 0;
        measurement(5) = rx_acc;

        Mat predictions = kfp.predict();
        Mat corrections = kfp.correct(measurement);
        Mat pred_t, mea_t, corr_t;
        transpose(predictions, pred_t);
        transpose(corrections, corr_t);
        transpose(measurement, mea_t);

        file << "predictions \n" << toString<float>(pred_t);
        file << "measurements \n" << toString<float>(mea_t);
        file << "corrections \n" << toString<float>(corr_t) << "\n";
    }

    file.close();

}

void test_eigen() {
  Matrix3f m;

  m(0,0) = 1;
  m(0,1) = -0.001;
  m(0,2) = 0.006;

  m(1,0) = 0.001;
  m(1,1) = 1;
  m(1,2) = -0.001;

  m(2,0) = -0.006;
  m(2,1) = 0.001;
  m(2,2) = 1;

  cout << "original rotation:" << endl;
  cout << m << endl << endl;

  Vector3f ea = m.eulerAngles(2, 1, 0);
  cout << "to Euler angles:" << endl;
  cout << ea << endl << endl;

  Matrix3f n;
  n = AngleAxisf(ea[0], Vector3f::UnitZ()) *
      AngleAxisf(ea[1], Vector3f::UnitY()) *
      AngleAxisf(ea[2], Vector3f::UnitX());
  cout << "recalc original rotation:" << endl;
  cout << n << endl;
}

float mouse_x, mouse_y;

void CallBackFunc(int event, int x, int y, int flags, void* userdata) {
  if (event == EVENT_LBUTTONDOWN) {
    //cout << "Left button of the mouse is clicked - position (" << x << ", " << y
    //     << ")" << endl;
  } else if (event == EVENT_RBUTTONDOWN) {
    //cout << "Right button of the mouse is clicked - position (" << x << ", "
    //     << y << ")" << endl;
  } else if (event == EVENT_MBUTTONDOWN) {
    //cout << "Middle button of the mouse is clicked - position (" << x << ", "
    //     << y << ")" << endl;
  } else if (event == EVENT_MOUSEMOVE) {
//    cout << "Mouse move over the window - position (" << x << ", " << y << ")"
//         << endl;
    mouse_x = x;
    mouse_y = y;
  }
}

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

    //testKalmanPose();

    //test_eigen();

  KalmanFilter KF(4, 2, 0);
  KF.transitionMatrix =
      *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);

  Mat_<float> measurement(2, 1);
  measurement.setTo(Scalar(0));

  // init...
  KF.statePre.at<float>(0) = 320;
  KF.statePre.at<float>(1) = 240;
  KF.statePre.at<float>(2) = 0;
  KF.statePre.at<float>(3) = 0;
  setIdentity(KF.measurementMatrix);
  setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
  setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
  setIdentity(KF.errorCovPost, Scalar::all(.1));

  cout << fixed << setprecision(2) << "process noise cov \n"
       << KF.processNoiseCov << " \n"
       << "measurementNoiseCov \n" << KF.measurementNoiseCov << "\n"
       << "errorCovPost \n" << KF.errorCovPost << endl;

  return 0;

  // Create a window
  namedWindow("Kalman tutorial", 1);

  // set the callback function for any mouse event
  setMouseCallback("Kalman tutorial", CallBackFunc, NULL);

  Mat res_image(480, 640, CV_8UC3, Scalar(0, 0, 0));

  mouse_x = 320;
  mouse_y = 240;

  vector<Point> mouse_line;
  vector<Point> estimated_line;

  bool running = true;
  while (running) {
    Mat res_image(480, 640, CV_8UC3, Scalar(0, 0, 0));

    // First predict, to update the internal statePre variable
    Mat prediction = KF.predict();
    Point predictPt(prediction.at<float>(0), prediction.at<float>(1));

    // Get mouse point
    measurement(0) = mouse_x;
    measurement(1) = mouse_y;

    Point measPt(measurement(0), measurement(1));

    // The "correct" phase that is going to use the predicted value and our
    // measurement
    Mat estimated = KF.correct(measurement);
    Point statePt(estimated.at<float>(0), estimated.at<float>(1));

    mouse_line.push_back(measPt);
    estimated_line.push_back(statePt);

    if (mouse_line.size() > 0) {
      polylines(res_image, mouse_line, false, Scalar(255, 0, 0));
      polylines(res_image, estimated_line, false, Scalar(0, 255, 0));
    }

    // circle(res_image, measPt, 1, Scalar(255,0,0), -1);
    // circle(res_image, statePt, 1, Scalar(0,255,0), -1);

    imshow("Kalman tutorial", res_image);

    auto c = waitKey(100);

    if (c == 'q') break;
  }

  return 0;
}


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

#define BOOST_TEST_MODULE GradTest
#include <boost/test/unit_test.hpp>

using namespace learning;

boost::function<float (int x)> f;

struct two_x_squared {
  float operator()(int x) const { return ((float)2*x*x); };
};

BOOST_AUTO_TEST_CASE(grad_test)
{
    f = two_x_squared();

    std::function<void(float)> f_prime = learning::grad(f)

	BOOST_CHECK_EQUAL( f_prime(2), 8 );
}


#include <vector>
#include "./helpers.hpp"

#include <algorithm>
#include <numeric>
#include<boost/foreach.hpp>
#include <random>

#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>

using namespace boost::accumulators;
using namespace std;

/*
Innercode: over GF2M
*/

template<class Innercode, class Outercode>
class EnDecode {
	//private:
	public:
	void outercode_stats(const vector< vector< typename Innercode::Symbol > >& segm_ordered);
	Innercode innercode;
	Outercode outercode;
	typedef typename Outercode::Symbol GFO;
	typedef typename Innercode::Symbol GFI;
	typedef typename Outercode::Symbol::uint uint;
	DNAmapGF<GFI> dnamap;
	vector< vector<GFI> > groundtruth;
	const unsigned l; // length of index
	//const uint gfim = GFI::m;
	//const unsigned len = 4;
	//const uint nbitsindex = 24;
	//const uint nbitsindex = 24;
	typedef GF2M<uint, 4*GFI::m ,0> GFUINT; // l*GFI::m = number bits of index
	typedef GF2M<uint, 4*GFO::m ,0> GFINT; // 48/12 = 4
	//typedef GF2M<uint,42,0> GFINT; // 56/14 = 4, 42/14 = 3

	unsigned m;
	unsigned nuss; // number of symbols of outer code per molecule
    unsigned numblocks;

	EnDecode(){};
	EnDecode(const Innercode& innercode_, const Outercode& outercode_ ,unsigned l_,unsigned nuss_):
	l(l_)
	{
		innercode = innercode_;
		outercode = outercode_;
		nuss = nuss_;
		assert(innercode.n>l);
		m = innercode.k - l;

	};

	void encode(string& str, vector<string>& urn);

	void decode(string& str, const vector<string>& drawnseg, const string& gtruthfile);
};

////////////////////////////
template<class Innercode, class Outercode>
void EnDecode<Innercode,Outercode>::encode(string& str, vector<string>& urn){

	typedef GF2M<uint,8,0> GFSTR;

	// convert the string to symbols in the outer code

	vector<GFSTR> strnu; // string in numbers
	for(unsigned char c: str){
		unsigned nu = (unsigned) c;
		assert(nu < 256);
		strnu.push_back(GFSTR( nu , 0));
	}
	vector<GFO> datao;

	GFM2GFN<GFSTR,GFO>(strnu,datao); // convert string

	// add the filelength at the beginning:
	assert(GFO::m >= 10); // then the filesize can be 2^(3*10)
	vector<GFO> filesize(3,GFO(0,0)); // length is 3
	vector<GFINT> filesizeint(1,GFINT( str.size() ,0));

	GFM2GFN<GFINT,GFO>( filesizeint ,filesize);
	filesize.resize(3); // make sure length is 3
	cout << "Filesize in Byte: " << str.size() << endl;
	datao.insert(datao.begin() , filesize.begin() , filesize.end() );

	// determine blocklength
	numblocks = 1;
	while( datao.size() > nuss * outercode.k * numblocks ) numblocks++;
	// datao has to grow such that nuss * Outercode::k * numblocks

	while( datao.size() > nuss * outercode.k * numblocks ) datao.push_back(GFO(0));

	urn.resize(numblocks*outercode.n);

	// add pseudorandom numbers
	mt19937 rng( 5489U ) ; // constructed with seed 5489U
	uniform_int_distribution<uint> randgfe(0, 1<<GFO::m - 1 );

    for(unsigned b=0;b<numblocks;++b){ // run over blocks
		cout << "encode block " << b << endl;
		// encode outer codes, add pseudorandom numbers
		vector< vector<GFO> > c(nuss); // outer codeword for given block
    	for(unsigned nb=0;nb<nuss;nb++){
		cout << "\t encode part  " << nb << endl;
			// outer information vector
        	vector<GFO> infvec( datao.begin()+(b*nuss+nb)*outercode.k ,
		                   		datao.begin()+(b*nuss+nb+1)*outercode.k );
			// encode the information, obtain outer codeword
			//cout << infvec << endl;
			outercode.RS_shortened_encode(infvec,c[nb]);
			// add pseudorandom element to each element of the codeword
			for(GFO& gfo: c[nb])
				// gfo += GFO(randgfe(rng),0);
				gfo += GFO( rng() % (1<<GFO::m) ,0);
		}

	// add index, encode inner code
        for(unsigned i=0;i<outercode.n;++i){ // run over elements in a block
		vector<GFO> infmol(nuss);
            	for(unsigned nb=0;nb<nuss;++nb){
			infmol[nb] = c[nb][i];
		}
		// tranform from GFO to GFI
		vector<GFI> infmolin;

		GFM2GFN<GFO,GFI>(infmol,infmolin);
		//cout << "infmolin: " << infmol << endl;
    // compute index
		vector<GFUINT> index(1, GFUINT( i+outercode.n*b ,0) );
		vector<GFI> indexgfi;
		GFM2GFN<GFUINT,GFI>(index,indexgfi);
		// add pseudorandom sequence to index
		mt19937 rng( 5489U ) ; // constructed with seed 5489U
		uniform_int_distribution<uint> randgfe(0, 1<<GFI::m - 1 );
		for(GFI& gfi: indexgfi)
			//gfi += GFI(randgfe(rng),0); // different behavior at diff system
			gfi += GFI( rng() % (1<<GFI::m) ,0);

		// construct information of inner codeword: [information | index | information]
		//vector<GFI> infveci;
		//infveci.insert(infveci.end(), infmolin.begin(), infmolin.begin() + infmolin.size()/2);
		//infveci.insert(infveci.end(), indexgfi.begin(), indexgfi.end());
		//infveci.insert(infveci.end(), infmolin.begin() + infmolin.size()/2, infmolin.end());

		// construct information of inner codeword: [information | index ]
		vector<GFI> infveci;

		// index first
		infveci.insert(infveci.end(), indexgfi.begin(), indexgfi.end());
		infveci.insert(infveci.end(), infmolin.begin(), infmolin.end());

		assert(infveci.size() == innercode.k);
    // encode the inner code, write it to urn[i]
    vector<GFI> icw;
		innercode.RS_shortened_encode(infveci,icw);
		dnamap.cw2frag(icw,urn[i+outercode.n*b]);
		flipvecdir(urn[i+outercode.n*b]);
        }
		cout << "encoded block " << b << " of " << numblocks << endl;
    }
}

template<class T>
bool pairCompare(const T & x, const T & y) {
  return x.second < y.second;
}

////////////////////////////
template<class Innercode, class Outercode>
void EnDecode<Innercode,Outercode>::decode(string& str, const vector<string>& drawnseg, const string& gtruthfile){
	/*
	Inner decoding, for each read:
		- inner decode, obtain inner information, extract index
		- for each index we keep a map which counts how many times an information has been
		  observed with that index. In that map, we only keep the information that has a
		  minimal number of errors, out ot all molecules we have seen with that index
	After going though all reads, choose the one for a given index, that has been observed
	most frequently.
	*/

   	////
	// read groundtruth if available for making tests
	if(gtruthfile.size() > 0){
		vector<string> drawnseg_gtruth;
		string sLine = "";
		ifstream in;
		in.open(gtruthfile.c_str());
		while (!in.eof()){
			getline(in, sLine);
			drawnseg_gtruth.push_back(sLine);
		}
		drawnseg_gtruth.resize(drawnseg_gtruth.size()-1); // erase the last, empty line
		// run over all reads
		for(const string& read: drawnseg_gtruth){
			vector<GFI> rececw;		// read: inner codeword
			string flipedread = read;
			flipvecdir(flipedread);
			dnamap.frag2cw(rececw,flipedread); 	// map read to GFI vector

			// decode molecule
			pair<unsigned,unsigned> erctrictmp; // for error counts
			vector<GFI> irec(innercode.k); // recovered information from inner codeword
			erctrictmp = innercode.RS_shortened_decode(irec,rececw);

		 	groundtruth.push_back(irec); // assume the groundtruth is in the right order according to the index
		}
		cout << "Read groundtruth for evaluation, " << drawnseg_gtruth.size() << " sequences" << endl;
	}

	//ofstream cand_file("candidates.dat");
	////

	// to keep track
	pair<unsigned,unsigned> erctric(0,0);
	vector<int> frag_freqs(numblocks*outercode.n, 0); // to count the frequencies of each DNA sequence
	unsigned ic_decerr = 0; // counts the inner decoding errors
	unsigned errctr = 0;
	unsigned numfrag = 0;

	typedef typename Innercode::Symbol GFI;
	typedef map<vector<GFI>, unsigned > seqctrmap; // information -> how many times it appeared
	vector< seqctrmap > segm_ordered_map(numblocks*outercode.n, seqctrmap() );
	// the minimal number of errors as reported by inner decoder for all molecules with given index
	vector<unsigned> curerr(numblocks*outercode.n, innercode.n ); // error of the element in segm_ordered_map;

	// run over all reads
	for(const string& read: drawnseg){
		vector<GFI> rececw;		// read: inner codeword
		string flipedread = read;
		flipvecdir(flipedread);
		dnamap.frag2cw(rececw,flipedread); 	// map read to GFI vector

		// decode molecule
		pair<unsigned,unsigned> erctrictmp; // for error counts
		vector<GFI> irec(innercode.k); // recovered information from inner codeword
		erctrictmp = innercode.RS_shortened_decode(irec,rececw);

		if(erctrictmp.second == innercode.n_u){ // decoding error
			ic_decerr++;
		} else {
			erctric.first += erctrictmp.first;
			erctric.second += erctrictmp.second;
			// extract index
			//// information part of the inner codeworkd is [information | index | information]
			//unsigned len = (innercode.k-l)/2;
			//vector<GFI> indexgfi(irec.begin()+len, irec.begin()+len+l);
			// information part of the inner codeworkd is [information | index]
			//vector<GFI> indexgfi(irec.end()-l-1, irec.end());
			vector<GFI> indexgfi(irec.begin(), irec.begin() + l);

			// remove pseudorandom sequence from index
			mt19937 rng( 5489U ) ; // constructed with seed 5489U
			uniform_int_distribution<uint> randgfe(0, 1<<GFI::m - 1 );
			for(GFI& gfi: indexgfi)
				//gfi -= GFI( randgfe(rng) ,0); // different behavior at diff system
				gfi -= GFI( rng() % (1<<GFI::m)  ,0);

			vector<GFUINT> index;
			GFM2GFN<GFI,GFUINT>(indexgfi,index);
			//unsigned maxindex = 1<<(GFI::m*l);
			//unsigned ind = ((int)index[0].el - maxindex/3); // % maxindex;
			//cout << ind << " " << maxindex << endl;
			unsigned ind = (unsigned)index[0].el; // index
			if(ind >= numblocks*outercode.n){
				// if the index is not in the appropriate range, there must be an error
				errctr ++;
			} else { // index can be right
				//curerr[ind] = 0;
				//////
				if(erctrictmp.second < curerr[ind]){
					// found information with smaller error than previously seen, thus discard map
					curerr[ind] = erctrictmp.second;
					segm_ordered_map[ind].empty();
					segm_ordered_map[ind].insert(make_pair(irec,1));
				} else if (erctrictmp.second == curerr[ind]){
					typename seqctrmap::iterator cur = segm_ordered_map[ind].find(irec);
					if(cur != segm_ordered_map[ind].end()){ // segment already appeared
						cur->second++;
					} else {
						segm_ordered_map[ind].insert(make_pair(irec,1));
					}
				} // else: (erctrictmp.second > curerr[ind]), nothing to do
				//////

				// Just decide by frequency:
				/*
				if(erctrictmp.second <= innercode.n - innercode.k){ // make sure that not really off..
					typename seqctrmap::iterator cur = segm_ordered_map[ind].find(irec);
					if(cur != segm_ordered_map[ind].end()){ // segment already appeared
						cur->second++;
					} else {
						segm_ordered_map[ind].insert(make_pair(irec,1));
					}
				}
				*/

				// if groundtruth given, compare to groundtruth whether there is a decoding error
				/*
				if(! groundtruth.empty() ){
					if(groundtruth[ind] != irec) {
						errctr++;
					} else {
						// this fragment has been decoded without error
						frag_freqs[ind]++;
					}
				}
				*/
				/////////////////////////
				/*
				// write to candidate output, as a test:
				assert(irec.size() == innercode.k);
				// encode the inner code, write it to urn[i]
				vector<GFI> icw;
				innercode.RS_shortened_encode(irec,icw);
				string str;
				dnamap.cw2frag(icw,str);
				flipvecdir(str);
				cand_file << str << endl;
				*/
				/////////////////////////

			}

		}
		numfrag++;
	} // run over all reads

	//cand_file.close();

	// take the one (out of all segments with smallest error) with largest frequency as the estimate,
	// store it in segm_ordered
	vector< vector<GFI> > segm_ordered(numblocks*outercode.n, vector<GFI>());
	for(unsigned ind=0;ind<segm_ordered_map.size();++ind){
		//cout << endl;
		//for (auto const& x : segm_ordered_map[ind]){
       	//	cout << x.second << endl;
		//}

		if(! segm_ordered_map[ind].empty() ){
			typename seqctrmap::iterator max = max_element(segm_ordered_map[ind].begin(), segm_ordered_map[ind].end(), pairCompare<typename seqctrmap::value_type> );
		//	cout << "best: " << max->second << endl;
			segm_ordered[ind] = max->first;
		}
	}

	//cout << "read error prob: " << (float) errctr / (float) numfrag << endl;
	cout << "number of reads: " << numfrag << endl;
	//cout << "inner code: " << (float)erctric.first / (float) numfrag << " erasures on average corrected" << endl;
	cout << "inner code: " << (float)erctric.second/ (float) numfrag << " errors on average corrected per sequence" << endl;
	//cout << "inner code, dec. errors: " << ic_decerr << endl;

	int ct = 0;
	for(unsigned i=0;i<curerr.size();++i){
		//cout << i << " " << curerr[i] << endl;
		if( curerr[i] >= (float)(innercode.n - innercode.k)/2.0 ){ // then better view as errasure
		//if( curerr[i] >= 1 ){
		//if( curerr[i] >= (innercode.n - innercode.k) ){
			segm_ordered_map[i] = seqctrmap();
			ct++;
			segm_ordered[i] = vector<GFI>();
		}
	}
	// cout << "errasures: " << ct << endl;

	if(gtruthfile.size() > 0){
		// error statistics based on groundtruth
		outercode_stats(segm_ordered);
	}

	///// decode the blocks
	vector<GFO> inf;

	// for removing pseudorandom numbers
	mt19937 rng( 5489U ) ; // constructed with seed 5489U
	uniform_int_distribution<uint> randgfe(0, 1<<GFO::m - 1 );

	for(unsigned b=0;b<numblocks;++b){ // run over blocks
		// obtain list of codewords
		vector< vector<GFO> > cw(nuss, vector<GFO>(outercode.n) ); // outer codewords for given block
		unsigned erasure_ctr = 0;
		for(unsigned i =0;i<outercode.n;++i){ // run over elements in a block
			if( ! segm_ordered[b*outercode.n+i].empty()){

				// extract information vector
				const vector<GFI>& inf = segm_ordered[b*outercode.n + i];
				//unsigned len = (innercode.k-l)/2;
				vector<GFI> infi;
				// information is [information | index]
				//infi.insert(infi.end(), inf.begin(), inf.begin() + len);
				//infi.insert(infi.end(), inf.begin()+len+l+1, inf.end());
				//infi.insert(infi.end(), inf.begin(), inf.end()-l);
				infi.insert(infi.end(), inf.begin()+l, inf.end());
				vector<GFO> info;
				GFM2GFN<GFI,GFO>(infi,info); // information vector in

				for(unsigned nb=0;nb<nuss;++nb)
					cw[nb][i] = info[nb];

			} else { // else we have an erasure
				for(unsigned nb=0;nb<nuss;++nb)
					cw[nb][i] = GFO();
				erasure_ctr++;
			}
		}

		cout << erasure_ctr << " many erasures in block " << b << " of length " << outercode.n <<  endl;

		///// decode outer codword
		cout << "remove random sequence" << endl;
		for(unsigned nb=0;nb<nuss;++nb){
			// add pseudorandom element to each element
			for(GFO& gfo: cw[nb])
				if( ! gfo.isempty() )
					// gfo -= GFO(randgfe(rng),0); // different behavior at diff system
					gfo -= GFO( rng() % (1<<GFO::m) ,0);
				else // errasure
					rng();
					//randgfe(rng); // draw to increase state of rng // different behavior at diff system
		}

		cout << "decoding block " << b << endl;
		vector< vector<GFO> > infrec(nuss);
		for(unsigned nb=0;nb<nuss;++nb){
			//pair<unsigned,unsigned> erctroc = outercode.RS_decode_spec(infrec[nb],cw[nb]);
			pair<unsigned,unsigned> erctroc = outercode.RS_shortened_decode(infrec[nb],cw[nb]);
			cout << "\tpart " << nb << "/" << nuss << endl;
			if(nb == nuss-1){
				cout << "\touter code: " << erctroc.first << " errasures, " << erctroc.second << " errors corrected"<< endl;
			}
		}

		// add information to string
		for( const vector<GFO>& cn : infrec )
			inf.insert( inf.end(), cn.begin(), cn.end() );

	} // for(unsigned b=0

	vector<GFINT> filesizeint(0,0);
	vector<GFO> filesizegfo(inf.begin(),inf.begin()+3);
	GFM2GFN<GFO,GFINT>(filesizegfo,filesizeint);
	unsigned filesize = filesizeint[0].el;
	inf = vector<GFO>(inf.begin()+3, inf.end());

	typedef GF2M<uint,8,0> GFSTR;
	// convert the string to symbols in the outer code
	vector<GFSTR> strnu; // string in numbers
	GFM2GFN<GFO,GFSTR>(inf,strnu);

	str.resize(0);
	for(GFSTR ch : strnu)
		str += (char)ch.el;
	str.resize(filesize);

}

///// determine error probabilities outer code, based on groundtruth
template<class Innercode, class Outercode>
void EnDecode<Innercode,Outercode>::outercode_stats(const vector< vector<typename Innercode::Symbol> >& segm_ordered){
	unsigned total_erasure_ctr = 0;
	unsigned total_error_ctr = 0;
	unsigned total_blockerror_ctr = 0;
	//// determine the number of errors and erasures in each block
	for(unsigned b=0;b<numblocks;++b){
		unsigned block_erasure_ctr = 0;
		unsigned block_error_ctr = 0;
		for(unsigned i =0;i<outercode.n;++i){ // run over elements in a block
			if( ! segm_ordered[b*outercode.n+i].empty()){
            	//compare to groundtruth
				if(! groundtruth.empty() ){
					if(groundtruth[b*outercode.n+i] != segm_ordered[b*outercode.n+i]) block_error_ctr++;
				}
			} // else we have an erasure
            else{
                block_erasure_ctr++;
            }
		}
		total_erasure_ctr += block_erasure_ctr;
		total_error_ctr += block_error_ctr;

		cout << b <<"-> errors: " << block_error_ctr << "	erasures: " << block_erasure_ctr <<"	total:" << block_error_ctr*2+block_erasure_ctr << endl;

		if(block_error_ctr*2+block_erasure_ctr > outercode.n-outercode.k){
			total_blockerror_ctr++;
		}
	}

	cout << "num block errors: " << total_blockerror_ctr << endl;
	cout << "OC: symbol error   prob.: " << (float) total_error_ctr / (float) (outercode.n*numblocks) << endl;
	cout << "OC: symbol erasure prob.: " << (float) total_erasure_ctr / (float) (outercode.n*numblocks) << endl;

	cout << "OC: symbol error   prob.: " << total_error_ctr  << endl;
	cout << "OC: symbol erasure prob.: " << total_erasure_ctr << endl;
}