// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_UPPER_TRISOLVE_INCLUDE
#define MTL_UPPER_TRISOLVE_INCLUDE

#include <boost/mpl/int.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/property_map.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/static_assert.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace mat {

namespace detail {

    /// Class that implements upper trisolver
    /** DiaTag can be tag::regular_diagonal, tag::unit_diagonal, or tag::inverse_diagonal.
	CompactStorage means that matrix contains only upper entries (strict upper when DiaTag == unit_diagonal).  \sa \ref trisolve_object **/
    template <typename Matrix, typename DiaTag, bool CompactStorage= false>
    struct upper_trisolve_t
    {
	MTL_STATIC_ASSERT((boost::is_same<DiaTag, tag::regular_diagonal>::value
			   || boost::is_same<DiaTag, tag::unit_diagonal>::value
			   || boost::is_same<DiaTag, tag::inverse_diagonal>::value),
			  "DiaTag must be either tag::regular_diagonal, tag::unit_diagonal, or tag::inverse_diagonal.");

	typedef typename Collection<Matrix>::value_type           value_type;
	typedef typename Collection<Matrix>::size_type            size_type;
	typedef typename OrientedCollection<Matrix>::orientation  my_orientation;
	typedef typename mtl::traits::category<Matrix>::type      my_category;
	typedef typename mtl::traits::range_generator<tag::major, Matrix>::type   a_cur_type; // row or col accordingly
	typedef typename mtl::traits::range_generator<tag::nz, a_cur_type>::type  a_icur_type;

	/// Construction from matrix \p A
	upper_trisolve_t(const Matrix& A) : A(A), value_a(A), col_a(A), row_a(A)
	{    MTL_THROW_IF(num_rows(A) != num_cols(A), matrix_not_square());	}

	template <typename M, typename D, bool C>
	struct generic_version
	  : boost::mpl::int_<mtl::traits::is_row_major<M>::value ? 1 : 2> {};

	template <typename M, typename D, bool C>
	struct version
	  : generic_version<M, D, C> {};

	template <typename Value, typename Para, typename D>
	struct version<compressed2D<Value, Para>, D, true>
	  : boost::mpl::if_<mtl::traits::is_row_major<Para>,
			    boost::mpl::int_<3>,
			    generic_version<compressed2D<Value, Para>, D, true>
	                   >::type {};

	/// Solve \p w = A * \p v
	template <typename VectorIn, typename VectorOut>
	void operator()(const VectorIn& v, VectorOut& w) const
	{
	    apply(v, w, version<Matrix, DiaTag, CompactStorage>());
	}

	/// Solves the upper triangular matrix A  with the rhs v returns the solution
	template <typename Vector>
	Vector operator()(const Vector& v) const
	{
	    Vector w(resource(v));
	    (*this)(v, w);
	    return w;
	}

    private:
	// Initialization for regular and inverse diagonal is the same
	template <typename Cursor, typename Value>
	void row_init(size_type MTL_DEBUG_ARG(r), Cursor& aic, Cursor& MTL_DEBUG_ARG(aiend), Value& dia, tag::universe_diagonal) const
	{
	    MTL_DEBUG_THROW_IF(aic == aiend || col_a(*aic) != r, missing_diagonal());
	    dia= value_a(*aic); ++aic;
	}

	template <typename Cursor, typename Value>
	void row_init(size_type, Cursor&, Cursor&, Value&, tag::unit_diagonal) const {}

	template <typename Value> void row_update(Value& res, Value& rr, const Value& dia, tag::regular_diagonal) const { res= rr / dia; }
	template <typename Value> void row_update(Value& res, Value& rr, const Value& dia, tag::inverse_diagonal) const { res= rr * dia; }
	template <typename Value> void row_update(Value& res, Value& rr, const Value&    , tag::unit_diagonal)    const { res= rr; }

	template <typename Tag> int dia_inc(Tag) const { return 0; }
	int dia_inc(tag::unit_diagonal) const { return 1; }

	// Generic row-major
	template <typename VectorIn, typename VectorOut>
	void inline apply(const VectorIn& v, VectorOut& w, boost::mpl::int_<1>) const
	{
	    // vampir_trace<5042> tracer;
	    using namespace tag;
	    typedef typename mtl::Collection<VectorOut>::value_type out_value_type;
	    a_cur_type ac= begin<row>(A), aend= end<row>(A);
	    for (size_type r= num_rows(A) - 1; ac != aend--; --r) {
		a_icur_type aic= CompactStorage ? begin<nz>(aend) : lower_bound<nz>(aend, r + dia_inc(DiaTag())),
		            aiend= end<nz>(aend);
		out_value_type rr= v[r], dia;
		row_init(r, aic, aiend, dia, DiaTag());
		for (; aic != aiend; ++aic) {
		    MTL_DEBUG_THROW_IF(col_a(*aic) <= r, logic_error("Matrix entries must be sorted for this."));
		    rr-= value_a(*aic) * w[col_a(*aic)];
		}
		row_update(w[r], rr, dia, DiaTag());
	    }
	}

	// Generic column-major
	template <typename VectorIn, typename VectorOut>
	void apply(const VectorIn& v, VectorOut& w, boost::mpl::int_<2>) const
	{
	    // vampir_trace<5043> tracer;
	    using namespace tag;
	    typedef typename mtl::Collection<VectorOut>::value_type out_value_type;
	    w= v;
	    a_cur_type ac= begin<col>(A), aend= end<col>(A);
	    for (size_type r= num_rows(A) - 1; ac != aend--; --r) {
		a_icur_type aic= begin<nz>(aend),
		            aiend= CompactStorage ? end<nz>(aend) : lower_bound<nz>(aend, r + 1 - dia_inc(DiaTag()));
		out_value_type rr;
		col_init(r, aic, aiend, rr, w[r], DiaTag());

		for (; aic != aiend; ++aic) {
		    MTL_DEBUG_THROW_IF(row_a(*aic) >= r, logic_error("Matrix entries must be sorted for this."));
		    w[row_a(*aic)]-= value_a(*aic) * rr;
		}
	    }
	}

	template <typename Value>
	void crs_row_init(size_type MTL_DEBUG_ARG(r), size_type& j0, size_type MTL_DEBUG_ARG(cj1), Value& dia, tag::universe_diagonal) const
	{
	    MTL_DEBUG_THROW_IF(j0 == cj1 || A.ref_minor()[j0] != r, missing_diagonal());
	    dia= A.data[j0++];
	}
	template <typename Value> void crs_row_init(size_type, size_type&, size_type, Value&, tag::unit_diagonal) const {}

	// Tuning for IC_0 and similar using compressed2D row-major compact
	template <typename VectorIn, typename VectorOut>
	void apply(const VectorIn& v, VectorOut& w, boost::mpl::int_<3>) const
	{
	    // vampir_trace<5046> tracer;
	    typedef typename mtl::Collection<VectorOut>::value_type out_value_type;
	    for (size_type r= num_rows(A); r-- > 0; ) {
		size_type j0= A.ref_major()[r];
		const size_type cj1= A.ref_major()[r+1];
		out_value_type rr= v[r], dia;
		crs_row_init(r, j0, cj1, dia, DiaTag());
		for (; j0 != cj1; ++j0) {
		    MTL_DEBUG_THROW_IF(A.ref_minor()[j0] <= r, logic_error("Matrix entries must be sorted for this."));
		    rr-= A.data[j0] * w[A.ref_minor()[j0]];
		}
		row_update(w[r], rr, dia, DiaTag());
	    }
	}

	template <typename Cursor, typename Value>
	void col_init(size_type MTL_DEBUG_ARG(r), Cursor& MTL_DEBUG_ARG(aic), Cursor& aiend, Value& rr, Value& res, tag::regular_diagonal) const
	{
	    MTL_DEBUG_THROW_IF(aic == aiend, missing_diagonal());
	    --aiend;
	    MTL_DEBUG_THROW_IF(row_a(*aiend) != r, missing_diagonal());
	    rr= res/= value_a(*aiend);
	}

	template <typename Cursor, typename Value>
	void col_init(size_type MTL_DEBUG_ARG(r), Cursor& MTL_DEBUG_ARG(aic), Cursor& aiend, Value& rr, Value& res, tag::inverse_diagonal) const
	{
	    MTL_DEBUG_THROW_IF(aic == aiend, missing_diagonal());
	    --aiend;
	    MTL_DEBUG_THROW_IF(row_a(*aiend) != r, missing_diagonal());
	    rr= res*= value_a(*aiend);
	}

	template <typename Cursor, typename Value>
	void col_init(size_type, Cursor&, Cursor&, Value& rr, Value& res, tag::unit_diagonal) const
	{
	    rr= res;
	}

	const Matrix& A;
	typename mtl::traits::const_value<Matrix>::type  value_a;
	typename mtl::traits::col<Matrix>::type          col_a;
	typename mtl::traits::row<Matrix>::type          row_a;
    };

}

/// Solves the upper triangular matrix A  with the rhs v and returns the solution vector
template <typename Matrix, typename Vector>
Vector inline upper_trisolve(const Matrix& A, const Vector& v)
{
    // vampir_trace<3043> tracer;
    return detail::upper_trisolve_t<Matrix, tag::regular_diagonal>(A)(v);
}

/// Solves the upper triangular matrix A  with the rhs v while solution vector w is passed as reference
template <typename Matrix, typename VectorIn, typename VectorOut>
void inline upper_trisolve(const Matrix& A, const VectorIn& v, VectorOut& w)
{
    // vampir_trace<3043> tracer;
    detail::upper_trisolve_t<Matrix, tag::regular_diagonal> solver(A); // use of anonymous variable causes weird error
    solver(v, w);
}

/// Solves the upper triangular matrix A (only one's in the diagonal) with the rhs v and returns the solution vector
template <typename Matrix, typename Vector>
Vector inline unit_upper_trisolve(const Matrix& A, const Vector& v)
{
    // vampir_trace<3044> tracer;
    return detail::upper_trisolve_t<Matrix, tag::unit_diagonal>(A)(v);
}

/// Solves the upper triangular matrix A (only one's in the diagonal) with the rhs v while solution vector w is passed as reference
template <typename Matrix, typename VectorIn, typename VectorOut>
void inline unit_upper_trisolve(const Matrix& A, const VectorIn& v, VectorOut& w)
{
    // vampir_trace<3044> tracer;
    detail::upper_trisolve_t<Matrix, tag::unit_diagonal> solver(A);
    solver(v, w);
}

/// Solves the upper triangular matrix A  (inverse the diagonal) with the rhs v and returns the solution vector
template <typename Matrix, typename Vector>
Vector inline inverse_upper_trisolve(const Matrix& A, const Vector& v)
{
    // vampir_trace<3045> tracer;
    return detail::upper_trisolve_t<Matrix, tag::inverse_diagonal>(A)(v);
}

/// Solves the upper triangular matrix A  (inverse the diagonal) with the rhs v while solution vector w is passed as reference
template <typename Matrix, typename VectorIn, typename VectorOut>
void inline inverse_upper_trisolve(const Matrix& A, const VectorIn& v, VectorOut& w)
{
    // vampir_trace<3045> tracer;
    detail::upper_trisolve_t<Matrix, tag::inverse_diagonal> solver(A);
    solver(v, w);
}

/// Solves the upper triangular matrix A  with the rhs v and returns the solution vector
template <typename Matrix, typename Vector, typename DiaTag>
Vector inline upper_trisolve(const Matrix& A, const Vector& v, DiaTag)
{
    // vampir_trace<3046> tracer;
    return detail::upper_trisolve_t<Matrix, DiaTag>(A)(v);
}

/// Solves the upper triangular matrix A  with the rhs v while solution vector w is passed as reference
template <typename Matrix, typename VectorIn, typename VectorOut, typename DiaTag>
void inline upper_trisolve(const Matrix& A, const VectorIn& v, VectorOut& w, DiaTag)
{
    // vampir_trace<3046> tracer;
    detail::upper_trisolve_t<Matrix, DiaTag> solver(A);
    solver(v, w);
}

}} // namespace mtl::matrix

#endif // MTL_UPPER_TRISOLVE_INCLUDE


#include "octree.h"
#include <pcl/console/print.h>
#include <pcl/console/time.h>
#include <pcl/pcl_macros.h>
#include <boost/shared_ptr.hpp>
#include <vector>
#include <stdint.h>
#include <cmath>
#include <fstream>
#include <iostream>

void
cpu_tsdf::OctreeNode::getCenter(float &x, float &y, float &z) const
{
	x = ctr_x_;
	y = ctr_y_;
	z = ctr_z_;
};

void
cpu_tsdf::OctreeNode::getSize(float &size_x, float &size_y, float &size_z) const
{
	size_x = size_;
	size_y = size_;
	size_z = size_;
}

float
cpu_tsdf::OctreeNode::getMaxSize() const
{
	return (std::sqrt(3) * size_);
}

float
cpu_tsdf::OctreeNode::getMinSize() const
{
	return (size_);
}

bool
cpu_tsdf::OctreeNode::hasChildren() const
{
	return (!children_.empty());
}

std::vector<cpu_tsdf::OctreeNode::Ptr>&
cpu_tsdf::OctreeNode::getChildren()
{
	return children_;
}

const std::vector<cpu_tsdf::OctreeNode::Ptr>&
cpu_tsdf::OctreeNode::getChildren() const
{
	return children_;
}

void
cpu_tsdf::OctreeNode::getLeaves(std::vector<cpu_tsdf::OctreeNode::Ptr> &leaves, int num_levels)
{
	for (size_t i = 0; i < children_.size(); i++)
	{
		const OctreeNode::Ptr &child = children_[i];
		if (child->hasChildren() && num_levels != 0)
			child->getLeaves(leaves, num_levels - 1);
		else
			leaves.push_back(child);
	}
}

// Get the voxel which contains this point
cpu_tsdf::OctreeNode*
cpu_tsdf::OctreeNode::getContainingVoxel(float x, float y, float z, float min_size)
{
	if (!hasChildren() || (min_size > 0 && size_ <= min_size))
		return (this);
	else
	{
		return children_[((x - ctr_x_) > 0) * 4 + ((y - ctr_y_) > 0) * 2 + (z - ctr_z_ > 0)]->getContainingVoxel(x, y, z, min_size);
	}
}

// Get the voxel which contains this point
const cpu_tsdf::OctreeNode*
cpu_tsdf::OctreeNode::getContainingVoxel(float x, float y, float z, float min_size) const
{
	if (!hasChildren() || (min_size > 0 && size_ <= min_size))
		return (this);
	else
	{
		return children_[((x - ctr_x_) > 0) * 4 + ((y - ctr_y_) > 0) * 2 + (z - ctr_z_ > 0)]->getContainingVoxel(x, y, z, min_size);
	}
}

bool
cpu_tsdf::OctreeNode::getData(float &d, float &w) const
{
	d = d_;
	w = w_;
	return (true);
}

bool
cpu_tsdf::OctreeNode::setData(float d, float w)
{
	d_ = d;
	w_ = w;
	return (true);
}

bool
cpu_tsdf::OctreeNode::addObservation(float d_new, float w_new, float max_weight)
{
	float d_old = d_;
	d_ = (d_ * w_ + d_new * w_new) / (w_ + w_new);
	w_ += w_new;
	if (w_ > max_weight)
		w_ = max_weight;
	M_ += w_new * (d_new - d_) * (d_new - d_old);
	++nsample_;
	return (true);
}

bool
cpu_tsdf::OctreeNode::addObservation(float d_new, float w_new, float max_weight,
	uint8_t r, uint8_t g, uint8_t b)
{
	return (addObservation(d_new, w_new, max_weight));
}

bool
cpu_tsdf::OctreeNode::getRGB(uint8_t &r, uint8_t &g, uint8_t &b) const
{
	r = g = b = 127;
	return (false);
}

cpu_tsdf::OctreeNode*
cpu_tsdf::OctreeNode::instantiateNode(float x, float y, float z, float sx, float sy, float sz)
{
	return (new OctreeNode(x, y, z, sx, sy, sz));
}

std::string
cpu_tsdf::OctreeNode::getTypeString()
{
	return ("NOCOLOR");
}

cpu_tsdf::OctreeNode*
cpu_tsdf::OctreeNode::instantiateByTypeString(const std::string &str)
{
	if (str == "NOCOLOR")
		return (new OctreeNode);
	else if (str == "RGB")
		return (new RGBNode);
	else if (str == "RGBNormalized")
		return (new RGBNormalized);
	else if (str == "LAB")
		return (new LABNode);
	// Handle more examples
	PCL_ERROR("[cpu_tsdf::OctreeNode::instantiateByTypeString] Requested invalid type string %s\n", str.c_str());
	return (NULL);
}

cpu_tsdf::OctreeNode*
cpu_tsdf::OctreeNode::instantiateByTypeString(const std::string &str,
	float x, float y, float z, float sx, float sy, float sz)
{
	cpu_tsdf::OctreeNode* empty_node = instantiateByTypeString(str);
	cpu_tsdf::OctreeNode* node = empty_node->instantiateNode(x, y, z, sx, sy, sz);
	delete empty_node;
	return (node);
}

void
cpu_tsdf::OctreeNode::updateAverage()
{
	if (children_.empty())
		return;

	float d_avg = 0;
	float w_avg = 0;
	int ngood = 0;
	for (size_t i = 0; i < children_.size(); ++i)
	{
		children_[i]->updateAverage();
		if (children_[i]->w_ > 0)
		{
			d_avg += children_[i]->d_;
			w_avg += children_[i]->w_;
			++ngood;
		}
	}
	if (ngood > 0)
	{
		d_ = d_avg / ngood;
		w_ = w_avg / ngood;
	}
}

std::vector<cpu_tsdf::OctreeNode::Ptr>&
cpu_tsdf::OctreeNode::split()
{
	children_.resize(8);
	//float off_x = size_x_ / 4;
	//float off_y = size_y_ / 4;
	//float off_z = size_z_ / 4;
	float off_x = size_ / 4;
	float off_y = size_ / 4;
	float off_z = size_ / 4;
	float newsize_x = size_ / 2;
	float newsize_y = size_ / 2;
	float newsize_z = size_ / 2;
	children_[0].reset(instantiateNode(ctr_x_ - off_x, ctr_y_ - off_y, ctr_z_ - off_z, newsize_x, newsize_y, newsize_z));
	children_[1].reset(instantiateNode(ctr_x_ - off_x, ctr_y_ - off_y, ctr_z_ + off_z, newsize_x, newsize_y, newsize_z));
	children_[2].reset(instantiateNode(ctr_x_ - off_x, ctr_y_ + off_y, ctr_z_ - off_z, newsize_x, newsize_y, newsize_z));
	children_[3].reset(instantiateNode(ctr_x_ - off_x, ctr_y_ + off_y, ctr_z_ + off_z, newsize_x, newsize_y, newsize_z));
	children_[4].reset(instantiateNode(ctr_x_ + off_x, ctr_y_ - off_y, ctr_z_ - off_z, newsize_x, newsize_y, newsize_z));
	children_[5].reset(instantiateNode(ctr_x_ + off_x, ctr_y_ - off_y, ctr_z_ + off_z, newsize_x, newsize_y, newsize_z));
	children_[6].reset(instantiateNode(ctr_x_ + off_x, ctr_y_ + off_y, ctr_z_ - off_z, newsize_x, newsize_y, newsize_z));
	children_[7].reset(instantiateNode(ctr_x_ + off_x, ctr_y_ + off_y, ctr_z_ + off_z, newsize_x, newsize_y, newsize_z));
	return (children_);
}

void
cpu_tsdf::OctreeNode::splitRecursive(int num_left)
{
	if (num_left <= 0)
		return;
	split();
	for (size_t i = 0; i < children_.size(); i++)
	{
		children_[i]->splitRecursive(num_left - 1);
	}
}

float
cpu_tsdf::OctreeNode::getVariance() const
{
	if (nsample_ < 5)
		return (std::numeric_limits<float>::infinity());
	return ((M_ / w_)*(nsample_ / (nsample_ - 1)));
}

void
cpu_tsdf::OctreeNode::serialize(std::ostream &f) const
{
	f.write((char*)&d_, sizeof(float));
	f.write((char*)&w_, sizeof(float));
	f.write((char*)&ctr_x_, sizeof(float));
	f.write((char*)&ctr_y_, sizeof(float));
	f.write((char*)&ctr_z_, sizeof(float));
	f.write((char*)&size_, sizeof(float));
	f.write((char*)&M_, sizeof(float));
	f.write((char*)&nsample_, sizeof(int));
	size_t nchild = children_.size();
	f.write((char*)&nchild, sizeof(size_t));
	for (size_t i = 0; i < nchild; ++i)
		children_[i]->serialize(f);
}

void
cpu_tsdf::OctreeNode::deserialize(std::istream &f)
{
	f.read((char*)&d_, sizeof(float));
	f.read((char*)&w_, sizeof(float));
	f.read((char*)&ctr_x_, sizeof(float));
	f.read((char*)&ctr_y_, sizeof(float));
	f.read((char*)&ctr_z_, sizeof(float));
	f.read((char*)&size_, sizeof(float));
	f.read((char*)&M_, sizeof(float));
	f.read((char*)&nsample_, sizeof(int));
	size_t nchild;
	f.read((char*)&nchild, sizeof(size_t));
	children_.resize(nchild);
	for (size_t i = 0; i < nchild; ++i)
	{
		children_[i].reset(instantiateNode(0, 0, 0, 0, 0, 0));
		children_[i]->deserialize(f);
	}
}

// RGBNode
bool
cpu_tsdf::RGBNode::addObservation(float d_new, float w_new, float max_weight,
	uint8_t r, uint8_t g, uint8_t b)
{
	float wsum = w_ + w_new;
	r_ = static_cast<uint8_t> ((w_*r_ + w_new*r) / wsum);
	g_ = static_cast<uint8_t> ((w_*g_ + w_new*g) / wsum);
	b_ = static_cast<uint8_t> ((w_*b_ + w_new*b) / wsum);
	return (OctreeNode::addObservation(d_new, w_new, max_weight));
}

bool
cpu_tsdf::RGBNode::getRGB(uint8_t &r, uint8_t &g, uint8_t &b) const
{
	r = r_;
	g = g_;
	b = b_;
	return (true);
}

cpu_tsdf::OctreeNode*
cpu_tsdf::RGBNode::instantiateNode(float x, float y, float z, float sx, float sy, float sz)
{
	return (new RGBNode(x, y, z, sx, sy, sz));
}

std::string
cpu_tsdf::RGBNode::getTypeString()
{
	return ("RGB");
}

void
cpu_tsdf::RGBNode::serialize(std::ostream &f) const
{
	f.write((char*)&r_, sizeof(uint8_t));
	f.write((char*)&g_, sizeof(uint8_t));
	f.write((char*)&b_, sizeof(uint8_t));
	OctreeNode::serialize(f);
}

void
cpu_tsdf::RGBNode::deserialize(std::istream &f)
{
	f.read((char*)&r_, sizeof(uint8_t));
	f.read((char*)&g_, sizeof(uint8_t));
	f.read((char*)&b_, sizeof(uint8_t));
	OctreeNode::deserialize(f);
}

// RGBNormalized
bool
cpu_tsdf::RGBNormalized::addObservation(float d_new, float w_new, float max_weight,
	uint8_t r, uint8_t g, uint8_t b)
{
	float wsum = w_ + w_new;
	float i = std::sqrt((float)r * (float)r + (float)g*(float)g + (float)b*(float)b);
	float r_f = r / i;
	float g_f = g / i;
	float b_f = b / i;
	r_n_ = (w_*r_n_ + w_new*r_f) / wsum;
	g_n_ = (w_*g_n_ + w_new*g_f) / wsum;
	b_n_ = (w_*b_n_ + w_new*b_f) / wsum;
	i_ = (w_*i_ + w_new*i) / wsum;
	return (OctreeNode::addObservation(d_new, w_new, max_weight));
}

bool
cpu_tsdf::RGBNormalized::getRGB(uint8_t &r, uint8_t &g, uint8_t &b) const
{
	r = r_n_ * i_;
	g = g_n_ * i_;
	b = b_n_ * i_;
	return (true);
}

cpu_tsdf::OctreeNode*
cpu_tsdf::RGBNormalized::instantiateNode(float x, float y, float z, float sx, float sy, float sz)
{
	return (new RGBNormalized(x, y, z, sx, sy, sz));
}

std::string
cpu_tsdf::RGBNormalized::getTypeString()
{
	return ("RGBNormalized");
}

void
cpu_tsdf::RGBNormalized::serialize(std::ostream &f) const
{
	f.write((char*)&r_n_, sizeof(uint8_t));
	f.write((char*)&g_n_, sizeof(uint8_t));
	f.write((char*)&b_n_, sizeof(uint8_t));
	f.write((char*)&i_, sizeof(uint8_t));
	OctreeNode::serialize(f);
}

void
cpu_tsdf::RGBNormalized::deserialize(std::istream &f)
{
	f.read((char*)&r_n_, sizeof(uint8_t));
	f.read((char*)&g_n_, sizeof(uint8_t));
	f.read((char*)&b_n_, sizeof(uint8_t));
	f.read((char*)&i_, sizeof(uint8_t));
	OctreeNode::deserialize(f);
}

void
cpu_tsdf::RGB2LAB(uint8_t r, uint8_t g, uint8_t b,
	float &L, float &A, float &B)
{
	// RGB to XYZ
	float rf = (static_cast<float> (r) / 255.);
	float gf = (static_cast<float> (g) / 255.);
	float bf = (static_cast<float> (b) / 255.);
	if (rf > 0.0405)
		rf = std::pow(((rf + 0.055) / 1.055), 2.4);
	else
		rf /= 12.92;
	if (gf > 0.0405)
		gf = std::pow(((gf + 0.055) / 1.055), 2.4);
	else
		gf /= 12.92;
	if (bf > 0.0405)
		bf = std::pow(((bf + 0.055) / 1.055), 2.4);
	else
		bf /= 12.92;
	rf *= 100;
	gf *= 100;
	bf *= 100;
	float X = rf * 0.4124 + gf * 0.3576 + bf * 0.1805;
	float Y = rf * 0.2126 + gf * 0.7152 + bf * 0.0722;
	float Z = rf * 0.0193 + gf * 0.1192 + bf * 0.9505;
	// XYZ to LAB
	X /= 95.047;
	Y /= 100.;
	Z /= 108.883;
	if (X > 0.008856)
		X = std::pow(static_cast<double> (X), 1 / 3.);
	else
		X = 7.787 * X + (16 / 116.);
	if (Y > 0.008856)
		Y = std::pow(static_cast<double> (Y), 1 / 3.);
	else
		Y = 7.787 * Y + (16 / 116.);
	if (Z > 0.008856)
		Z = std::pow(static_cast<double> (Z), 1 / 3.);
	else
		Z = 7.787 * Z + (16 / 116.);
	L = (116 * Y) - 16;
	A = 500 * (X - Y);
	B = 200 * (Y - Z);
}

void
cpu_tsdf::LAB2RGB(float L, float A, float B,
	uint8_t &r, uint8_t &g, uint8_t &b)
{
	// LAB to XYZ
	float Y = (L + 16) / 116.;
	float X = A / 500. + Y;
	float Z = Y - (B / 200.);
	if (std::pow(X, 3) > 0.008856)
		X = std::pow(X, 3);
	else
		X = (X - 16 / 116.) / 7.787;
	if (std::pow(Y, 3) > 0.008856)
		Y = std::pow(Y, 3);
	else
		Y = (Y - 16 / 116.) / 7.787;
	if (std::pow(Z, 3) > 0.008856)
		Z = std::pow(Z, 3);
	else
		Z = (Z - 16 / 116.) / 7.787;
	X *= 95.047;
	Y *= 100.;
	Z *= 108.883;
	// XYZ to RGB
	X /= 100;
	Y /= 100;
	Z /= 100;
	float rf = X * +3.2406 + Y * -1.5372 + Z * -0.4986;
	float gf = X * -0.9689 + Y * +1.8758 + Z * +0.0415;
	float bf = X * +0.0557 + Y * -0.2040 + Z * +1.0570;
	if (rf > 0.0031308)
		rf = 1.055 * std::pow(static_cast<double> (rf), 1. / 2.4) - 0.055;
	else
		rf *= 12.92;
	if (gf > 0.0031308)
		gf = 1.055 * std::pow(static_cast<double> (gf), 1. / 2.4) - 0.055;
	else
		gf *= 12.92;
	if (bf > 0.0031308)
		bf = 1.055 * std::pow(static_cast<double> (bf), 1. / 2.4) - 0.055;
	else
		bf *= 12.92;
	r = static_cast<uint8_t> (rf * 255);
	g = static_cast<uint8_t> (gf * 255);
	b = static_cast<uint8_t> (bf * 255);
}

// LABNode
bool
cpu_tsdf::LABNode::addObservation(float d_new, float w_new, float max_weight,
	uint8_t r, uint8_t g, uint8_t b)
{
	float wsum = w_ + w_new;
	float L_new, A_new, B_new;
	RGB2LAB(r, g, b, L_new, A_new, B_new);
	uint8_t r_reconv, g_reconv, b_reconv;
	LAB2RGB(L_new, A_new, B_new, r_reconv, g_reconv, b_reconv);
	L_ = (w_*L_ + w_new*L_new) / wsum;
	A_ = (w_*A_ + w_new*A_new) / wsum;
	B_ = (w_*B_ + w_new*B_new) / wsum;
	return (OctreeNode::addObservation(d_new, w_new, max_weight));
}

bool
cpu_tsdf::LABNode::getRGB(uint8_t &r, uint8_t &g, uint8_t &b) const
{
	LAB2RGB(L_, A_, B_, r, g, b);
	return (true);
}

cpu_tsdf::OctreeNode*
cpu_tsdf::LABNode::instantiateNode(float x, float y, float z, float sx, float sy, float sz)
{
	return (new LABNode(x, y, z, sx, sy, sz));
}

std::string
cpu_tsdf::LABNode::getTypeString()
{
	return ("LAB");
}

void
cpu_tsdf::LABNode::serialize(std::ostream &f) const
{
	f.write((char*)&L_, sizeof(uint8_t));
	f.write((char*)&A_, sizeof(uint8_t));
	f.write((char*)&B_, sizeof(uint8_t));
	OctreeNode::serialize(f);
}

void
cpu_tsdf::LABNode::deserialize(std::istream &f)
{
	f.read((char*)&L_, sizeof(uint8_t));
	f.read((char*)&A_, sizeof(uint8_t));
	f.read((char*)&B_, sizeof(uint8_t));
	OctreeNode::deserialize(f);
}

// Octree
void
cpu_tsdf::Octree::init(int num_splits)
{
	// Starts with just one root node
	root_.reset(OctreeNode::instantiateByTypeString
	(voxel_type_, 0, 0, 0, size_x_, size_y_, size_z_));
	root_->splitRecursive(num_splits);
}

void
cpu_tsdf::Octree::init(float max_size_x, float max_size_y, float max_size_z)
{
	int desired_res = std::max(size_x_ / max_size_x, std::max(size_y_ / max_size_y, size_z_ / max_size_z));
	int num_levels = std::ceil(std::log(desired_res) / std::log(2));
	init(num_levels);
}

cpu_tsdf::OctreeNode::Ptr&
cpu_tsdf::Octree::getRoot()
{
	return root_;
}

void
cpu_tsdf::Octree::getLeaves(std::vector<OctreeNode::Ptr> &leaves, int num_levels) const
{
	// Recursively find leaf nodes
	pcl::console::TicToc tt;
	tt.tic();
	if (num_levels == 0)
		leaves.push_back(root_);
	else
		root_->getLeaves(leaves, num_levels - 1);
}

void
cpu_tsdf::Octree::getLeaves(std::vector<OctreeNode::Ptr> &leaves, float max_size_x, float max_size_y, float max_size_z) const
{
	int desired_res = std::max(size_x_ / max_size_x, std::max(size_y_ / max_size_y, size_z_ / max_size_z));
	int num_levels = std::ceil(std::log(desired_res) / std::log(2));
	getLeaves(leaves, num_levels);
}

// Get the voxel which contains this point
const cpu_tsdf::OctreeNode*
cpu_tsdf::Octree::getContainingVoxel(float x, float y, float z, float min_size) const
{
	if (pcl_isnan(z) || std::fabs(x) > size_x_ / 2 || std::fabs(y) > size_y_ / 2 || std::fabs(z) > size_z_ / 2)
		return (NULL);
	return (root_->getContainingVoxel(x, y, z, min_size));
}

// Get the voxel which contains this point
cpu_tsdf::OctreeNode*
cpu_tsdf::Octree::getContainingVoxel(float x, float y, float z, float min_size)
{
	if (pcl_isnan(z) || std::fabs(x) > size_x_ / 2 || std::fabs(y) > size_y_ / 2 || std::fabs(z) > size_z_ / 2)
		return (NULL);
	return (root_->getContainingVoxel(x, y, z, min_size));
}

void
cpu_tsdf::Octree::serialize(std::ostream &f) const
{
	f << root_->getTypeString() << std::endl;
	f << "#OCTREEBINARY" << std::endl;
	f.write((char*)&res_x_, sizeof(size_t));
	f.write((char*)&res_y_, sizeof(size_t));
	f.write((char*)&res_z_, sizeof(size_t));
	f.write((char*)&size_x_, sizeof(float));
	f.write((char*)&size_y_, sizeof(float));
	f.write((char*)&size_z_, sizeof(float));
	root_->serialize(f);
}

void
cpu_tsdf::Octree::deserialize(std::istream &f)
{
	std::string root_type;
	f >> root_type;
	root_.reset(OctreeNode::instantiateByTypeString(root_type));
	char tmp[1024];
	do
	{
		f.getline(tmp, 1024);
	} while (!(tmp[0] == '#' && tmp[1] == 'O'));
	f.read((char*)&res_x_, sizeof(size_t));
	f.read((char*)&res_y_, sizeof(size_t));
	f.read((char*)&res_z_, sizeof(size_t));
	f.read((char*)&size_x_, sizeof(float));
	f.read((char*)&size_y_, sizeof(float));
	f.read((char*)&size_z_, sizeof(float));
	root_->deserialize(f);
}


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

#include "tabulatedpotential.h"
#include "../../include/votca/csg/version.h"
#include "analysistool.h"
#include "bondedstatistics.h"
#include <boost/lexical_cast.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <votca/tools/constants.h>
#include <votca/tools/histogram.h>

using namespace std;
using namespace votca::tools;

namespace votca {
namespace csg {
/******************************************************************************
 * Public Facing Methods
 ******************************************************************************/
TabulatedPotential::TabulatedPotential() {
  tab_smooth1_ = tab_smooth2_ = 0;
  Temperature_ = 300;
}

void TabulatedPotential::Register(map<string, AnalysisTool *> &lib) {
  lib["tab"] = this;
  lib["hist"] = this;
}

void TabulatedPotential::Command(BondedStatistics &bs, const string &cmd,
                                 vector<string> &args) {
  if (args[0] == "set") {
    if (cmd == "hist") {
      SetOption_(hist_options_, args);
    } else if (cmd == "tab") {
      if (!SetOption_(tab_options_, args)) {
        if (args.size() > 2) {
          if (args[1] == "smooth_pdf") {
            tab_smooth1_ = boost::lexical_cast<Index>(args[2]);
          } else if (args[1] == "smooth_pot") {
            tab_smooth2_ = boost::lexical_cast<Index>(args[2]);
          } else if (args[1] == "T") {
            Temperature_ = boost::lexical_cast<double>(args[2]);
          } else {
            cout << "unknown option " << args[2] << endl;
            return;
          }
        }
      }
      if (args.size() <= 2) {
        cout << "smooth_pdf: " << tab_smooth1_ << endl;
        cout << "smooth_pot: " << tab_smooth2_ << endl;
        cout << "T: " << Temperature_ << endl;
      }
    }
  } else if (args.size() >= 2) {
    if (cmd == "hist") {
      WriteHistogram(bs, args);
    } else if (cmd == "tab") {
      WritePotential(bs, args);
    }
  } else {
    cout << "wrong number of arguments" << endl;
  }
}

void TabulatedPotential::Help(const string &cmd, vector<string> &args) {
  if (args.size() == 0) {
    if (cmd == "tab") {
      cout << "tab <file> <selection>\n"
           << "Calculate tabulated potential by inverting the distribution "
              "function. Statistics is calculated using all interactions in "
              "selection.\nsee also: help tab set\n\nexample:\ntab set scale "
              "bond\ntab U_bond.txt *:bond:*\n";
    }
    if (cmd == "hist") {
      cout << "hist <file> <selection>\n"
           << "Calculate distribution function for selection. Statistics is "
              "calculated using all interactions in selection.\n see also: help"
              " hist set\n\nexample:hist U_bond.txt *:bond:*\n";
    }
    return;
  }
  if (args[0] == "set") {
    if (args.size() == 1) {
      cout << cmd << " set <option> <value>\n"
           << "set option for this command. Use \"" << cmd
           << " set\" for a list of available options. To get help on a "
              "specific option use e.g.\n"
           << cmd << " set periodic\n";
      return;
    }
    if (args[1] == "n") {
      cout << cmd << "set n <integer>\n"
           << "set number of bins for table\n";
      return;
    }
    if (args[1] == "min") {
      cout << cmd << "set min <value>\n"
           << "minimum value of interval for histogram (see also periodic, "
              "extend)\n";
      return;
    }
    if (args[1] == "max") {
      cout << cmd << "set max <value>\n"
           << "maximum value of interval for histogram (see also periodic, "
              "extend)\n";
      return;
    }
    if (args[1] == "periodic") {
      cout << cmd << "set periodic <value>\n"
           << "can be 1 for periodic interval (e.g. dihedral) or 0 for "
              "non-periodic (e.g. bond)\n";
      return;
    }
    if (args[1] == "auto") {
      cout << cmd
           << "set auto <value>\n"
              "can be 1 for automatically determine the interval for the table "
              "(min, max, extend will be ignored) or 0 to use min/max as "
              "specified\n";
      return;
    }
    if (args[1] == "extend") {
      cout << cmd
           << "set extend <value>\n"
              "should only be used with auto=0. Can be 1 for extend the "
              "interval if values are out of bounds (min/max) or 0 to "
              "ignore values which are out of the interal\n";
      return;
    }
    if (args[1] == "scale") {
      cout << cmd
           << "set scale <value>\n"
              "volume normalization of pdf. Can be no (no scaling), bond "
              "(1/r^2) or angle ( 1/sin(phi) ). See VOTCA manual, section "
              "theoretical background for details\n";
      return;
    }
    if (args[1] == "normalize") {
      cout
          << cmd
          << "set normalize <value>\n"
             "can be 1 for a normalized histogram or 0 to skip normalization\n";
      return;
    }

    if (cmd == "tab") {
      if (args[1] == "smooth_pdf") {
        cout << "tab set smooth_pdf <value>\n"
                "Perform so many smoothing iterations on the distribution "
                "function before inverting the potential\n";
        return;
      }
      if (args[1] == "smooth_pot") {
        cout << "tab set smooth_pot <value>\n"
                "Perform so many smoothing iterations on tabulated potential "
                "after inverting the potential\n";
        return;
      }
      if (args[1] == "T") {
        cout << "tab set T <value>\n"
                "Temperature in Kelvin the simulation was performed\n";
        return;
      }
    }
  }

  cout << "no help text available" << endl;
}

double TabulatedPotential::getTemperature() const { return Temperature_; }

pair<Index, Index> TabulatedPotential::getSmoothIterations() const {
  return pair<Index, Index>(tab_smooth1_, tab_smooth2_);
}

void TabulatedPotential::WriteHistogram(BondedStatistics &bs,
                                        vector<string> &args) {
  ofstream out;
  DataCollection<double>::selection *sel = nullptr;

  for (size_t i = 1; i < args.size(); i++) {
    sel = bs.BondedValues().select(args[i], sel);
  }
  Histogram h(hist_options_);
  h.ProcessData(sel);
  out.open(args[0]);
  out << h;
  out.close();
  cout << "histogram created using " << sel->size() << " data-rows, written to "
       << args[0] << endl;
  delete sel;
}

void TabulatedPotential::WritePotential(BondedStatistics &bs,
                                        vector<string> &args) {
  ofstream out;
  DataCollection<double>::selection *sel = nullptr;

  // Appends all the interactions that are specified in args to selection
  // pointer given by sel

  for (size_t i = 1; i < args.size(); i++) {
    sel = bs.BondedValues().select(args[i], sel);
  }

  Histogram h(tab_options_);

  h.ProcessData(sel);
  for (Index i = 0; i < tab_smooth1_; ++i) {
    Smooth_(h.getPdf(), tab_options_.periodic_);
  }
  BoltzmannInvert_(h.getPdf());
  for (Index i = 0; i < tab_smooth2_; ++i) {
    Smooth_(h.getPdf(), tab_options_.periodic_);
  }
  out.open(args[0]);

  vector<double> F;
  assert(h.getInterval() > 0 && "Interval for pdf histogram is 0");
  CalcForce_(h.getPdf(), F, h.getInterval(), tab_options_.periodic_);
  for (Index i = 0; i < h.getN(); i++) {
    out << h.getMin() + h.getInterval() * ((double)i) << " " << h.getPdf()[i]
        << " " << F[i] << endl;
  }
  out.close();
  cout << "histogram created using " << sel->size() << " data-rows, written to "
       << args[0] << endl;
  delete sel;
}

/******************************************************************************
 * Private Facing Methods
 ******************************************************************************/
bool TabulatedPotential::SetOption_(Histogram::options_t &op,
                                    const vector<string> &args) {
  if (args.size() > 2) {
    if (args[1] == "n") {
      op.n_ = boost::lexical_cast<Index>(args[2]);
    } else if (args[1] == "min") {
      op.min_ = boost::lexical_cast<double>(args[2]);
    } else if (args[1] == "max") {
      op.max_ = boost::lexical_cast<double>(args[2]);
    } else if (args[1] == "periodic") {
      op.periodic_ = boost::lexical_cast<bool>(args[2]);
    } else if (args[1] == "auto") {
      op.auto_interval_ = boost::lexical_cast<bool>(args[2]);
    } else if (args[1] == "extend") {
      op.extend_interval_ = boost::lexical_cast<bool>(args[2]);
    } else if (args[1] == "normalize") {
      op.normalize_ = boost::lexical_cast<bool>(args[2]);
    } else if (args[1] == "scale") {
      if (args[2] == "no" || args[2] == "bond" || args[2] == "angle") {
        op.scale_ = args[2];
      } else {
        cout << "scale can be: no, bond or angle\n";
      }
    } else {
      return false;
    }
  } else {
    cout << "n: " << op.n_ << endl;
    cout << "min: " << op.min_ << endl;
    cout << "max: " << op.max_ << endl;
    cout << "periodic: " << op.periodic_ << endl;
    cout << "auto: " << op.auto_interval_ << endl;
    cout << "extend: " << op.extend_interval_ << endl;
    cout << "scale: " << op.scale_ << endl;
    cout << "normalize: " << op.normalize_ << endl;
  }
  return true;
}

void TabulatedPotential::CalcForce_(vector<double> &U, vector<double> &F,
                                    double dx, bool bPeriodic) {
  size_t n = U.size();
  double f = 0.5 / dx;
  F.resize(n);
  if (bPeriodic) {
    F[n - 1] = F[0] = -(U[1] - U[n - 2]) * f;
  } else {
    F[0] = -(U[1] - U[0]) * 2 * f;
    F[n - 1] = -(U[n - 1] - U[n - 2]) * 2 * f;
  }
  for (size_t i = 1; i < n - 1; i++) {
    F[i] = -(U[i + 1] - U[i - 1]) * f;
  }
}

void TabulatedPotential::Smooth_(vector<double> &data, bool bPeriodic) {
  double old[3];
  Index n = Index(data.size());
  if (bPeriodic) {
    old[0] = data[n - 3];
    old[1] = data[n - 2];
  } else {
    old[0] = data[0];
    old[1] = data[0];
  }
  Index i;
  for (i = 0; i < Index(data.size()) - 2; i++) {
    old[2] = data[i];
    data[i] =
        (old[0] + 2. * old[1] + 3. * data[i] + 2. * data[i + 1] + data[i + 2]) /
        9.;
    old[0] = old[1];
    old[1] = old[2];
  }
  if (bPeriodic) {
    data[i] =
        (old[0] + 2. * old[1] + 3. * data[i] + 2. * data[i + 1] + data[0]) / 9.;
    data[n - 1] = data[0];
  } else {
    data[i] = (old[0] + 2. * old[1] + 3. * data[i] + 3. * data[i + 1]) / 9.;
    old[0] = old[1];
    old[1] = data[i];
    i++;
    data[i] = (old[0] + 2. * old[1] + 6. * data[i]) / 9.;
  }
}

void TabulatedPotential::BoltzmannInvert_(vector<double> &data) {

  double min = std::numeric_limits<double>::max();
  double max = std::numeric_limits<double>::min();

  for (double i : data) {
    max = std::max(i, max);
    if (i > 0) {
      min = std::min(i, min);
    }
  }
  max = -conv::kB * conv::ev2kj_per_mol * Temperature_ * std::log(max);
  min = -conv::kB * conv::ev2kj_per_mol * Temperature_ * std::log(min) - max;

  for (double &i : data) {
    if (i == 0) {
      i = min;
    } else {
      i = -conv::kB * conv::ev2kj_per_mol * Temperature_ * std::log(i) - max;
    }
  }
}

}  // namespace csg
}  // namespace votca


/*****************************************************************************
 *   GATB : Genome Assembly Tool Box
 *   Copyright (C) 2014  INRIA
 *   Authors: R.Chikhi, G.Rizk, E.Drezen
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Affero General Public License as
 *  published by the Free Software Foundation, either version 3 of the
 *  License, or (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Affero General Public License for more details.
 *
 *  You should have received a copy of the GNU Affero General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*****************************************************************************/

/** \file Integer.hpp
 *  \date 01/03/2013
 *  \author edrezen
 *  \brief Entry point class for large integer usage
 */
#ifndef _GATB_CORE_TOOLS_MATH_INTEGER_HPP_
#define _GATB_CORE_TOOLS_MATH_INTEGER_HPP_

/********************************************************************************/
#include <gatb/tools/math/LargeInt.hpp>
#include <gatb/system/api/Exception.hpp>

#include <boost/variant.hpp>

/********************************************************************************/
namespace gatb  {  namespace core  { namespace tools {  namespace math  {
/********************************************************************************/

/** \brief Class for large integers calculus
 *
 * The IntegerTemplate is implemented as a boost variant, which means that it can act like T1, T2, T3 or T4 type
 * according to the configuration.
 *
 * The IntegerTemplate should be specialized with 4 different LargeInt implementation
 * classes.
 *
 * All the methods are implemented through a boost variant visitor.
 *
 *  According to the INTEGER_KIND compilation flag, we define the Integer class
 *  as an alias of one from several possible implementations.
 *
 *  Note that we have 2 possible native implementations (NativeInt64 and NativeInt128)
 *  that rely on native types uint64_t and __uint128_t.
 *
 *  For larger integer, a multi-precision LargeInt is used.
 *
 *  From the user point of view, [s]he has just to include this file and use the Integer
 *  class.
 *
 */
template<typename T1, typename T2, typename T3, typename T4>
class IntegerTemplate
{
private:

    typedef boost::variant<T1,T2,T3,T4> Type;
    Type v;

          Type& operator *()       { return v; }
    const Type& operator *() const { return v; }

public:

    /** Get a number telling which template class is actually used by the variant.
     * \return an integer n, meaning that the Tn template type is used.
     */
    static char& getType()  {  static char instance = 0; return instance; }

    /** Set the type of which template class has to be used by the variant.
     * \param[in] type : integer value
     */
    static void setType (char type)  {  getType() = type; }

    /** Constructor. Note that the type (see getType and setType) has to be first initialized
     * otherwise no instance can be created (exception thrown).
     * \param[in] n : value for initialization of the integer.
     */
    IntegerTemplate (int64_t n=0)
    {
        switch (getType())
        {
        case PREC_1: v = T1(n); break;
        case PREC_2: v = T2(n); break;
        case PREC_3: v = T3(n); break;
        case PREC_4: v = T4(n); break;
        default:  if (getType()<=PREC_4) { v = T4(n);  break; }
                  else { throw system::Exception ("class Integer not initialized"); }
        }
    }

    /** Copy constructor. Relies on the copy constructor of boost variant
     * \param[in] t : the object to be used for initialization
     */
    template<typename T>  explicit IntegerTemplate (const T& t) : v (t)  {}

    /** Affectation operator. Relies on the affectation operator of boost variant
     * \param[in] t : object to be copied
     * \return the current object.
     */
    template<typename T>
    IntegerTemplate& operator=(const T& t)
    {
        v = t;
        return *this;
    }

    /** Get the name of the class used by the variant (ie. one of the Ti template class parameters)
     * \return the class name.
     */
    const char*  getName ()         { return boost::apply_visitor (Integer_name(),  *(*this)); }

    /** Get the size of an instance of the class used by the variant  (ie. one of the Ti template class parameters)
     * \return the size of an object (in bits).
     */
    const size_t getSize ()         { return boost::apply_visitor (Integer_size(),  *(*this)); }

    /** Get the HDF5 type for the the class used by the variant  (ie. one of the Ti template class parameters)
     * \param[in] isCompound : tells whether the type is composed or not
     * \return a HDF5 identifier for the type.
     */
    hid_t hdf5 (bool& isCompound)   { return boost::apply_visitor (Integer_hdf5(isCompound),  *(*this)); }

    /** Operator +
     * \param[in] a : first operand
     * \param[in] b : second operand
     * \return sum of the two operands.
     */
    inline friend IntegerTemplate   operator+  (const IntegerTemplate& a, const IntegerTemplate& b)  {  return  boost::apply_visitor (Integer_plus(),  *a, *b);  }

    /** Operator -
     * \param[in] a : first operand
     * \param[in] b : second operand
     * \return substraction of the two operands.
     */
    inline friend IntegerTemplate   operator-  (const IntegerTemplate& a, const IntegerTemplate& b)  {  return  boost::apply_visitor (Integer_minus(), *a, *b);  }

    /** Operator |
     * \param[in] a : first operand
     * \param[in] b : second operand
     * \return 'or' of the two operands.
     */
    inline friend IntegerTemplate   operator|  (const IntegerTemplate& a, const IntegerTemplate& b)  {  return  boost::apply_visitor (Integer_or(),    *a, *b);  }

    /** Operator ^
     * \param[in] a : first operand
     * \param[in] b : second operand
     * \return 'xor' of the two operands.
     */
    inline friend IntegerTemplate   operator^  (const IntegerTemplate& a, const IntegerTemplate& b)  {  return  boost::apply_visitor (Integer_xor(),   *a, *b);  }

    /** Operator &
     * \param[in] a : first operand
     * \param[in] b : second operand
     * \return 'and' of the two operands.
     */
    inline friend IntegerTemplate   operator&  (const IntegerTemplate& a, const IntegerTemplate& b)  {  return  boost::apply_visitor (Integer_and(),   *a, *b);  }

    /** Operator ~
     * \param[in] a : operand
     * \return negation of the operand
     */
    inline friend IntegerTemplate   operator~  (const IntegerTemplate& a)                    {  Integer_compl v; return  boost::apply_visitor (v, *a);  }

    /** Operator ==
     * \param[in] a : first operand
     * \param[in] b : second operand
     * \return equality of the two operands.
     */
    inline friend bool      operator== (const IntegerTemplate& a, const IntegerTemplate& b)  {  return  boost::apply_visitor (Integer_equals(), *a, *b);  }

    /** Operator !=
     * \param[in] a : first operand
     * \param[in] b : second operand
     * \return inequality of the two operands.
     */
    inline friend bool      operator!= (const IntegerTemplate& a, const IntegerTemplate& b)  {  return  ! (*a==*b);  }

    /** Operator <
     * \param[in] a : first operand
     * \param[in] b : second operand
     * \return '<' of the two operands.
     */
    inline friend bool      operator<  (const IntegerTemplate& a, const IntegerTemplate& b)  {  return  boost::apply_visitor (Integer_less(),   *a, *b);  }

    /** Operator <=
     * \param[in] a : first operand
     * \param[in] b : second operand
     * \return '<=' of the two operands.
     */
    inline friend bool      operator<= (const IntegerTemplate& a, const IntegerTemplate& b)  {  return  boost::apply_visitor (Integer_lesseq(), *a, *b);  }

    /** Operator *
     * \param[in] a : first operand
     * \param[in] c : second operand
     * \return multiplication of the two operands.
     */
    inline friend IntegerTemplate   operator*  (const IntegerTemplate& a, const int&       c)  {  return  boost::apply_visitor (Integer_mult(c), *a);  }

    /** Operator /
     * \param[in] a : first operand
     * \param[in] c : second operand
     * \return division of the two operands.
     */
    inline friend IntegerTemplate   operator/  (const IntegerTemplate& a, const u_int32_t& c)  {  return  boost::apply_visitor (Integer_div(c),  *a);  }

    /** Operator %
     * \param[in] a : first operand
     * \param[in] c : second operand
     * \return modulo of the two operands.
     */
    inline friend u_int32_t operator%  (const IntegerTemplate& a, const u_int32_t& c)  {  return  boost::apply_visitor (Integer_mod(c),  *a);  }

    /** Operator >>
     * \param[in] a : first operand
     * \param[in] c : second operand
     * \return right shift of the two operands.
     */
    inline friend IntegerTemplate   operator>> (const IntegerTemplate& a, const int& c)  {  return  boost::apply_visitor (Integer_shiftLeft(c),   *a);  }

    /** Operator <<
     * \param[in] a : first operand
     * \param[in] c : second operand
     * \return left shift of the two operands.
     */
    inline friend IntegerTemplate   operator<< (const IntegerTemplate& a, const int& c)  {  return  boost::apply_visitor (Integer_shiftRight(c),  *a);  }

    /** Operator +=
     * \param[in] a : first operand
     * \return addition and affectation.
     */
    void operator+= (const IntegerTemplate& a)  {  boost::apply_visitor (Integer_plusaffect(),  *(*this), *a);  }

    /** Operator ^=
     * \param[in] a : first operand
     * \return xor and affectation.
     */
    void operator^= (const IntegerTemplate& a)  {  boost::apply_visitor (Integer_xoraffect(),   *(*this), *a);  }

    /** Operator[] access the ith nucleotide in the given integer. For instance a[4] get the 5th nucleotide of
     * a kmer encoded as an Integer object.
     * \param[in] idx : index of the nucleotide to be retrieved
     * \return the nucleotide value as follow: A=0, C=1, T=2 and G=3
     */
    u_int8_t  operator[]  (size_t idx) const   { return  boost::apply_visitor (Integer_value_at(idx), *(*this)); }

    /** Get the reverse complement of a kmer encoded as an IntegerTemplate object. Note that the kmer size must be known.
     * \param[in] a : kmer value to be reversed-complemented
     * \param[in] sizeKmer : size of the kmer
     * \return the reverse complement kmer as a IntegerTemplate value
     */
    friend IntegerTemplate revcomp (const IntegerTemplate& a,  size_t sizeKmer)  {  return  boost::apply_visitor (Integer_revomp(sizeKmer),  *a);  }

    /** Get a hash value on 64 bits for a given IntegerTemplate object.
     * \param[in] a : the integer value
     * \param[in] seed : some seed value used for the hash computation.
     * \return the hash value on 64 bits.
     */
    friend u_int64_t hash1        (const IntegerTemplate& a,  u_int64_t seed)  {  return  boost::apply_visitor (Integer_hash1(seed),  *a);          }

    /** Get a hash value on 64 bits for a given IntegerTemplate object.
     * \param[in] a : the integer value
     * \return the hash value on 64 bits.
     */
    friend u_int64_t oahash       (const IntegerTemplate& a)                   {  return  boost::apply_visitor (Integer_oahash(), *a);              }

    /** Get a hash value on 64 bits for a given IntegerTemplate object.
     * Note: although we return 64 bits, only the first 16 bits are set
     * \param[in] a : the integer value
     * \param[in] shift : some value used for the hash computation.
     * \return the hash value on 64 bits.
     */
    friend u_int64_t simplehash16 (const IntegerTemplate& a,  int shift)       {  return  boost::apply_visitor (Integer_simplehash16(shift),  *a);  }

    /** Get an ASCII string representation of a kmer encoded as a IntegerTemplate object
     * \param[in] sizeKmer : size of the kmer
     * \return the ASCII representation of the kmer.
     */
    std::string toString (size_t sizeKmer) const  {  return boost::apply_visitor (Integer_toString(sizeKmer), *(*this)); }

    /** Output stream operator for the IntegerTemplate class
     * \param[in] s : the output stream to be used.
     * \param[in] a : the object to output
     * \return the modified output stream.
     */
    friend std::ostream & operator<<(std::ostream & s, const IntegerTemplate& a)  {  s << *a;  return s;  }

    /** Get the value of the IntegerTemplate object as a U type, U being one of the T1,T2,T3,T4
     * template class parameters. This method can be seen as a converter from the IntegerTemplate class
     * to a specific U type (given as a template parameter of this method).
     * \return  the converted value as a U type.
     */
    template<typename U>
    const U& get ()  const  {  return * boost::get<U>(&v);  }

private:

    struct Integer_name : public boost::static_visitor<const char*>    {
        template<typename T>  const char* operator() (const T& a) const { return a.getName();  }};

    struct Integer_size : public boost::static_visitor<const size_t>    {
        template<typename T>  const size_t operator() (const T& a) const  { return a.getSize();  }};

    struct Integer_plus : public boost::static_visitor<IntegerTemplate>    {
        template<typename T>              IntegerTemplate operator() (const T& a, const T& b) const  { return IntegerTemplate(a + b);  }
        template<typename T, typename U>  IntegerTemplate operator() (const T& a, const U& b) const  { return IntegerTemplate();       }
    };

    struct Integer_minus : public boost::static_visitor<IntegerTemplate>    {
        template<typename T>              IntegerTemplate operator() (const T& a, const T& b) const  { return IntegerTemplate(a - b);  }
        template<typename T, typename U>  IntegerTemplate operator() (const T& a, const U& b) const  { return IntegerTemplate();  }
    };

    struct Integer_or : public boost::static_visitor<IntegerTemplate>    {
        template<typename T>              IntegerTemplate operator() (const T& a, const T& b) const  { return IntegerTemplate(a | b);  }
        template<typename T, typename U>  IntegerTemplate operator() (const T& a, const U& b) const  { return IntegerTemplate();  }
    };

    struct Integer_xor : public boost::static_visitor<IntegerTemplate>    {
        template<typename T>              IntegerTemplate operator() (const T& a, const T& b) const  { return IntegerTemplate(a ^ b);  }
        template<typename T, typename U>  IntegerTemplate operator() (const T& a, const U& b) const  { return IntegerTemplate();  }
    };

    struct Integer_and : public boost::static_visitor<IntegerTemplate>    {
        template<typename T>              IntegerTemplate operator() (const T& a, const T& b) const  { return IntegerTemplate(a & b);  }
        template<typename T, typename U>  IntegerTemplate operator() (const T& a, const U& b) const  { return IntegerTemplate();  }
    };

    struct Integer_less : public boost::static_visitor<bool>    {
        template<typename T>              bool operator() (const T& a, const T& b) const  { return a < b;  }
        template<typename T, typename U>  bool operator() (const T& a, const U& b) const  { return false;  }
    };

    struct Integer_lesseq : public boost::static_visitor<bool>    {
        template<typename T>              bool operator() (const T& a, const T& b) const  { return a <= b;  }
        template<typename T, typename U>  bool operator() (const T& a, const U& b) const  { return false;   }
    };

    struct Integer_equals : public boost::static_visitor<bool>    {
        template<typename T>              bool operator() (const T& a, const T& b) const  { return a == b;  }
        template<typename T, typename U>  bool operator() (const T& a, const U& b) const  { return false;   }
    };

    struct Integer_plusaffect : public boost::static_visitor<>    {
        template<typename T>              void operator() ( T& a, const T& b) const  { a += b;  }
        template<typename T, typename U>  void operator() ( T& a, const U& b) const  {   }
    };

    struct Integer_xoraffect : public boost::static_visitor<>    {
        template<typename T>              void operator() ( T& a, const T& b) const  { a ^= b;  }
        template<typename T, typename U>  void operator() ( T& a, const U& b) const  {   }
    };

    struct Integer_compl : public boost::static_visitor<IntegerTemplate>    {
        template<typename T>  IntegerTemplate operator() (const T& a)  { return IntegerTemplate(~a);  }};

    template<typename Result, typename Arg>
    struct Visitor : public boost::static_visitor<Result>
    {
        Visitor (Arg a=Arg()) : arg(a) {}
        Arg arg;
    };

    struct Integer_hdf5 : public Visitor<hid_t,bool&>   {
        Integer_hdf5 (bool& c) : Visitor<IntegerTemplate,bool&>(c) {}
        template<typename T>  hid_t operator() (const T& a)  { return a.hdf5 (this->arg);  }};

    struct Integer_mult : public Visitor<IntegerTemplate,const int>    {
        Integer_mult (const int& c) : Visitor<IntegerTemplate,const int>(c) {}
        template<typename T>  IntegerTemplate operator() (const T& a) const  { return IntegerTemplate(a*this->arg);  }};

    struct Integer_div : public Visitor<IntegerTemplate,const u_int32_t>    {
        Integer_div (const u_int32_t& c) : Visitor<IntegerTemplate,const u_int32_t>(c) {}
        template<typename T>  IntegerTemplate operator() (const T& a) const  { return IntegerTemplate(a/this->arg);  }};

    struct Integer_mod : public Visitor<u_int32_t,const u_int32_t>    {
        Integer_mod (const u_int32_t& c) : Visitor<u_int32_t,const u_int32_t>(c) {}
        template<typename T>  u_int32_t operator() (const T& a) const  { return (a%this->arg);  }};

    struct Integer_shiftLeft : public Visitor<IntegerTemplate,const int>    {
        Integer_shiftLeft (const int& c) : Visitor<IntegerTemplate,const int>(c) {}
        template<typename T>  IntegerTemplate operator() (const T& a) const  { return IntegerTemplate (a >> this->arg);  }};

    struct Integer_shiftRight : public Visitor<IntegerTemplate,const int>    {
        Integer_shiftRight (const int& c) : Visitor<IntegerTemplate,const int>(c) {}
        template<typename T>  IntegerTemplate operator() (const T& a) const  { return IntegerTemplate (a << this->arg);  }};

    struct Integer_revomp : public Visitor<IntegerTemplate,size_t>    {
        Integer_revomp (const size_t& c) : Visitor<IntegerTemplate,size_t>(c) {}
        template<typename T>  IntegerTemplate operator() (const T& a) const  { return IntegerTemplate (revcomp(a,this->arg));  }};

    struct Integer_hash1 : public Visitor<u_int64_t,u_int64_t>    {
        Integer_hash1 (const u_int64_t& c) : Visitor<u_int64_t,u_int64_t>(c) {}
        template<typename T>  u_int64_t operator() (const T& a) const  { return (hash1(a,this->arg));  }};

    struct Integer_oahash : public boost::static_visitor<u_int64_t>    {
        template<typename T>  u_int64_t operator() (const T& a) const  { return (oahash(a));  }};

    struct Integer_simplehash16 : public Visitor<u_int64_t,int>    {
        Integer_simplehash16 (const int& c) : Visitor<u_int64_t,int>(c) {}
        template<typename T>  u_int64_t operator() (const T& a) const  { return (simplehash16(a,this->arg));  }};

    struct Integer_value_at : public Visitor<u_int8_t,size_t>   {
        Integer_value_at (size_t idx) : Visitor<u_int8_t,size_t>(idx) {}
        template<typename T>  u_int8_t operator() (const T& a) const { return a[this->arg];  }};

    struct Integer_toString : public Visitor<std::string,size_t>   {
        Integer_toString (size_t c) : Visitor<std::string,size_t>(c) {}
        template<typename T>  std::string operator() (const T& a) const  { return a.toString(this->arg);  }};
};

/********************************************************************************/

#define INTEGER_TYPES   LargeInt<PREC_1>,LargeInt<PREC_2>,LargeInt<PREC_3>,LargeInt<PREC_4>

typedef IntegerTemplate <INTEGER_TYPES> Integer;

/********************************************************************************/
}}}};
/********************************************************************************/

#endif /* _GATB_CORE_TOOLS_MATH_INTEGER_HPP_ */


/*=============================================================================
  Copyright (c) 2010-2016 Bolero MURAKAMI
  https://github.com/bolero-MURAKAMI/Sprig

  Distributed under the Boost Software License, Version 1.0. (See accompanying
  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/
#ifndef SPRIG_NTL_FROM_ZZ_HPP
#define SPRIG_NTL_FROM_ZZ_HPP

#include <sprig/config/config.hpp>

#ifdef SPRIG_USING_PRAGMA_ONCE
#	pragma once
#endif	// #ifdef SPRIG_USING_PRAGMA_ONCE

#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_signed.hpp>
#include <boost/type_traits/is_unsigned.hpp>
#include <boost/type_traits/is_float.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/not.hpp>
#include <boost/utility/enable_if.hpp>
#include <sprig/external/ntl/zz.hpp>
#include <sprig/external/ntl/rr.hpp>
#include <sprig/external/ntl/xdouble.hpp>
#include <sprig/external/ntl/quad_float.hpp>

namespace sprig {
	//
	// from_ZZ
	//
	// specialization from_ZZ: T == NTL::ZZ
	//
	template<typename T>
	SPRIG_INLINE T from_ZZ(
		NTL::ZZ const& target,
		typename boost::enable_if<
			boost::is_same<typename boost::remove_const<T>::type, NTL::ZZ>
		>::type* = 0
		)
	{
		return target;
	}
	//
	// specialization from_ZZ: T == NTL::RR
	//
	template<typename T>
	SPRIG_INLINE T from_ZZ(
		NTL::ZZ const& target,
		typename boost::enable_if<
			boost::is_same<typename boost::remove_const<T>::type, NTL::RR>
		>::type* = 0
		)
	{
		return NTL::to_RR(target);
	}
	//
	// specialization from_ZZ: T == NTL::xdouble
	//
	template<typename T>
	SPRIG_INLINE T from_ZZ(
		NTL::ZZ const& target,
		typename boost::enable_if<
			boost::is_same<typename boost::remove_const<T>::type, NTL::xdouble>
		>::type* = 0
		)
	{
		return NTL::to_xdouble(target);
	}
	//
	// specialization from_ZZ: T == NTL::quad_float
	//
	template<typename T>
	SPRIG_INLINE T from_ZZ(
		NTL::ZZ const& target,
		typename boost::enable_if<
			boost::is_same<typename boost::remove_const<T>::type, NTL::quad_float>
		>::type* = 0
		)
	{
		return NTL::to_quad_float(target);
	}
	//
	// specialization from_ZZ: T == int
	//
	template<typename T>
	SPRIG_INLINE T from_ZZ(
		NTL::ZZ const& target,
		typename boost::enable_if<
			boost::is_same<typename boost::remove_const<T>::type, int>
		>::type* = 0
		)
	{
		return NTL::to_int(target);
	}
	//
	// specialization from_ZZ: is_signed(T) && T != int
	//
	template<typename T>
	SPRIG_INLINE T from_ZZ(
		NTL::ZZ const& target,
		typename boost::enable_if<
			boost::mpl::and_<
				boost::is_signed<T>,
				boost::mpl::not_<
					boost::is_same<typename boost::remove_const<T>::type, int>
				>
			>
		>::type* = 0
		)
	{
		return NTL::to_long(target);
	}
	//
	// specialization from_ZZ: T == unsigned int
	//
	template<typename T>
	SPRIG_INLINE T from_ZZ(
		NTL::ZZ const& target,
		typename boost::enable_if<
			boost::is_same<typename boost::remove_const<T>::type, unsigned int>
		>::type* = 0
		)
	{
		return NTL::to_uint(target);
	}
	//
	// specialization from_ZZ: is_unsigned(T) && T != unsigned int
	//
	template<typename T>
	SPRIG_INLINE T from_ZZ(
		NTL::ZZ const& target,
		typename boost::enable_if<
			boost::mpl::and_<
				boost::is_unsigned<T>,
				boost::mpl::not_<
					boost::is_same<typename boost::remove_const<T>::type, unsigned int>
				>
			>
		>::type* = 0
		)
	{
		return NTL::to_ulong(target);
	}
	//
	// specialization from_ZZ: T == float
	//
	template<typename T>
	SPRIG_INLINE T from_ZZ(
		NTL::ZZ const& target,
		typename boost::enable_if<
			boost::is_same<typename boost::remove_const<T>::type, float>
		>::type* = 0
		)
	{
		return NTL::to_float(target);
	}
	//
	// specialization from_ZZ: is_float(T) && T != float
	//
	template<typename T>
	SPRIG_INLINE T from_ZZ(
		NTL::ZZ const& target,
		typename boost::enable_if<
			boost::mpl::and_<
				boost::is_float<T>,
				boost::mpl::not_<
					boost::is_same<typename boost::remove_const<T>::type, float>
				>
			>
		>::type* = 0
		)
	{
		return NTL::to_double(target);
	}
}	// namespace sprig

#endif	// #ifndef SPRIG_NTL_FROM_ZZ_HPP


#pragma once
#include <boost/program_options.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <fstream>
#include <math.h>

/*
README
To avoid confusion in indexing:
  x goes along heigth
  y goes along width
*/

namespace utils {

  struct Options {
    // Command line arguments (with default values)
    unsigned num_ipus;
    unsigned height;
    unsigned width;
    unsigned num_iterations;
    float my1;
    float my2;
    float k;
    float epsilon;
    float b;
    float a;
    float h;
    float dt;
    float delta;
    bool cpu;
    bool save_cpu;
    // Other arguments which are a consequence of the code or environment
    std::string architecture; // Assigned when creating device
    unsigned num_tiles_available = 0;
    std::size_t halo_volume = 0;
    unsigned nh = 0;
    unsigned nw = 0;
    float upper_bound_dt;
    std::vector<std::size_t> smallest_slice = {std::numeric_limits<size_t>::max(),1};
    std::vector<std::size_t> largest_slice = {0,0};
  };

  inline
  Options parseOptions(int argc, char** argv) {
    Options options;
    namespace po = boost::program_options;
    po::options_description desc("Flags");
    // Construction of exactly cubic sub-grids
    desc.add_options()
    ("help", "Show command help.")
    (
      "num-ipus",
      po::value<unsigned>(&options.num_ipus)->default_value(1),
      "Number of IPUs to use."
    )
    (
      "height",
      po::value<unsigned>(&options.height)->default_value(7000),
      "Heigth of a custom 2D grid."
    )
    (
      "width",
      po::value<unsigned>(&options.width)->default_value(7000),
      "Width of a custom 2D grid."
    )
    (
      "num-iterations",
      po::value<unsigned>(&options.num_iterations)->default_value(1000),
      "PDE: number of iterations to execute on grid."
    )
    (
      "my1",
      po::value<float>(&options.my1)->default_value(0.07),
      "A constant in the forward Euler Aliev-Panfilov equations."
    )
    (
      "my2",
      po::value<float>(&options.my2)->default_value(0.3),
      "A constant in the forward Euler Aliev-Panfilov equations."
    )
    (
      "k",
      po::value<float>(&options.k)->default_value(8.0),
      "A constant in the forward Euler Aliev-Panfilov equations."
    )
    (
      "epsilon",
      po::value<float>(&options.epsilon)->default_value(0.01),
      "A constant in the forward Euler Aliev-Panfilov equations."
    )
    (
      "b",
      po::value<float>(&options.b)->default_value(0.1),
      "A constant in the forward Euler Aliev-Panfilov equations."
    )
    (
      "a",
      po::value<float>(&options.a)->default_value(0.1),
      "A constant in the forward Euler Aliev-Panfilov equations."
    )
    (
      "dt",
      po::value<float>(&options.dt)->default_value(0.0001),
      "A constant in the forward Euler Aliev-Panfilov equations."
    )
    (
      "h",
      po::value<float>(&options.h)->default_value(0.000143),
      "A constant in the forward Euler Aliev-Panfilov equations."
    )
    (
      "delta",
      po::value<float>(&options.delta)->default_value(5.0e-5),
      "A constant in the forward Euler Aliev-Panfilov equations."
    )
    (
      "cpu",
      po::bool_switch(&options.cpu)->default_value(false),
      "Also perform CPU execution to control results from IPU."
    )
    (
      "save-cpu",
      po::bool_switch(&options.save_cpu)->default_value(false),
      "Save CPU results to csv files in data folder."
    ); // NOTE: remember to remove this semicolon if more options are added in future
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n";
      throw std::runtime_error("Show help");
    }
    po::notify(vm);

    return options;
  }

} // End of namespace Utils

poplar::Device getDevice(unsigned numIpus) {
  /* return a Poplar device with the desired number of IPUs */
  auto manager = poplar::DeviceManager::createDeviceManager();
  auto devices = manager.getDevices(poplar::TargetType::IPU, numIpus);
  // Use the first available device
  for (auto &device : devices)
    if (device.attach())
      return std::move(device);

  throw std::runtime_error("No hardware device available.");
}

inline float randomFloat() {
  return static_cast <float> (rand() / static_cast <float> (RAND_MAX));
}

inline static unsigned index(unsigned x, unsigned y, unsigned width) {
  return y + (x*width);
}

inline static unsigned block_low(unsigned id, unsigned p, unsigned n) {
  return (id*n)/p;
}

inline static unsigned block_high(unsigned id, unsigned p, unsigned n) {
  return block_low(id+1, p, n);
}

inline static unsigned block_size(unsigned id, unsigned p, unsigned n) {
  return block_high(id, p, n) - block_low(id, p, n);
}

std::size_t area(std::vector<std::size_t> shape) {
  // return area of shape vector (2D)
  return shape[0]*shape[1];
}

void workDivision(utils::Options &options) {
  /* Function UPDATES options.nh and options.nw
   * nh and nw will be chosen so that
   * 1) all tiles will be used, hence options.num_tiles_available must
   *    previously be updated by using the target object
   * 2) find work division which results in most square patches (sub-grids)
   */
  unsigned tile_count = options.num_tiles_available;
  unsigned H = options.height - 2; // Actual height (boundaries wont be computed)
  unsigned W = options.width - 2; // Actual width
  float best_patch_ratio = std::numeric_limits<float>::infinity();
  bool height_dominant = (H >= W);

  // Try all unique combinations where i*j = tile_count
  for (unsigned i = 1; i*i <= tile_count; ++i) {
    if ((tile_count % i) == 0) {
      unsigned j = tile_count / i; // j >= i
      unsigned nh = height_dominant ? j : i;
      unsigned nw = height_dominant ? i : j;

      unsigned h = float(H) / nh;
      unsigned w = float(W) / nw;
      // largest side length / smallest side length
      // this will result in a ratio >= 1, where the ratio
      // which is closest to 1, has the most square patches (1=perfect square)
      float patch_ratio = (h > w) ? float(h)/w : float(w)/h;
      if (patch_ratio < best_patch_ratio) {
        best_patch_ratio = patch_ratio;
        options.nh = nh;
        options.nw = nw;
      }
    }
  }

  if (options.nw == 0 || options.nh == 0) {
    std::cout << "Work division went wrong. Using 1 tile.\n";
    options.nh = 1;
    options.nw = 1;
  }
}

void print2dVector(std::vector<float> grid, utils::Options &options, std::string name) {
  int h = options.height;
  int w = options.width;
  std::cout << name << ":\n";
  for (int i = 0; i < h; ++i) {
    if (i == 0) {
      std::cout << "[";
    } else {
      std::cout << " ";
    }
    for (int j = 0; j < w; ++j) {
      if (j == 0) std::cout << "[";
      std::cout << grid[j + i*w];
      if (j < w - 1) {
        std::cout << ", ";
      } else {
        std::cout << "]";
      }
    }
    if (i < h - 1) {
      std::cout << "\n";
    } else {
      std::cout << "]\n\n";
    }
  }
}

void save2dVector(std::vector<float> grid, utils::Options &options, std::string filename) {
  int h = options.height;
  int w = options.width;
  std::ofstream outfile;
  outfile.open(filename);
  for (int i = 0; i < h; ++i) {
    for (int j = 0; j < w; ++j) {
      outfile << grid[j + i*w];
      if (j < w - 1)
        outfile << ",";
    }
    if (i < h - 1) outfile << "\n";
  }
  outfile.close();
}

void solveAlievPanfilovCpu(
  const std::vector<float> initial_e, const std::vector<float> initial_r,
  std::vector<float> &cpu_e, std::vector<float> &cpu_r, utils::Options &options) {

  int N = initial_e.size();
  int w = options.width;
  int h = options.height;
  float rhs;
  std::vector<float> temp_e(N);
  for (int i = 0 ; i < N; ++i) {
    cpu_e[i] = initial_e[i];
    cpu_r[i] = initial_r[i];
    temp_e[i] = initial_e[i];
  }
  const float d_h2 = options.delta/(options.h*options.h);
  const float minus_epsilon = -options.epsilon;
  const float b_plus_one = options.b + 1;
  float west, north, east, south;
  int c = 0;

  for (int t = 0; t < options.num_iterations; ++t) {
    for (int i = 0; i < h; ++i) {
      for (int j = 0; j < w; ++j) {

        // Boundary condition (zero gradient)
        west = (j == 0) ? cpu_e[(j+1)+i*w] : cpu_e[(j-1)+i*w];
        east = (j == w - 1) ? cpu_e[(j-1)+i*w] : cpu_e[(j+1)+i*w];
        north = (i == 0) ? cpu_e[j+(i+1)*w] : cpu_e[j+(i-1)*w];
        south = (i == h - 1) ? cpu_e[j+(i-1)*w] : cpu_e[j+(i+1)*w];

        // Computation of new e
        rhs = d_h2*(-4*cpu_e[j+i*w] + west + east + south + north);
        rhs -= options.k*cpu_e[j+i*w]*(cpu_e[j+i*w] - options.a)*(cpu_e[j+i*w] - 1);
        rhs -= cpu_e[j+i*w]*cpu_r[j+i*w];
        temp_e[j+i*w] = cpu_e[j+i*w] + rhs*options.dt;

        // Computation of new r
        rhs = minus_epsilon - options.my1*cpu_r[j+i*w]/(options.my2 + cpu_e[j+i*w]);
        rhs *= cpu_r[j+i*w] + options.k*cpu_e[j+i*w]*(cpu_e[j+i*w] - b_plus_one);
        cpu_r[j+i*w] += rhs*options.dt;
      }
    }
    for (int i = 0; i < h; ++i) {
      for (int j = 0; j < w; ++j) {
        cpu_e[j+i*w] = temp_e[j+i*w];
      }
    }
    if (t % 500 == 0 && options.save_cpu) {
      save2dVector(cpu_e, options, "./data/e"+std::to_string(c)+".csv");
      save2dVector(cpu_r, options, "./data/r"+std::to_string(c)+".csv");
      c++; // nice
    }
  }
}

void printGeneralInfo(utils::Options &options) {
  std::cout
    << "\nAliev-Panfilov Forward Euler Method"
    << "\n-----------------------------------"
    << "\n\nProblem"
    << "\n-------"
    << "\n2D Grids = " << options.height << "x" << options.width << " elements"
    << "\nWork Division = " << options.nh << "x" << options.nw << " partitions"
    << "\nNo. Time Steps = " << options.num_iterations
    << "\n\nConstants"
    << "\n---------"
    << "\ndelta = " << options.delta
    << "\nmy1 = " << options.my1
    << "\nmy2 = " << options.my2
    << "\na = " << options.a
    << "\nb = " << options.b
    << "\nk = " << options.k
    << "\ndx = " << options.h
    << "\ndy = " << options.h
    << "\ndt = " << options.dt << " (upper bound = " << options.upper_bound_dt << ")"
    << "\nepsilon = " << options.epsilon
    << "\n\n";
}

void testUpperBoundDt(utils::Options &options) {
  float ka = options.k*options.a;
  float k_1_a = options.k*(1 - options.a);
  float max = (ka > k_1_a) ? ka : k_1_a;
  float lambda = options.delta/(options.h*options.h);
  float r_plus = options.k*(options.b+1)*(options.b+1)/4.0;
  options.upper_bound_dt = 1.0/(4*lambda + max + r_plus);
  if (options.dt > options.upper_bound_dt)
    throw std::runtime_error(
      "Forward Euler method is not stable, because dt ("+std::to_string(options.dt)+
      ") > upper bound ("+std::to_string(options.upper_bound_dt)+")."
    );
}

void reportCpuVsIpu(std::vector<float> cpu_e, std::vector<float> cpu_r,
  std::vector<float> ipu_e, std::vector<float> ipu_r, utils::Options &options) {

  std::size_t w = options.width;
  std::size_t h = options.height;
  float MSE_e = 0;
  float MSE_r = 0;
  for (int i = 0; i < options.height; ++i) {
    for (int j = 0; j < options.width; ++j) {
      float diff_e = cpu_e[j + i*w] - ipu_e[j + i*w];
      float diff_r = cpu_r[j + i*w] - ipu_r[j + i*w];
      MSE_e += diff_e*diff_e;
      MSE_r += diff_r*diff_r;
    }
  }
  MSE_e /= w*h;
  MSE_r /= w*h;
  std::cout
    << "\nAliev-Panfilov IPU vs. CPU error"
    << "\n--------------------------------"
    << "\nMSE of e = " << MSE_e;
  if (MSE_e == 0) std::cout << " (exactly)";
  std::cout << "\nMSE of r = " << MSE_r;
  if (MSE_r == 0) std::cout << " (exactly)";
  std::cout << "\n\n";
}

void printPerformance(double wall_time, utils::Options &options) {
  double flops_per_element = 28.0;
  double elements_per_time = (double) options.height * (double) options.width * (double) options.num_iterations / (double) wall_time;
  double flops = flops_per_element * elements_per_time;
  double comp_mem_bw = 8*elements_per_time*sizeof(float); // 6 loads (5-point stencil for e, and r) + 2 stores (e and r)
  double comm_mem_bw = 2.0*(double)options.halo_volume*sizeof(float)*(double) options.num_iterations / (double) wall_time;
  double minimal_bw = comp_mem_bw + comm_mem_bw;
  std::cout
    << "\nPerformance"
    << "\n-----------" << std::fixed
    << "\nTime       = " << std::setprecision(2) << wall_time << " s"
    << "\nThroughput = " << std::setprecision(2) << flops*1e-12 << " TFLOPS"
    << "\nMinimal BW = " << std::setprecision(2) << minimal_bw*1e-12 << " TB/s"
    << "\n\n";
}


#ifndef ALEPH_TOPOLOGY_SIMPLEX_HH__
#define ALEPH_TOPOLOGY_SIMPLEX_HH__

#include <boost/functional/hash.hpp>

#include <boost/iterator/iterator_adaptor.hpp>
#include <boost/iterator/filter_iterator.hpp>

#include <algorithm>
#include <initializer_list>
#include <iosfwd>
#include <stdexcept>
#include <vector>

namespace aleph
{

namespace topology
{

/**
  @class Simplex

  This class describes an abstract simplex of arbitrary dimensionality
  with an optional *weight* or *data* value. This makes it possible to
  use the class together with persistent homology calculations.

  @see SimplicialComplex

  @tparam D Data (weight) type, e.g. `double`
  @tparam V Vertex type; usually, you do not have to change this type,
            except if you want to change the memory footprint.
*/

template <
  class D,
  class V = unsigned short
>
class Simplex
{
public:

  // Aliases & declarations -------------------------------------------
  //
  // Note that these aliases follow the STL conventions in order to make it
  // easier to use the class with STL algorithms.

  using DataType                      = D;          ///< Data type alias
  using VertexType                    = V;          ///< Vertex type alias

  using data_type                     = DataType;   ///< Data type alias, STL-style
  using vertex_type                   = VertexType; ///< Vertex type alias, STL-style

  using vertex_container_type         = std::vector<vertex_type>;
  using vertex_iterator               = typename vertex_container_type::iterator;
  using const_vertex_iterator         = typename vertex_container_type::const_iterator;
  using reverse_vertex_iterator       = typename vertex_container_type::reverse_iterator;
  using const_reverse_vertex_iterator = typename vertex_container_type::const_reverse_iterator;

  // I cannot describe the class inline because boost::iterator_adaptor expects
  // a _complete_ class.
  class boundary_iterator;

  // Constructors ------------------------------------------------------

  /** Creates an empty simplex */
  Simplex()
    : _data( DataType() )
  {
  }

  /**
    Creates a new 0-simplex from the given vertex.

    @param u    Vertex
    @param data Data to assign to simplex
  */

  Simplex( VertexType u, DataType data = DataType() )
    : _vertices( 1, u )
    , _data( data )
  {
  }

  /**
    Creates a new simplex from another simplex while setting the data for the
    new simplex. This is handy for copying the vertices but not the data of a
    given simplex, which occurs in cases where the data for the new simplex is
    \i not determined by the simplex that is to be copied.

    @param simplex Simplex to copy vertices from
    @param data    Data to assign new simplex
  */

  explicit Simplex( const Simplex<DataType, VertexType>& simplex, DataType data )
    : _vertices( simplex._vertices )
    , _data( data )
  {
  }

  /**
    Creates a new simplex from a range of vertices. The input iterators are
    supposed to belong to a range of vertices for the simplex. This range need
    not be ordered. Note that the simplex will \b not check for duplicate
    values.

    @param begin Iterator to begin of vertex range
    @param end   Iterator to end of vertex range
    @param data  Data to assign to simplex
  */

  template <class InputIterator>
  Simplex( InputIterator begin, InputIterator end,
           DataType data = DataType() )
    : _vertices( begin, end )
    , _data( data )
  {
    std::sort( _vertices.begin(), _vertices.end(), std::greater<VertexType>() );

    // Ensures that the simplex does not contain the same vertex
    // multiple times. This may result in the empty simplex, but
    // there is no way around that.
    _vertices.erase(
      std::unique( _vertices.begin(), _vertices.end() ),
      _vertices.end()
    );
  }

  /**
    Creates a new simplex from a range of vertices. The vertices are not
    assumed to be ordered. It must be possible for me to convert them to
    the vertex type of the simplex.

    @param vertices Vertices
    @param data     Data to assign to simplex
  */

  template <class Vertex>
  Simplex( const std::initializer_list<Vertex>& vertices,
           DataType data = DataType() )
    : Simplex( vertices.begin(), vertices.end(), data )
  {
  }

  // vertices ----------------------------------------------------------

  /** @returns Iterator to begin of simplex vertex range */
  const_vertex_iterator begin() const
  {
    return _vertices.begin();
  }

  /** @returns Iterator to end of simplex vertex range */
  const_vertex_iterator end() const
  {
    return _vertices.end();
  }

  /** @returns Reverse begin iterator of simplex vertex range */
  const_reverse_vertex_iterator rbegin() const
  {
    return _vertices.rbegin();
  }

  /** @returns Reverse end iterator of simplex vertex range */
  const_reverse_vertex_iterator rend() const
  {
    return _vertices.rend();
  }

  /**
    Checks whether the current simplex contains a given vertex. This is
    required for intersection queries, for example.

    @param vertex Vertex to search for

    @returns true if the simplex contains the current vertex at least once,
    else false.
  */

  bool contains( VertexType vertex ) const
  {
    return std::find( this->begin(), this->end(), vertex ) != this->end();
  }

  // boundary ----------------------------------------------------------

  /** @returns Boundary iterator to begin of boundary */
  boundary_iterator begin_boundary() const
  {
    // Check dimension directly in order to handle the empty simplex
    if( _vertices.empty() || _vertices.size() <= 1 )
      return this->end_boundary();

    return boundary_iterator( _vertices.begin(), _vertices);
  }

  /** @returns Boundary iterator to end of boundary */
  boundary_iterator end_boundary() const
  {
    return boundary_iterator( _vertices.end(), _vertices);
  }

  // Data --------------------------------------------------------------

  /**
    Assigns the simplex a new value for its data object. This function does not
    perform any sanity checks. The value is simply copied and stored.

    @param data Data to assign
  */

  void setData( DataType data = DataType() )
  {
    _data = data;
  }

  /** @returns Current value of simplex data object */
  DataType data() const
  {
    return _data;
  }

  // Attribute access --------------------------------------------------

  /** @returns true if the simplex is empty, i.e. it has no vertices */
  bool empty() const
  {
    return _vertices.empty();
  }

  /**
    @returns true if the simplex is valid, i.e. it is not empty. This function
    allows the simplex class to be used in expressions such as this one:

    @code
    Simplex<double> mySimplex( 2, 2.0 ); // 0-simplex
    if( mySimplex )
    {
      // Do stuff with a valid simplex.
    }
    @endcode
  */

  explicit operator bool() const
  {
    // A bit verbose but more readable :)
    return this->empty() ? false : true;
  }

  /**
    @returns Dimension of simplex

    @throws std::runtime_error if the dimension of the empty simplex is
    queried. If you do this, it's your own fault.
  */

  std::size_t dimension() const
  {
    if( _vertices.empty() )
      throw std::runtime_error( "Querying dimension of empty simplex" );
    else
      return _vertices.size() - 1;
  }

  /** @returns Number of vertices of the simplex */
  std::size_t size() const
  {
    return _vertices.size();
  }

  /**
    Returns a vertex (specified by an index) of the current simplex.

    @param   index Index of vertex in simplex
    @returns Vertex of simplex, specified by an index.
    @throws  std::out_of_range if the index is out of range.
  */

  VertexType operator[]( std::size_t index ) const
  {
    return _vertices.at( index );
  }

  // Comparison operators ----------------------------------------------

  /**
    Checks whether two simplices are equal. Two simplices are considered equal
    if their vertices are being equal. Simplex data is \b not checked by this
    function as this would complicate simplex queries in a simplicial complex.

    @param other Simplex to check for equality
    @returns true if the two simplices are equal (see above), else false.
  */

  bool operator==( const Simplex& other ) const
  {
    return this->_vertices == other._vertices;
  }

  /**
    Checks whether two simplices are inequal. This function is simply the
    negation of operator==.

    @param other Simplex to check for inequality
    @returns true if the two simplices differ, else false.
  */

  bool operator!=( const Simplex& other ) const
  {
    return !this->operator==( other );
  }

  /**
    Comparison operator for simplices. Uses lexicographical comparison to
    obtain a weak ordering of the simplices. This is used in many algorithms.

    @param other Simplex to compare current simplex to
    @returns  true if current simplex is to be sorted before other simplex.
  */

  bool operator<( const Simplex& other ) const
  {
    return std::lexicographical_compare( this->_vertices.begin(), this->_vertices.end(),
                                         other._vertices.begin(), other._vertices.end() );
  }

  // Convenience functions ---------------------------------------------

  template <class DataType>                   friend std::size_t hash_value( const Simplex<DataType>& s );
  template <class DataType, class VertexType> friend std::size_t hash_value( const Simplex<DataType, VertexType>& s );

private:

  /**
    The vertices making up the current simplex. Their values are irrelevant,
    though it is assumed that the vertices represent some data points stored
    _outside_ the simplex.
  */

  vertex_container_type _vertices;

  /**
    Data stored within the simplex. The type and semantics of this member
    variable depend on the template parameters of the simplex class. For
    example, a \c double value is used at several places in order to add a \i
    weight or a \i distance for the simplex.
  */

  DataType _data;
};

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

template <class DataType, class VertexType>
std::size_t hash_value( const Simplex<DataType, VertexType>& s )
{
  boost::hash< typename Simplex<DataType, VertexType>::vertex_container_type > hasher;
  return hasher( s._vertices );
}

template <class DataType> std::size_t hash_value( const Simplex<DataType>& s )
{
  return hash_value<DataType, typename Simplex<DataType>::vertex_type>( s );
}

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

/**
  @class boundary_iterator
  @brief Iterator for traversing the boundary of a given simplex

  This iterator, inspired by Dmitriy Morozov's "Dionysus" framework, calculates
  the boundary of a given simplex while traversing it. Since the boundary
  simplices are created from scratch, they will _not_ have the correct weights
  set. Hence, the boundary iterator is only useful if some kind of lookup of
  generated simplices exists---as is the case for a simplicial complex, for
  example.
*/

template <
    class DataType,
    class VertexType
>
class Simplex<DataType, VertexType>::boundary_iterator
  : public boost::iterator_adaptor<boundary_iterator,
                                   const_vertex_iterator,
                                   Simplex<DataType, VertexType>,
                                   boost::use_default,
                                   Simplex<DataType, VertexType> >
{
public:

  using Iterator = const_vertex_iterator ;
  using Parent   = boost::iterator_adaptor<boundary_iterator,
                                           Iterator,
                                           Simplex<DataType, VertexType>,
                                           boost::use_default,
                                           Simplex<DataType, VertexType> >;

  /**
    Creates a new boundary iterator from a parent iterator (i.e. a simplex) and a
    set of vertices (i.e. the vertices of the parent simplex).

    @param it       Parent iterator
    @param vertices VertexType set
  */

  explicit boundary_iterator( Iterator it, const vertex_container_type& vertices )
    : Parent(it)
    , _vertices(vertices)
  {
  }

private:

  friend class boost::iterator_core_access;

  /** @returns Current boundary simplex */
  Simplex<DataType, VertexType> dereference() const
  {
    // This returns a new simplex. The simplex is created from a set of
    // vertices, which in turn is created by applying a filter to the set of
    // vertices stored in this iterator: Namely, the filter iterator will
    // return all vertices that are _not_ equal to its current position.

    using std::placeholders::_1;

    vertex_container_type vertices(
          boost::make_filter_iterator( std::bind( std::not_equal_to<vertex_type>(), _1, *( this->base() ) ),
                                                     _vertices.begin(),
                                                     _vertices.end() ),
          boost::make_filter_iterator( std::bind( std::not_equal_to<vertex_type>(), _1, *( this->base() ) ),
                                                     _vertices.end(),
                                                     _vertices.end() )
          );

    return Simplex<DataType, VertexType>( vertices.begin(), vertices.end() );
  }

  /**
    Reference to vertex set; this is required because the boundary iterator
    iterates over a set of vertices and may thus not exist without one.
  */

  const vertex_container_type& _vertices;
};

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

/**
  Outputs a simplex to an ostream. This is used for debugging purposes.

  @param o Output stream
  @param s Simplex to be added to o

  @returns Output stream with information about simplex s.
*/

template <class DataType, class VertexType>
std::ostream& operator<<( std::ostream& o, const topology::Simplex<DataType, VertexType>& s )
{
  auto numVertices = s.size();

  o << "{";

  for( decltype(numVertices) i = 0; i < numVertices; i++ )
  {
    if( i != 0 )
      o << " ";

    o << s[i];
  }

  if( s.data() != DataType() )
    o << " (" << s.data() << ")";

  o << "}";

  return o;
}

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

} // namespace topology

} // namespace aleph

namespace std
{

/**
  This specialization permits using simplices in std::unordered_map and
  std::unordered_set. This struct wraps the hash value function defined
  above.
*/

template<class DataType, class VertexType> struct hash<aleph::topology::Simplex<DataType, VertexType> >
{
  using argument_type = aleph::topology::Simplex<DataType, VertexType>;
  using result_type   = std::size_t;

  result_type operator()( const argument_type& simplex ) const noexcept
  {
    return aleph::topology::hash_value( simplex );
  }
};

} // namespace std

#endif