#pragma once
// posit.hpp: definition of arbitrary posit number configurations
//
// Copyright (C) 2017 Stillwater Supercomputing, Inc.
//
// This file is part of the universal numbers project, which is released under an MIT Open Source license.

#include <cmath>
#include <cassert>
#include <iostream>
#include <limits>
#include <boost/multiprecision/cpp_dec_float.hpp>

using boost::multiprecision::cpp_dec_float_50;
using boost::multiprecision::cpp_dec_float_100;

// to yield a fast regression environment for productive development
// we want to leverage the IEEE floating point hardware available on x86 and ARM.
// Problem is that neither support a true IEEE 128bit long double.
// x86 provides a irreproducible x87 80bit format that is susceptible to inconsistent results due to multi-programming
// ARM only provides a 64bit double format.
// This conditional section is intended to create a unification of a long double format across
// different compilation environments that creates a fast verification environment through consistent hw support.
// Another option is to use a multiprecision floating point emulation layer.
// Side note: the performance of the bitset<> manipulation is slower than a multiprecision floating point implementation
// so this comment is talking about issues that will come to pass when we transition to a high performance sw emulation.

// 128bit double-double
struct __128bitdd {
	double upper;
	double lower;
};

#if defined(__clang__)
/* Clang/LLVM. ---------------------------------------------- */
typedef __128bitdd double_double;

#elif defined(__ICC) || defined(__INTEL_COMPILER)
/* Intel ICC/ICPC. ------------------------------------------ */
typedef __128bitdd double_double;

#elif defined(__GNUC__) || defined(__GNUG__)
/* GNU GCC/G++. --------------------------------------------- */
typedef __128bitdd double_double;

#elif defined(__HP_cc) || defined(__HP_aCC)
/* Hewlett-Packard C/aC++. ---------------------------------- */

#elif defined(__IBMC__) || defined(__IBMCPP__)
/* IBM XL C/C++. -------------------------------------------- */

#elif defined(_MSC_VER)
/* Microsoft Visual Studio. --------------------------------- */
typedef __128bitdd double_double;

#elif defined(__PGI)
/* Portland Group PGCC/PGCPP. ------------------------------- */

#elif defined(__SUNPRO_C) || defined(__SUNPRO_CC)
/* Oracle Solaris Studio. ----------------------------------- */

#endif

// Posits encode error conditions as NaR (Not a Real), propagating the error through arithmetic operations is preferred
#include "exceptions.hpp"
#include "../bitblock/bitblock.hpp"
#include "bit_functions.hpp"
#include "trace_constants.hpp"
#include "posit_functions.hpp"
#include "value.hpp"
#include "fraction.hpp"
#include "exponent.hpp"
#include "regime.hpp"

namespace sw {
	namespace unum {

// Forward definitions
template<size_t nbits, size_t es> class posit;
template<size_t nbits, size_t es> posit<nbits, es> abs(const posit<nbits, es>& p);
template<size_t nbits, size_t es> posit<nbits, es> sqrt(const posit<nbits, es>& p);
template<size_t nbits, size_t es> posit<nbits, es> minpos();
template<size_t nbits, size_t es> posit<nbits, es> maxpos();

// Not A Real is the posit encoding for INFINITY and arithmetic errors that can propagate
// The symbol NAR can be used to initialize a posit, i.e., posit<nbits,es>(NAR), or posit<nbits,es> p = NAR
#define NAR INFINITY

// define to non-zero if you want to enable arithmetic and logic literals
// POSIT_ENABLE_LITERALS

/*
 class posit represents arbitrary configuration posits and their basic arithmetic operations (add/sub, mul/div)
 */
template<size_t nbits, size_t es>
class posit {

	static_assert(es + 3 <= nbits, "Value for 'es' is too large for this 'nbits' value");
//	static_assert(sizeof(long double) == 16, "Posit library requires compiler support for 128 bit long double.");
//	static_assert((sizeof(long double) == 16) && (std::numeric_limits<long double>::digits < 113), "C++ math library for long double does not support 128-bit quad precision floats.");

	template <typename T>
	posit<nbits, es>& float_assign(const T& rhs) {
		constexpr int dfbits = std::numeric_limits<T>::digits - 1;
		value<dfbits> v((T)rhs);

		// special case processing
		if (v.isZero()) {
			setToZero();
			return *this;
		}
		if (v.isInfinite() || v.isNaN()) {  // posit encode for FP_INFINITE and NaN as NaR (Not a Real)
			setToNaR();
			return *this;
		}

		convert(v);
		return *this;
	}

public:
	static constexpr size_t rbits   = nbits - 1;
	static constexpr size_t ebits   = es;
	static constexpr size_t fbits   = nbits - 3 - es;
	static constexpr size_t abits   = fbits + 4;       // size of the addend
	static constexpr size_t fhbits  = fbits + 1;       // size of fraction + hidden bit
	static constexpr size_t mbits   = 2 * fhbits;      // size of the multiplier output
	static constexpr size_t divbits = 3 * fhbits + 4;  // size of the divider output

	posit() { setToZero();  }

	posit(const posit&) = default;
	posit(posit&&) = default;

	posit& operator=(const posit&) = default;
	posit& operator=(posit&&) = default;

	/// Construct posit from its components
	posit(bool sign, const regime<nbits, es>& r, const exponent<nbits, es>& e, const fraction<fbits>& f)
          : _sign(sign), _regime(r), _exponent(e), _fraction(f) {
		// generate raw bit representation
		_raw_bits = _sign ? twos_complement(collect()) : collect();
		_raw_bits.set(nbits - 1, _sign);
	}
	/// Construct posit from raw bits
	posit(const std::bitset<nbits>& raw_bits) {
		*this = set(raw_bits);
	}
	// initializers for native types
	posit(signed char initial_value)        { *this = initial_value; }
	posit(short initial_value)              { *this = initial_value; }
	posit(int initial_value)                { *this = initial_value; }
	posit(long long initial_value)          { *this = initial_value; }
	posit(unsigned long long initial_value) { *this = initial_value; }
	posit(float initial_value)              { *this = initial_value; }
	posit(double initial_value)             { *this = initial_value; }
	posit(long double initial_value)        { *this = initial_value; }
	// assignment operators for native types
	posit& operator=(signed char rhs) {
		value<8> v(rhs);
		if (v.isZero()) {
			setToZero();
			return *this;
		}
		else if (v.isNegative()) {
			convert(v);
			take_2s_complement();
		}
		else {
			convert(v);
		}
		return *this;
	}
	posit<nbits, es>& operator=(short rhs) {
		value<16> v(rhs);
		if (v.isZero()) {
			setToZero();
			return *this;
		}
		else if (v.isNegative()) {
			convert(v);
			take_2s_complement();
		}
		else {
			convert(v);
		}
		return *this;
	}
	posit<nbits, es>& operator=(int rhs) {
		value<32> v(rhs);
		if (v.isZero()) {
			setToZero();
			return *this;
		}
		else if (v.isNegative()) {
			convert(v);
			take_2s_complement();
		}
		else {
			convert(v);
		}
		return *this;
	}
	posit<nbits, es>& operator=(long long rhs) {
		value<64> v(rhs);
		if (v.isZero()) {
			setToZero();
			return *this;
		}
		else if (v.isNegative()) {
			convert(v);
			take_2s_complement();
		}
		else {
			convert(v);
		}
		return *this;
	}
	posit<nbits, es>& operator=(unsigned long long rhs) {
		value<64> v(rhs);
		if (v.isZero()) {
			setToZero();
			return *this;
		}
		else {
			convert(v);
		}
		convert(v);
		return *this;
	}
	posit<nbits, es>& operator=(float rhs) {
		return float_assign(rhs);
	}
	posit<nbits, es>& operator=(double rhs) {
		return float_assign(rhs);
	}
	posit<nbits, es>& operator=(long double rhs) {
       		return float_assign(rhs);
	}

	// prefix operator
	posit<nbits, es> operator-() const {
		if (isZero()) {
			return *this;
		}
		if (isNaR()) {
			return *this;
		}
		posit<nbits, es> negated;
		negated.decode(twos_complement(_raw_bits));
		return negated;
	}

	// we model a hw pipeline with register assignments, functional block, and conversion
	posit<nbits, es>& operator+=(const posit& rhs) {
		if (_trace_add) std::cout << "---------------------- ADD -------------------" << std::endl;
		// special case handling of the inputs
		if (isNaR() || rhs.isNaR()) {
			setToNaR();
			return *this;
		}
		if (isZero()) {
			*this = rhs;
			return *this;
		}
		if (rhs.isZero()) return *this;

		// arithmetic operation
		value<abits + 1> sum;
		value<fbits> a, b;
		// transform the inputs into (sign,scale,fraction) triples
		normalize(a);
		rhs.normalize(b);
		module_add<fbits,abits>(a, b, sum);		// add the two inputs

		// special case handling of the result
		if (sum.isZero()) {
			setToZero();
		}
		else if (sum.isInfinite()) {
			setToNaR();
		}
		else {
			convert(sum);
		}
		return *this;
	}
	posit<nbits, es>& operator+=(double rhs) {
		return *this += posit<nbits, es>(rhs);
	}
	posit<nbits, es>& operator-=(const posit& rhs) {
		if (_trace_sub) std::cout << "---------------------- SUB -------------------" << std::endl;
		// special case handling of the inputs
		if (isNaR() || rhs.isNaR()) {
			setToNaR();
			return *this;
		}
		if (isZero()) {
			*this = -rhs;
			return *this;
		}
		if (rhs.isZero()) return *this;

		// arithmetic operation
		value<abits + 1> difference;
		value<fbits> a, b;
		// transform the inputs into (sign,scale,fraction) triples
		normalize(a);
		rhs.normalize(b);
		module_subtract<fbits, abits>(a, b, difference);	// add the two inputs

		// special case handling of the result
		if (difference.isZero()) {
			setToZero();
		}
		else if (difference.isInfinite()) {
			setToNaR();
		}
		else {
			convert(difference);
		}
		return *this;
	}
	posit<nbits, es>& operator-=(double rhs) {
		return *this -= posit<nbits, es>(rhs);
	}
	posit<nbits, es>& operator*=(const posit& rhs) {
		static_assert(fhbits > 0, "posit configuration does not support multiplication");
		if (_trace_mul) std::cout << "---------------------- MUL -------------------" << std::endl;
		// special case handling of the inputs
		if (isNaR() || rhs.isNaR()) {
			setToNaR();
			return *this;
		}
		if (isZero() || rhs.isZero()) {
			setToZero();
			return *this;
		}

		// arithmetic operation
		value<mbits> product;
		value<fbits> a, b;
		// transform the inputs into (sign,scale,fraction) triples
		normalize(a);
		rhs.normalize(b);

		module_multiply(a, b, product);    // multiply the two inputs

		// special case handling on the output
		if (product.isZero()) {
			setToZero();
		}
		else if (product.isInfinite()) {
			setToNaR();
		}
		else {
			convert(product);
		}
		return *this;
	}
	posit<nbits, es>& operator*=(double rhs) {
		return *this *= posit<nbits, es>(rhs);
	}
	posit<nbits, es>& operator/=(const posit& rhs) {
		if (_trace_div) std::cout << "---------------------- DIV -------------------" << std::endl;
		// since we are encoding error conditions as NaR (Not a Real), we need to process that condition first
		if (rhs.isZero()) {
			setToNaR();
			return *this;
			//throw divide_by_zero{};    not throwing is a quiet signalling NaR
		}
		if (rhs.isNaR()) {
			setToNaR();
			return *this;
		}
		if (isZero() || isNaR()) {
			return *this;
		}

		value<divbits> ratio;
		value<fbits> a, b;
		// transform the inputs into (sign,scale,fraction) triples
		normalize(a);
		rhs.normalize(b);

		module_divide(a, b, ratio);

		// special case handling on the output
		if (ratio.isZero()) {
			throw "result can't be zero";
			setToZero();  // this can't happen as we would project back onto minpos
		}
		else if (ratio.isInfinite()) {
			throw "result can't be NaR";
			setToNaR();  // this can't happen as we would project back onto maxpos
		}
		else {
			convert<divbits>(ratio);
		}

		return *this;
	}
	posit<nbits, es>& operator/=(double rhs) {
		return *this /= posit<nbits, es>(rhs);
	}
	posit<nbits, es>& operator++() {
		increment_posit();
		return *this;
	}
	posit<nbits, es> operator++(int) {
		posit tmp(*this);
		operator++();
		return tmp;
	}
	posit<nbits, es>& operator--() {
		decrement_posit();
		return *this;
	}
	posit<nbits, es> operator--(int) {
		posit tmp(*this);
		operator--();
		return tmp;
	}

	posit<nbits, es> reciprocate() const {
		if (_trace_reciprocate) std::cout << "-------------------- RECIPROCATE ----------------" << std::endl;
		posit<nbits, es> p;
		// special case of NaR (Not a Real)
		if (isNaR()) {
			p.setToNaR();
			return p;
		}
		if (isZero()) {
			p.setToNaR();
			return p;
		}
		// compute the reciprocal
		bool old_sign = _sign;
		bitblock<nbits> raw_bits;
		if (isPowerOf2()) {
			raw_bits = twos_complement(_raw_bits);
			raw_bits.set(nbits-1, old_sign);
			p.set(raw_bits);
		}
		else {
			constexpr size_t operand_size = fhbits;
			bitblock<operand_size> one;
			one.set(operand_size - 1, true);
			bitblock<operand_size> frac;
			copy_into(_fraction.get(), 0, frac);
			frac.set(operand_size - 1, true);
			constexpr size_t reciprocal_size = 3 * fbits + 4;
			bitblock<reciprocal_size> reciprocal;
			divide_with_fraction(one, frac, reciprocal);
			if (_trace_reciprocate) {
				std::cout << "one    " << one << std::endl;
				std::cout << "frac   " << frac << std::endl;
				std::cout << "recip  " << reciprocal << std::endl;
			}

			// radix point falls at operand size == reciprocal_size - operand_size - 1
			reciprocal <<= operand_size - 1;
			if (_trace_reciprocate) std::cout << "frac   " << reciprocal << std::endl;
			int new_scale = -scale();
			int msb = findMostSignificantBit(reciprocal);
			if (msb > 0) {
				int shift = reciprocal_size - msb;
				reciprocal <<= shift;
				new_scale -= (shift-1);
				if (_trace_reciprocate) std::cout << "result " << reciprocal << std::endl;
			}
			//std::bitset<operand_size> tr;
			//truncate(reciprocal, tr);
			//std::cout << "tr     " << tr << std::endl;
			p.convert(_sign, new_scale, reciprocal);
		}
		return p;
	}
	// SELECTORS
	bool isNaR() const {
		return (_sign & _regime.isZero());
	}
	bool isZero() const {
		return (!_sign & _regime.isZero());
	}
	bool isOne() const { // pattern 010000....
		bitblock<nbits> tmp(_raw_bits);
		tmp.set(nbits - 2, false);
		bool oneBitSet = tmp.none();
		return !_sign & oneBitSet;
	}
	bool isMinusOne() const { // pattern 110000...
		bitblock<nbits> tmp(_raw_bits);
		tmp.set(nbits - 1, false);
		tmp.set(nbits - 2, false);
		bool oneBitSet = tmp.none();
		return _sign & oneBitSet;
	}
	bool isNegative() const {
		return _sign;
	}
	bool isPositive() const {
		return !_sign;
	}
	bool isPowerOf2() const {
		return _fraction.none();
	}

	inline int	      sign_value() const {
		return (_sign ? -1 : 1);
	}
	inline double   regime_value() const {
		return _regime.value();
	}
	inline double exponent_value() const {
		return _exponent.value();
	}
	inline double fraction_value() const {
		return _fraction.value();
	}

	int				   regime_k() const {
		return _regime.regime_k();
	}
	int                get_scale() const { return _regime.scale() + _exponent.scale(); }
	bool               get_sign() const { return _sign;  }
	regime<nbits, es>  get_regime() const {
		return _regime;
	}
	exponent<nbits,es> get_exponent() const {
		return _exponent;
	}
	fraction<fbits>    get_fraction() const {
		return _fraction;
	}
	bitblock<nbits>    get() const {
		return _raw_bits;
	}
	bitblock<nbits>    get_decoded() const {
		bitblock<rbits> r = _regime.get();
		size_t nrRegimeBits = _regime.nrBits();
		bitblock<es> e = _exponent.get();
		size_t nrExponentBits = _exponent.nrBits();
		bitblock<fbits> f = _fraction.get();
		size_t nrFractionBits = _fraction.nrBits();

		bitblock<nbits> _Bits;
		_Bits.set(nbits - 1, _sign);
		int msb = nbits - 2;
		for (size_t i = 0; i < nrRegimeBits; i++) {
			_Bits.set(std::size_t(msb--), r[nbits - 2 - i]);
		}
		if (msb < 0)
                    return _Bits;
		for (size_t i = 0; i < nrExponentBits && msb >= 0; i++) {
			_Bits.set(std::size_t(msb--), e[es - 1 - i]);
		}
		if (msb < 0) return _Bits;
		for (size_t i = 0; i < nrFractionBits && msb >= 0; i++) {
			_Bits.set(std::size_t(msb--), f[fbits - 1 - i]);
		}
		return _Bits;
	}
	std::string        get_quadrant() const {
		posit<nbits, es> pOne(1), pMinusOne(-1);
		if (_sign) {
			// west
			if (*this > pMinusOne) {
				return "SW";
			}
			else {
				return "NW";
			}
		}
		else {
			// east
			if (*this < pOne) {
				return "SE";
			}
			else {
				return "NE";
			}
		}
	}
	long long          get_encoding_as_integer() const {
		if (nbits > 64) throw "encoding cannot be represented by a 64bit integer";
		long long as_integer = 0;
		unsigned long long mask = 1;
		for (unsigned i = 0; i < nbits; i++) {
			if (_raw_bits[i]) as_integer |= mask;
			mask <<= 1;
		}
		return as_integer;
	}
	// MODIFIERS
	inline void clear() {
		_sign = false;
		_regime.reset();
		_exponent.reset();
		_fraction.reset();
		_raw_bits.reset();
	}
	inline void setToZero() {
		_sign = false;
		_regime.setToZero();
		_exponent.reset();
		_fraction.reset();
		_raw_bits.reset();
	}
	inline void setToNaR() {
		_sign = true;
		_regime.setToInfinite();
		_exponent.reset();
		_fraction.reset();
		_raw_bits.reset();
		_raw_bits.set(nbits - 1, true);
	}
	posit<nbits, es>& set(const bitblock<nbits>& raw_bits) {
		decode(raw_bits);
		return *this;
	}
	// Set the raw bits of the posit given a binary pattern
	posit<nbits,es>& set_raw_bits(uint64_t value) {
		clear();
		bitblock<nbits> raw_bits;
		uint64_t mask = 1;
		for ( int i = 0; i < nbits; i++ ) {
			raw_bits.set(i,(value & mask));
			mask <<= 1;
		}
		// decode to cache the posit number interpretation
		decode(raw_bits);
		return *this;
	}
	int decode_regime(bitblock<nbits>& raw_bits) {
		// let m be the number of identical bits in the regime
		int m = 0;   // regime runlength counter
		int k = 0;   // converted regime scale
		if (raw_bits[nbits - 2] == 1) {   // run length of 1's
			m = 1;   // if a run of 1's k = m - 1
			for (int i = nbits - 3; i >= 0; --i) {
				if (raw_bits[i] == 1) {
					m++;
				}
				else {
					break;
				}
			}
			k = m - 1;
		}
		else {
			m = 1;  // if a run of 0's k = -m
			for (int i = nbits - 3; i >= 0; --i) {
				if (raw_bits[i] == 0) {
					m++;
				}
				else {
					break;
				}
			}
			k = -m;
		}
		return k;
	}
	// decode takes the raw bits representing a posit coming from memory
	// and decodes the regime, the exponent, and the fraction.
	// This function has the functionality of the posit register-file load.
	void extract_fields(const bitblock<nbits>& raw_bits) {
		bitblock<nbits> tmp(raw_bits);
		if (_sign) tmp = twos_complement(tmp);
		size_t nrRegimeBits = _regime.assign_regime_pattern(decode_regime(tmp));

		// get the exponent bits
		// start of exponent is nbits - (sign_bit + regime_bits)
		int msb = int(int(nbits) - 1 - (1 + nrRegimeBits));
		size_t nrExponentBits = 0;
		if (es > 0) {
			bitblock<es> _exp;
			if (msb >= 0 && es > 0) {
				nrExponentBits = (msb >= es - 1 ? es : msb + 1);
				for (size_t i = 0; i < nrExponentBits; i++) {
					_exp[es - 1 - i] = tmp[msb - i];
				}
			}
			_exponent.set(_exp, nrExponentBits);
		}

		// finally, set the fraction bits
		// we do this so that the fraction is right extended with 0;
		// The max fraction is <nbits - 3 - es>, but we are setting it to <nbits - 3> and right-extent
		// The msb bit of the fraction represents 2^-1, the next 2^-2, etc.
		// If the fraction is empty, we have a fraction of nbits-3 0 bits
		// If the fraction is one bit, we have still have fraction of nbits-3, with the msb representing 2^-1, and the rest are right extended 0's
		bitblock<fbits> _frac;
		msb = msb - int(nrExponentBits);
		size_t nrFractionBits = (msb < 0 ? 0 : msb + 1);
		if (msb >= 0) {
			for (int i = msb; i >= 0; --i) {
				_frac[fbits - 1 - (msb - i)] = tmp[i];
			}
		}
		_fraction.set(_frac, nrFractionBits);
	}
	void decode(const bitblock<nbits>& raw_bits) {
		_raw_bits = raw_bits;	// store the raw bits for reference
		// check special cases
		_sign     = raw_bits.test(nbits - 1);
		// check for special cases
		bool special = false;
		if (_sign) {
			std::bitset<nbits> tmp(raw_bits);
			tmp.reset(nbits - 1);
			if (tmp.none()) {
				setToNaR();  // special case = NaR (Not a Real)
			}
			else {
				extract_fields(raw_bits);
			}
		}
		else {
			if (raw_bits.none()) {  // special case = 0
				setToZero();
			}
			else {
				extract_fields(raw_bits);
			}
		}
		if (_trace_decode) std::cout << "raw bits: " << _raw_bits << " posit bits: " << (_sign ? "1|" : "0|") << _regime << "|" << _exponent << "|" << _fraction << " posit value: " << *this << std::endl;

		// we are storing both the raw bit representation and the decoded form
		// so no need to transform back via 2's complement of regime/exponent/fraction
	}

	// Maybe remove explicit, MTL compiles, but we have lots of double computation then
	explicit operator cpp_dec_float_50() const { return to_cpp_dec_float_50(); }
	explicit operator cpp_dec_float_100() const { return to_cpp_dec_float_100(); }
	explicit operator long double() const { return to_long_double(); }
	explicit operator double() const { return to_double(); }
	explicit operator float() const { return to_float(); }
	explicit operator long long() const { return to_long_long(); }
	explicit operator long() const { return to_long(); }
	explicit operator int() const { return to_int(); }

	// currently, size is tied to fbits size of posit config. Is there a need for a case that captures a user-defined sized fraction?
	value<fbits> convert_to_scientific_notation() const {
		return value<fbits>(_sign, scale(), get_fraction().get(), isZero(), isNaR());
	}
	value<fbits> to_value() const {
		return value<fbits>(_sign, scale(), get_fraction().get(), isZero(), isNaR());
	}
	void normalize(value<fbits>& v) const {
		v.set(_sign, scale(), _fraction.get(), isZero(), isNaR());
	}
	template<size_t tgt_fbits>
	void normalize_to(value<tgt_fbits>& v) const {
		bitblock<tgt_fbits> _fr;
		bitblock<fbits> _src = _fraction.get();
		int tgt, src;
		for (tgt = int(tgt_fbits) - 1, src = int(fbits) - 1; tgt >= 0, src >= 0; tgt--, src--) _fr[tgt] = _src[src];
		v.set(_sign, scale(), _fr, isZero(), isNaR());
	}
	// collect the posit components into a bitset
	bitblock<nbits> collect() {
		bitblock<rbits> r = _regime.get();
		size_t nrRegimeBits = _regime.nrBits();
		bitblock<es> e = _exponent.get();
		size_t nrExponentBits = _exponent.nrBits();
		bitblock<fbits> f = _fraction.get();
		size_t nrFractionBits = _fraction.nrBits();
		bitblock<nbits> raw_bits;
		// collect
		raw_bits.set(nbits - 1, _sign);
		int msb = int(nbits) - 2;
		for (size_t i = 0; i < nrRegimeBits; i++) {
			raw_bits.set(msb--, r[nbits - 2 - i]);
		}
		if (msb >= 0) {
			for (size_t i = 0; i < nrExponentBits; i++) {
				raw_bits.set(msb--, e[es - 1 - i]);
			}
		}
		if (msb >= 0) {
			for (size_t i = 0; i < nrFractionBits; i++) {
				raw_bits.set(msb--, f[fbits - 1 - i]);
			}
		}
		return raw_bits;
	}
	// given a decoded posit, take its 2's complement
	void take_2s_complement() {
		// transform back through 2's complement
		bitblock<rbits> r = _regime.get();
		size_t nrRegimeBits = _regime.nrBits();
		bitblock<es> e = _exponent.get();
		size_t nrExponentBits = _exponent.nrBits();
		bitblock<fbits> f = _fraction.get();
		size_t nrFractionBits = _fraction.nrBits();
		bitblock<nbits> raw_bits;
		// collect
		raw_bits.set(int(nbits) - 1, _sign);
		int msb = int(nbits) - 2;
		for (size_t i = 0; i < nrRegimeBits; i++) {
			raw_bits.set(msb--, r[nbits - 2 - i]);
		}
		if (msb >= 0) {
			for (size_t i = 0; i < nrExponentBits; i++) {
				raw_bits.set(msb--, e[es - 1 - i]);
			}
		}
		if (msb >= 0) {
			for (size_t i = 0; i < nrFractionBits; i++) {
				raw_bits.set(msb--, f[fbits - 1 - i]);
			}
		}
		// transform
		raw_bits = twos_complement(raw_bits);
		// distribute
		bitblock<nbits - 1> regime_bits;
		for (unsigned int i = 0; i < nrRegimeBits; i++) {
			regime_bits.set(nbits - 2 - i, raw_bits[nbits - 2 - i]);
		}
		_regime.set(regime_bits, nrRegimeBits);
		if (es > 0 && nrExponentBits > 0) {
			bitblock<es> exponent_bits;
			for (size_t i = 0; i < nrExponentBits; i++) {
				exponent_bits.set(es - 1 - i, raw_bits[nbits - 2 - nrRegimeBits - i]);
			}
			_exponent.set(exponent_bits, nrExponentBits);
		}
		if (nrFractionBits > 0) {
			bitblock<fbits> fraction_bits;   // was nbits - 2
			for (size_t i = 0; i < nrFractionBits; i++) {
				// fraction_bits.set(nbits - 3 - i, raw_bits[nbits - 2 - nrRegimeBits - nrExponentBits - i]);
				fraction_bits.set(fbits - 1 - i, raw_bits[nbits - 2 - nrRegimeBits - nrExponentBits - i]);
			}
			_fraction.set(fraction_bits, nrFractionBits);
		}
	}
	// scale returns the shifts to normalize the number =  regime + exponent shifts
	int scale() const {
		// how many shifts represent the regime?
		// regime = useed ^ k = 2 ^ (k*(2 ^ e))
		// scale = useed ^ k * 2^e
		return _regime.scale() + _exponent.scale();
	}
	unsigned int exp() const {
		return _exponent.scale();
	}
	// special case check for projecting values between (0, minpos] to minpos and [maxpos, inf) to maxpos
	// Returns true if the scale is too small or too large for this posit config
	// DO NOT USE the k value for this, as the k value encodes the useed regions and thus is too coarse to make this decision
	// Using the scale directly is the simplest expression of the inward projection test.
	bool check_inward_projection_range(int scale) {
		// calculate the max k factor for this posit config
		int posit_size = nbits;
		int k = scale < 0 ?	-(posit_size - 2) : (posit_size - 2);
		return scale < 0 ? scale < k*(1<<es) : scale > k*(1<<es);
	}
	// project to the next 'larger' posit: this is 'pushing away' from zero, projecting to the next bigger scale
	void project_up() {
		bool carry = _fraction.increment();
		if (carry && es > 0)
			carry = _exponent.increment();
		if (carry)
                    _regime.increment();
	}
	// step up to the next posit in a lexicographical order
	void increment_posit() {
		bitblock<nbits> raw(_raw_bits);
		increment_bitset(raw);
		decode(raw);
	}
	// step down to the previous posit in a lexicographical order
	void decrement_posit() {
		bitblock<nbits> raw(_raw_bits);
		decrement_bitset(raw);
		decode(raw);
	}

	// Generalized version
	template <size_t FBits>
	inline void convert(const value<FBits>& v) {
		if (v.isZero()) {
			setToZero();
			return;
		}
		if (v.isNaN() || v.isInfinite()) {
			setToNaR();
			return;
		}
		convert(v.sign(), v.scale(), v.fraction());
    }
	// convert assumes that ZERO and NaR cases are handled. Only non-zero and non-NaR values are allowed.
	template<size_t input_fbits>
	void convert(bool sign, int scale, bitblock<input_fbits> input_fraction) {
		clear();
		if (_trace_conversion) std::cout << "------------------- CONVERT ------------------" << std::endl;
		if (_trace_conversion) std::cout << "sign " << (sign ? "-1 " : " 1 ") << "scale " << std::setw(3) << scale << " fraction " << input_fraction << std::endl;

		// construct the posit
		_sign = sign;
		int k = calculate_unconstrained_k<nbits, es>(scale);
		// interpolation rule checks
		if (check_inward_projection_range(scale)) {    // regime dominated
			if (_trace_conversion) std::cout << "inward projection" << std::endl;
			// we are projecting to minpos/maxpos
			_regime.assign_regime_pattern(k);
			// store raw bit representation
			_raw_bits = _sign ? twos_complement(collect()) : collect();
			_raw_bits.set(nbits - 1, _sign);
			// we are done
			if (_trace_rounding) std::cout << "projection  rounding ";
		}
		else {
			const size_t pt_len = nbits + 3 + es;
			bitblock<pt_len> pt_bits;
			bitblock<pt_len> regime;
			bitblock<pt_len> exponent;
			bitblock<pt_len> fraction;
			bitblock<pt_len> sticky_bit;

			bool s = sign;
			int e = scale;
			bool r = (e >= 0);

			unsigned run = (r ? 1 + (e >> es) : -(e >> es));
			regime.set(0, 1 ^ r);
			for (unsigned i = 1; i <= run; i++) regime.set(i, r);

			unsigned esval = e % (uint32_t(1) << es);
			exponent = convert_to_bitblock<pt_len>(esval);
			unsigned nf = (unsigned)std::max<int>(0, (nbits + 1) - (2 + run + es));
			// TODO: what needs to be done if nf > fbits?
			//assert(nf <= input_fbits);
			// copy the most significant nf fraction bits into fraction
			unsigned lsb = nf <= input_fbits ? 0 : nf - input_fbits;
			for (unsigned i = lsb; i < nf; i++) fraction[i] = input_fraction[input_fbits - nf + i];

			bool sb = anyAfter(input_fraction, input_fbits - 1 - nf);

			// construct the untruncated posit
			// pt    = BitOr[BitShiftLeft[reg, es + nf + 1], BitShiftLeft[esval, nf + 1], BitShiftLeft[fv, 1], sb];
			regime <<= es + nf + 1;
			exponent <<= nf + 1;
			fraction <<= 1;
			sticky_bit.set(0, sb);

			pt_bits |= regime;
			pt_bits |= exponent;
			pt_bits |= fraction;
			pt_bits |= sticky_bit;

			unsigned len = 1 + std::max<unsigned>((nbits + 1), (2 + run + es));
			bool blast = pt_bits.test(len - nbits);
			bool bafter = pt_bits.test(len - nbits - 1);
			bool bsticky = anyAfter(pt_bits, len - nbits - 1 - 1);

			bool rb = (blast & bafter) | (bafter & bsticky);

			pt_bits <<= pt_len - len;
			bitblock<nbits> ptt;
			truncate(pt_bits, ptt);

			if (rb) increment_bitset(ptt);
			if (s) ptt = twos_complement(ptt);
			decode(ptt);
		}
	}

private:
	bitblock<nbits>        _raw_bits;	// raw bit representation
	bool				   _sign;       // decoded posit representation
	regime<nbits, es>	   _regime;		// decoded posit representation
	exponent<nbits, es>    _exponent;	// decoded posit representation
	fraction<fbits> 	   _fraction;	// decoded posit representation

	// HELPER methods
	// Conversion functions
	int         to_int() const {
		if (isZero()) return 0;
		if (isNaR()) throw "NaR (Not a Real)";
		return int(to_float());
	}
	long        to_long() const {
		if (isZero()) return 0;
		if (isNaR()) throw "NaR (Not a Real)";
		return long(to_double());
	}
	long long   to_long_long() const {
		if (isZero()) return 0;
		if (isNaR()) throw "NaR (Not a Real)";
		return long(to_long_double());
	}
	float       to_float() const {
		return (float)to_double();
	}
	double      to_double() const {
		if (isZero())	return 0.0;
		if (isNaR())	return NAN;
		return sign_value() * regime_value() * exponent_value() * (1.0 + fraction_value());
	}
	long double to_long_double() const {
		if (isZero())  return 0.0;
		if (isNaR())   return NAN;
		int s = sign_value();
		double r = regime_value(); // regime value itself will fit in a double
		double e = exponent_value(); // same with exponent
		long double f = (long double)(1.0) + _fraction.value();
		return s * r * e * f;
	}
	cpp_dec_float_50 to_cpp_dec_float_50() const {
		if (isZero())  return 0.0;
		if (isNaR())   return NAN;
		int s = sign_value();
		cpp_dec_float_50 r = regime_value(); // regime value itself will fit in a double
		cpp_dec_float_50 e = exponent_value(); // same with exponent
		cpp_dec_float_50 f = (cpp_dec_float_50)(1.0) + _fraction.value();
		return s * r * e * f;
	}
	cpp_dec_float_100 to_cpp_dec_float_100() const {
		if (isZero())  return 0.0;
		if (isNaR())   return NAN;
		int s = sign_value();
		cpp_dec_float_100 r = regime_value(); // regime value itself will fit in a double
		cpp_dec_float_100 e = exponent_value(); // same with exponent
		cpp_dec_float_100 f = (cpp_dec_float_100)(1.0) + _fraction.value();
		return s * r * e * f;
	}

	// friend functions
    // template parameters need names different from class template parameters (for gcc and clang)
	template<size_t nnbits, size_t ees>
	friend std::ostream& operator<< (std::ostream& ostr, const posit<nnbits, ees>& p);
	template<size_t nnbits, size_t ees>
	friend std::istream& operator>> (std::istream& istr, posit<nnbits, ees>& p);

	// posit - posit logic functions
	template<size_t nnbits, size_t ees>
	friend bool operator==(const posit<nnbits, ees>& lhs, const posit<nnbits, ees>& rhs);
	template<size_t nnbits, size_t ees>
	friend bool operator!=(const posit<nnbits, ees>& lhs, const posit<nnbits, ees>& rhs);
	template<size_t nnbits, size_t ees>
	friend bool operator< (const posit<nnbits, ees>& lhs, const posit<nnbits, ees>& rhs);
	template<size_t nnbits, size_t ees>
	friend bool operator> (const posit<nnbits, ees>& lhs, const posit<nnbits, ees>& rhs);
	template<size_t nnbits, size_t ees>
	friend bool operator<=(const posit<nnbits, ees>& lhs, const posit<nnbits, ees>& rhs);
	template<size_t nnbits, size_t ees>
	friend bool operator>=(const posit<nnbits, ees>& lhs, const posit<nnbits, ees>& rhs);

#if POSIT_ENABLE_LITERALS
	// posit - literal logic functions
	// posit - int
	template<size_t nnbits, size_t ees>
	friend bool operator==(const posit<nnbits, ees>& lhs, int rhs);
	template<size_t nnbits, size_t ees>
	friend bool operator!=(const posit<nnbits, ees>& lhs, int rhs);
	template<size_t nnbits, size_t ees>
	friend bool operator< (const posit<nnbits, ees>& lhs, int rhs);
	template<size_t nnbits, size_t ees>
	friend bool operator> (const posit<nnbits, ees>& lhs, int rhs);
	template<size_t nnbits, size_t ees>
	friend bool operator<=(const posit<nnbits, ees>& lhs, int rhs);
	template<size_t nnbits, size_t ees>
	friend bool operator>=(const posit<nnbits, ees>& lhs, int rhs);
	// posit - double
	template<size_t nnbits, size_t ees>
	friend bool operator==(const posit<nnbits, ees>& lhs, double rhs);
	template<size_t nnbits, size_t ees>
	friend bool operator!=(const posit<nnbits, ees>& lhs, double rhs);
	template<size_t nnbits, size_t ees>
	friend bool operator< (const posit<nnbits, ees>& lhs, double rhs);
	template<size_t nnbits, size_t ees>
	friend bool operator> (const posit<nnbits, ees>& lhs, double rhs);
	template<size_t nnbits, size_t ees>
	friend bool operator<=(const posit<nnbits, ees>& lhs, double rhs);
	template<size_t nnbits, size_t ees>
	friend bool operator>=(const posit<nnbits, ees>& lhs, double rhs);
    // posit - long double
    template<size_t nnbits, size_t ees>
    friend bool operator> (long double lhs, const posit<nnbits, ees>& rhs);
#endif // POSIT_ENABLE_LITERALS

};

////////////////// POSIT operators

// stream operators
template<size_t nbits, size_t es>
inline std::ostream& operator<<(std::ostream& ostr, const posit<nbits, es>& p) {
	if (p.isZero()) {
		ostr << double(0.0);
		return ostr;
	}
	else if (p.isNaR()) {
		ostr << "NaR";
		return ostr;
	}
	ostr << p.to_double();
	return ostr;
}

// TODO: this needs an implementation
template<size_t nbits, size_t es>
inline std::istream& operator>> (std::istream& istr, const posit<nbits, es>& p) {
	istr >> p._Bits;
	return istr;
}

// posit - posit binary logic operators
template<size_t nbits, size_t es>
inline bool operator==(const posit<nbits, es>& lhs, const posit<nbits, es>& rhs) {
	return lhs._raw_bits == rhs._raw_bits;
}
template<size_t nbits, size_t es>
inline bool operator!=(const posit<nbits, es>& lhs, const posit<nbits, es>& rhs) {
	return !operator==(lhs, rhs);
}
template<size_t nbits, size_t es>
inline bool operator< (const posit<nbits, es>& lhs, const posit<nbits, es>& rhs) {
	return lessThan(lhs._raw_bits, rhs._raw_bits);
}
template<size_t nbits, size_t es>
inline bool operator> (const posit<nbits, es>& lhs, const posit<nbits, es>& rhs) {
	return operator< (rhs, lhs);
}
template<size_t nbits, size_t es>
inline bool operator<=(const posit<nbits, es>& lhs, const posit<nbits, es>& rhs) {
	return operator< (lhs, rhs) || operator==(lhs, rhs);
}
template<size_t nbits, size_t es>
inline bool operator>=(const posit<nbits, es>& lhs, const posit<nbits, es>& rhs) {
	return !operator< (lhs, rhs);
}
template<size_t nbits, size_t es>
inline bool operator> (long double lhs, const posit<nbits, es>& rhs) {
    return operator< (rhs, posit<nbits, es>(lhs));
}

// posit - posit binary arithmetic operators
// BINARY ADDITION
template<size_t nbits, size_t es>
inline posit<nbits, es> operator+(const posit<nbits, es>& lhs, const posit<nbits, es>& rhs) {
	posit<nbits, es> sum = lhs;
	sum += rhs;
	return sum;
}
// BINARY SUBTRACTION
template<size_t nbits, size_t es>
inline posit<nbits, es> operator-(const posit<nbits, es>& lhs, const posit<nbits, es>& rhs) {
	posit<nbits, es> diff = lhs;
	diff -= rhs;
	return diff;
}
// BINARY MULTIPLICATION
template<size_t nbits, size_t es>
inline posit<nbits, es> operator*(const posit<nbits, es>& lhs, const posit<nbits, es>& rhs) {
	posit<nbits, es> mul = lhs;
	mul *= rhs;
	return mul;
}
// BINARY DIVISION
template<size_t nbits, size_t es>
inline posit<nbits, es> operator/(const posit<nbits, es>& lhs, const posit<nbits, es>& rhs) {
	posit<nbits, es> ratio = lhs;
	ratio /= rhs;
	return ratio;
}

#if POSIT_ENABLE_LITERALS

// posit - literal int logic operators
template<size_t nbits, size_t es>
inline bool operator==(const posit<nbits, es>& lhs, int rhs) {
	return lhs == posit<nbits, es>(rhs);
}
template<size_t nbits, size_t es>
inline bool operator!=(const posit<nbits, es>& lhs, int rhs) {
	return !operator==(lhs, posit<nbits, es>(rhs));
}
template<size_t nbits, size_t es>
inline bool operator< (const posit<nbits, es>& lhs, int rhs) {
	return lessThan(lhs._raw_bits, posit<nbits, es>(rhs)._raw_bits);
}
template<size_t nbits, size_t es>
inline bool operator> (const posit<nbits, es>& lhs, int rhs) {
	return operator< (posit<nbits, es>(rhs), lhs);
}
template<size_t nbits, size_t es>
inline bool operator<=(const posit<nbits, es>& lhs, int rhs) {
	return operator< (lhs, posit<nbits, es>(rhs)) || operator==(lhs, posit<nbits, es>(rhs));
}
template<size_t nbits, size_t es>
inline bool operator>=(const posit<nbits, es>& lhs, int rhs) {
	return !operator<(lhs, posit<nbits, es>(rhs));
}

// posit - literal double logic operators
template<size_t nbits, size_t es>
inline bool operator==(const posit<nbits, es>& lhs, double rhs) {
	return lhs == posit<nbits, es>(rhs);
}
template<size_t nbits, size_t es>
inline bool operator!=(const posit<nbits, es>& lhs, double rhs) {
	return !operator==(lhs, posit<nbits, es>(rhs));
}
template<size_t nbits, size_t es>
inline bool operator< (const posit<nbits, es>& lhs, double rhs) {
	return lessThan(lhs._raw_bits, posit<nbits, es>(rhs)._raw_bits);
}
template<size_t nbits, size_t es>
inline bool operator> (const posit<nbits, es>& lhs, double rhs) {
	return operator< (posit<nbits, es>(rhs), lhs);
}
template<size_t nbits, size_t es>
inline bool operator<=(const posit<nbits, es>& lhs, double rhs) {
	return operator< (lhs, posit<nbits, es>(rhs)) || operator==(lhs, posit<nbits, es>(rhs));
}
template<size_t nbits, size_t es>
inline bool operator>=(const posit<nbits, es>& lhs, double rhs) {
	return !operator<(lhs, posit<nbits, es>(rhs));
}

// BINARY ADDITION
template<size_t nbits, size_t es>
inline posit<nbits, es> operator+(const posit<nbits, es>& lhs, double rhs) {
	posit<nbits, es> sum = lhs;
	sum += rhs;
	return sum;
}
template<size_t nbits, size_t es>
inline posit<nbits, es> operator+(double lhs, const posit<nbits, es>& rhs) {
	posit<nbits, es> sum = lhs;
	sum += rhs;
	return sum;
}

// BINARY SUBTRACTION
template<size_t nbits, size_t es>
inline posit<nbits, es> operator-(double lhs, const posit<nbits, es>& rhs) {
	posit<nbits, es> sum = lhs;
	sum -= rhs;
	return sum;
}
template<size_t nbits, size_t es>
inline posit<nbits, es> operator-(const posit<nbits, es>& lhs, double rhs) {
	posit<nbits, es> diff = lhs;
	diff -= rhs;
	return diff;
}
// BINARY MULTIPLICATION
template<size_t nbits, size_t es>
inline posit<nbits, es> operator*(double lhs, const posit<nbits, es>& rhs) {
	posit<nbits, es> sum = lhs;
	sum *= rhs;
	return sum;
}
template<size_t nbits, size_t es>
inline posit<nbits, es> operator*(const posit<nbits, es>& lhs, double rhs) {
	posit<nbits, es> mul = lhs;
	mul *= rhs;
	return mul;
}
// BINARY DIVISION
template<size_t nbits, size_t es>
inline posit<nbits, es> operator/(double lhs, const posit<nbits, es>& rhs) {
	posit<nbits, es> sum = lhs;
	sum /= rhs;
	return sum;
}
template<size_t nbits, size_t es>
inline posit<nbits, es> operator/(const posit<nbits, es>& lhs, double rhs) {
	posit<nbits, es> ratio = lhs;
	ratio /= rhs;
	return ratio;
}

#endif // POSIT_ENABLE_LITERALS

// Magnitude of a posit (equivalent to turning the sign bit off).
template<size_t nbits, size_t es>
posit<nbits, es> abs(const posit<nbits, es>& p) {
    return posit<nbits, es>(false, p.get_regime(), p.get_exponent(), p.get_fraction());
}

// generate a posit representing minpos
template<size_t nbits, size_t es>
posit<nbits, es> minpos() {
	posit<nbits, es> p;
	p++;
	return p;
}

// generate a posit representing maxpos
template<size_t nbits, size_t es>
posit<nbits, es> maxpos() {
	posit<nbits, es> p;
	p.setToNaR();
	--p;
	return p;
}

// QUIRE OPERATORS
// Why are they defined here and not in quire.hpp? TODO

template<size_t nbits, size_t es>
value<nbits - es + 2> quire_add(const posit<nbits, es>& lhs, const posit<nbits, es>& rhs) {
	static constexpr size_t fbits = nbits - 3 - es;
	static constexpr size_t abits = fbits + 4;       // size of the addend
	value<abits + 1> sum;
	value<fbits> a, b;

	if (lhs.isZero() && rhs.isZero()) return sum;

	// transform the inputs into (sign,scale,fraction) triples
	a.set(lhs.get_sign(), lhs.scale(), lhs.get_fraction().get(), lhs.isZero(), lhs.isNaR());;
	b.set(rhs.get_sign(), rhs.scale(), rhs.get_fraction().get(), rhs.isZero(), rhs.isNaR());;

	module_add<fbits, abits>(a, b, sum);		// add the two inputs

	return sum;
}

template<size_t nbits, size_t es>
value<2*(nbits - 2 - es)> quire_mul(const posit<nbits, es>& lhs, const posit<nbits, es>& rhs) {
	static constexpr size_t fbits = nbits - 3 - es;
	static constexpr size_t fhbits = fbits + 1;      // size of fraction + hidden bit
	static constexpr size_t mbits = 2 * fhbits;      // size of the multiplier output

	value<mbits> product;
	value<fbits> a, b;

	if (lhs.isZero() || rhs.isZero()) return product;

	// transform the inputs into (sign,scale,fraction) triples
	a.set(lhs.get_sign(), lhs.scale(), lhs.get_fraction().get(), lhs.isZero(), lhs.isNaR());;
	b.set(rhs.get_sign(), rhs.scale(), rhs.get_fraction().get(), rhs.isZero(), rhs.isNaR());;

	module_multiply(a, b, product);    // multiply the two inputs

	return product;
}

	}  // namespace unum

}  // namespace sw


#include <iostream>
#include <string>
#include <fstream>
#include <algorithm>
#include <vector>
#include <iterator>
#include <numeric>
#include <memory>
#include <chrono>

#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "VNS.hpp"
#include "Solution.hpp"
#include "Helper.hpp"

using namespace std;
namespace ublas = boost::numeric::ublas;

/*********************************************************************************/
/*                   CONSTRUCTOR AND MEMBER FUNCTIONS OF VNS                     */
/*********************************************************************************/

VNS::VNS(const string &directory,
	const unsigned int &kMin,
	const unsigned int &kMax,
	const unsigned int &tMax,
	const unsigned int &shaking_mode,
	const unsigned int &local_search_mode,
	const unsigned int &update_xij_mode,
	const unsigned int &initial_mode,
	const unsigned int &time):
	// Init VNS and TestSet
		TestSet(directory), m_k(kMin - 1), m_kMax(kMax), m_kMin(kMin), m_tMax(tMax),
		m_shaking_mode(shaking_mode), m_local_search_mode(local_search_mode), m_update_xij_mode(update_xij_mode),
		m_incumbent_solution(m_locationNumber, 1), m_perturbed_solution(m_locationNumber, 1),
		m_fx(DBL_MAX), m_best_fx(DBL_MAX), m_sum_dj(accumulate(m_dj.begin(), m_dj.end(), 0.0)),
		m_initial_mode(initial_mode), m_maxNH(pow(m_kMax, 2)),m_timeMax(time), m_rMax(0),
		m_neighborhood_k(m_maxNH, vector<bool>(m_locationNumber, 1)), m_flow_tpl(0), m_initial_fx(DBL_MAX)
{
}

void VNS::initialSolution(
	vector <bool> &incumbent_solution,
	const vector<double> &bi,
	const vector<double> &fi,
	const vector<double> &dj,
	ublas::matrix<double> &cij,
	const double &sum_dj,
	const unsigned int &init_mode)
{
	if (init_mode == 0)
		initialGreedy(incumbent_solution, bi, fi, dj, cij, sum_dj);
	else
		initialRVNS(incumbent_solution, bi,  dj, cij);

	if (m_best_fx <= m_fx)
		m_initial_fx = m_best_fx;
	else
		m_initial_fx = m_fx;
}

// Move or not
void VNS::neighborhoodChange(
	vector<bool> &incumbent_solution,
	const vector<bool> &local_solution,
	unsigned int &k,
	const double &fx,
	double &best_fx,
	const double &kMin)
{
	if (fx < best_fx)
	{
		incumbent_solution = local_solution;
		k = kMin - 1;
		best_fx = fx;
	}
	else
		k++;
}

// Creates Perturbed Solution S'
vector<bool> VNS::shaking(
	const vector<bool> &incumbent_solution,
	const vector<double> &bi,
	const vector<double> &dj,
	ublas::matrix<double> &cij,
	const double &k,
	double &fx,
	const unsigned int &shaking_mode)
{
	if (shaking_mode == 0)
		return shakingKOperations(incumbent_solution, bi, m_dj, m_cij, m_k, fx);
	else if (shaking_mode == 1)
		return shakingKMaxOperations(incumbent_solution, bi, m_dj, m_cij, m_k, fx);
	else if (shaking_mode == 2)
		return shakingAssignments(incumbent_solution, bi, m_dj, m_cij, m_k, fx);
	else
		return shakingCosts(incumbent_solution, bi, m_dj, m_cij, m_k, fx);
}

// Getter f(x)
const double VNS::getFx() const
{
	return m_best_fx;
}

// Getter initial f(x)
const double VNS::getInitialFx() const
{
	return m_initial_fx;
}


#include <qubus/qtl/kronecker_delta_folding_pass.hpp>

#include <qubus/IR/qir.hpp>
#include <qubus/pattern/IR.hpp>
#include <qubus/pattern/core.hpp>

#include <qubus/qtl/pattern/all.hpp>

#include <boost/optional.hpp>

#include <algorithm>
#include <vector>

namespace qubus
{
namespace qtl
{

namespace
{

using bubble_t = std::tuple<variable_declaration, variable_declaration>;

bubble_t make_bubble(variable_declaration first, variable_declaration second)
{
    return std::less<variable_declaration>()(first, second)
               ? std::make_tuple(std::move(first), std::move(second))
               : std::make_tuple(std::move(second), std::move(first));
}

struct collect_bubbles_context
{
    std::vector<bubble_t> bubbles;
};

void collect_bubbles(const expression& expr, collect_bubbles_context& ctx)
{
    using pattern::delta;
    using pattern::index;
    using pattern::sum_multi;
    using qubus::pattern::_;
    using qubus::pattern::value;
    using qubus::pattern::variable;

    variable<const expression&> a, b;
    variable<variable_declaration> i, j;
    variable<std::vector<variable_declaration>> indices;

    auto m =
        qubus::pattern::make_matcher<expression, void>()
            .case_(sum_multi(a, indices),
                   [&] {
                       collect_bubbles(a.get(), ctx);

                       for (const auto& index : indices.get())
                       {
                           auto last = std::remove_if(ctx.bubbles.begin(), ctx.bubbles.end(),
                                                      [&](const bubble_t& value) {
                                                          return index == std::get<0>(value) ||
                                                                 index == std::get<1>(value);
                                                      });

                           ctx.bubbles.erase(last, ctx.bubbles.end());
                       }
                   })
            .case_(binary_operator(value(binary_op_tag::plus) || value(binary_op_tag::minus), a, b),
                   [&] {
                       collect_bubbles_context ctx_left;
                       collect_bubbles_context ctx_right;

                       collect_bubbles(a.get(), ctx_left);
                       collect_bubbles(b.get(), ctx_right);

                       for (const auto& bubble : ctx_left.bubbles)
                       {
                           auto iter = std::find(ctx_right.bubbles.begin(), ctx_right.bubbles.end(),
                                                 bubble);

                           if (iter != ctx_right.bubbles.end())
                           {
                               ctx.bubbles.push_back(bubble);
                           }
                       }
                   })
            .case_(binary_operator(value(binary_op_tag::divides), a, _),
                   [&] { collect_bubbles(a.get(), ctx); })
            .case_(binary_operator(value(binary_op_tag::multiplies), a, b),
                   [&] {
                       collect_bubbles(a.get(), ctx);
                       collect_bubbles(b.get(), ctx);
                   })
            .case_(delta(index(i), index(j)), [&] {
                // Eliminating contractions is only valid if the indices differ. Therefore,
                // we only produce a bubble in this case.
                if (i.get() != j.get())
                {
                    ctx.bubbles.push_back(make_bubble(i.get(), j.get()));
                }
            });

    qubus::pattern::try_match(expr, m);
}

boost::optional<variable_declaration>
contains_bubble_with_index(const variable_declaration& index, const std::vector<bubble_t>& bubbles)
{
    for (const auto& bubble : bubbles)
    {
        if (std::get<0>(bubble) == index)
        {
            return std::get<1>(bubble);
        }
        else if (std::get<1>(bubble) == index)
        {
            return std::get<0>(bubble);
        }
    }

    return boost::none;
}

std::unique_ptr<expression> eliminate_trivial_deltas(const expression& expr)
{
    using pattern::delta;
    using pattern::index;
    using qubus::pattern::variable;

    variable<variable_declaration> i;

    auto m3 = qubus::pattern::make_matcher<expression, std::unique_ptr<expression>>().case_(
        delta(index(i), index(i)), [&] { return integer_literal(1); });

    return qubus::pattern::substitute(expr, m3);
}

std::unique_ptr<expression> substitute_index(const expression& expr,
                                             const variable_declaration& idx,
                                             const variable_declaration& other_idx)
{
    using pattern::index;
    using qubus::pattern::value;

    auto m2 = qubus::pattern::make_matcher<expression, std::unique_ptr<expression>>().case_(
        index(value(idx)), [&] { return var(other_idx); });

    return qubus::pattern::substitute(expr, m2);
}
}

std::unique_ptr<expression> fold_kronecker_deltas(const expression& expr)
{
    using pattern::sum_multi;
    using qubus::pattern::variable;

    variable<const expression&> body;
    variable<std::vector<variable_declaration>> indices;

    auto m = qubus::pattern::make_matcher<expression, std::unique_ptr<expression>>().case_(
        sum_multi(body, indices), [&]() -> std::unique_ptr<expression> {
            std::unique_ptr<expression> new_body = clone(body.get());

            collect_bubbles_context ctx;

            collect_bubbles(*new_body, ctx);

            std::vector<variable_declaration> surviving_indices;

            for (const auto& index : indices.get())
            {
                if (auto other_index = contains_bubble_with_index(index, ctx.bubbles))
                {
                    new_body = substitute_index(*new_body, index, *other_index);

                    new_body = eliminate_trivial_deltas(*new_body);
                }
                else
                {
                    surviving_indices.push_back(index);
                }
            }

            if (surviving_indices.empty())
            {
                return new_body;
            }
            else
            {
                return sum(surviving_indices, std::move(new_body));
            }
        });

    return qubus::pattern::substitute(expr, m);
}

}
}


/* Copyright (c) 2016, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */
#include <thread>
#include <pangolin/pangolin.h>
#include <pangolin/video/video_record_repeat.h>
#include <pangolin/gl/gltexturecache.h>
#include <pangolin/gl/glpixformat.h>
#include <pangolin/handler/handler_image.h>
#include <pangolin/utils/file_utils.h>
#include <pangolin/utils/timer.h>
#include <pangolin/gl/gl.h>
#include <pangolin/gl/glcuda.h>
#include <pangolin/gl/gldraw.h>

#include <Eigen/Dense>
#include <tdp/camera/rig.h>
#include <tdp/data/image.h>
#include <tdp/data/managed_image.h>
#include <tdp/data/managed_pyramid.h>
#include <tdp/data/managed_volume.h>
#include <tdp/data/pyramid.h>
#include <tdp/data/volume.h>
#include <tdp/gl/gl_draw.h>
#include <tdp/gui/gui_base.hpp>
#include <tdp/gui/quickView.h>
#include <tdp/icp/icp.h>
#include <tdp/icp/icpRot.h>
#include <tdp/icp/icpGrad3d.h>
#include <tdp/icp/icpTexture.h>
#include <tdp/manifold/SE3.h>
#include <tdp/nvidia/helper_cuda.h>
#include <tdp/preproc/depth.h>
#include <tdp/preproc/normals.h>
#include <tdp/preproc/pc.h>
#include <tdp/utils/Stopwatch.h>
#include <tdp/inertial/imu_factory.h>
#include <tdp/inertial/imu_interpolator.h>
#include <tdp/manifold/SO3.h>
#include <tdp/preproc/grad.h>
#include <tdp/preproc/grey.h>
#include <tdp/preproc/mask.h>
#include <tdp/gl/shaders.h>
#include <tdp/utils/colorMap.h>
#include <tdp/camera/photometric.h>

typedef tdp::CameraPoly3f CameraT;
//typedef tdp::Cameraf CameraT;

int main( int argc, char* argv[] )
{
  std::string input_uri = "openni2://";
  std::string output_uri = "pango://video.pango";
  std::string calibPath = "";
  std::string imu_input_uri = "";
  std::string tsdfOutputPath = "tsdf.raw";
  bool runOnce = false;

  if( argc > 1 ) {
    input_uri = std::string(argv[1]);
    calibPath = (argc > 2) ? std::string(argv[2]) : "";
    if (argc > 3 && std::string(argv[3]).compare("-1") == 0 ) runOnce = true;
//    imu_input_uri =  (argc > 3)? std::string(argv[3]) : "";
  }

  pangolin::Uri uri = pangolin::ParseUri(input_uri);
  if (!uri.scheme.compare("file")) {
    std::cout << uri.scheme << std::endl;
    if (pangolin::FileExists(uri.url+std::string("imu.pango"))
     && pangolin::FileExists(uri.url+std::string("video.pango"))) {
//      imu_input_uri = input_uri + std::string("imu.pango");
      tsdfOutputPath = uri.url + tsdfOutputPath;
      input_uri = input_uri + std::string("video.pango");
    } else if (pangolin::FileExists(uri.url+std::string("video.pango"))) {
      input_uri = input_uri + std::string("video.pango");
    }
  }

  std::cout << input_uri << std::endl;
  std::cout << imu_input_uri << std::endl;

  Stopwatch::getInstance().setCustomSignature(82043984912);

  // Open Video by URI
  pangolin::VideoRecordRepeat video(input_uri, output_uri);
  const size_t num_streams = video.Streams().size();

  if(num_streams == 0) {
    pango_print_error("No video streams from device.\n");
    return 1;
  }

  // optionally connect to IMU if it is found.
  tdp::ImuInterface* imu = nullptr;
  if (imu_input_uri.size() > 0)
    imu = tdp::OpenImu(imu_input_uri);
  if (imu) imu->Start();
  tdp::ImuInterpolator imuInterp(imu,nullptr);
  imuInterp.Start();

  tdp::GuiBase gui(1200,800,video);
  gui.container().SetLayout(pangolin::LayoutEqual);

  tdp::Rig<CameraT> rig;
  if (calibPath.size() > 0) {
    rig.FromFile(calibPath,false);
    std::vector<pangolin::VideoInterface*>& streams = video.InputStreams();
    rig.CorrespondOpenniStreams2Cams(streams);
  } else {
    return 2;
  }

  size_t w = video.Streams()[gui.iRGB[0]].Width();
  size_t h = video.Streams()[gui.iRGB[0]].Height();
  size_t wc = (w+w%64); // for convolution
  size_t hc = rig.NumCams()*(h+h%64);
  wc += wc%64;
  hc += hc%64;

  tdp::Camera<float> camView(Eigen::Vector4f(220,220,319.5,239.5));
  // Define Camera Render Object (for view / scene browsing)
  pangolin::OpenGlRenderState s_cam(
      pangolin::ProjectionMatrix(640,480,420,420,319.5,239.5,0.1,1000),
      pangolin::ModelViewLookAt(0,0.5,-3, 0,0,0, pangolin::AxisNegY)
      );
  // Add named OpenGL viewport to window and provide 3D Handler
  pangolin::View& viewPc3D = pangolin::CreateDisplay()
    .SetHandler(new pangolin::Handler3D(s_cam));
  gui.container().AddDisplay(viewPc3D);

  pangolin::OpenGlRenderState camNormals(
      pangolin::ProjectionMatrix(640,3*480,420,3*420,320,3*240,0.1,1000),
      pangolin::ModelViewLookAt(0,0.5,-3, 0,0,0, pangolin::AxisNegY)
      );
  pangolin::View& viewNormals = pangolin::CreateDisplay()
    .SetHandler(new pangolin::Handler3D(camNormals));
  gui.container().AddDisplay(viewNormals);
  viewNormals.Show(false);

  pangolin::View& plotters = pangolin::Display("plotters");
  plotters.SetLayout(pangolin::LayoutEqualVertical);
  pangolin::DataLog logInliers;
  pangolin::Plotter plotInliers(&logInliers, -100.f,1.f, 0, 130000.f,
      10.f, 0.1f);
  plotters.AddDisplay(plotInliers);
  pangolin::DataLog logCost;
  pangolin::Plotter plotCost(&logCost, -100.f,1.f, -10.f,1.f, 10.f, 0.1f);
  plotters.AddDisplay(plotCost);
  pangolin::DataLog logRmse;
  pangolin::Plotter plotRmse(&logRmse, -100.f,1.f, 0.f,0.2f, 0.1f, 0.1f);
  plotters.AddDisplay(plotRmse);
  pangolin::DataLog logOverlap;
  pangolin::Plotter plotOverlap(&logOverlap, -100.f,1.f, 0.f,1.f, .1f, 0.1f);
  plotters.AddDisplay(plotOverlap);
  gui.container().AddDisplay(plotters);

  pangolin::View& containerTracking = pangolin::Display("tracking");
  containerTracking.SetLayout(pangolin::LayoutEqual);
  tdp::QuickView viewModel(wc, hc);
  containerTracking.AddDisplay(viewModel);
  tdp::QuickView viewCurrent(wc, hc);
  containerTracking.AddDisplay(viewCurrent);
  gui.container().AddDisplay(containerTracking);

  pangolin::View& containerLoopClosure = pangolin::Display("loopClosures");
  containerLoopClosure.SetLayout(pangolin::LayoutEqual);
  pangolin::OpenGlRenderState camLoopClose(
      pangolin::ProjectionMatrix(640,3*480,420,3*420,320,3*240,0.1,1000),
      pangolin::ModelViewLookAt(0,0.5,-3, 0,0,0, pangolin::AxisNegY)
      );
  pangolin::View& viewLoopClose = pangolin::CreateDisplay()
    .SetHandler(new pangolin::Handler3D(camLoopClose));
  containerLoopClosure.AddDisplay(viewLoopClose);
  tdp::QuickView viewDebugA(wc, hc);
  containerLoopClosure.AddDisplay(viewDebugA);
  tdp::QuickView viewDebugB(wc, hc);
  containerLoopClosure.AddDisplay(viewDebugB);
  tdp::QuickView viewDebugC(wc, hc);
  containerLoopClosure.AddDisplay(viewDebugC);
  tdp::QuickView viewDebugD(3*wc/2, hc);
  containerLoopClosure.AddDisplay(viewDebugD);

  tdp::QuickView viewDebugE(3*wc/2, hc);
  containerLoopClosure.AddDisplay(viewDebugE);
  tdp::QuickView viewDebugF(wc, hc);
  containerLoopClosure.AddDisplay(viewDebugF);

  gui.container().AddDisplay(containerLoopClosure);

  tdp::ManagedHostImage<float> d(wc, hc);
  tdp::ManagedHostImage<tdp::Vector3bda> n2D(wc,hc);
  memset(n2D.ptr_,0,n2D.SizeBytes());
  tdp::ManagedHostImage<tdp::Vector3fda> n2Df(wc,hc);
  tdp::ManagedHostImage<tdp::Vector3fda> n(wc,hc);
  tdp::ManagedHostImage<tdp::Vector3bda> rgb(wc,hc);
  tdp::ManagedHostImage<tdp::Vector3bda> rgb_m(wc,hc);
  tdp::ManagedHostImage<tdp::Vector3fda> pc(wc, hc);

  tdp::ManagedDeviceImage<tdp::Vector3bda> cuRgb(wc,hc);
  tdp::ManagedDeviceImage<float> cuGrey(wc,hc);
  tdp::ManagedDeviceImage<float> cuGreyDv(wc,hc);
  tdp::ManagedDeviceImage<float> cuGreyDu(wc,hc);
  tdp::ManagedDeviceImage<tdp::Vector2fda> cuGradGrey(wc,hc);
  tdp::ManagedDeviceImage<tdp::Vector3fda> cuGrad3D(wc,hc);

  tdp::ManagedHostImage<float> grey_m(wc,hc);
  tdp::ManagedHostImage<float> greyDu(wc, hc);
  tdp::ManagedHostImage<float> greyDv(wc, hc);

  tdp::ManagedDeviceImage<uint16_t> cuDraw(wc, hc);
  tdp::ManagedDeviceImage<float> cuD(wc, hc);

  tdp::ManagedDeviceImage<float> cuIrmse(wc, hc);
  tdp::ManagedHostImage<float> Irmse(wc, hc);

  tdp::ManagedDevicePyramid<uint8_t,3> cuPyrMask(wc, hc);
  tdp::ManagedDeviceImage<uint8_t> cuMask(wc, hc);
  tdp::ManagedHostImage<uint8_t> mask(wc, hc);

  // ICP stuff
  tdp::ManagedHostPyramid<float,3> dPyr(wc,hc);
  tdp::ManagedHostPyramid<float,3> dPyrEst(wc,hc);
  tdp::ManagedDevicePyramid<float,3> cuDPyr(wc,hc);
  tdp::ManagedDevicePyramid<float,3> cuDPyrEst(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> pcs_m(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> pcs_c(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> ns_m(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> ns_c(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> gs_m(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> gs_c(wc,hc);

  tdp::ManagedDevicePyramid<float,3> cuPyrGrey_c(wc,hc);
  tdp::ManagedDevicePyramid<float,3> cuPyrGrey_m(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector2fda,3> cuPyrGradGrey_c(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector2fda,3> cuPyrGradGrey_m(wc,hc);

  tdp::ManagedDeviceImage<tdp::Vector2fda> cuGrad2D(3*wc/2, hc);
  tdp::ManagedDeviceImage<tdp::Vector3bda> cuGrad2DImg(3*wc/2, hc);
  tdp::ManagedHostImage<tdp::Vector3bda> grad2DImg(3*wc/2, hc);
  tdp::ManagedHostImage<float> greyPyrImg(3*wc/2, hc);

  pangolin::GlBufferCudaPtr cuPcbuf(pangolin::GlArrayBuffer, wc*hc,
      GL_FLOAT, 3, cudaGraphicsMapFlagsNone, GL_DYNAMIC_DRAW);

  pangolin::GlBuffer vbo(pangolin::GlArrayBuffer,wc*hc,GL_FLOAT,3);
  pangolin::GlBuffer cbo(pangolin::GlArrayBuffer,wc*hc,GL_UNSIGNED_BYTE,3);

  tdp::ManagedHostImage<float> dispDepthPyr(dPyr.Width(0)+dPyr.Width(1), hc);

  tdp::ManagedDeviceImage<tdp::Vector3fda> cuDispNormalsPyr(ns_m.Width(0)+ns_m.Width(1), hc);
  tdp::ManagedDeviceImage<tdp::Vector3bda> cuDispNormals2dPyr(ns_m.Width(0)+ns_m.Width(1), hc);
  tdp::ManagedHostImage<tdp::Vector3bda> dispNormals2dPyr(ns_m.Width(0)+ns_m.Width(1), hc);

  tdp::QuickView viewGrad3DPyr(dispNormals2dPyr.w_,dispNormals2dPyr.h_);
  gui.container().AddDisplay(viewGrad3DPyr);
  viewGrad3DPyr.Show(false);

  pangolin::Var<float> depthSensorScale("ui.depth sensor scale",1e-3,1e-4,1e-3);
  pangolin::Var<float> dMin("ui.d min",0.10,0.0,0.1);
  pangolin::Var<float> dMax("ui.d max",6.,0.1,10.);

  pangolin::Var<bool>  randomSubsample("ui.random subsample", false, true);
  pangolin::Var<float> subsample("ui.subsample %",0.001,0.0001,0.1);

  pangolin::Var<bool> dispNormalsPyrEst("ui.disp normal est", false, true);
  pangolin::Var<int>   dispLvl("ui.disp lvl",0,0,2);

  pangolin::Var<bool> newKf("ui.new KF", true, true);

  pangolin::Var<bool>  icpFrame2Frame("ui.run frame2frame ICP", false, true);
  pangolin::Var<bool>  icpRgb("ui.run ICP RGB", false, true);
//  pangolin::Var<bool>  icpGrad3D("ui.run ICP Grad3D", false, true);
  pangolin::Var<bool>  icpRot("ui.run ICP Rot", false, true);

  pangolin::Var<bool> resetICP("ui.reset ICP",false,false);
  pangolin::Var<float> icpAngleThr_deg("ui.icp angle thr",15,0.,90.);
  pangolin::Var<float> icpDistThr("ui.icp dist thr",0.10,0.,1.);
  pangolin::Var<int>   icpIter0("ui.ICP iter lvl 0",10,0,10);
  pangolin::Var<int>   icpIter1("ui.ICP iter lvl 1",7,0,10);
  pangolin::Var<int>   icpIter2("ui.ICP iter lvl 2",5,0,10);

  pangolin::Var<float> icpRgbLambda("ui.icp rgb lamb",.1,0.,1.);

  pangolin::Var<float> gradNormThr("ui.grad3d norm thr",6.,0.,10.);

  pangolin::Var<bool> showPcModel("ui.show model",true,true);
  pangolin::Var<bool> showPcCurrent("ui.show current",false,true);

  pangolin::Var<float> rmseView("ui.rmse",0.,0.,0.);

  tdp::SE3f T_wc_0;
  tdp::SE3f T_wc = T_wc_0;
  std::vector<tdp::SE3f> T_wcs;
  tdp::SE3f T_mc; // current to model

  gui.verbose = false;
  if (gui.verbose) std::cout << "starting main loop" << std::endl;

  // Stream and display video
  while(!pangolin::ShouldQuit())
  {

    if (pangolin::Pushed(resetICP)) {
      T_mc = tdp::SE3f();
      T_wc = T_wc_0;
      T_wcs.clear();
      gui.Seek(0);
    }

    if (gui.frame > 0 && !gui.paused() && newKf) {
//      for (size_t lvl=0; lvl<3; ++lvl) {
//        tdp::Image<tdp::Vector3fda> pc = pcs_c.GetImage(lvl);
//        tdp::Image<tdp::Vector3fda> n = ns_c.GetImage(lvl);
//        tdp::Image<tdp::Vector3fda> g = gs_c.GetImage(lvl);
//        tdp::TransformPc(T_wc, pc);
//        tdp::TransformPc(T_wc.rotation(), n);
//        tdp::TransformPc(T_wc.rotation(), g);
//      }

      pcs_m.CopyFrom(pcs_c);
      ns_m.CopyFrom(ns_c);
      gs_m.CopyFrom(gs_c);
      cuPyrGrey_m.CopyFrom(cuPyrGrey_c);
      cuPyrGradGrey_m.CopyFrom(cuPyrGradGrey_c);
      rgb_m.CopyFrom(rgb);

      if (randomSubsample) {
        tdp::RandomMaskCpu(mask, subsample);
        size_t numObs = 0;
        for (size_t i=0; i<mask.Area(); ++i) {
          if (mask[i]) numObs++;
        }
        std::cout << "num obs in lvl 0 " << numObs
          << " " << subsample << std::endl;
        cuMask.CopyFrom(mask);
        tdp::ConstructPyramidFromMask(cuMask, cuPyrMask);
        tdp::ApplyMask(cuPyrMask, pcs_m);
        tdp::ApplyMask(cuPyrMask, ns_m);
        tdp::ApplyMask(cuPyrMask, gs_m);
//        tdp::ApplyMask(cuMask, rgb_m);
        tdp::ApplyMask(cuPyrMask, cuPyrGrey_m);
        tdp::ApplyMask(cuPyrMask, cuPyrGradGrey_m);
      }
//      T_wc = T_wc_0;
    }

    glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
    glColor3f(1.0f, 1.0f, 1.0f);

    gui.NextFrames();

    int64_t t_host_us_d = 0;
    if (gui.verbose) std::cout << "setup pyramids" << std::endl;
    TICK("Setup Pyramids");
    if (gui.verbose) std::cout << "collect d" << std::endl;
    rig.CollectD(gui, dMin, dMax, cuDraw, cuD, t_host_us_d);
    if (gui.verbose) std::cout << "compute pc" << std::endl;
    rig.ComputePc(cuD, true, pcs_c);
    if (gui.verbose) std::cout << "compute n" << std::endl;
    rig.ComputeNormals(cuD, true, ns_c);
    if (gui.verbose) std::cout << "collect rgb" << std::endl;
    rig.CollectRGB(gui, rgb) ;
    cuRgb.CopyFrom(rgb);
    tdp::Rgb2Grey(cuRgb,cuGrey, 1./255.);

    tdp::Image<tdp::Vector2fda> cuGradGrey_c = cuPyrGradGrey_c.GetImage(0);
    tdp::Gradient(cuGrey, cuGreyDu, cuGreyDv, cuGradGrey_c);
    greyDu.CopyFrom(cuGreyDu);
    greyDv.CopyFrom(cuGreyDv);
    tdp::ConstructPyramidFromImage(cuGrey, cuPyrGrey_c);
    tdp::CompletePyramid(cuPyrGradGrey_c);

//    tdp::Image<tdp::Vector3fda> cuNs = ns_c.GetImage(0);
//    tdp::Image<tdp::Vector3fda> cuGs = gs_c.GetImage(0);
//    tdp::Gradient3D(cuGrey, cuD, cuNs, camD, gradNormThr, cuGreydu,
//        cuGreydv, cuGs);
//    tdp::CompletePyramid(gs_c);
    TOCK("Setup Pyramids");

    std::vector<size_t> maxIt{icpIter0,icpIter1,icpIter2};
    std::vector<float> errPerLvl;
    std::vector<float> countPerLvl;
    T_mc = tdp::SE3f();
    Eigen::Matrix<float,6,6> Sigma_mc;
    TICK("ICP");
    if(icpFrame2Frame) {
      if (gui.verbose) std::cout << "icp" << std::endl;
      tdp::ICP::ComputeProjective<CameraT>(pcs_m, ns_m, pcs_c, ns_c,
          rig, rig.rgbStream2cam_, maxIt, icpAngleThr_deg, icpDistThr,
          gui.verbose, T_mc, Sigma_mc, errPerLvl, countPerLvl);
    } else if (icpRgb) {
      tdp::IcpTexture::ComputeProjective<CameraT>(pcs_m, ns_m,
          cuPyrGradGrey_m, cuPyrGrey_m, pcs_c, ns_c, cuPyrGradGrey_c,
          cuPyrGrey_c, rig, rig.rgbStream2cam_, maxIt, icpAngleThr_deg,
          icpDistThr, icpRgbLambda, gui.verbose, T_mc, Sigma_mc,
          errPerLvl, countPerLvl);
//    } else if (icpGrad3D) {
//      tdp::IcpGrad3d::ComputeProjective<CameraT::NumParams,CameraT>(
//          pcs_m, ns_m, pcs_c, ns_c,
//          rig, rig.dStream2cam_, maxIt, icpAngleThr_deg, icpDistThr,
//          gui.verbose, T_mc, Sigma_mc, errPerLvl, countPerLvl);
    } else if (icpRot) {
      tdp::ComputeProjectiveRotation<CameraT::NumParams,CameraT>(
          ns_m,  ns_c, pcs_c,
          T_mc, rig.T_rcs_[rig.rgbStream2cam_[0]].Inverse(),
          rig.cams_[rig.rgbStream2cam_[0]],
          maxIt, icpAngleThr_deg);
    }
    TOCK("ICP");
    if (!gui.paused()) {
      T_wc = T_wc*T_mc;
      T_wcs.push_back(T_wc);
    }

    logInliers.Log(countPerLvl);
    logCost.Log(errPerLvl);

    float overlap, rmse;
    cudaMemset(cuIrmse.ptr_, 0, cuIrmse.SizeBytes());
    tdp::OverlapGpu(cuPyrGrey_m.GetImage(0), cuPyrGrey_c.GetImage(0),
        pcs_m.GetImage(0), pcs_c.GetImage(0), T_mc, rig,
        overlap, rmse, &cuIrmse);
//    tdp::OverlapGpu(cuPyrGrey_m.GetImage(0), cuPyrGrey_c.GetImage(0),
//        pcs_m.GetImage(0), pcs_c.GetImage(0),
//        rig.T_rcs_[rig.rgbStream2cam_[0]].Inverse()* T_mc,
//        rig.cams_[rig.rgbStream2cam_[0]],
//        overlap, rmse, &cuIrmse);
    Irmse.CopyFrom(cuIrmse);
    rmseView = rmse;
    logRmse.Log(rmse);
    logOverlap.Log(overlap);

    if (gui.verbose) std::cout << "draw 3D" << std::endl;
    TICK("Draw 3D");
    glEnable(GL_DEPTH_TEST);
    if (viewPc3D.IsShown()) {
      viewPc3D.Activate(s_cam);

      pangolin::glDrawAxis(T_wc.matrix(), 0.05f);
      glColor4f(1.,1.,0.,0.6);
      glDrawPoses(T_wcs,30);

      // render model and observed point cloud
      if (showPcModel) {
        {
          pangolin::CudaScopedMappedPtr cuPcbufp(cuPcbuf);
          cudaMemset(*cuPcbufp,0,hc*wc*sizeof(tdp::Vector3fda));
          tdp::Image<tdp::Vector3fda> pc0 = pcs_m.GetImage(dispLvl);
          cudaMemcpy(*cuPcbufp, pc0.ptr_, pc0.SizeBytes(),
              cudaMemcpyDeviceToDevice);
        }
        glColor3f(0,1,0);
        pangolin::glSetFrameOfReference((T_wc*T_mc.Inverse()).matrix());
        if (dispLvl == 0) {
          cbo.Upload(rgb_m.ptr_, rgb_m.SizeBytes(), 0);
          pangolin::RenderVboCbo(cuPcbuf, cbo, true);
        } else {
          pangolin::RenderVbo(cuPcbuf);
        }
        pangolin::glUnsetFrameOfReference();
      }
      // render current camera second in the propper frame of
      // reference
      if (showPcCurrent) {
        {
          pangolin::CudaScopedMappedPtr cuPcbufp(cuPcbuf);
          cudaMemset(*cuPcbufp,0,hc*wc*sizeof(tdp::Vector3fda));
          tdp::Image<tdp::Vector3fda> pc0 = pcs_c.GetImage(dispLvl);
          cudaMemcpy(*cuPcbufp, pc0.ptr_, pc0.SizeBytes(),
              cudaMemcpyDeviceToDevice);
        }
        cbo.Upload(rgb.ptr_, rgb.SizeBytes(), 0);
        glColor3f(1,0,0);
        pangolin::glSetFrameOfReference(T_wc.matrix());
        pangolin::RenderVboCbo(cuPcbuf, cbo, true);
        pangolin::glUnsetFrameOfReference();
      }
    }
    TOCK("Draw 3D");

    if (gui.verbose) std::cout << "draw 2D" << std::endl;
    TICK("Draw 2D");
    glLineWidth(1.5f);
    glDisable(GL_DEPTH_TEST);

    if (viewModel.IsShown()) {
      if (gui.verbose) std::cout << "model grey image" << std::endl;
      grey_m.CopyFrom(cuPyrGrey_m.GetImage(0));
      viewModel.SetImage(grey_m);
    }
    if (viewCurrent.IsShown()) {
      viewCurrent.SetImage(rgb);
    }
    if (viewDebugA.IsShown()) {
      viewDebugA.SetImage(greyDu);
    }
    if (viewDebugB.IsShown()) {
      viewDebugB.SetImage(greyDv);
    }
    if (viewDebugC.IsShown()) {
      viewDebugC.SetImage(Irmse);
    }
    if (viewDebugD.IsShown()) {
      if (gui.verbose) std::cout << "gradient image" << std::endl;
      tdp::PyramidToImage(cuPyrGradGrey_c, cuGrad2D);
      tdp::Grad2Image(cuGrad2D, cuGrad2DImg);
      grad2DImg.CopyFrom(cuGrad2DImg);
      viewDebugD.SetImage(grad2DImg);
    }
    if (viewDebugE.IsShown()) {
      if (gui.verbose) std::cout << "current grey image" << std::endl;
      tdp::PyramidToImage(cuPyrGrey_c, greyPyrImg);
      viewDebugE.SetImage(greyPyrImg);
    }
    if (viewDebugF.IsShown()) {
      viewDebugF.RenderImage();
    }

    if (viewGrad3DPyr.IsShown()) {
      if (gui.verbose) std::cout << "3D gradient image" << std::endl;
      tdp::PyramidToImage<tdp::Vector3fda,3>(gs_c,cuDispNormalsPyr);
      tdp::RenormalizeSurfaceNormals(cuDispNormalsPyr, 1e-3);
      tdp::Normals2Image(cuDispNormalsPyr, cuDispNormals2dPyr);
      dispNormals2dPyr.CopyFrom(cuDispNormals2dPyr);
      viewGrad3DPyr.SetImage(dispNormals2dPyr);
    }

    plotInliers.ScrollView(1,0);
    plotCost.ScrollView(1,0);
    plotRmse.ScrollView(1,0);
    plotOverlap.ScrollView(1,0);
    TOCK("Draw 2D");

    if (gui.verbose) std::cout << "finished one iteration" << std::endl;
    // leave in pixel orthographic for slider to render.
    pangolin::DisplayBase().ActivatePixelOrthographic();
    if(video.IsRecording()) {
      pangolin::glRecordGraphic(pangolin::DisplayBase().v.w-14.0f,
          pangolin::DisplayBase().v.h-14.0f, 7.0f);
    }
    Stopwatch::getInstance().sendAll();
    pangolin::FinishFrame();
  }

  imuInterp.Stop();
  if (imu) imu->Stop();
  delete imu;
  std::this_thread::sleep_for(std::chrono::microseconds(500));
  return 0;
}


#include "test.h"
#include <iostream>

#include "vi/io/model.h"
#include "vi/la/cpu/cpu_context.h"
#include "vi/nn/activation_function.h"

#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;

TEST(model, round_trip) {
  const auto temp_dir_path = boost::filesystem::temp_directory_path();
  fs::path model_path(temp_dir_path);
  model_path /= "round_trip_test.model";

  vi::la::cpu_context context;
  vi::nn::network out_network;
  out_network.add(std::make_shared<vi::nn::layer>(
      context, std::make_shared<vi::nn::linear_activation>(), 5, 5));
  out_network.add(std::make_shared<vi::nn::layer>(
      context, std::make_shared<vi::nn::hyperbolic_tangent>(), 5, 5));
  out_network.add(std::make_shared<vi::nn::layer>(
      context, std::make_shared<vi::nn::sigmoid_activation>(), 4, 5));
  out_network.add(std::make_shared<vi::nn::layer>(
      context, std::make_shared<vi::nn::softmax_activation>(), 2, 4));
  vi::io::model model(model_path.string());
  model.store(out_network);

  vi::io::model in_model(model_path.string());
  vi::nn::network in_network;
  in_model.load(in_network, context);

  EXPECT_EQ(out_network.size(), in_network.size());

  vi::nn::network::const_iterator out_iterator = out_network.begin();
  vi::nn::network::const_iterator in_iterator = in_network.begin();
  for (; out_iterator != out_network.end(); ++out_iterator, ++in_iterator) {
    std::shared_ptr<vi::nn::layer> out_layer = *out_iterator;
    std::shared_ptr<vi::nn::layer> in_layer = *in_iterator;

    EXPECT_MATRIX_EQ(out_layer->weights(), in_layer->weights());
    EXPECT_EQ(typeid(out_layer->activation()), typeid(in_layer->activation()));
  }

  fs::remove_all(model_path);
}


/* ----------------------------------------------------------------------------
 * Copyright 2018, Ross Hartley <m.ross.hartley@gmail.com>
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 *  @file   kinematics.cpp
 *  @author Ross Hartley
 *  @brief  Example of invariant filtering for contact-aided inertial navigation
 *  @date   September 25, 2018
 **/

#include <iostream>
#include <fstream>
#include <string>
#include <cstdlib>
#include <Eigen/Dense>
#include <boost/algorithm/string.hpp>
#include <vector>
#include "InEKF.h"

#define DT_MIN 1e-6
#define DT_MAX 1

using namespace std;
using namespace inekf;

double stod98(const std::string &s) {
    return atof(s.c_str());
}

int stoi98(const std::string &s) {
    return atoi(s.c_str());
}

int main() {
    //  ---- Initialize invariant extended Kalman filter ----- //
    RobotState initial_state;

    // Initialize state mean
    Eigen::Matrix3d R0;
    Eigen::Vector3d v0, p0, bg0, ba0;
    R0 << 1, 0, 0, // initial orientation
          0, -1, 0, // IMU frame is rotated 90deg about the x-axis
          0, 0, -1;
    v0 << 0,0,0; // initial velocity
    p0 << 0,0,0; // initial position
    bg0 << 0,0,0; // initial gyroscope bias
    ba0 << 0,0,0; // initial accelerometer bias
    initial_state.setRotation(R0);
    initial_state.setVelocity(v0);
    initial_state.setPosition(p0);
    initial_state.setGyroscopeBias(bg0);
    initial_state.setAccelerometerBias(ba0);

    // Initialize state covariance
    NoiseParams noise_params;
    noise_params.setGyroscopeNoise(0.01);
    noise_params.setAccelerometerNoise(0.1);
    noise_params.setGyroscopeBiasNoise(0.00001);
    noise_params.setAccelerometerBiasNoise(0.0001);
    noise_params.setContactNoise(0.01);

    // Initialize filter
    InEKF filter(initial_state, noise_params);
    cout << "Noise parameters are initialized to: \n";
    cout << filter.getNoiseParams() << endl;
    cout << "Robot's state is initialized to: \n";
    cout << filter.getState() << endl;

    // Open data file
    ifstream infile("../src/data/imu_kinematic_measurements.txt");
    string line;
    Eigen::Matrix<double,6,1> imu_measurement = Eigen::Matrix<double,6,1>::Zero();
    Eigen::Matrix<double,6,1> imu_measurement_prev = Eigen::Matrix<double,6,1>::Zero();
    double t = 0;
    double t_prev = 0;

    // ---- Loop through data file and read in measurements line by line ---- //
    while (getline(infile, line)){
        vector<string> measurement;
        boost::split(measurement,line,boost::is_any_of(" "));
        // // Handle measurements
        if (measurement[0].compare("IMU")==0){
            cout << "Received IMU Data, propagating state\n";
            assert((measurement.size()-2) == 6);
            t = atof(measurement[1].c_str());
            // Read in IMU data
            imu_measurement << stod98(measurement[2]),
                               stod98(measurement[3]),
                               stod98(measurement[4]),
                               stod98(measurement[5]),
                               stod98(measurement[6]),
                               stod98(measurement[7]);

            // Propagate using IMU data
            double dt = t - t_prev;
            if (dt > DT_MIN && dt < DT_MAX) {
                filter.Propagate(imu_measurement_prev, dt);
            }

        }
        else if (measurement[0].compare("CONTACT")==0){
            cout << "Received CONTACT Data, setting filter's contact state\n";
            assert((measurement.size()-2)%2 == 0);
            vector<pair<int,bool> > contacts;
            int id;
            bool indicator;
            t = stod98(measurement[1]);
            // Read in contact data
            for (int i=2; i<measurement.size(); i+=2) {
                id = stoi98(measurement[i]);
                indicator = bool(stod98(measurement[i+1]));
                contacts.push_back(pair<int,bool> (id, indicator));
            }
            // Set filter's contact state
            filter.setContacts(contacts);
        }
        else if (measurement[0].compare("KINEMATIC")==0){
            cout << "Received KINEMATIC observation, correcting state\n";
            assert((measurement.size()-2)%44 == 0);
            int id;
            Eigen::Quaternion<double> q;
            Eigen::Vector3d p;
            Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
            Eigen::Matrix<double,6,6> covariance;
            vectorKinematics measured_kinematics;
            t = stod98(measurement[1]);
            // Read in kinematic data
            for (int i=2; i<measurement.size(); i+=44) {
                id = stoi98(measurement[i]);
                q = Eigen::Quaternion<double> (stod98(measurement[i+1]),stod98(measurement[i+2]),stod98(measurement[i+3]),stod98(measurement[i+4]));
                q.normalize();
                p << stod98(measurement[i+5]),stod98(measurement[i+6]),stod98(measurement[i+7]);
                pose.block<3,3>(0,0) = q.toRotationMatrix();
                pose.block<3,1>(0,3) = p;
                for (int j=0; j<6; ++j) {
                    for (int k=0; k<6; ++k) {
                        covariance(j,k) = stod98(measurement[i+8 + j*6+k]);
                    }
                }
                Kinematics frame(id, pose, covariance);
                measured_kinematics.push_back(frame);
            }
            // Correct state using kinematic measurements
            filter.CorrectKinematics(measured_kinematics);
        }

        // Store previous timestamp
        t_prev = t;
        imu_measurement_prev = imu_measurement;
    }

    // Print final state
    cout << filter.getState() << endl;
    return 0;
}


#include <Eigen/Core>
#include <Eigen/Sparse>
#include <iostream>
#include <mex.h>
#include <igl/C_STR.h>
#include <igl/matlab/mexErrMsgTxt.h>
#undef assert
#define assert( isOK ) ( (isOK) ? (void)0 : (void) ::mexErrMsgTxt(C_STR(__FILE__<<":"<<__LINE__<<": failed assertion `"<<#isOK<<"'"<<std::endl) ) )
#include <igl/matlab/MexStream.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/prepare_lhs.h>

void mexFunction(
         int          nlhs,
         mxArray      *plhs[],
         int          nrhs,
         const mxArray *prhs[]
         )
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;
  using namespace Eigen;
  igl::matlab::MexStream mout;
  std::streambuf *outbuf = std::cout.rdbuf(&mout);

  mexErrMsgTxt(nrhs==5,"nrhs should be == 5");
  // unnecessary copy
  int m,n;
  m = (double)*mxGetPr(prhs[3]);
  n = (double)*mxGetPr(prhs[4]);
  mexErrMsgTxt(mxGetM(prhs[0]) == mxGetM(prhs[1]) && mxGetM(prhs[0]) == mxGetM(prhs[2]),"I J V must be same #rows");
  mexErrMsgTxt(mxGetN(prhs[0]) == mxGetN(prhs[1]) && mxGetN(prhs[0]) == mxGetN(prhs[2]),"I J V must be same #cols");
  const int nr = mxGetM(prhs[0]);
  const int nc = mxGetN(prhs[0]);
  std::vector<Eigen::Triplet<double> > IJV;
  IJV.reserve(nr*nc);
  for(int i = 0;i<nr;i++)
  {
    for(int j = 0;j<nc;j++)
    {
      IJV.emplace_back(
        mxGetPr(prhs[0])[i+nr*j]-1,
        mxGetPr(prhs[1])[i+nr*j]-1,
        mxGetPr(prhs[2])[i+nr*j]);
    }
  }
  Eigen::SparseMatrix<double> S(m,n);
  // S.reserve doesn't seem to help much
  S.setFromTriplets(IJV.begin(),IJV.end());
  // This final copy is not that expensive (e.g., 0.5s gather, 1.1s scatter,
  // 0.05s prepare lhs)
  switch(nlhs)
  {
    case 1:
      prepare_lhs_double(S,plhs+0);
    default:break;
  }
  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
  return;
}


/*
 Copyright (C) 2018 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/test/unit_test.hpp>
#include <oret/toplevelfixture.hpp>

#include <boost/algorithm/string/replace.hpp>
#include <boost/make_shared.hpp>
#include <ored/configuration/conventions.hpp>
#include <ql/currencies/all.hpp>
#include <ql/time/calendars/all.hpp>
#include <ql/time/daycounters/all.hpp>

using namespace std;
using namespace boost::unit_test_framework;
using namespace QuantLib;
using namespace ore::data;
using boost::algorithm::replace_all_copy;

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, ore::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(ConventionsTests)

BOOST_AUTO_TEST_CASE(testCrossCcyFixFloatSwapConventionConstruction) {

    BOOST_TEST_MESSAGE("Testing cross currency fix float convention construction");

    // Check construction raises no errors
    boost::shared_ptr<CrossCcyFixFloatSwapConvention> convention;
    BOOST_CHECK_NO_THROW(
        convention = boost::make_shared<CrossCcyFixFloatSwapConvention>("USD-TRY-XCCY-FIX-FLOAT", "2", "US,UK,TRY", "F",
                                                                        "TRY", "Annual", "F", "A360", "USD-LIBOR-3M"));

    // Check object
    BOOST_CHECK_EQUAL(convention->id(), "USD-TRY-XCCY-FIX-FLOAT");
    BOOST_CHECK_EQUAL(convention->settlementDays(), 2);
    BOOST_CHECK_EQUAL(convention->settlementCalendar(), JointCalendar(UnitedStates(), UnitedKingdom(), Turkey()));
    BOOST_CHECK_EQUAL(convention->settlementConvention(), Following);
    BOOST_CHECK_EQUAL(convention->fixedCurrency(), TRYCurrency());
    BOOST_CHECK_EQUAL(convention->fixedFrequency(), Annual);
    BOOST_CHECK_EQUAL(convention->fixedConvention(), Following);
    BOOST_CHECK_EQUAL(convention->fixedDayCounter(), Actual360());
    BOOST_CHECK_EQUAL(convention->index()->name(), "USDLibor3M Actual/360");
    BOOST_CHECK(!convention->eom());

    // Check end of month when not default
    BOOST_CHECK_NO_THROW(
        convention = boost::make_shared<CrossCcyFixFloatSwapConvention>(
            "USD-TRY-XCCY-FIX-FLOAT", "2", "US,UK,TRY", "F", "TRY", "Annual", "F", "A360", "USD-LIBOR-3M", "false"));
    BOOST_CHECK(!convention->eom());

    BOOST_CHECK_NO_THROW(
        convention = boost::make_shared<CrossCcyFixFloatSwapConvention>(
            "USD-TRY-XCCY-FIX-FLOAT", "2", "US,UK,TRY", "F", "TRY", "Annual", "F", "A360", "USD-LIBOR-3M", "true"));
    BOOST_CHECK(convention->eom());
}

BOOST_AUTO_TEST_CASE(testCrossCcyFixFloatSwapConventionFromXml) {

    BOOST_TEST_MESSAGE("Testing parsing of cross currency fix float convention from XML");

    // XML string convention
    string xml;
    xml.append("<CrossCurrencyFixFloat>");
    xml.append("  <Id>USD-TRY-XCCY-FIX-FLOAT</Id>");
    xml.append("  <SettlementDays>2</SettlementDays>");
    xml.append("  <SettlementCalendar>US,UK,TRY</SettlementCalendar>");
    xml.append("  <SettlementConvention>F</SettlementConvention>");
    xml.append("  <FixedCurrency>TRY</FixedCurrency>");
    xml.append("  <FixedFrequency>Annual</FixedFrequency>");
    xml.append("  <FixedConvention>F</FixedConvention>");
    xml.append("  <FixedDayCounter>A360</FixedDayCounter>");
    xml.append("  <Index>USD-LIBOR-3M</Index>");
    xml.append("</CrossCurrencyFixFloat>");

    // Parse convention from XML
    boost::shared_ptr<CrossCcyFixFloatSwapConvention> convention = boost::make_shared<CrossCcyFixFloatSwapConvention>();
    BOOST_CHECK_NO_THROW(convention->fromXMLString(xml));

    // Check parsed object
    BOOST_CHECK_EQUAL(convention->id(), "USD-TRY-XCCY-FIX-FLOAT");
    BOOST_CHECK_EQUAL(convention->settlementDays(), 2);
    BOOST_CHECK_EQUAL(convention->settlementCalendar(), JointCalendar(UnitedStates(), UnitedKingdom(), Turkey()));
    BOOST_CHECK_EQUAL(convention->settlementConvention(), Following);
    BOOST_CHECK_EQUAL(convention->fixedCurrency(), TRYCurrency());
    BOOST_CHECK_EQUAL(convention->fixedFrequency(), Annual);
    BOOST_CHECK_EQUAL(convention->fixedConvention(), Following);
    BOOST_CHECK_EQUAL(convention->fixedDayCounter(), Actual360());
    BOOST_CHECK_EQUAL(convention->index()->name(), "USDLibor3M Actual/360");
    BOOST_CHECK(!convention->eom());

    // Check end of month also
    xml = replace_all_copy(xml, "</CrossCurrencyFixFloat>", "<EOM>false</EOM></CrossCurrencyFixFloat>");
    BOOST_TEST_MESSAGE("xml is: " << xml);
    convention->fromXMLString(xml);
    BOOST_CHECK(!convention->eom());

    xml = replace_all_copy(xml, "<EOM>false</EOM>", "<EOM>true</EOM>");
    convention->fromXMLString(xml);
    BOOST_CHECK(convention->eom());
}

BOOST_AUTO_TEST_CASE(testCrossCcyFixFloatSwapConventionToXml) {

    BOOST_TEST_MESSAGE("Testing writing of cross currency fix float convention to XML");

    // Construct the convention
    boost::shared_ptr<CrossCcyFixFloatSwapConvention> convention;
    BOOST_CHECK_NO_THROW(
        convention = boost::make_shared<CrossCcyFixFloatSwapConvention>("USD-TRY-XCCY-FIX-FLOAT", "2", "US,UK,TRY", "F",
                                                                        "TRY", "Annual", "F", "A360", "USD-LIBOR-3M"));

    // Write the convention to a string
    string xml = convention->toXMLString();

    // Read the convention back from the string using fromXMLString
    boost::shared_ptr<CrossCcyFixFloatSwapConvention> readConvention =
        boost::make_shared<CrossCcyFixFloatSwapConvention>();
    BOOST_CHECK_NO_THROW(readConvention->fromXMLString(xml));

    // The read convention should equal the original convention
    BOOST_CHECK_EQUAL(convention->id(), readConvention->id());
    BOOST_CHECK_EQUAL(convention->settlementDays(), readConvention->settlementDays());
    BOOST_CHECK_EQUAL(convention->settlementCalendar(), readConvention->settlementCalendar());
    BOOST_CHECK_EQUAL(convention->settlementConvention(), readConvention->settlementConvention());
    BOOST_CHECK_EQUAL(convention->fixedCurrency(), readConvention->fixedCurrency());
    BOOST_CHECK_EQUAL(convention->fixedFrequency(), readConvention->fixedFrequency());
    BOOST_CHECK_EQUAL(convention->fixedConvention(), readConvention->fixedConvention());
    BOOST_CHECK_EQUAL(convention->fixedDayCounter(), readConvention->fixedDayCounter());
    BOOST_CHECK_EQUAL(convention->index()->name(), readConvention->index()->name());
    BOOST_CHECK_EQUAL(convention->eom(), readConvention->eom());
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


/**************************************************************************************************
 Software License Agreement (BSD License)

 Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
 All rights reserved.

 Redistribution and use in source and binary forms, with or without modification, are permitted
 provided that the following conditions are met:

  *Redistributions of source code must retain the above copyright notice, this list of
   conditions and the following disclaimer.
  *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.
  *Neither the name of the University of Aveiro 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.
***************************************************************************************************/
/*! \brief Code for feature extraction
 *
 *  extract features from targets received from MTT wrt the robot
 *  the features are printed in the command line and should be stored
 *  in a file (txt) for further processing with matlab
 *  the features will be used for training a classifier
 *
 */

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "tf/transform_listener.h"
#include "mtt/TargetList.h"
#include <boost/circular_buffer.hpp>
#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/weighted_sum.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <numeric>

using namespace boost::accumulators;

ros::Time time_last_msg(0);
ros::Duration time_elapsed;

tf::TransformListener *p_listener;
tf::StampedTransform transform;

geometry_msgs::Twist twist;
geometry_msgs::Twist features;

//statics
boost::circular_buffer<double> robot_posex_buffer(30);

double robot_x, robot_y;
double robot_vel, robot_theta;

double target_x, target_y;
double target_theta, target_vel;
double distance_to_robot, lateral_disp, sagittal_disp;
double relative_heading, angle_to_robot;
double relative_vel_x, relative_vel_y;

uint leader_tag = 0;
uint target_id = 0;

void targetsCallback(const mtt::TargetList& list)
{
  static ros::Time start_time = ros::Time::now();
  time_elapsed = ros::Time::now() - start_time;

  /// /// ROBOT PART //////
  //use transformations to extract robot features
  try{
    p_listener->lookupTransform("/map", "/base_link", ros::Time(0), transform);
    p_listener->lookupTwist("/map", "/base_link", ros::Time(0), ros::Duration(0.5), twist);
  }
  catch (tf::TransformException ex){
    ROS_ERROR("%s",ex.what());
  }

  robot_x = transform.getOrigin().x();

  robot_posex_buffer.push_back(robot_x);

  robot_y = transform.getOrigin().y();
  robot_theta = tf::getYaw(transform.getRotation());
  robot_vel = sqrt(pow(twist.linear.x, 2) + pow(twist.linear.y, 2));

  //robot output line
  /// uncomment the following for training!
  printf("%d,%d,%.10f,%.10f,%.10f,%.10f,%.10f,0,0,0,0,0,0,0,0\n",
         -1, leader_tag, time_elapsed.toSec(),
         robot_x, robot_y, robot_vel, robot_theta);

  //testing new features extraction
//   accumulator_set<double, stats<tag::variance> > acc;
//   for_each(robot_posex_buffer.begin(), robot_posex_buffer.end(), boost::bind<void>(boost::ref(acc), _1));
//   printf("%f,%f,%f\n", robot_x, mean(acc), sqrt(variance(acc)));

  /// /// TARGETS PART //////
  //sweeps target list and extract features
  for(uint i = 0; i < list.Targets.size(); i++){
    target_id = list.Targets[i].id;
    target_x = list.Targets[i].pose.position.x;
    target_y = list.Targets[i].pose.position.y;
    target_theta = tf::getYaw(list.Targets[i].pose.orientation);

    //velocity of target
    target_vel = sqrt(pow(list.Targets[i].velocity.linear.x,2)+
                      pow(list.Targets[i].velocity.linear.y,2));

    //angle from target to robot's ref frame
    angle_to_robot = -robot_theta + atan2(target_y - robot_y,
                                        target_x - robot_x );

    //robot and target difference in heading
    relative_heading = robot_theta - target_theta;

    //velocity diff from target to robot ref. frame in X axis
    relative_vel_x = (robot_vel - target_vel)*cos(relative_heading);

    //velocity diff from target to robot ref. frame in Y axis
    relative_vel_y = (robot_vel - target_vel)*sin(relative_heading);

    //distance between robot and target
    distance_to_robot = sqrt(pow(robot_x - target_x,2)+
                        pow(robot_y - target_y,2));

    //distance form robot along X axis (robot ref frame)
    lateral_disp = distance_to_robot * sin(angle_to_robot);

    //distance form robot along X axis (robot ref frame)
    sagittal_disp = distance_to_robot * cos(angle_to_robot);

    //target output (to be used in adaboost training)

    // % output file format:
    // % 1: id
    // % 2: good/bad tag
    // % 3: time
    // % 4: pos x
    // % 5: pos y
    // % 6: heading

    // % 7: velocity
    // % 8: lateral disp
    // % 9: relative heading
    // %10: angle to robot
    // %12: distance to robot
    // %13: relative vel x
    // %14: relative vel y
    // %15: sagittal_disp

    /// uncomment the following to generate training file!
    printf("%d,%d,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f\n",
      target_id, leader_tag, time_elapsed.toSec(),
      target_x, target_y, target_theta,
      target_vel,
      lateral_disp,
      relative_heading,
      angle_to_robot,
      distance_to_robot,
      relative_vel_x,
      relative_vel_y,
      sagittal_disp);

  }
}

void tagCallback(const std_msgs::Header& tag)
{
  //ROS_INFO("tag received");
  //only works for a transition good/bad leader
  leader_tag = 1;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "process_target");
  ros::NodeHandle n;

  tf::TransformListener listener;
  p_listener=&listener;

  ros::Subscriber sub = n.subscribe("/targets", 1000, targetsCallback);
  ros::Subscriber sub_tag = n.subscribe("/timetag", 1000, tagCallback);

  ros::spin();

  return 0;
}