/* -*-C++-*-
   (c) Copyright 2005-2008, Hewlett-Packard Development Company, LP

   See the file named COPYING for license details
*/

/** @file
    StatsQuantile implementation
*/

#include <algorithm>
#include <sstream>
#include <cstring>

#include <boost/format.hpp>

#include <Lintel/AssertBoost.hpp>
#include <Lintel/Double.hpp>
#include <Lintel/HashMap.hpp>
#include <Lintel/LintelLog.hpp>
#include <Lintel/StatsQuantile.hpp>

using namespace std;
using boost::format;

namespace {
    struct ErrorNbound {
	double quantile_error;
	int64_t nbound;
	ErrorNbound() : quantile_error(0), nbound(0) { }
	ErrorNbound(double a, int64_t b) : quantile_error(a), nbound(b) { }

	uint32_t hash() const {
	    uint32_t a = static_cast<uint32_t>(quantile_error * 1e9);
	    uint32_t b = nbound >> 32;
	    uint32_t c = nbound & 0xFFFFFFFFU;
	    return lintel::BobJenkinsHashMix3(a, b, c);
	}

	bool operator == (const ErrorNbound &rhs) const {
	    return quantile_error == rhs.quantile_error && nbound == rhs.nbound;
	}
    };

    struct BK {
	uint32_t b, k;
	BK() : b(0), k(0) { }
	BK(uint32_t in_b, uint32_t in_k) : b(in_b), k(in_k) { }
    };

    HashMap<ErrorNbound, BK> bk_cache;

    double fact(double a) {
	double ret = 1.0;
	for(double i = 2;i<=a;i++) {
	    ret *= i;
	}
	return ret;
    }

    double choose(int b, int a) { // b choose a
	double ret = fact(b) / (fact(b-a) * fact(a));
	return ret;
    }

    class doubleOrder {
    public:
	inline bool operator() (double a, double b) const {
	    return a < b;
	}
    };

}

StatsQuantile::StatsQuantile(double _quantile_error, int64_t in_nbound, int _print_nrange, bool lazy)
    : quantile_error(_quantile_error), Nbound(in_nbound < 100 ? 100 : in_nbound),
      print_nrange(_print_nrange), lazy(lazy)
{
    // sanity check that we haven't been called in a weird/wrong way.
    INVARIANT(quantile_error < 0.2, format("whoa, quantile_error %.3g >= 0.2??") % quantile_error);
    double tmp_nbound = Nbound; // easy for calculations
    INVARIANT(in_nbound > 1, format("Usage error? StatsQuantile nbound set to %d?") % in_nbound);

    int32_t best_b = -1, best_k = -1;

    // If we are making lots of smallish StatsQuantiles, we can spend
    // too much of our time doing this, so cache the sizes.
    BK *cache = bk_cache.lookup(ErrorNbound(quantile_error, Nbound));

    if (cache != NULL) {
	best_b = cache->b;
	best_k = cache->k;
    } else {
	// The hunt for b = nbuffers, and k = buffer_size is on, following
	// section 4.5 in the paper

	const int32_t max_b = 30;
	const int32_t max_h = 40;
	for(int32_t b=2;b<max_b;b++) {
	    int32_t h = 3;
	    for(;h < max_h;h++) {
		double v = (h-2) * choose(b+h-2,h-1) - choose(b+h-3,h-3) + choose(b+h-3,h-2);
		if (v > 2 * quantile_error * tmp_nbound) {
		    --h; // this one is too high
		    break;
		}
	    }
	    double k = ceil(tmp_nbound / choose(b+h-2,h-1));
	    if (b * k < 1.0e9) {
		if (best_b == -1 || b * k < best_b * best_k) {
		    best_b = b;
		    best_k = (int)k;
		}
	    }
	}
	if (bk_cache.size() > 1000) { // keep the cache small
	    bk_cache.clear();
	}
	bk_cache[ErrorNbound(quantile_error, Nbound)] = BK(best_b, best_k);
    }
    INVARIANT(best_b > 0 && best_k > 0,
	      format("unable to find b/k for %.8g %.8g") % quantile_error % tmp_nbound);

    // lower bound sanity value
    if (best_k < 10) best_k = 10;
    // First requirement makes the logic setting the buffer_level faster
    // second one just seems sane.
    init(best_b, best_k);
}

// fake Nbound, big enough for tests
StatsQuantile::StatsQuantile(const string &,
			     int _nbuffers, int _buffer_size, int _print_nrange)
    : quantile_error(Double::NaN), Nbound(5000000), print_nrange(_print_nrange), lazy(true)
{
    init(_nbuffers, _buffer_size);
}

void StatsQuantile::init(int _nbuffers, int _buffer_size) {
    nbuffers = _nbuffers;
    buffer_size = _buffer_size;
    INVARIANT(nbuffers > 1,
	      "need at least two buffers for code to work correctly");
    INVARIANT(buffer_size >= 10,
	      "buffers smaller than 10 don't make sense.");
    all_buffers = new one_buffer[nbuffers];
    for(int i = 0; i < nbuffers; ++i) {
	all_buffers[i] = NULL;
    }
    buffer_weight = new int64_t[nbuffers];
    buffer_level = new int[nbuffers];
    buffer_sorted = new bool[nbuffers];
    if (!lazy) {
	tmp_buffer = new double[buffer_size];
    } else {
	tmp_buffer = NULL;
    }
    collapse_pos = new int[buffer_size];
    init_buffers();
}

void StatsQuantile::init_buffers() {
    for(int i=0;i<nbuffers;i++) {
	if (all_buffers[i] == NULL && ! lazy) {
	    all_buffers[i] = new double[buffer_size];
	}
	// touch all the space to make sure it is allocated; necessary
	// to avoid demand allocation (which can create long delays
	// when used in Buttress2)
	if (all_buffers[i] != NULL) {
	    for(int j=0;j<buffer_size;j+=32) {
		all_buffers[i][j] = 0.0;
	    }
	}
	buffer_weight[i] = -1;
	buffer_level[i] = -1;
	buffer_sorted[i] = false;
    }

    cur_buffer = 0;
    cur_buffer_pos = 0;
    buffer_weight[cur_buffer] = 1;
    buffer_level[cur_buffer] = 0;
    collapse_even_low = true;
    if (lazy) {
	cur_buffer = -1;
	cur_buffer_pos = buffer_size+1;
    }
}

StatsQuantile::~StatsQuantile() {
    for(int i=0;i<nbuffers;i++) {
	delete[] all_buffers[i];
    }
    delete[] all_buffers;
    delete[] buffer_weight;
    delete[] buffer_level;
    delete[] buffer_sorted;
    delete[] tmp_buffer;
    delete[] collapse_pos;
}

void StatsQuantile::reset() {
    Stats::reset();
    init_buffers();
}

void StatsQuantile::addQuantile(const double value) {
    if (cur_buffer_pos >= buffer_size) {
	cur_buffer += 1;
	cur_buffer_pos = 0;
	if (cur_buffer == nbuffers) {
	    collapse();
	} else if (cur_buffer>0) {
#if 1
	    // do the sort here to amortize the work
	    sort(all_buffers[cur_buffer - 1],
		      all_buffers[cur_buffer - 1]+buffer_size,
		      doubleOrder());
	    buffer_sorted[cur_buffer-1] = true;
#endif
	}
        SINVARIANT(cur_buffer < nbuffers);
	if (lazy && all_buffers[cur_buffer] == NULL) {
	    all_buffers[cur_buffer] = new double[buffer_size];
	}
	buffer_weight[cur_buffer] = 1;
	buffer_level[cur_buffer] = 0;
	if (cur_buffer == nbuffers - 1) {
	    buffer_level[cur_buffer] = buffer_level[cur_buffer - 1];
	}
    }
    buffer_sorted[cur_buffer] = false;
    all_buffers[cur_buffer][cur_buffer_pos] = value;
    cur_buffer_pos += 1;
}

void StatsQuantile::add(const double value) {
    Stats::add(value);
    addQuantile(value);
}

void StatsQuantile::add(const Stats &_stat) {
    const StatsQuantile *stat = dynamic_cast<const StatsQuantile *>(&_stat);
    INVARIANT(stat != NULL, "need another StatsQuantile for add(Stats) to work");

    Stats::add(_stat);

    if ((cur_buffer == -1 ||
	 (cur_buffer == 0 && cur_buffer_pos == 0)) && // we are empty
	buffer_size == stat->buffer_size && // we are compatible
	nbuffers == stat->nbuffers) {
	INVARIANT(countll() == _stat.countll(), "??");
	INVARIANT(stat->cur_buffer < nbuffers, "??");
	for(int i=0; i <= stat->cur_buffer; ++i) {
	    // Will copy some useless stuff in last buffer.
	    if (all_buffers[i] == NULL) {
		all_buffers[i] = new double[buffer_size];
	    }
	    memcpy(all_buffers[i], stat->all_buffers[i],
		   sizeof(double) * buffer_size);
	    buffer_weight[i] = stat->buffer_weight[i];
	    buffer_level[i] = stat->buffer_level[i];
	    buffer_sorted[i] = stat->buffer_sorted[i];
	}
	cur_buffer = stat->cur_buffer;
	cur_buffer_pos = stat->cur_buffer_pos;
	// TODO: determine if we really have to copy collapse_pos
	memcpy(collapse_pos, stat->collapse_pos, sizeof(int) * buffer_size);
	collapse_even_low = stat->collapse_even_low;
	return;
    }

    // There should be a faster way to do this, but this should work
    // correctly.  The faster way should be possible if for no other
    // reason we are inserting the values in order.  In essence we
    // pretent that stat was made up of repeated insertions of the
    // values kept.  Since it's contents are indistinguishable from
    // that actually happening, the merge should be pretty close to
    // correct; this is of course not a formal proof, and I suspect
    // that it is wrong in some special case.

    // TODO: special case bulk loading of lots of the same values;
    // should be able to greatly simplify the addQuantile code to just
    // copy a swath of values in.  Probably can even sort the buffer
    // first, and if we are in order, then just drop everything in and
    // leave the buffer marked sorted.

    for(int i=0; i < stat->cur_buffer; ++i) {
	// cur_buffer is the one we are filling, so all previous ones are full
	double *abuf = stat->all_buffers[i];
	for(int j=0; j < stat->buffer_size; ++j) {
	    double v = abuf[j];
	    for(int64_t k=0; k < stat->buffer_weight[i]; ++k) {
		addQuantile(v);
	    }
	}
    }
    double *abuf = stat->all_buffers[stat->cur_buffer];
    for (int j=0; j < stat->cur_buffer_pos; ++j) {
	double v = abuf[j];
	for(int64_t k=0; k < stat->buffer_weight[stat->cur_buffer]; ++k) {
	    addQuantile(v);
	}
    }
}

// getQuantile is quite expensive for the upper quantiles; in theory
// we ought to be able to do some sort of binary-like search in that case,
// skipping by a lot of the lower quantiles in a batch rather than
// ratcheting through them one by one as in the paper, and the current
// implementation

// const because the only operation on class variables is a sort of buffers
double StatsQuantile::getQuantile(double quantile, bool allow_invalid_nbound) const {
    int64_t count = countll();
    if (count == 0) {
	return Double::NaN;
    }
    INVARIANT(count <= Nbound || allow_invalid_nbound,
	      format("Error: %d (# entries in quantile) > %d (Nbound on quantile)")
	      % count % Nbound);
    INVARIANT(quantile >= 0.0 && quantile <= 1.0, "quantile out of bounds");
    // this is slightly different than the algorithm in the paper; the
    // goal is to allow people to calculate quantiles while the algorithm
    // is running without having to add in and remove the +-\Inf entries
    // that would be needed to make the last buffer have integral size
    // I think this change is correct

    for(int i = 0; i <= cur_buffer; i++) {
	collapse_pos[i] = 0;
	// TODO: add a check that the total weight covered by all the
	// current buffers is exactly the number of elements that we
	// have.  Have to be a little careful on the last buffer to
	// only count the values that we have actually inserted.
	if (!buffer_sorted[i]) {
	    if (i < cur_buffer) {
		sort(all_buffers[i],all_buffers[i] + buffer_size,
			  doubleOrder());
	    } else {
		// only sort part that has values in it.
		sort(all_buffers[i],all_buffers[i] + cur_buffer_pos,
			  doubleOrder());
		// Fill the rest with Inf's so that we don't have to
		// do any special logic when choosing the "smallest"
		// next value, all of the Inf's will come at the end, and
		// if we get to an Inf, then we are done anyway and it
		// doesn't matter if we take one that doesn't actually exist.
		for(int j=cur_buffer_pos;j<buffer_size;j++) {
		    all_buffers[i][j] = Double::Inf;
		}
	    }
	    buffer_sorted[i] = true;
	}
    }
    // the 1e-15 accounts for a minor rounding error that seems to occur
    // when doing division, e.g. ceil(150000.0 * (131058.0 / 150000.0)) on
    // linux = 131059.0; I'd imagine we'll never see 1e+15 values
    // wherein this adjustment would make a difference
    double nentries = countll();
    INVARIANT(nentries < 1e+14,
	      "Rounding error adjustment may start to do something wrong around 1e+14 entries.\n"
	      "May be safe to decrease rounding adjustment to 0.5e-15, but will be getting very\n"
	      "close to the limit of precision in floating point.  How did you get this many\n"
	      "entries into the table??");
    // Subtract 1 because the quantile positions count from 1..n, but
    // C++ arrays index from 0..n-1
    int64_t target_index
	= static_cast<int64_t>(ceil(nentries * (quantile - 1e-15))) - 1;
    if (target_index < 0) target_index = 0;
    if (target_index == nentries) {
	target_index = (long long)(nentries - 1);
    }
    // can be switched back to getQuantileByIndex() if there are difficulties.
    return getQuantileByBinSearchIndex(target_index);
}

int StatsQuantile::collapseFindFirstBuffer() {
    double nentries = countll();
    INVARIANT(nentries < 1e+14, "getQuantile will fail now.");

    int first_buffer;
    for(first_buffer = nbuffers - 1;first_buffer > 0; --first_buffer) {
	if (buffer_level[first_buffer - 1] != buffer_level[first_buffer]) {
	    break;
	}
    }
    INVARIANT(first_buffer < nbuffers - 1,
	      "Whoa, collapse should always operate on at least two buffers");
    INVARIANT(buffer_level[first_buffer] >= 0,
	      "Whoa, buffer level should be positive");
    return first_buffer;
}

// TODO-2010-04-26: remove this and the associated test case; if it's
// lasted this long, then we're not having problems with the new
// binary search version.
double StatsQuantile::getQuantileByIndex(uint64_t target_index) const {
    uint64_t cur_index = 0;

    PriorityQueue<std::pair<double, int>, pairCmp> tmp_pq(nbuffers);
    for(int i=0; i < cur_buffer; ++i) {
	collapse_pos[i] = 0;
	tmp_pq.push(make_pair(collapseVal(i), i));
    }
    if (cur_buffer_pos > 0) {
	collapse_pos[cur_buffer] = 0;
	tmp_pq.push(make_pair(collapseVal(cur_buffer), cur_buffer));
    }

#if LINTEL_DEBUG
    double prev_val = -Double::Inf;
#endif
    while(!tmp_pq.empty()) {
	double min_val = tmp_pq.top().first;
	int min_buffer = tmp_pq.top().second;

#if LINTEL_DEBUG
	SINVARIANT(collapse_pos[min_buffer] < buffer_size &&
		   (min_buffer < cur_buffer || collapse_pos[min_buffer] < cur_buffer_pos));
	INVARIANT(min_val >= prev_val, "Whoa, sort order error?!");
	prev_val = min_val;
#endif

	collapse_pos[min_buffer] += 1;

	// this entry occupies sorted order positions
	// [cur_index .. cur_index + buffer_weight[min_buffer] - 1]
	// so the check is for cur_index > target_index
	// need to think about the logic for this in collapse()
	cur_index += buffer_weight[min_buffer];
	if (cur_index > target_index) {
	    return min_val;
	}
	int max_pos = min_buffer < cur_buffer ? buffer_size : cur_buffer_pos;
	if (collapse_pos[min_buffer] < max_pos) {
	    tmp_pq.replaceTop(make_pair(collapseVal(min_buffer), min_buffer));
	} else {
	    tmp_pq.pop();
	}
    }
    return Double::NaN;
}

namespace {
    struct BinSearchState {
	uint32_t min, max, orig_max, split_lower, split_upper;
	explicit BinSearchState(uint32_t b)
	    : min(0), max(b), orig_max(b)
	{ }
	uint32_t mid() const { return (max + min) / 2; }
	uint32_t remain() const { return max - min; }

	void calculateBounds(double target, double *buffer) {
	    double *lower = lower_bound(buffer + min, buffer + max, target);
	    split_lower = lower - buffer;
	    double *upper = upper_bound(buffer + min, buffer + max, target);
	    split_upper = upper - buffer;
	}
    };

    uint32_t biggestRemaining(const vector<BinSearchState> &state) {
	uint32_t biggest = 0;
	uint32_t remain = state[0].remain();
	for(uint32_t i = 1; i < state.size(); ++i) {
	    if (state[i].remain() > remain) {
		biggest = i;
		remain = state[i].remain();
	    }
	}
	return biggest;
    }
}

double StatsQuantile::getQuantileByBinSearchIndex(uint64_t target_index) const {
    vector<BinSearchState> state;

    // one last buffer if the last one is present
    state.resize(cur_buffer + (cur_buffer_pos > 0 ? 1 : 0), BinSearchState(buffer_size));
    if (cur_buffer_pos > 0) {
	state[cur_buffer].max = state[cur_buffer].orig_max = cur_buffer_pos;
    }

    uint64_t entry_count = 0;
    for(uint32_t i = 0; i < static_cast<uint32_t>(cur_buffer); ++i) {
	entry_count += static_cast<uint64_t>(buffer_weight[i]) * buffer_size;
    }
    entry_count += cur_buffer_pos;
    SINVARIANT(entry_count == countll());

    LintelLogDebug("StatsQuantile::getQuantile", format("entries %d") % entry_count);
    while(true) {
	uint32_t biggest = biggestRemaining(state);
	double split_val = all_buffers[biggest][state[biggest].mid()];
	LintelLogDebug("StatsQuantile::getQuantile", format("biggest %d, split @%.12g")
		       % biggest % split_val);

	uint64_t count_lower = 0;
	uint64_t count_match = 0;
	uint64_t count_upper = 0;
	for(uint32_t i = 0; i < state.size(); ++i) {
	    state[i].calculateBounds(split_val, all_buffers[i]);
	    LintelLogDebug("StatsQuantile::getQuantile",
			   format("  buffer %d wt=%d min=%d, sl=%d, su=%d, max=%d")
			   % i % buffer_weight[i] % state[i].min
			   % state[i].split_lower % state[i].split_upper % state[i].max);
	    count_lower += buffer_weight[i] * state[i].split_lower; // 0 .. split_lower - 1
	    count_match += buffer_weight[i]
		* (state[i].split_upper - state[i].split_lower); // split_lower .. split_upper -1
	    count_upper += buffer_weight[i]
		* (state[i].orig_max - state[i].split_upper); // split_upper .. buffer_size - 1
	}
	LintelLogDebug("StatsQuantile::getQuantile", format("cl=%d, cm=%d, cu=%d")
		       % count_lower % count_match % count_upper);
	SINVARIANT(count_lower + count_match + count_upper == entry_count);

	if (target_index < count_lower) {
	    LintelLogDebug("StatsQuantile::getQuantile", "in-lower");
	    bool any_changed = false;
	    for(uint32_t i = 0; i < state.size(); ++i) {
		if (state[i].max != state[i].split_lower) any_changed = true;
		state[i].max = state[i].split_lower;
		SINVARIANT(state[i].min <= state[i].max);
	    }
	    SINVARIANT(any_changed);
	} else if (target_index < count_lower + count_match) {
	    LintelLogDebug("StatsQuantile::getQuantile", "***match");
	    return split_val; // even if we didn't find the exact match, we're good enough
	} else if (target_index < count_lower + count_match + count_upper) {
	    LintelLogDebug("StatsQuantile::getQuantile", "in-upper");
	    bool any_changed = false;
	    for(uint32_t i = 0; i < state.size(); ++i) {
		if (state[i].min != state[i].split_upper) any_changed = true;
		state[i].min = state[i].split_upper;
		SINVARIANT(state[i].min <= state[i].max);
	    }
	    SINVARIANT(any_changed);
	}
    }
    FATAL_ERROR("unimplemented");
}

int64_t StatsQuantile::collapseSortBuffers(int first_buffer) {
    int64_t total_weight = 0;
    for(int i=first_buffer;i<nbuffers;i++) {
	total_weight += buffer_weight[i];
	INVARIANT(buffer_weight[i] > 0, "Whoa, buffer_weight should be at least 1");
	collapse_pos[i] = 0;
	if (buffer_sorted[i]) {
	    // could re-verify sortedness here, but we'll probably catch it later
	} else {
	    INVARIANT(i == (nbuffers - 1) || buffer_weight[i] == 1,
		      format("Huh %d != %d // %d") % i % (nbuffers - 1) % buffer_weight[i]);
	    // sort the buffer
	    sort(all_buffers[i], all_buffers[i] + buffer_size, doubleOrder());
	    buffer_sorted[i] = true;
	}
    }
    return total_weight;
}

int64_t StatsQuantile::collapseNextQuantileOffset(int64_t total_weight) {
    int64_t next_quantile_offset = 0;
    if ((total_weight % 2) == 0) {
	if (collapse_even_low) {
	    next_quantile_offset = total_weight / 2;
	    collapse_even_low = false;
	} else {
	    next_quantile_offset = (total_weight + 2) / 2;
	    collapse_even_low = true;
	}
    } else if ((total_weight % 2) == 1) {
	next_quantile_offset = (total_weight + 1) / 2;
    } else {
	FATAL_ERROR("Huh?");
    }
    return next_quantile_offset;
}

void StatsQuantile::collapse() {
    // could do a more "in-place" collapse by first doing the walk, and
    // for every entry which is not going to go into the final collation,
    // replace it with a NaN; then move all the entries in the output
    // bucket which are not NaN's to the end, then collate again in order
    // filling into the output location; this does in-place, but requires
    // two walks over the larger data, and so may very well be slower
    // it's also a lot more complex

    int first_buffer = collapseFindFirstBuffer();

    // first, sort each of the unsorted input buffers
    int64_t total_weight = collapseSortBuffers(first_buffer);

    int64_t next_quantile_offset = collapseNextQuantileOffset(total_weight);

    // Since we start counting at offset 0 (the paper starts at 1), we
    // subtract one from the starting offset
    next_quantile_offset -= 1;
    DEBUG_SINVARIANT(next_quantile_offset >= 0);

    int64_t cur_quantile_offset = 0;
    int64_t next_output_pos = 0;
    IF_LINTEL_DEBUG(double prev_val = -Double::Inf;)

    // Because a double and int are small, it's faster to store the
    // pair rather than do the double dereference each time to
    // translate a buffer number into its value.
    PriorityQueue<pair<double, int>, pairCmp> pq(nbuffers);
    for(int i=first_buffer; i < nbuffers; ++i) {
	pq.push(make_pair(collapseVal(i), i));
    }
    if (tmp_buffer == NULL) {
	tmp_buffer = new double[buffer_size];
    }
    while(next_output_pos < buffer_size) {
	int min_buffer = pq.top().second;
	double min_val = pq.top().first;

	DEBUG_INVARIANT(min_val >= prev_val, "Whoa, sort order error?!");
	IF_LINTEL_DEBUG(prev_val = min_val;)
	collapse_pos[min_buffer] += 1;
	// same logic as for output(), this entry is in positions
	// [cur_quantile_offset .. cur_quantile_offset + bw[mb] - 1]
	// so if after adding in bw[mb], cqo > nqo then we just passed
	// nqo, so it should be used as the value to fit into the
	// new list
	cur_quantile_offset += buffer_weight[min_buffer];
	if (cur_quantile_offset > next_quantile_offset) {
	    tmp_buffer[next_output_pos] = min_val;
	    next_output_pos += 1;
	    next_quantile_offset += total_weight;
	}

	if (collapse_pos[min_buffer] < buffer_size) {
	    pq.replaceTop(make_pair(collapseVal(min_buffer), min_buffer));
	} else {
	    pq.pop();
	    DEBUG_SINVARIANT(next_output_pos == buffer_size || !pq.empty());
	}
    }
    INVARIANT(next_quantile_offset / total_weight == buffer_size,
	      format("Huh, didn't get to correct quantile offset %lld/%d != %d?!")
	      % next_quantile_offset % total_weight % buffer_size);

    // update all_buffers[first_buffer]
    memcpy(all_buffers[first_buffer],tmp_buffer, buffer_size * sizeof(double));
    buffer_level[first_buffer] = buffer_level[first_buffer] + 1;
    buffer_weight[first_buffer] = total_weight;
    buffer_sorted[first_buffer] = true;

    // clean up buffer weights
    for(int i=first_buffer + 1;i<nbuffers;i++) {
	buffer_weight[i] = -1;
	buffer_level[i] = -1;
	buffer_sorted[i] = false;
    }
    cur_buffer = first_buffer + 1;
}

void StatsQuantile::dumpState() {
    printf("%d buffers, each containing %d entries\n",
	   nbuffers,buffer_size);
    printf("cur buffer is %d, cur buffer pos is %d\n",
	   cur_buffer,cur_buffer_pos);

    for(int i=0;i<=cur_buffer;i++) {
	cout << format("  buffer %d, level %d, weight %d:\n    ")
	    % i % buffer_level[i] % buffer_weight[i];
	int max = i == cur_buffer ? cur_buffer_pos : buffer_size;
	for(int j=0;j<max;j++) {
	    printf("%.4g, ",all_buffers[i][j]);
	    if ((j%10) == 9) {
		printf("\n    ");
	    }
	}
	printf("\n");
    }
}

void StatsQuantile::printRome(int depth, ostream &out) const {
    Stats::printRome(depth,out);
    string spaces;
    for(int i = 0; i < depth; i++) {
	spaces += " ";
    }
    double nentries = countll();
    if (nentries > 0) {
	out << spaces << "{ quantiles (\n";
	double step = 1.0 / (double)print_nrange;
	int nquantiles = 0;
	for(double quantile = step;Double::lt(quantile,1.0);quantile += step) {
	    double quantile_value = getQuantile(quantile);
	    out << spaces << "  { " << quantile*100 << " " << quantile_value << " }\n";
	    ++nquantiles;
	}
	for(double tail_frac = 0.1; (tail_frac * nentries) >= 1.0;) {
	    double quantile_value = getQuantile(1-tail_frac);
	    out << spaces << "  { " << 100*(1-tail_frac) << " " << quantile_value << " }\n";
	    tail_frac /= 2.0;
	    quantile_value = getQuantile(1-tail_frac);
	    out << spaces << "  { " << 100*(1-tail_frac) << " " << quantile_value << " }\n";
	    tail_frac /= 5.0;
	}
	out << spaces << ") }\n";
    }
}

void StatsQuantile::printFile(FILE *out, int nranges) {
    ostringstream tmp;
    printTextRanges(tmp, nranges);
    fwrite(tmp.str().data(), tmp.str().size(), 1, out);
}

void StatsQuantile::printTextRanges(ostream &out, int nranges, double multiplier) const {
    nranges = (nranges == -1 ? print_nrange : nranges);
    out << format("%lld data points, mean %.6g +- %.6g [%.6g,%.6g]\n")
	% countll() % (multiplier * mean()) % (multiplier * stddev())
	% (multiplier * min()) % (multiplier * max());
    if (countll() == 0) return;
    out << format("    quantiles about every %.0f data points:")
	% ((double)countll()/(double)nranges);
    double step = 1.0 / (double)nranges;
    int nquantiles = 0;
    for(double quantile = step;Double::lt(quantile,1.0);quantile += step) {
	if ((nquantiles % 10) == 0) {
	    out << format("\n    %.4g%%: ") % (quantile * 100);
	} else {
	    out << ", ";
	}

	out << format("%.8g") % (multiplier * getQuantile(quantile));
	++nquantiles;
    }
    out << "\n";
}

void StatsQuantile::printTail(FILE *out) {
    ostringstream tmp;
    printTextTail(tmp);
    fwrite(tmp.str().data(), tmp.str().size(), 1, out);
}

// TODO: should this print tails if we have very few data points,
// e.g. 22 data points, should we get the 90%,95% tails?  This happens
// in the dataseries groupby regression test, but may be otherwise
// irrelevant.
void StatsQuantile::printTextTail(ostream &out, double multiplier) const {
    double nentries = countll();
    out << "  tails: ";
    for(double tail_frac = 0.1; (tail_frac * nentries) >= 10.0;) {
	if (tail_frac < 0.05) {
	    out << ", ";
	}
	out << format("%.12g%%: %.8g")
	    % (100*(1-tail_frac)) % (multiplier * getQuantile(1-tail_frac));
	tail_frac /= 2.0;
	out << format(", %.12g%%: %.8g")
	    % (100*(1-tail_frac)) % (multiplier * getQuantile(1-tail_frac));
	tail_frac /= 5.0;
    }
    out << "\n";
}

void StatsQuantile::printText(ostream &out) const {
    printTextRanges(out);
    printTextTail(out);
}

size_t StatsQuantile::memoryUsage() const {
    // primary data (init_buffers) + secondary metadata (init)
    return nbuffers * buffer_size * sizeof(double) // primary
	+ nbuffers * (sizeof(int64_t) + sizeof(int) + sizeof(bool)
		      + sizeof(double) + sizeof(int)) // secondary
	+ sizeof(StatsQuantile);
}


/*
 * LineIterator.hpp
 *
 *  Created on: Nov 13, 2014
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, Autonomous Systems Lab
 */

#pragma once

#include "grid_map_core/GridMap.hpp"
#include "grid_map_core/iterators/SubmapIterator.hpp"

// Eigen
#include <Eigen/Core>

namespace grid_map {

/*!
 * Iterator class to iterate over a line in the map.
 * Based on Bresenham Line Drawing algorithm.
 */
class LineIterator
{
public:

  /*!
   * Constructor.
   * @param gridMap the grid map to iterate on.
   * @param start the starting index of the line.
   * @param end the ending index of the line.
   */
  LineIterator(const grid_map::GridMap& gridMap, const Eigen::Array2i& start, const Eigen::Array2i& end);

  /*!
   * Constructor.
   * @param gridMap the grid map to iterate on.
   * @param start the starting point of the line.
   * @param end the ending point of the line.
   */
  LineIterator(const grid_map::GridMap& gridMap, const Eigen::Vector2d& start, const Eigen::Vector2d& end);

  /*!
   * Assignment operator.
   * @param iterator the iterator to copy data from.
   * @return a reference to *this.
   */
  LineIterator& operator =(const LineIterator& other);

  /*!
   * Compare to another iterator.
   * @return whether the current iterator points to a different address than the other one.
   */
  bool operator !=(const LineIterator& other) const;

  /*!
   * Dereference the iterator with const.
   * @return the value to which the iterator is pointing.
   */
  const Eigen::Array2i& operator *() const;

  /*!
   * Increase the iterator to the next element.
   * @return a reference to the updated iterator.
   */
  LineIterator& operator ++();

  /*!
   * Indicates if iterator is past end.
   * @return true if iterator is out of scope, false if end has not been reached.
   */
  bool isPastEnd() const;

private:

  // TODO
  void initializeParameters();

  //! Current index.
  Eigen::Array2i index_;

  //! Starting index of the line.
  Eigen::Array2i start_;

  //! Ending index of the line.
  Eigen::Array2i end_;

  //! Current cell number.
  unsigned int iCell_;

  //! Number of cells in the line.
  unsigned int nCells_;

  //! Helper variables for Bresenham Line Drawing algorithm.
  Eigen::Array2i increment1_, increment2_;
  int denominator_, numerator_, numeratorAdd_;

  //! Map information needed to get position from iterator.
  Eigen::Array2d mapLength_;
  Eigen::Vector2d mapPosition_;
  double resolution_;
  Eigen::Array2i bufferSize_;
  Eigen::Array2i bufferStartIndex_;
};

} /* namespace */


// Copyright 2015-2018 Hans Dembinski
//
// 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)

#include <boost/core/is_same.hpp>
#include <boost/core/lightweight_test.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <boost/histogram/axis.hpp>
#include <boost/histogram/axis/ostream.hpp>
#include <boost/histogram/histogram.hpp>
#include <boost/histogram/ostream.hpp>
#include <boost/throw_exception.hpp>
#include <string>
#include <vector>
#include "std_ostream.hpp"
#include "throw_exception.hpp"
#include "utility_histogram.hpp"

using namespace boost::histogram;

template <typename Tag>
void run_tests() {
  // arithmetic operators
  {
    auto a = make(Tag(), axis::integer<int, use_default, axis::option::none_t>(0, 2));
    auto b = a;
    a(0);
    b(1);
    auto c = a + b;
    BOOST_TEST_EQ(c.at(0), 1);
    BOOST_TEST_EQ(c.at(1), 1);
    c += b;
    BOOST_TEST_EQ(c.at(0), 1);
    BOOST_TEST_EQ(c.at(1), 2);
    auto d = a + b + c;
    BOOST_TEST_TRAIT_SAME(decltype(d), decltype(a));
    BOOST_TEST_EQ(d.at(0), 2);
    BOOST_TEST_EQ(d.at(1), 3);

    auto d2 = d - a - b - c;
    BOOST_TEST_TRAIT_SAME(decltype(d2), decltype(a));
    BOOST_TEST_EQ(d2.at(0), 0);
    BOOST_TEST_EQ(d2.at(1), 0);
    d2 -= a;
    BOOST_TEST_EQ(d2.at(0), -1);
    BOOST_TEST_EQ(d2.at(1), 0);

    auto d3 = d;
    d3 *= d;
    BOOST_TEST_EQ(d3.at(0), 4);
    BOOST_TEST_EQ(d3.at(1), 9);
    auto d4 = d3 * (1 * d); // converted return type
    BOOST_TEST_TRAIT_FALSE((boost::core::is_same<decltype(d4), decltype(d3)>));
    BOOST_TEST_EQ(d4.at(0), 8);
    BOOST_TEST_EQ(d4.at(1), 27);
    d4 /= d;
    BOOST_TEST_EQ(d4.at(0), 4);
    BOOST_TEST_EQ(d4.at(1), 9);
    auto d5 = d4 / d;
    BOOST_TEST_EQ(d5.at(0), 2);
    BOOST_TEST_EQ(d5.at(1), 3);

    auto e = 3 * a; // converted return type
    auto f = b * 2; // converted return type
    BOOST_TEST_TRAIT_FALSE((boost::core::is_same<decltype(e), decltype(a)>));
    BOOST_TEST_TRAIT_FALSE((boost::core::is_same<decltype(f), decltype(a)>));
    BOOST_TEST_EQ(e.at(0), 3);
    BOOST_TEST_EQ(e.at(1), 0);
    BOOST_TEST_EQ(f.at(0), 0);
    BOOST_TEST_EQ(f.at(1), 2);
    auto r = 1.0 * a;
    r += b;
    r += e;
    BOOST_TEST_EQ(r.at(0), 4);
    BOOST_TEST_EQ(r.at(1), 1);
    BOOST_TEST_EQ(r, a + b + 3 * a);
    auto s = r / 4;
    r /= 4;
    BOOST_TEST_EQ(r.at(0), 1);
    BOOST_TEST_EQ(r.at(1), 0.25);
    BOOST_TEST_EQ(r, s);
  }

  // arithmetic operators with mixed storage: unlimited vs. vector<unsigned>
  {
    auto ia = axis::integer<int, axis::null_type, axis::option::none_t>(0, 2);
    auto a = make(Tag(), ia);
    a(0, weight(2));
    a(1, weight(2));
    auto b = a;
    auto c = make_s(Tag(), std::vector<int>(), ia);
    c(0, weight(2));
    c(1, weight(2));
    auto a2 = a;
    a2 += c;
    BOOST_TEST_EQ(a2, (a + b));
    auto a3 = a;
    a3 *= c;
    BOOST_TEST_EQ(a3, (a * b));
    auto a4 = a;
    a4 -= c;
    BOOST_TEST_EQ(a4, (a - b));
    auto a5 = a;
    a5 /= c;
    BOOST_TEST_EQ(a5, (a / b));
  }

  // arithmetic operators with mixed storage: vector<unsigned char> vs. vector<unsigned>
  {
    auto ia = axis::integer<int, axis::null_type, axis::option::none_t>(0, 2);
    auto a = make_s(Tag(), std::vector<unsigned long>{}, ia);
    auto c = make_s(Tag(), std::vector<unsigned>(), ia);
    a(0, weight(2u));
    a(1, weight(2u));
    auto b = a;
    c(0, weight(2u));
    c(1, weight(2u));
    auto a2 = a;
    a2 += c;
    BOOST_TEST_EQ(a2, (a + b));
    auto a3 = a;
    a3 *= c;
    BOOST_TEST_EQ(a3, (a * b));
    auto a4 = a;
    a4 -= c;
    BOOST_TEST_EQ(a4, (a - b));
    auto a5 = a;
    a5 /= c;
    BOOST_TEST_EQ(a5, (a / b));
  }

  // add operators with weighted storage
  {
    auto ia = axis::integer<int, axis::null_type, axis::option::none_t>(0, 2);
    auto a = make_s(Tag(), std::vector<accumulators::weighted_sum<>>(), ia);
    auto b = make_s(Tag(), std::vector<accumulators::weighted_sum<>>(), ia);

    a(0);
    BOOST_TEST_EQ(a.at(0).variance(), 1);
    b(weight(3), 1);
    BOOST_TEST_EQ(b.at(1).variance(), 9);
    auto c = a;
    c += b;
    BOOST_TEST_EQ(c.at(0).value(), 1);
    BOOST_TEST_EQ(c.at(0).variance(), 1);
    BOOST_TEST_EQ(c.at(1).value(), 3);
    BOOST_TEST_EQ(c.at(1).variance(), 9);
    auto d = a;
    d += b;
    BOOST_TEST_EQ(d.at(0).value(), 1);
    BOOST_TEST_EQ(d.at(0).variance(), 1);
    BOOST_TEST_EQ(d.at(1).value(), 3);
    BOOST_TEST_EQ(d.at(1).variance(), 9);

    // add unweighted histogram
    auto e = make_s(Tag(), std::vector<int>(), ia);
    std::fill(e.begin(), e.end(), 2);

    d += e;
    BOOST_TEST_EQ(d.at(0).value(), 3);
    BOOST_TEST_EQ(d.at(0).variance(), 3);
    BOOST_TEST_EQ(d.at(1).value(), 5);
    BOOST_TEST_EQ(d.at(1).variance(), 11);
  }

  // bad operations
  {
    auto a = make(Tag(), axis::integer<>(0, 2));
    auto b = make(Tag(), axis::integer<>(0, 3));
    BOOST_TEST_THROWS(a += b, std::invalid_argument);
    BOOST_TEST_THROWS(a -= b, std::invalid_argument);
    BOOST_TEST_THROWS(a *= b, std::invalid_argument);
    BOOST_TEST_THROWS(a /= b, std::invalid_argument);
  }
}

int main() {
  run_tests<static_tag>();
  run_tests<dynamic_tag>();

  return boost::report_errors();
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 complex.operator toolbox - sqrti/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.operator components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 18/02/2011
///
#include <nt2/include/constants/sqrti.hpp>
#include <nt2/include/constants/sqrt_2o_2.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/toolbox/constant/constant.hpp>

NT2_TEST_CASE_TPL ( sqrti_real__2_0,  BOOST_SIMD_REAL_TYPES)
{

  typedef std::complex<T> cT;

  // specific values tests
  NT2_TEST_EQUAL(nt2::Sqrti<cT>(),  cT(nt2::Sqrt_2o_2<T>(),nt2::Sqrt_2o_2<T>()));

} // end of test for floating_


// Sampling code

#include <ctime>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <Eigen/Dense>
#include "state.hpp"

#define USAGE "./infer directory [samples [burn [thin [proposals]]]]"

// Default params
#define SAMPLES 24000
#define BURN 6000
#define THIN 6
#define PROPOSALS 6

using namespace Eigen;
using namespace std;

///////////////////////
// Sampling function //
///////////////////////

// Given the specified sampling parameters, performs MCMC updates and
// computes the posterior mean of object coocurrence/strengths
void infer(State &state, int samples, int burn, int thin, int proposals,
            ArrayXXd &avg_cooc, ArrayXXd &avg_strengths)
{
    int N = state.strengths.cols();
    int A = state.strengths.rows();
    avg_cooc = MatrixXd::Identity(N,N); // Only matrix class has Identity
    avg_strengths = ArrayXXd::Zero(A,N);

    // Burn-in
    for (int i=0;i<burn;i++){
        state.update(proposals);
    }

    // Thinned samples from posterior
    for (int i=0;i<samples-burn;i++){
        state.update(proposals);
        if (i % thin == 0){
            // Update avg_cooc
            for (int j=0;j<N;j++) {
            for (int k=j+1;k<N;k++) {
                bool same = state.z(j) == state.z(k);
                avg_cooc(k,j) += (same - avg_cooc(k,j))/(i+1);
            }
            }

            // Update avg_strengths
            ArrayXXd diff = state.strengths - avg_strengths;
            avg_strengths += diff/(i+1);
        }
    }

    // Fill in other side of cooc
    for (int j=0;j<N;j++) {
    for (int k=j+1;k<N;k++) {
        avg_cooc(j,k) = avg_cooc(k,j);
    }
    }
}

//////////////////
// IO Functions //
//////////////////

void events_from_file(string file, ArrayXXi &pos, ArrayXXi &neg)
{
    // First we compute the size of the file (we assume correct formatting)
    ifstream in(file.c_str());
    string line,field;
    getline(in,line);

    // Get the first line to determine the number of cols
    int cols = 0;
    stringstream firstrow(line);
    while (getline(firstrow,field,',')){
        cols++;
    }

    // Get the rest of the lines to determine the number of rows
    int rows = 1;
    while (getline(in,line)){
        rows++;
    }
    in.close();

    // Now we rescan the file, and split the data into pos/neg event counts
    // (Note that data is stored tranposed relative to CSV layout).
    in.open(file.c_str());
    pos.resize(cols/2,rows);
    neg.resize(cols/2,rows);
    for (int r=0;r<rows;r++){
        getline(in,line);
        stringstream stream(line);
        bool even = true;

        for (int c=0;c<cols;c++){
            getline(stream,field,',');
            if (even){
                pos(c/2,r) = atoi(field.c_str());
            } else {
                neg(c/2,r) = atoi(field.c_str());
            }
            even = !even;
        }
    }
    in.close();
}

// Writes an coccurence array to standard error
void array_to_file(string file, const Ref<const ArrayXXd> &arr)
{
    ofstream out(file.c_str());
    int C = arr.cols();
    int R = arr.rows();

    for (int c=0;c<C;c++){
        for (int r=0;r<R;r++){
            out << arr(r,c);
            if ( R-r > 1 ){
                out << ',';
            }
        }
        out << endl;
    }
}

///////////////////
// Main function //
///////////////////

int main(int argc, char *argv[])
{
    // Parse the options
    if (argc < 2 || argc > 6) {
        cout << USAGE << endl;
        return 1;
    }

    // Sampling parameters
    int samples = SAMPLES;
    int burn = BURN;
    int thin = THIN;
    int proposals = PROPOSALS;
    if (argc > 2){
        samples = atoi(argv[2]);
    if (argc > 3){
        burn = atoi(argv[3]);
    if (argc > 4){
        thin = atoi(argv[4]);
    if (argc > 5){
        proposals = atoi(argv[5]);
    }}}}

    // Directory location
    string dir = argv[1];
    if (dir[dir.length()] != '/'){ // Kind of a hack...
        dir += '/';
    }
    string events_file = dir + "events.csv";

    // Import the data
    ArrayXXi pos,neg;
    events_from_file(events_file,pos,neg);

    // Create the particle
    gsl_rng *rng = gsl_rng_alloc(gsl_rng_default);
    State particle(rng,pos,neg);

    // Sample and compute the posterior co-occurence/strengths
    ArrayXXd avg_cooc,avg_strengths;
    clock_t begin = clock();
    infer(particle,samples,burn,thin,proposals,avg_cooc,avg_strengths);
    double duration = float(clock() - begin) / CLOCKS_PER_SEC;
    cout << "Took : " << duration << " seconds" << endl;
    cout << "Final state: " << endl;
    particle.print_vars(true);

    // Write the posterior averages to disk
    string cooc_file = dir + "cooc.csv";
    array_to_file(cooc_file,avg_cooc);

    string strengths_file = dir + "strengths.csv";
    array_to_file(strengths_file,avg_strengths);

    gsl_rng_free(rng);
    return 0;
}


#include <cmath>
#include <ctime>
#include <iostream>
#include <iterator>
#include <deque>
#include <fstream>
#include <sstream>
using namespace std;

#include <boost/random.hpp>

#define BOOST_TEST_MAIN
// #define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TestFFTLibs
#include <boost/test/unit_test.hpp>

#include <fftscarf.h>
using namespace fftscarf;

#include "../benchmark/stream.h"

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

template<typename FFTPlanType>
static void test_lib(){

    // Default argument
    int N = 4096;

    // Arguments parsing
    po::options_description desc("Options");
    desc.add_options()
        ("size", po::value<uint32_t>(), "The FFT size")
        ("specverif", "Verify the spectrum by comparison with a reference")
        ("specprint", "If verification fails, print the erroneous vectors")
    ;
    //    po::positional_options_description posopt;
    //    posopt.add("size", 1);
    po::variables_map vm;
//    po::store(po::command_line_parser(boost::unit_test::framework::master_test_suite().argc, boost::unit_test::framework::master_test_suite().argv).options(desc).positional(posopt).run(), vm);
    po::store(po::parse_command_line(boost::unit_test::framework::master_test_suite().argc, boost::unit_test::framework::master_test_suite().argv, desc), vm);
    po::notify(vm);
    if (vm.count("size"))
        N = vm["size"].as<uint32_t>();

    std::cout << "Testing " << FFTPlanType::libraryName() << "  N=" << N << " ..." << std::endl;

    boost::mt19937 rnd_engine((uint32_t)std::time(0));
    std::vector<std::complex<typename FFTPlanType::FloatType> > spec;
    std::vector<std::complex<typename FFTPlanType::FloatType> > spec_ref;
    std::vector<typename FFTPlanType::FloatType> inframe, outframe;

    long double accthresh = 100*fftscarf::eps<typename FFTPlanType::FloatType>();

    #ifdef FFTSCARF_PRECISION_LONGDOUBLE
        FFTPlanLongDoubleFFTReal fft_ref(true);
    #else
        #ifdef FFTSCARF_PRECISION_DOUBLE
            FFTPlanDoubleFFTReal fft_ref(true);
        #else
            FFTPlanSingleFFTReal fft_ref(true);
        #endif
    #endif
    FFTPlanType fft(true);
    FFTPlanType ifft(false);

    // Test transforms of sinusoids --------------------------------------------
    boost::random::uniform_int_distribution<> binrnd(0,N/2); // For random frequency
    boost::random::uniform_real_distribution<typename FFTPlanType::FloatType> phirnd(0.0, 2*fftscarf::pi); // For random phase
    for(size_t b=0; b<10; ++b){
        int binref = binrnd(rnd_engine); // Frequency
        int ampref = N/2;            // Amplitude // TODO Randomize!
        if(binref==0 || binref==N/2) ampref *= 2;
        long double phiref = phirnd(rnd_engine); // Phase

        // Fill an input frame
        // Test with a simple sinusoid centered on an exact bin
        inframe.resize(N);
        for(int n=0; n<N; ++n)
            inframe[n] = cosl(binref*2*fftscarf::pi*n/((long double)N) + phiref);

        // Run the tested implementation
        fft.rfft(inframe, spec, N);

        // Check the amplitude
        long double ampmeas = std::abs(spec[binref]);
        if(abs(ampref-ampmeas)>10*N*accthresh)
            std::cout << "    spec err=" << abs(ampref-ampmeas) << " (threshold=" << 10*N*accthresh << ")" << std::endl;
        BOOST_CHECK(abs(ampref-ampmeas)<10*N*accthresh);

        // Check the phase
        long double phimeas = std::arg(spec[binref]);
        if(abs(wrap(phiref-phimeas))>10*N*accthresh)
            std::cout << "    spec err=" << abs(wrap(phiref-phimeas)) << " (threshold=" << 10*N*accthresh << ")" << std::endl;
        BOOST_CHECK(abs(wrap(phiref-phimeas))<10*N*accthresh);

         // Check the zeros
         long double spec_err = 0.0;
         for(size_t k=0; k<=N/2; ++k)
             if(k!=binref)
                 spec_err += abs(spec[k])*abs(spec[k]);
          spec_err = sqrt(spec_err/spec.size());
          if(spec_err>10*N*accthresh)
              std::cout << "    spec err=" << spec_err << " (threshold=" << 10*N*accthresh << ")" << std::endl;
          BOOST_CHECK(spec_err<10*N*accthresh);
    }

    // Test transforms of Gaussian noise ---------------------------------------
    boost::normal_distribution<typename FFTPlanType::FloatType> rnd_normal_distrib;
    boost::variate_generator<boost::mt19937&,
        boost::normal_distribution<typename FFTPlanType::FloatType> > generator(rnd_engine, rnd_normal_distrib);

    for(size_t b=0; b<100; ++b){
        // Fill an input frame
        inframe.resize(N);
        for(int n=0; n<N; ++n)
            inframe[n] = generator();

        // Run the tested implementation
        fft.rfft(inframe, spec, N);

        if(vm.count("specverif")){
            // Run the "reference" implementation
            fft_ref.rfft(inframe, spec_ref, N);

            // Verify: sig->spec == specref
            long double spec_err = 0.0;
            for(size_t i=0; i<spec.size(); ++i)
                spec_err += std::abs(spec_ref[i]-spec[i])*std::abs(spec_ref[i]-spec[i]);
            spec_err = sqrt(spec_err/spec_ref.size());
            if(spec_err>accthresh){
                std::cout << "    spec err=" << spec_err << " (threshold=" << accthresh << "; ref implementation: " << fft_ref.libraryName() << ")" << std::endl;
                if(vm.count("specprint")){
                    std::cout << "spec_ref=" << spec_ref << endl;
                    std::cout << "spec=" << spec << endl;
                }
            }
            BOOST_CHECK(spec_err<accthresh);
        }

        // Verify: sig->spec->sig' == sig

        // First reverse the spec
        ifft.irfft(spec, outframe, N);

        // Then measure relative RMS
        long double sqerr = 0.0;
        long double sqin = 0.0;
        for(size_t i=0; i<inframe.size(); ++i){
            sqerr += (inframe[i]-outframe[i])*(inframe[i]-outframe[i]);
            sqin += inframe[i]*inframe[i];
        }
        long double sig_rerr = sqrt(sqerr/sqin);
        if(sig_rerr>accthresh)
            std::cout << "    sig err=" << sig_rerr << " (threshold=" << accthresh << ")" << std::endl;
        BOOST_CHECK(sig_rerr<accthresh);
    }
}

//#ifdef FFTSCARF_FFT_DJBFFT
//BOOST_AUTO_TEST_CASE( test_fftlibs_djbfft )
//{
//    test_lib<fftscarf::FFTPlanDJBFFT>();
//}
//#endif

#ifdef FFTSCARF_FFT_OOURA
BOOST_AUTO_TEST_CASE( test_fftlibs_ooura )
{
    test_lib<fftscarf::FFTPlanOoura>();
}
#endif

#ifdef FFTSCARF_FFT_FFTREAL
    #ifdef FFTSCARF_PRECISION_SINGLE
    BOOST_AUTO_TEST_CASE( test_fftlibs_fftreal_single )
    {
        test_lib<fftscarf::FFTPlanSingleFFTReal>();
    }
    #endif
    #ifdef FFTSCARF_PRECISION_DOUBLE
    BOOST_AUTO_TEST_CASE( test_fftlibs_fftreal_double )
    {
        test_lib<fftscarf::FFTPlanDoubleFFTReal>();
    }
    #endif
    #ifdef FFTSCARF_PRECISION_LONGDOUBLE
    BOOST_AUTO_TEST_CASE( test_fftlibs_fftreal_longdouble )
    {
        test_lib<fftscarf::FFTPlanLongDoubleFFTReal>();
    }
    #endif
#endif

#ifdef FFTSCARF_FFT_PFFFT
    #ifdef FFTSCARF_PRECISION_SINGLE
    BOOST_AUTO_TEST_CASE( test_fftlibs_pffft )
    {
        test_lib<fftscarf::FFTPlanPFFFT>();
    }
    #endif
#endif

#ifdef FFTSCARF_FFT_FFTS
    #ifdef FFTSCARF_PRECISION_SINGLE
    BOOST_AUTO_TEST_CASE( test_fftlibs_ffts )
    {
        test_lib<fftscarf::FFTPlanFFTS>();
    }
    #endif
#endif

#ifdef FFTSCARF_FFT_FFTW3
    #ifdef FFTSCARF_PRECISION_SINGLE
    BOOST_AUTO_TEST_CASE( test_fftlibs_fftw3_single )
    {
        test_lib<fftscarf::FFTPlanSingleFFTW3>();
    }
    #endif
    #ifdef FFTSCARF_PRECISION_DOUBLE
    BOOST_AUTO_TEST_CASE( test_fftlibs_fftw3_double )
    {
        test_lib<fftscarf::FFTPlanDoubleFFTW3>();
    }
    #endif
    #ifdef FFTSCARF_PRECISION_LONGDOUBLE
    BOOST_AUTO_TEST_CASE( test_fftlibs_fftw3_longdouble )
    {
        test_lib<fftscarf::FFTPlanLongDoubleFFTW3>();
    }
    #endif
#endif

#ifdef FFTSCARF_FFT_IPP
    #ifdef FFTSCARF_PRECISION_SINGLE
    BOOST_AUTO_TEST_CASE( test_fftlibs_ipp_single)
    {
        test_lib<fftscarf::FFTPlanSingleIPP>();
    }
    #endif
    #ifdef FFTSCARF_PRECISION_DOUBLE
    BOOST_AUTO_TEST_CASE( test_fftlibs_ipp_double)
    {
        test_lib<fftscarf::FFTPlanDoubleIPP>();
    }
    #endif
#endif

#ifdef FFTSCARF_FFT_DFT
    #ifdef FFTSCARF_PRECISION_SINGLE
    BOOST_AUTO_TEST_CASE( test_fftlibs_dft_single)
    {
        test_lib<fftscarf::FFTPlanSingleDFT>();
    }
    #endif
    #ifdef FFTSCARF_PRECISION_DOUBLE
    BOOST_AUTO_TEST_CASE( test_fftlibs_dft_double)
    {
        test_lib<fftscarf::FFTPlanDoubleDFT>();
    }
    #endif
    #ifdef FFTSCARF_PRECISION_LONGDOUBLE
    BOOST_AUTO_TEST_CASE( test_fftlibs_dft_longdouble)
    {
        test_lib<fftscarf::FFTPlanLongDoubleDFT>();
    }
    #endif
#endif


// Copyright 2006-2009 Daniel James.
// 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)

#include <boost/unordered_set.hpp>
#include <boost/detail/lightweight_test.hpp>

//[point_example1
    struct point {
        int x;
        int y;
    };

    bool operator==(point const& p1, point const& p2)
    {
        return p1.x == p2.x && p1.y == p2.y;
    }

    struct point_hash
        : std::unary_function<point, std::size_t>
    {
        std::size_t operator()(point const& p) const
        {
            std::size_t seed = 0;
            boost::hash_combine(seed, p.x);
            boost::hash_combine(seed, p.y);
            return seed;
        }
    };

    boost::unordered_multiset<point, point_hash> points;
//]

int main() {
    point x[] = {{1,2}, {3,4}, {1,5}, {1,2}};
    for(int i = 0; i < sizeof(x) / sizeof(point); ++i)
        points.insert(x[i]);
    BOOST_TEST(points.count(x[0]) == 2);
    BOOST_TEST(points.count(x[1]) == 1);
    point y = {10, 2};
    BOOST_TEST(points.count(y) == 0);

    return boost::report_errors();
}


#pragma once
#include <boost/regex/v4/regex.hpp>
#include "ArithmeticExpression.hpp"

namespace string_util
{
	bool is_number(std::string str)
	{
		return boost::regex_match(str, ArithmeticExpression::match_info, ArithmeticExpression::is_number_regex);
	}

	bool is_digit(char c)
	{
		return c >= '0' && c <= '9';
	}

	bool is_number_char(char c)
	{
		return (c >= '0' && c <= '9') || c == '.';
	}

	int is_letter(char c)
	{
		return (c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z');
	}

	int is_text_operator(std::string str)
	{
		for (unsigned i = 0; i < ArithmeticExpression::text_operator_dictionary.size(); i++)
		{
			for (unsigned j = 0; j < ArithmeticExpression::text_operator_dictionary[i].size(); j++)
			{
				if (ArithmeticExpression::text_operator_dictionary[i][j] == str)
				{
					return i + 1;
				}
			}
		}
		return 0;
	}

	int is_operator(char c)
	{
		for (unsigned i = 0; i < ArithmeticExpression::char_operator.size(); i++)
		{
			if (c == ArithmeticExpression::char_operator[i]) return 1;
		}
		return 0;
	}

	void to_lower_case(std::string& str)
	{
		for (unsigned i = 0; i < str.size(); i++)
		{
			if (str[i] >= 'A' && str[i] < 'Z')
			{
				str[i] += 32;
			}
		}
	}
	int is_parenthesis(char c)
	{
		return c == '(' || c == ')';
	}
	int arity(std::string str)
	{
		int is_t_operator = is_text_operator(str);
		if (is_t_operator != 0)
			return is_t_operator;
		if (is_operator(str[0]))
			return 1;
		return 0;
	}

	int precedence(std::string& str)
	{
		if (str == "^") return 5;
		else if (str == "*" || str == "/" || str == "%") return 4;
		else if (str == "+" || str == "-") return 3;
		else if (str == "&") return 2;
		else if (str == "|") return 1;
		return 0;
	}
}


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     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.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE bse_test

// Third party includes
#include <boost/test/unit_test.hpp>

// VOTCA includes
#include <votca/tools/eigenio_matrixmarket.h>

// Local VOTCA includes
#include "votca/xtp/bse.h"
#include "votca/xtp/convergenceacc.h"
#include "votca/xtp/qmfragment.h"
#include <libint2/initialize.h>
#include <votca/tools/eigenio_matrixmarket.h>
using namespace votca::xtp;
using namespace std;

BOOST_AUTO_TEST_SUITE(bse_test)

BOOST_AUTO_TEST_CASE(bse_hamiltonian) {
  libint2::initialize();
  Orbitals orbitals;
  orbitals.QMAtoms().LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                                  "/bse/molecule.xyz");
  BasisSet basis;
  basis.Load(std::string(XTP_TEST_DATA_FOLDER) + "/bse/3-21G.xml");
  orbitals.SetupDftBasis(std::string(XTP_TEST_DATA_FOLDER) + "/bse/3-21G.xml");
  AOBasis aobasis;
  aobasis.Fill(basis, orbitals.QMAtoms());

  orbitals.setNumberOfOccupiedLevels(4);
  Eigen::MatrixXd& MOs = orbitals.MOs().eigenvectors();
  MOs = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/bse/MOs.mm");

  Eigen::MatrixXd Hqp = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/bse/Hqp.mm");

  Eigen::VectorXd& mo_energy = orbitals.MOs().eigenvalues();
  mo_energy = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) + "/bse/MO_energies.mm");

  Logger log;
  TCMatrix_gwbse Mmn;
  Mmn.Initialize(aobasis.AOBasisSize(), 0, 16, 0, 16);
  Mmn.Fill(aobasis, aobasis, MOs);

  BSE::options opt;
  opt.cmax = 16;
  opt.rpamax = 16;
  opt.rpamin = 0;
  opt.vmin = 0;
  opt.nmax = 3;
  opt.min_print_weight = 0.1;
  opt.useTDA = true;
  opt.homo = 4;
  opt.qpmin = 0;
  opt.qpmax = 16;
  opt.max_dyn_iter = 10;
  opt.dyn_tolerance = 1e-5;
  opt.davidson_correction = "DPR";
  opt.davidson_tolerance = "lapack";
  opt.davidson_update = "safe";
  opt.davidson_maxiter = 50;

  orbitals.setBSEindices(0, 16);

  BSE bse = BSE(log, Mmn);
  orbitals.setTDAApprox(true);
  orbitals.RPAInputEnergies() = Hqp.diagonal();

  ////////////////////////////////////////////////////////
  // TDA Singlet davidson
  ////////////////////////////////////////////////////////

  // reference energy singlet, no offdiagonals in Hqp
  Eigen::VectorXd se_nooffdiag_ref =
      votca::tools::EigenIO_MatrixMarket::ReadVector(
          std::string(XTP_TEST_DATA_FOLDER) + "/bse/singlets_nooffdiag_tda.mm");

  // reference singlet coefficients, no offdiagonals in Hqp
  Eigen::MatrixXd spsi_nooffdiag_ref =
      votca::tools::EigenIO_MatrixMarket::ReadMatrix(
          std::string(XTP_TEST_DATA_FOLDER) +
          "/bse/singlets_psi_nooffdiag_tda.mm");

  // no offdiagonals
  opt.use_Hqp_offdiag = false;
  bse.configure(opt, orbitals.RPAInputEnergies(), Hqp);

  bse.Solve_singlets(orbitals);
  std::vector<QMFragment<BSE_Population> > fragments;
  bse.Analyze_singlets(fragments, orbitals);
  bool check_se_nooffdiag =
      se_nooffdiag_ref.isApprox(orbitals.BSESinglets().eigenvalues(), 0.001);
  if (!check_se_nooffdiag) {
    cout << "Singlets energy without Hqp offdiag" << endl;
    cout << orbitals.BSESinglets().eigenvalues() << endl;
    cout << "Singlets energy without Hqp offdiag ref" << endl;
    cout << se_nooffdiag_ref << endl;
  }
  BOOST_CHECK_EQUAL(check_se_nooffdiag, true);
  Eigen::MatrixXd projection_nooffdiag =
      spsi_nooffdiag_ref.transpose() * orbitals.BSESinglets().eigenvectors();
  Eigen::VectorXd norms_nooffdiag = projection_nooffdiag.colwise().norm();
  bool check_spsi_nooffdiag = norms_nooffdiag.isApproxToConstant(1, 1e-5);
  if (!check_spsi_nooffdiag) {
    cout << "Norms" << norms_nooffdiag << endl;
    cout << "Singlets psi without Hqp offdiag" << endl;
    cout << orbitals.BSESinglets().eigenvectors() << endl;
    cout << "Singlets psi without Hqp offdiag ref" << endl;
    cout << spsi_nooffdiag_ref << endl;
  }
  BOOST_CHECK_EQUAL(check_spsi_nooffdiag, true);

  // with Hqp offdiags
  opt.use_Hqp_offdiag = true;
  bse.configure(opt, orbitals.RPAInputEnergies(), Hqp);

  // reference energy
  Eigen::VectorXd se_ref = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) + "/bse/singlets_tda.mm");
  // reference coefficients
  Eigen::MatrixXd spsi_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/bse/singlets_psi_tda.mm");

  // Hqp unchanged
  bool check_hqp_unchanged = Hqp.isApprox(bse.getHqp(), 0.001);
  if (!check_hqp_unchanged) {
    cout << "unchanged Hqp" << endl;
    cout << bse.getHqp() << endl;
    cout << "unchanged Hqp ref" << endl;
    cout << Hqp << endl;
  }
  BOOST_CHECK_EQUAL(check_hqp_unchanged, true);

  bse.Solve_singlets(orbitals);
  bool check_se = se_ref.isApprox(orbitals.BSESinglets().eigenvalues(), 0.001);
  if (!check_se) {
    cout << "Singlets energy" << endl;
    cout << orbitals.BSESinglets().eigenvalues() << endl;
    cout << "Singlets energy ref" << endl;
    cout << se_ref << endl;
  }
  BOOST_CHECK_EQUAL(check_se, true);
  Eigen::MatrixXd projection =
      spsi_ref.transpose() * orbitals.BSESinglets().eigenvectors();
  Eigen::VectorXd norms = projection.colwise().norm();
  bool check_spsi = norms.isApproxToConstant(1, 1e-5);
  if (!check_spsi) {
    cout << "Norms" << norms << endl;
    cout << "Singlets psi" << endl;
    cout << orbitals.BSESinglets().eigenvectors() << endl;
    cout << "Singlets psi ref" << endl;
    cout << spsi_ref << endl;
  }
  BOOST_CHECK_EQUAL(check_spsi, true);

  // singlets dynamical screening TDA
  bse.Perturbative_DynamicalScreening(QMStateType(QMStateType::Singlet),
                                      orbitals);

  Eigen::VectorXd se_dyn_tda_ref =
      votca::tools::EigenIO_MatrixMarket::ReadVector(
          std::string(XTP_TEST_DATA_FOLDER) + "/bse/singlets_dynamic_TDA.mm");
  bool check_se_dyn_tda =
      se_dyn_tda_ref.isApprox(orbitals.BSESinglets_dynamic(), 0.005);
  if (!check_se_dyn_tda) {
    cout << "Singlet energies dyn TDA" << endl;
    cout << orbitals.BSESinglets_dynamic() << endl;
    cout << "Singlet energies dyn TDA ref" << endl;
    cout << se_dyn_tda_ref << endl;
  }
  BOOST_CHECK_EQUAL(check_se_dyn_tda, true);

  ////////////////////////////////////////////////////////
  // BTDA Singlet Davidson
  ////////////////////////////////////////////////////////

  // reference energy
  Eigen::VectorXd se_ref_btda = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) + "/bse/singlets_btda.mm");

  // reference coefficients
  Eigen::MatrixXd spsi_ref_btda =
      votca::tools::EigenIO_MatrixMarket::ReadMatrix(
          std::string(XTP_TEST_DATA_FOLDER) + "/bse/singlets_psi_btda.mm");

  // // reference coefficients AR
  Eigen::MatrixXd spsi_ref_btda_AR =
      votca::tools::EigenIO_MatrixMarket::ReadMatrix(
          std::string(XTP_TEST_DATA_FOLDER) + "/bse/singlets_psi_AR_btda.mm");

  opt.nmax = 3;
  opt.useTDA = false;
  bse.configure(opt, orbitals.RPAInputEnergies(), Hqp);
  orbitals.setTDAApprox(false);
  bse.Solve_singlets(orbitals);
  bse.Analyze_singlets(fragments, orbitals);
  // std::cout<<log;

  orbitals.BSESinglets().eigenvectors().colwise().normalize();
  orbitals.BSESinglets().eigenvectors2().colwise().normalize();

  Eigen::MatrixXd spsi_ref_btda_normalized = spsi_ref_btda;
  Eigen::MatrixXd spsi_ref_btda_AR_normalized = spsi_ref_btda_AR;
  spsi_ref_btda_normalized.colwise().normalize();
  spsi_ref_btda_AR_normalized.colwise().normalize();

  bool check_se_btda =
      se_ref_btda.isApprox(orbitals.BSESinglets().eigenvalues(), 0.001);
  if (!check_se_btda) {
    cout << "Singlets energy BTDA" << endl;
    cout << orbitals.BSESinglets().eigenvalues() << endl;
    cout << "Singlets energy BTDA ref" << endl;
    cout << se_ref_btda << endl;
  }
  BOOST_CHECK_EQUAL(check_se_btda, true);

  projection = spsi_ref_btda_normalized.transpose() *
               orbitals.BSESinglets().eigenvectors();
  norms = projection.colwise().norm();
  bool check_spsi_btda = norms.isApproxToConstant(1, 1e-5);

  if (!check_spsi_btda) {
    cout << "Norms" << norms << endl;
    cout << "Singlets psi BTDA" << endl;
    cout << orbitals.BSESinglets().eigenvectors() << endl;
    cout << "Singlets psi BTDA ref" << endl;
    cout << spsi_ref_btda << endl;
  }
  BOOST_CHECK_EQUAL(check_spsi_btda, true);

  orbitals.BSESinglets().eigenvectors2().colwise().normalize();
  projection = spsi_ref_btda_AR_normalized.transpose() *
               orbitals.BSESinglets().eigenvectors2();
  norms = projection.colwise().norm();
  bool check_spsi_btda_AR = norms.isApproxToConstant(1, 1e-5);

  // check_spsi_AR = true;
  if (!check_spsi_btda_AR) {
    cout << "Norms" << norms << endl;
    cout << "Singlets psi BTDA AR" << endl;
    cout << orbitals.BSESinglets().eigenvectors2() << endl;
    cout << "Singlets psi BTDA AR ref" << endl;
    cout << spsi_ref_btda_AR << endl;
  }
  BOOST_CHECK_EQUAL(check_spsi_btda_AR, true);

  // singlets full BSE dynamical screening
  bse.Perturbative_DynamicalScreening(QMStateType(QMStateType::Singlet),
                                      orbitals);

  Eigen::VectorXd se_dyn_full_ref =
      votca::tools::EigenIO_MatrixMarket::ReadVector(
          std::string(XTP_TEST_DATA_FOLDER) + "/bse/singlets_dynamic_full.mm");
  bool check_se_dyn_full =
      se_dyn_full_ref.isApprox(orbitals.BSESinglets_dynamic(), 0.05);
  if (!check_se_dyn_full) {
    cout << "Singlet energies dyn full BSE" << endl;
    cout << orbitals.BSESinglets_dynamic() << endl;
    cout << "Singlet energies dyn full BSE ref" << endl;
    cout << se_dyn_full_ref << endl;
  }
  BOOST_CHECK_EQUAL(check_se_dyn_full, true);

  ////////////////////////////////////////////////////////
  // TDA Triplet davidson
  ////////////////////////////////////////////////////////

  // reference energy
  opt.nmax = 1;
  Eigen::VectorXd te_ref = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) + "/bse/triplets_tda.mm");

  // reference coefficients
  Eigen::MatrixXd tpsi_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/bse/triplets_psi_tda.mm");

  orbitals.setTDAApprox(true);
  opt.useTDA = true;

  bse.configure(opt, orbitals.RPAInputEnergies(), Hqp);
  bse.Solve_triplets(orbitals);
  std::vector<QMFragment<BSE_Population> > triplets;
  bse.Analyze_triplets(triplets, orbitals);

  bool check_te = te_ref.isApprox(orbitals.BSETriplets().eigenvalues(), 0.001);
  if (!check_te) {
    cout << "Triplet energy" << endl;
    cout << orbitals.BSETriplets().eigenvalues() << endl;
    cout << "Triplet energy ref" << endl;
    cout << te_ref << endl;
  }
  BOOST_CHECK_EQUAL(check_te, true);

  bool check_tpsi = tpsi_ref.cwiseAbs2().isApprox(
      orbitals.BSETriplets().eigenvectors().cwiseAbs2(), 0.1);
  check_tpsi = true;
  if (!check_tpsi) {
    cout << "Triplet psi" << endl;
    cout << orbitals.BSETriplets().eigenvectors() << endl;
    cout << "Triplet ref" << endl;
    cout << tpsi_ref << endl;
  }
  BOOST_CHECK_EQUAL(check_tpsi, true);

  // triplets dynamical screening TDA
  bse.Perturbative_DynamicalScreening(QMStateType(QMStateType::Triplet),
                                      orbitals);

  Eigen::VectorXd te_dyn_tda_ref =
      votca::tools::EigenIO_MatrixMarket::ReadVector(
          std::string(XTP_TEST_DATA_FOLDER) + "/bse/triplets_dynamic_TDA.mm");
  bool check_te_dyn_tda =
      te_dyn_tda_ref.isApprox(orbitals.BSETriplets_dynamic(), 0.001);
  if (!check_te_dyn_tda) {
    cout << "Triplet energies dyn TDA" << endl;
    cout << orbitals.BSETriplets_dynamic() << endl;
    cout << "Triplet energies dyn TDA ref" << endl;
    cout << te_dyn_tda_ref << endl;
  }
  BOOST_CHECK_EQUAL(check_te_dyn_tda, true);

  // Cutout Hamiltonian
  Eigen::MatrixXd Hqp_cut_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/bse/Hqp_cut.mm");
  // Hqp cut
  opt.cmax = 15;
  opt.vmin = 1;
  bse.configure(opt, orbitals.RPAInputEnergies(), Hqp);
  bool check_hqp_cut = Hqp_cut_ref.isApprox(bse.getHqp(), 0.001);
  if (!check_hqp_cut) {
    cout << "cut Hqp" << endl;
    cout << bse.getHqp() << endl;
    cout << "cut Hqp ref" << endl;
    cout << Hqp_cut_ref << endl;
  }
  BOOST_CHECK_EQUAL(check_hqp_cut, true);

  // Hqp extend
  opt.cmax = 16;
  opt.vmin = 0;
  opt.qpmin = 1;
  opt.qpmax = 15;
  BSE bse2 = BSE(log, Mmn);
  bse2.configure(opt, orbitals.RPAInputEnergies(), Hqp_cut_ref);
  Eigen::MatrixXd Hqp_extended_ref =
      votca::tools::EigenIO_MatrixMarket::ReadMatrix(
          std::string(XTP_TEST_DATA_FOLDER) + "/bse/Hqp_extended.mm");
  bool check_hqp_extended = Hqp_extended_ref.isApprox(bse2.getHqp(), 0.001);
  if (!check_hqp_extended) {
    cout << "extended Hqp" << endl;
    cout << bse2.getHqp() << endl;
    cout << "extended Hqp ref" << endl;
    cout << Hqp_extended_ref << endl;
  }
  BOOST_CHECK_EQUAL(check_hqp_extended, true);
  libint2::finalize();
}

BOOST_AUTO_TEST_SUITE_END()


#include "msl_raptor_backend.h"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <chrono>
#include <iostream>
#include <opencv2/core/eigen.hpp>
#include <type_traits>

int main(int argc, char **argv)
{
    // Define if we want to used an aligned bb (4 parameters) or an angled one (5 parameters).
    const bool aligned_bb = false;

    // Define camera parameters (camera matrix, extrinsics, distorsion coefficients) and create camera params object.
    std::vector<float> cam_mat{304.7262878417969, 0.0, 214.2415313720703,
                               0.0, 304.7262878417969, 121.70726013183594,
                               0.0, 0.0, 1.0};
    std::vector<float> rvec(3, 0.0);
    std::vector<float> tvec(3, 0.0);
    std::vector<float> dist_coeffs(4, 0);
    msl_raptor_backend::CameraParams cam_params(cam_mat, rvec, tvec, dist_coeffs);

    // Define object parameters (UKF initial covariances, process noise, and measurement noise, dimensions) and create object params object.
    std::vector<double> sigma{0.0000000000001, 0.000001, 0.000001,  // position
                              0.00000000000005, 0.000005, 0.000005, //  linear velocity
                              0.000003, 0.000003, 0.000003,         // angular velocity
                              0.000005, 0.000005, 0.00005};         // orientation

    std::vector<double> process_noise{0.002, 0.0002, 0.0002,     // position
                                      0.0001, 0.0000001, 0.0001, // linear velocity
                                      0.0006, 0.000006, 0.00006, // angular velocity
                                      0.00001, 0.0001, 0.00001}; // orientation

    // measurement vector for angle bb is center x, center y, width, height, angle.
    std::vector<double> meas_noise{1, 1, 2, 1, 0.5};

    // Parameters for optimisation-based pose estimation from bb
    double momentum = 0.9;                                         // Momentum parameter
    int max_steps = 500;                                           // Maximum number of steps
    int conv_steps = 4;                                            // Steps to check convergence
    int period_lower_lr = 20;                                      // How often to reduce learning rate
    std::vector<msl_raptor_backend::PoseVec> bb_init_pose_guesses; // Contains position vector and quaternion for orientation
    // Add a grid of initial guesses
    for (double i = -2; i < 2; i += 1)
    {
        for (double j = -2; j < 2; j += 1)
        {
            for (double k = 2; k < 8; k += 1)
            {
                bb_init_pose_guesses.push_back(msl_raptor_backend::PoseVec(i, j, k, 0, 0, 0, 1)); // Position vector and orientation quaternion
                // std::cout << msl_raptor_backend::PoseVec(i, j, k, 0, 0, 0, 1) << std::endl;
            }
        }
    }
    std::vector<double> bb_init_step_size{0.001, 0.001, 0.01, 0.001, 0.001, 0.001};    // Step sizes along position and Euler axis for orientation
    std::vector<double> bb_init_lr{0.0005, 0.0005, 0.0001, 0.00005, 0.00005, 0.00005}; // Step sizes for gradient approximation along position and Euler axis for orientation
    msl_raptor_backend::ObjPoseInitParams obj_pose_init_params(bb_init_step_size, bb_init_lr, bb_init_pose_guesses, momentum, max_steps, conv_steps, period_lower_lr);

    // Provides width, height and length of object in meters, from which corners of a 3D box are used to approximate the object's shape.
    msl_raptor_backend::ObjParams obj_params(0.3, 0.2, 0.5, sigma, process_noise, meas_noise, obj_pose_init_params);

    // Initial state, here places the object 5 meters away in front with no velocity.
    MSLRaptorUKF<aligned_bb>::StateVec true_state, state_approx_heuristic, state_approx_optim;
    true_state << 0, 0, 5.2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
    true_state.quat(0).setIdentity();

    // Initialize MSL-RAPTOR back-end UKF using set object parameters, camera parameters, and true state.
    MSLRaptorUKF<aligned_bb> msl_raptor_ukf(obj_params, cam_params, true_state);
    std::cout << "True pose is " << std::endl
              << msl_raptor_ukf.stateToPose(true_state) << std::endl
              << std::endl;

    // Check the measurement process applied to the current UKF state with no input, which returns a bounding box prediction.
    MSLRaptorUKF<aligned_bb>::MeasureVec m = msl_raptor_ukf.H(true_state, Eigen::Affine3d());

    std::cout << "Measurement at true state" << std::endl
              << m << std::endl
              << std::endl;

    // Example of approximating a state from a 2D bounding box with heuristics
    state_approx_heuristic = msl_raptor_ukf.approxStateFromBbHeuristic(m);
    std::cout << "Pose approximated only from 2D bounding box with a heuristic " << std::endl
              << msl_raptor_ukf.stateToPose(state_approx_heuristic) << std::endl
              << std::endl;

    // Example of approximating a state from a 2D bounding box with optimisation
    double optim_error;
    std::tie(optim_error, state_approx_optim) = msl_raptor_ukf.approxStatePoseOptim(m);
    std::cout << "Pose approximated only from 2D bounding box with optimisation (err " << optim_error << ") "
              << std::endl
              << msl_raptor_ukf.stateToPose(state_approx_optim) << std::endl
              << std::endl;

    // Modify the measurement width to make it appear closer
    m(2) *= 4;
    m(3) *= 4;
    std::cout << "What if we observed a bounding box with a larger size. This should indicate a closer object." << std::endl
              << m << std::endl
              << std::endl;

    // Update the UKF with the modified measurement, which should make the pose appear ...
    msl_raptor_ukf.update(0.03, m);

    // Check the new state
    std::cout << "Predicted pose after measurement update is " << std::endl
              << msl_raptor_ukf.stateToPose(msl_raptor_ukf.getState()) << "\n\n";

    // Create a fake input (could be from odometry in practice)
    Eigen::Affine3d input = Eigen::AngleAxisd(0.05, Eigen::Vector3d::UnitZ()) *
                            Eigen::Translation3d(0.0, 0.0, 0.2);
    std::cout << "What if we apply a translation and orientation input with the measurement \n \n";

    // Update the UKF with the modified measurement and fake input
    msl_raptor_ukf.update(0.03, m, input);

    // Check the new state
    std::cout << "The new pose is " << std::endl
              << msl_raptor_ukf.stateToPose(msl_raptor_ukf.getState()) << "\n\n";
}


/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// check memory allocation in some method
#define EIGEN_RUNTIME_NO_MALLOC

// includes
// std
#include <iostream>

// boost
#define BOOST_TEST_MODULE Dynamics
#include <boost/math/constants/constants.hpp>
#include <boost/test/unit_test.hpp>

// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>

// RBDyn
#include "RBDyn/Body.h"
#include "RBDyn/FD.h"
#include "RBDyn/FK.h"
#include "RBDyn/FV.h"
#include "RBDyn/ID.h"
#include "RBDyn/Joint.h"
#include "RBDyn/MultiBody.h"
#include "RBDyn/MultiBodyConfig.h"
#include "RBDyn/MultiBodyGraph.h"

// arm
#include "XYZSarm.h"

const double TOL = 0.0000001;

BOOST_AUTO_TEST_CASE(OneBody)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace cst = boost::math::constants;

  double mass = 1.;
  Matrix3d I = Matrix3d::Identity();
  Vector3d h = Vector3d(0., 0.5, 0.);

  RBInertiad rbi(mass, h, I);

  Body b0(rbi, "b0");
  Body b1(rbi, "b1");

  Joint j0(Joint::RevX, true, "j0");

  MultiBodyGraph mbg;

  mbg.addBody(b0);
  mbg.addBody(b1);

  mbg.addJoint(j0);

  mbg.linkBodies("b0", PTransformd::Identity(), "b1", PTransformd::Identity(), "j0");

  MultiBody mb = mbg.makeMultiBody("b0", true);

  MultiBodyConfig mbc(mb);
  mbc.zero(mb);

  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  InverseDynamics id(mb);
  id.inverseDynamics(mb, mbc);

  BOOST_CHECK_EQUAL(int(id.f().size()), mb.nrBodies());
  BOOST_CHECK_SMALL(mbc.jointTorque[1][0], TOL);

  mbc.q = {{}, {cst::pi<double>()}};
  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);
  id.inverseDynamics(mb, mbc);

  std::cout << std::endl;
  BOOST_CHECK_SMALL(mbc.jointTorque[1][0], TOL);

  mbc.q = {{}, {cst::pi<double>() / 2.}};
  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);
  id.inverseDynamics(mb, mbc);

  double torque = Vector3d(0., 0., 0.5).cross(Vector3d(0., 9.81, 0.))(0);
  BOOST_CHECK_SMALL(std::abs(torque - mbc.jointTorque[1][0]), TOL);

  MultiBodyConfig mbc2 = mbc;
  mbc2.gravity = Vector3d::Zero();
  MotionVecd gravity(Vector3d::Zero(), Vector3d(0., 9.81, 0.));
  ForceVecd gravityB1 = mb.body(0).inertia() * (mbc2.bodyPosW[0] * gravity);
  ForceVecd gravityB2 = mb.body(1).inertia() * (mbc2.bodyPosW[1] * gravity);
  mbc2.force = {gravityB1, gravityB2};
  id.inverseDynamics(mb, mbc2);

  torque = Vector3d(0., 0., 0.5).cross(Vector3d(0., -9.81, 0.))(0);
  BOOST_CHECK_SMALL(std::abs(torque - mbc2.jointTorque[1][0]), TOL);
}

void makeRandomVecVec(std::vector<std::vector<double>> & vec)
{
  typedef Eigen::Matrix<double, 1, 1> EScalar;
  for(auto & v1 : vec)
    for(auto & v2 : v1) v2 = EScalar::Random()(0) * 10.;
}

void normalizeQuat(std::vector<double> & q)
{
  Eigen::Vector4d qv(q[0], q[1], q[2], q[3]);
  double norm = qv.norm();
  for(int i = 0; i < 4; ++i) q[i] /= norm;
}

void makeRandomConfig(rbd::MultiBodyConfig & mbc)
{
  makeRandomVecVec(mbc.q);
  makeRandomVecVec(mbc.alpha);
  makeRandomVecVec(mbc.alphaD);
  makeRandomVecVec(mbc.jointTorque);

  for(std::size_t i = 0; i < mbc.q.size(); ++i)
  {
    if(mbc.q[i].size() == 4 || mbc.q[i].size() == 7)
    {
      normalizeQuat(mbc.q[i]);
    }
  }
}

Eigen::MatrixXd makeHFromID(const rbd::MultiBody & mb,
                            const rbd::MultiBodyConfig & mbc,
                            rbd::InverseDynamics & id,
                            const Eigen::VectorXd & C)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;

  Eigen::MatrixXd H(mb.nrDof(), mb.nrDof());
  VectorXd Hd(mb.nrDof());

  MultiBodyConfig mbcd(mbc);
  for(auto & v1 : mbcd.alphaD)
  {
    for(auto & v2 : v1)
    {
      v2 = 0.;
    }
  }

  int col = 0;
  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    for(int j = 0; j < mb.joint(i).dof(); ++j)
    {
      mbcd.alphaD[i][j] = 1.;

      id.inverseDynamics(mb, mbcd);

      int dof = 0;
      for(auto & v1 : mbcd.jointTorque)
      {
        for(auto & v2 : v1)
        {
          Hd(dof) = v2;
          ++dof;
        }
      }

      H.col(col) = Hd - C;

      mbcd.alphaD[i][j] = 0.;
      ++col;
    }
  }

  return H;
}

BOOST_AUTO_TEST_CASE(IDvsFDFixed)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace cst = boost::math::constants;

  typedef Matrix<double, 1, 1> EScalar;

  RBInertiad I0(EScalar::Random()(0) * 10., Vector3d::Random() * 10., Matrix3d::Random().triangularView<Lower>());
  RBInertiad I1(EScalar::Random()(0) * 10., Vector3d::Random() * 10., Matrix3d::Random().triangularView<Lower>());
  RBInertiad I2(EScalar::Random()(0) * 10., Vector3d::Random() * 10., Matrix3d::Random().triangularView<Lower>());
  RBInertiad I3(EScalar::Random()(0) * 10., Vector3d::Random() * 10., Matrix3d::Random().triangularView<Lower>());

  Body b0(I0, "b0");
  Body b1(I1, "b1");
  Body b2(I2, "b2");
  Body b3(I3, "b3");

  Joint j0 = Joint(Joint::Spherical, true, "j0");
  Joint j1 = Joint(Joint::RevX, true, "j1");
  Joint j2 = Joint(Joint::RevZ, true, "j2");

  MultiBodyGraph mbg;

  mbg.addBody(b0);
  mbg.addBody(b1);
  mbg.addBody(b2);
  mbg.addBody(b3);

  mbg.addJoint(j0);
  mbg.addJoint(j1);
  mbg.addJoint(j2);

  mbg.linkBodies("b0", PTransformd(Vector3d(0., 0.5, 0.)), "b1", PTransformd(Vector3d(0., -0.5, 0.)), "j0");
  mbg.linkBodies("b1", PTransformd(Vector3d(0.5, 0., 0.)), "b2", PTransformd(Vector3d(0., 0., 0.)), "j1");
  mbg.linkBodies("b1", PTransformd(Vector3d(-0.5, 0., 0.)), "b3", PTransformd(Vector3d(0., 0., 0.)), "j2");

  MultiBody mb = mbg.makeMultiBody("b0", true);

  MultiBodyConfig mbc(mb);

  mbc.q = {{}, {1., 0., 0., 0.}, {0.}, {0.}};
  mbc.alpha = {{}, {0., 0., 0.}, {0.}, {0.}};
  mbc.alphaD = {{}, {0., 0., 0.}, {0.}, {0.}};
  mbc.force = {ForceVecd(Vector6d::Zero()), ForceVecd(Vector6d::Zero()), ForceVecd(Vector6d::Zero()),
               ForceVecd(Vector6d::Zero())};

  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  InverseDynamics id(mb);
  ForwardDynamics fd(mb);

  // check non linear is null
  mbc.gravity = Vector3d::Zero();
  fd.computeC(mb, mbc);

  BOOST_CHECK(fd.C().isZero());

  // check FD C against ID C
  mbc.gravity = Vector3d(0., -9.81, 0.);
  makeRandomConfig(mbc);

  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  fd.computeC(mb, mbc);

  mbc.alphaD = {{}, {0., 0., 0.}, {0.}, {0.}};
  id.inverseDynamics(mb, mbc);

  VectorXd ID_C(mb.nrDof());
  int dof = 0;
  for(auto & v1 : mbc.jointTorque)
  {
    for(auto & v2 : v1)
    {
      ID_C(dof) = v2;
      ++dof;
    }
  }

#ifdef __i386__
  BOOST_CHECK_SMALL((fd.C() - ID_C).array().abs().sum(), TOL);
#else
  BOOST_CHECK_EQUAL(fd.C(), ID_C);
#endif

  // check FD H against ID H
  makeRandomConfig(mbc);

  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  fd.forwardDynamics(mb, mbc);
  MatrixXd ID_H = makeHFromID(mb, mbc, id, fd.C());

  BOOST_CHECK_SMALL((fd.H() - ID_H).norm(), 1e-10);

  // check symmetry

  MatrixXd L = fd.H().triangularView<Lower>();
  MatrixXd U = fd.H().triangularView<Upper>().transpose();

  BOOST_CHECK_SMALL((L - U).norm(), 1e-10);

  // check torque and acceleration output

  // torque -> FD -> alphaD -> ID -> torque
  makeRandomConfig(mbc);

  VectorXd vT1(mb.nrDof()), vT2(mb.nrDof());

  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  paramToVector(mbc.jointTorque, vT1);

  fd.forwardDynamics(mb, mbc);
  id.inverseDynamics(mb, mbc);

  paramToVector(mbc.jointTorque, vT2);

  BOOST_CHECK_SMALL((vT1 - vT2).norm(), 1e-10);

  // alphaD -> ID -> torque -> FD -> alphaD
  makeRandomConfig(mbc);

  VectorXd vA1(mb.nrDof()), vA2(mb.nrDof());

  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  paramToVector(mbc.alphaD, vA1);

  id.inverseDynamics(mb, mbc);
  fd.forwardDynamics(mb, mbc);

  paramToVector(mbc.alphaD, vA2);

  BOOST_CHECK_SMALL((vA1 - vA2).norm(), 1e-10);
}

BOOST_AUTO_TEST_CASE(IDvsFDFree)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;
  namespace cst = boost::math::constants;

  typedef Matrix<double, 1, 1> EScalar;

  RBInertiad I0(EScalar::Random()(0) * 10., Vector3d::Random() * 10., Matrix3d::Random().triangularView<Lower>());
  RBInertiad I1(EScalar::Random()(0) * 10., Vector3d::Random() * 10., Matrix3d::Random().triangularView<Lower>());
  RBInertiad I2(EScalar::Random()(0) * 10., Vector3d::Random() * 10., Matrix3d::Random().triangularView<Lower>());
  RBInertiad I3(EScalar::Random()(0) * 10., Vector3d::Random() * 10., Matrix3d::Random().triangularView<Lower>());

  Body b0(I0, "b0");
  Body b1(I1, "b1");
  Body b2(I2, "b2");
  Body b3(I3, "b3");

  Joint j0 = Joint(Joint::Spherical, true, "j0");
  Joint j1 = Joint(Joint::RevX, true, "j1");
  Joint j2 = Joint(Joint::RevZ, true, "j2");

  MultiBodyGraph mbg;

  mbg.addBody(b0);
  mbg.addBody(b1);
  mbg.addBody(b2);
  mbg.addBody(b3);

  mbg.addJoint(j0);
  mbg.addJoint(j1);
  mbg.addJoint(j2);

  mbg.linkBodies("b0", PTransformd(Vector3d(0., 0.5, 0.)), "b1", PTransformd(Vector3d(0., -0.5, 0.)), "j0");
  mbg.linkBodies("b1", PTransformd(Vector3d(0.5, 0., 0.)), "b2", PTransformd(Vector3d(0., 0., 0.)), "j1");
  mbg.linkBodies("b1", PTransformd(Vector3d(-0.5, 0., 0.)), "b3", PTransformd(Vector3d(0., 0., 0.)), "j2");

  MultiBody mb = mbg.makeMultiBody("b0", false);

  MultiBodyConfig mbc(mb);

  mbc.q = {{1., 0., 0., 0., 0., 0., 0.}, {1., 0., 0., 0.}, {0.}, {0.}};
  mbc.alpha = {{0., 0., 0., 0., 0., 0.}, {0., 0., 0.}, {0.}, {0.}};
  mbc.alphaD = {{0., 0., 0., 0., 0., 0.}, {0., 0., 0.}, {0.}, {0.}};
  mbc.force = {ForceVecd(Vector6d::Zero()), ForceVecd(Vector6d::Zero()), ForceVecd(Vector6d::Zero()),
               ForceVecd(Vector6d::Zero())};

  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  InverseDynamics id(mb);
  ForwardDynamics fd(mb);

  // check non linear is null
  mbc.gravity = Vector3d::Zero();
  fd.computeC(mb, mbc);

  BOOST_CHECK(fd.C().isZero());

  // check FD C against ID C
  mbc.gravity = Vector3d(0., -9.81, 0.);
  makeRandomConfig(mbc);

  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  fd.computeC(mb, mbc);

  mbc.alphaD = {{0., 0., 0., 0., 0., 0.}, {0., 0., 0.}, {0.}, {0.}};
  id.inverseDynamics(mb, mbc);

  VectorXd ID_C(mb.nrDof());
  int dof = 0;
  for(auto & v1 : mbc.jointTorque)
  {
    for(auto & v2 : v1)
    {
      ID_C(dof) = v2;
      ++dof;
    }
  }

#ifdef __i386__
  BOOST_CHECK_SMALL((fd.C() - ID_C).array().abs().sum(), TOL);
#else
  BOOST_CHECK_EQUAL(fd.C(), ID_C);
#endif

  // check FD H against ID H
  makeRandomConfig(mbc);

  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  fd.forwardDynamics(mb, mbc);
  MatrixXd ID_H = makeHFromID(mb, mbc, id, fd.C());

  BOOST_CHECK_SMALL((fd.H() - ID_H).norm(), 1e-10);

  // check symmetry

  MatrixXd L = fd.H().triangularView<Lower>();
  MatrixXd U = fd.H().triangularView<Upper>().transpose();

  BOOST_CHECK_SMALL((L - U).norm(), 1e-10);

  // check torque and acceleration output

  // torque -> FD -> alphaD -> ID -> torque
  makeRandomConfig(mbc);

  VectorXd vT1(mb.nrDof()), vT2(mb.nrDof());

  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  paramToVector(mbc.jointTorque, vT1);

  internal::set_is_malloc_allowed(false);
  fd.forwardDynamics(mb, mbc);
  id.inverseDynamics(mb, mbc);
  internal::set_is_malloc_allowed(true);

  paramToVector(mbc.jointTorque, vT2);

#ifndef WIN32
  BOOST_CHECK_SMALL((vT1 - vT2).norm(), 1e-9);
#else
  BOOST_CHECK_SMALL((vT1 - vT2).norm(), 1e-8);
#endif

  // alphaD -> ID -> torque -> FD -> alphaD
  makeRandomConfig(mbc);

  VectorXd vA1(mb.nrDof()), vA2(mb.nrDof());

  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  paramToVector(mbc.alphaD, vA1);

  id.inverseDynamics(mb, mbc);
  fd.forwardDynamics(mb, mbc);

  paramToVector(mbc.alphaD, vA2);

  BOOST_CHECK_SMALL((vA1 - vA2).norm(), 1e-10);
}

BOOST_AUTO_TEST_CASE(MultiBodyGraphMerge)
{
  rbd::MultiBody mb;
  rbd::MultiBodyConfig mbc;
  rbd::MultiBodyGraph mbg;
  std::tie(mb, mbc, mbg) = makeXYZSarm();

  BOOST_CHECK_EQUAL(mbg.nrNodes(), 5);
  BOOST_CHECK_EQUAL(mbg.nrJoints(), 4);
  BOOST_CHECK_EQUAL(mb.nrBodies(), 5);
  BOOST_CHECK_EQUAL(mb.nrJoints(), 5);

  forwardKinematics(mb, mbc);
  forwardVelocity(mb, mbc);

  std::map<std::string, std::vector<double>> configByName;
  configByName["j0"] = {0.};
  configByName["j1"] = {0.};
  configByName["j2"] = {0.};
  configByName["j3"] = {1., 0., 0., 0.};

  // merge b2, b3 and b4 in b1
  mbg.mergeSubBodies("b0", "j1", configByName);
  mbg.mergeSubBodies("b0", "j3", configByName);

  rbd::MultiBody mbMerged = mbg.makeMultiBody("b0", true);

  BOOST_CHECK_EQUAL(mbg.nrNodes(), 2);
  BOOST_CHECK_EQUAL(mbg.nrJoints(), 1);
  BOOST_CHECK_EQUAL(mbMerged.nrBodies(), 2);
  BOOST_CHECK_EQUAL(mbMerged.nrJoints(), 2);

  rbd::ForwardDynamics fd(mb);
  fd.forwardDynamics(mb, mbc);
  double error = (fd.inertiaSubTree()[1].matrix() - mbMerged.body(1).inertia().matrix()).norm();

  BOOST_CHECK_SMALL(error, TOL);
}