#pragma once

#include <crab/common/types.hpp>

#include <forward_list>
#include <map>
#include <memory>
#include <vector>

#include <boost/container/flat_set.hpp>
#include <boost/optional.hpp>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-compare"

namespace crab {

namespace domains {

namespace term {
typedef int term_id;
typedef int var_id;

enum term_kind { TERM_CONST, TERM_VAR, TERM_APP };

template <class Num, class Ftor> class term {
public:
  typedef term<Num, Ftor> term_t;
  typedef std::shared_ptr<term_t> term_ptr;

  virtual ~term() {}

  virtual term_kind kind(void) = 0;

  bool operator<(term_t &other);

  virtual void write(crab_os &o) const = 0;

  int depth;
};

template <class Num, class Ftor>
inline crab::crab_os &operator<<(crab::crab_os &o, const term<Num, Ftor> &t) {
  t.write(o);
  return o;
}

template <class Num, class Ftor> class term_ref {
public:
  typedef term<Num, Ftor> term_t;
  typedef std::shared_ptr<term_t> term_ptr;

  term_ref(term_ptr _p) : p(_p) {}

  term_ref(term_t *_p) : p(_p) {}

  bool operator<(const term_ref &other) const { return *p < *(other.p); }

  term_ptr p;
};

// Dispatch for different kinds of terms.
template <class Num, class Ftor> class const_term : public term<Num, Ftor> {
public:
  const_term(Num _val) : val(_val) {}
  term_kind kind(void) { return TERM_CONST; }

  void write(crab_os &o) const { o << "c(" << val << ")"; }
  Num val;
};

template <class Num, class Ftor> class var_term : public term<Num, Ftor> {
public:
  var_term(var_id _var) : var(_var) {}
  term_kind kind(void) { return TERM_VAR; }
  void write(crab_os &o) const { o << "v(" << var << ")"; }
  var_id var;
};

template <class Num, class Ftor> class ftor_term : public term<Num, Ftor> {
public:
  ftor_term(Ftor _f, std::vector<term_id> &_args) : ftor(_f), args(_args) {}
  term_kind kind(void) { return TERM_APP; }
  void write(crab_os &o) const {
    // o << "<op>(";
    o << ftor << "(";
    bool first = true;
    for (term_id c : args) {
      if (first)
        first = false;
      else
        o << ", ";
      o << c;
    }
    o << ")";
  }

  Ftor ftor;
  std::vector<term_id> args;
};

// These functions are super-unsafe.
template <class Num, class Ftor> var_id term_var(term<Num, Ftor> *term) {
  return static_cast<var_term<Num, Ftor> *>(term)->var;
}
template <class Num, class Ftor> Num term_const(term<Num, Ftor> *term) {
  return static_cast<const_term<Num, Ftor> *>(term)->val;
}

template <class Num, class Ftor> Ftor term_ftor(term<Num, Ftor> *term) {
  return static_cast<ftor_term<Num, Ftor> *>(term)->ftor;
}

template <class Num, class Ftor>
std::vector<term_id> &term_args(term<Num, Ftor> *term) {
  return static_cast<ftor_term<Num, Ftor> *>(term)->args;
}

// Term ordering
template <class Num, class Ftor>
bool term<Num, Ftor>::operator<(term_t &other) {
  if (kind() != other.kind())
    return kind() < other.kind();

  switch (kind()) {
  case TERM_CONST: {
    return term_const(this) < term_const(&other);
  } break;
  case TERM_VAR: {
    return term_var(this) < term_var(&other);
  }
  case TERM_APP: {
    if (term_ftor(this) != term_ftor(&other))
      return term_ftor(this) < term_ftor(&other);

    std::vector<int> &xargs(term_args(this));
    std::vector<int> &yargs(term_args(&other));

    if (xargs.size() != yargs.size())
      return xargs.size() < yargs.size();

    for (unsigned int ii = 0; ii < xargs.size(); ii++) {
      if (xargs[ii] != yargs[ii])
        return xargs[ii] < yargs[ii];
    }
    return false;
  }
  default:
    CRAB_ERROR("Unsupported term kind.");
  }
}

template <typename TermTable> class congruence_closure_solver;

template <class Num, class Ftor> class term_table {
public:
  typedef term_table<Num, Ftor> term_table_t;
  typedef term<Num, Ftor> term_t;
  typedef std::shared_ptr<term_t> term_ptr;
  typedef term_ref<Num, Ftor> term_ref_t;
  typedef term_id term_id_t;

  typedef const_term<Num, Ftor> const_term_t;
  typedef var_term<Num, Ftor> var_term_t;
  typedef ftor_term<Num, Ftor> ftor_term_t;

  // For establishing a mapping between tables
  typedef std::map<term_id, term_id> term_map_t;
  typedef std::map<std::pair<term_id, term_id>, term_id> gener_map_t;

  term_table(void) : free_var(0) {}

  term_table(const term_table_t &o)
      : free_var(o.free_var), _map(o._map), terms(o.terms),
        _parents(o._parents), _ref_count(o._ref_count), _depth(o._depth),
        free_terms(o.free_terms) {}

  term_table_t &operator=(const term_table_t &o) {
    free_var = o.free_var;
    _map = o._map;
    terms = o.terms;
    _ref_count = o._ref_count;
    _parents = o._parents;
    _depth = o._depth;
    free_terms = o.free_terms;
    return *this;
  }

  term_id make_const(const Num &n) {
    term_ref_t ref(new const_term_t(n));
    return add_term(ref);
  };
  boost::optional<term_id> find_const(Num &n) {
    term_ref_t ref(new const_term_t(n));
    return find_term(ref);
  }

  term_id make_var(var_id &n) {
    free_var = std::max(free_var, n + 1);
    term_ref_t ref(new var_term_t(n));
    return add_term(ref);
  };

  term_id fresh_var(void) {
    var_id v = free_var++;
    return make_var(v);
  }

  template <typename T, typename... Types>
  void collect_args(std::vector<T> &vec, T &x, Types... rest) {
    vec.push_back(x);
    collect_args(vec, rest...);
  }

  template <typename T> void collect_args(std::vector<T> &vec) {}

  term_id apply_ftor(const Ftor &f, std::vector<term_id> &ids) {
    term_ref_t ref(new ftor_term_t(f, ids));
    return add_term(ref);
  }

  template <typename... Types>
  term_id apply_ftor(const Ftor &f, Types... args) {
    std::vector<term_id> ids;
    collect_args(ids, args...);
    return apply_ftor(f, ids);
  }

  boost::optional<term_id> find_ftor(Ftor &f, std::vector<term_id> &ids) {
    term_ref_t ref(new ftor_term_t(f, ids));
    return find_term(ref);
  }

  template <typename... Types>
  boost::optional<term_id> find_ftor(Ftor &f, Types... args) {
    std::vector<term_id> ids;
    collect_args(ids, args...);
    return find_ftor(f, ids);
  }

  term_t *get_term_ptr(term_id t) { return terms[t].p.get(); }

  void add_ref(term_id t) { _ref_count[t]++; }

  void deref(term_id t, std::vector<term_id> &forgotten) {
    assert(_ref_count[t]);
    _ref_count[t]--;
    if (!_ref_count[t]) {
      forgotten.push_back(t);
      free_terms.push_back(t);
      term_ref_t ref(terms[t]);
      _map.erase(ref);
      if (ref.p.get()->kind() == TERM_APP) {
        for (term_id c : term_args(ref.p.get())) {
          // remove t as parent of c before going down child
          auto &c_parents = parents(c);
          c_parents.remove(t);
          deref(c, forgotten);
        }
      }
    }
  }

  // Check if a tx is a generalization of ty, given an existing context.
  bool map_leq(term_table_t &y, term_id tx, term_id ty, term_map_t &map) {
    auto it = map.find(ty);
    if (it != map.end())
      return (*it).second == tx;

    term_ref_t ry(y.terms[ty]);
    term_ref_t rx(terms[tx]);
    switch (ry.p.get()->kind()) {
    case TERM_CONST: {
      if (rx.p.get()->kind() != TERM_CONST ||
          term_const(rx.p.get()) != term_const(ry.p.get()))
        return false;
    } break;
    case TERM_APP: {
      if (rx.p.get()->kind() != TERM_APP ||
          term_ftor(rx.p.get()) != term_ftor(ry.p.get()))
        return false;

      std::vector<int> &xargs(term_args(rx.p.get()));
      std::vector<int> &yargs(term_args(ry.p.get()));

      if (xargs.size() != yargs.size())
        return false;

      for (unsigned int ii = 0; ii < xargs.size(); ii++) {
        if (!map_leq(y, xargs[ii], yargs[ii], map))
          return false;
      }
    } break;
    case TERM_VAR:
      break;
    }
    map[ty] = tx;
    return true;
  }

  term_id_t generalize(term_table_t &y, term_id tx, term_id ty,
                       term_table_t &out, gener_map_t &g_map) {
    auto txy = std::make_pair(tx, ty);
    auto it = g_map.find(txy);
    if (it != g_map.end()) {
      return (*it).second;
    } else {
      // Haven't found this pair yet.
      // Generalize the arguments.
      auto px(terms[tx].p.get());
      auto py(y.terms[ty].p.get());

      term_kind kx = px->kind();
      term_kind ky = py->kind();

      // If either is a variable, we just create a fresh variable
      term_id_t ret;
      do {
        if (kx == TERM_VAR || ky == TERM_VAR || kx != ky) {
          ret = out.fresh_var();
          break;
        }

        if (kx == TERM_CONST) {
          if (term_const(px) != term_const(py)) {
            ret = out.fresh_var();
          } else {
            ret = out.make_const(term_const(px));
          }
        } else {
          assert(kx == TERM_APP);
          if (term_ftor(px) != term_ftor(py)) {
            ret = out.fresh_var();
            break;
          }

          std::vector<term_id> &xargs(term_args(px));
          std::vector<term_id> &yargs(term_args(py));
          if (xargs.size() != yargs.size()) {
            ret = out.fresh_var();
            break;
          }

          std::vector<term_id> xyargs;
          for (unsigned int ii = 0; ii < xargs.size(); ii++)
            xyargs.push_back(generalize(y, xargs[ii], yargs[ii], out, g_map));
          ret = out.apply_ftor(term_ftor(px), xyargs);
        }
      } while (0);

      g_map[txy] = ret;
      return ret;
    }
  }

  // Copy term x from table tx
  term_id_t copy_term(term_table_t &tx, term_id x, term_map_t &ren_map) {
    auto it = ren_map.find(x);
    if (it != ren_map.end()) {
      return (*it).second;
    } else {
      auto px(tx.terms[x].p.get());
      term_kind kx = px->kind();
      term_id_t ret;
      if (kx == TERM_VAR) {
        ret = fresh_var();
      } else if (kx == TERM_CONST) {
        ret = make_const(term_const(px));
      } else {
        assert(kx == TERM_APP);
        std::vector<term_id> &xargs(term_args(px));
        std::vector<term_id> yargs;
        for (unsigned int ii = 0; ii < xargs.size(); ii++)
          yargs.push_back(copy_term(tx, xargs[ii], ren_map));
        ret = apply_ftor(term_ftor(px), yargs);
      }

      ren_map[x] = ret;
      return ret;
    }
  }

  std::forward_list<term_id_t> &parents(term_id_t id) { return _parents[id]; }

  void write(crab_os &o) const {
    bool first = true;
    for (unsigned int ti = 0; ti < terms.size(); ti++) {
      if (first)
        first = false;
      else
        o << ", ";
      term_t *p(terms[ti].p.get());
      o << ti << " -> " << *p;
    }
  }

  int size() { return terms.size(); }

  int depth(term_id t) { return _depth[t]; }

protected:
  boost::optional<term_id> find_term(term_ref_t ref) {
    auto it = _map.find(ref);
    if (it != _map.end())
      return boost::optional<term_id>(it->second);
    else
      return boost::optional<term_id>();
  }

  // When we know that a germ doesn't already exist.
  term_id fresh_term(term_ref_t ref) {
    if (free_terms.size() > 0) {
      term_id t = free_terms.back();
      free_terms.pop_back();
      terms[t] = ref;
      _parents[t].clear();
      _depth[t] = 0;
      _ref_count[t] = 0;
      return t;
    } else {
      term_id t = terms.size();
      terms.push_back(ref);
      _ref_count.push_back(0);
      _parents.push_back(std::forward_list<term_id>());
      _depth.push_back(0);
      return t;
    }
  }

  term_id add_term(term_ref_t ref) {
    auto it = _map.find(ref);
    if (it != _map.end()) {
      return (*it).second;
    } else {
      term_id id = fresh_term(ref);
      _map[ref] = id;
      if (ref.p.get()->kind() == TERM_APP) {
        unsigned int c_depth = 0;
        for (term_id c : term_args(ref.p.get())) {
          assert(c < _ref_count.size());
          _ref_count[c] += 1;
          // do not keep order between parents
          _parents[c].push_front(id);
          c_depth = std::max(c_depth, _depth[c]);
        }
        _depth[id] = 1 + c_depth;
      }
      /* Not true, as we're garbage collecting terms */
      // assert(_map.size() == id+1);
      return id;
    }
  }

  int free_var;
  std::map<term_ref_t, term_id> _map;
  std::vector<term_ref_t> terms;
  std::vector<std::forward_list<term_id_t>> _parents;
  std::vector<unsigned int> _ref_count;
  std::vector<unsigned int> _depth;
  std::vector<term_id> free_terms;
};

template <class Num, class Ftor>
inline crab::crab_os &operator<<(crab::crab_os &o,
                                 const term_table<Num, Ftor> &t) {
  t.write(o);
  return o;
}

template <typename TermTable> class congruence_closure_solver {

  typedef congruence_closure_solver<TermTable> this_type;

public:
  typedef typename TermTable::term_id_t term_id_t;
  typedef typename TermTable::term_t term_t;
  typedef boost::container::flat_set<term_id_t> term_set_t;
  typedef std::map<term_id_t, term_set_t> ccpar_map_t;
  typedef std::map<term_id_t, term_id_t> find_map_t;
  typedef std::pair<term_id_t, term_id_t> equation_t;
  typedef std::map<term_id_t, std::vector<term_id_t>> member_map_t;

private:
  TermTable *_ttbl;
  ccpar_map_t _ccpar_map;
  find_map_t _find_map;
  std::vector<equation_t> _eqs;
  term_set_t _terms;
  member_map_t _members;

public:
  congruence_closure_solver(TermTable *ttbl) : _ttbl(ttbl) {}

  congruence_closure_solver(const this_type &o) = delete;

  this_type &operator=(const this_type &o) = delete;

  void operator+=(equation_t eq) {
    _terms.insert(eq.first);
    _terms.insert(eq.second);
    _eqs.push_back(eq);
  }

  void run() {
    for (auto eq : _eqs) {
      term_id_t x = eq.first;
      term_id_t y = eq.second;
      merge(x, y);
    }
  }

  void run(std::vector<equation_t> &eqs) {
    for (auto e : eqs) {
      *this += e;
    }
    run();
  }

  // return the equivalence class associated with t
  term_id_t get_class(term_id_t t) { return find(t); }

  // return the members of an equivalence class of k
  std::vector<term_id_t> &get_members(term_id_t t) {
    auto it = _members.find(t);
    if (it != _members.end()) {
      return it->second;
    } else {
      t = find(t); // find representative of the t's class
      std::vector<term_id_t> members;
      for (auto x : _terms) {
        if (find(x) == t /*find (t)*/)
          members.push_back(x);
      }
      auto res = _members.insert(std::make_pair(t, members));
      return (res.first)->second;
    }
  }

  // return the size of the equivalence class of k
  size_t get_size(term_id_t t) { return get_members(t).size(); }

  void write(crab_os &o) const {
    for (auto t1 : _terms) {
      o << "t" << t1 << " --> {";
      for (auto t2 : _terms) {
        if (find(t1) == find(t2))
          o << "t" << t2 << ";";
      }
      o << "}\n";
    }
  }

  std::forward_list<term_id_t> &get_parents(term_id_t t) {
    return _ttbl->parents(t);
  }

private:
  term_set_t &get_ccpar(term_id_t t) {
    auto it = _ccpar_map.find(t);
    if (it == _ccpar_map.end()) {
      auto res = _ccpar_map.insert(std::make_pair(t, term_set_t()));
      return (res.first)->second;
    } else {
      return it->second;
    }
  }

  // return the representative of i
  term_id_t find(term_id_t t) {
    auto it = _find_map.find(t);
    if (it == _find_map.end()) {
      _find_map.insert(std::make_pair(t, t));
      return t;
    }

    if (it->second == t) {
      return it->second;
    } else {
      return find(it->second);
    }
  }

  // i2 is the new representative
  void do_union(term_id_t t1, term_id_t t2) {
    _find_map[t1] = _find_map[t2];

    term_set_t &ccpar1 = get_ccpar(t1);
    term_set_t &ccpar2 = get_ccpar(t2);
    ccpar2.insert(ccpar1.begin(), ccpar1.end());

    term_t *t1_ptr = _ttbl->get_term_ptr(t1);
    assert(t1_ptr);
    if (t1_ptr->kind() == TERM_APP) {
      auto &t1_parents = get_parents(t1);
      ccpar2.insert(t1_parents.begin(), t1_parents.end());
    }

    term_t *t2_ptr = _ttbl->get_term_ptr(t2);
    assert(t2_ptr);
    if (t2_ptr->kind() == TERM_APP) {
      auto &t2_parents = get_parents(t2);
      ccpar2.insert(t2_parents.begin(), t2_parents.end());
    }
    ccpar1.clear();
  }

  // pre: t1 and t2 are TERM_APP
  bool is_congruent(term_id_t t1, term_id_t t2) {

    term_t *t1_ptr = _ttbl->get_term_ptr(t1);
    term_t *t2_ptr = _ttbl->get_term_ptr(t2);

    assert(t1_ptr->kind() == TERM_APP);
    assert(t2_ptr->kind() == TERM_APP);

    if (term_ftor(t1_ptr) != term_ftor(t2_ptr))
      return false;

    std::vector<term_id_t> &xargs(term_args(t1_ptr));
    std::vector<term_id_t> &yargs(term_args(t2_ptr));

    if (xargs.size() != yargs.size())
      return false;

    for (unsigned i = 0; i < xargs.size(); i++) {
      if (find(xargs[i]) != find(yargs[i]))
        return false;
    }
    return true;
  }

  void merge(term_id_t t1, term_id_t t2) {

    if (find(t1) == find(t2))
      return;

    do_union(t1, t2);
    auto &ps1 = get_ccpar(t1);
    auto &ps2 = get_ccpar(t2);
    for (auto p1 : ps1) {
      for (auto p2 : ps2) {
        if ((find(p1) != find(p2)) && is_congruent(p1, p2))
          merge(p1, p2);
      }
    }
  }
}; // end congruence_closure_solver

} // end namespace term
} // namespace domains
} // end namespace crab
#pragma GCC diagnostic pop


//==================================================================================================
/*!
  @file

  @copyright 2012-2015 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generates a value of the chosen type which all bits are set to 1.

    @par Semantic:

    @code
    T r = Allbits<T>();
    @endcode

    is equivalent to:

    - if T is floating point:
      @code
      T r =  @ref Nan<T>();
      @endcode

    - otherwise:
      @code
      T r = T(~0);
      @endcode

    @return A value of type @c T with all its bits set to 1

    @see functional::allbits
  **/
  template<typename T> T Allbits();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generates a value of the chosen type which all bits are set to 1.

      @par Semantic:

      For any value @c x of type @c T:
      @code
      T r = simd::functional::allbits( boost::simd::as(x));
      @endcode

      is equivalent to:

      @code
      T r = simd::Allbits<T>();
      @endcode

      @return A value of type @c T with all its bits set to 1

      @see Allbits
    **/
    const boost::dispatch::functor<tag::allbits_> allbits = {};
  }
} }
#endif

#include <boost/simd/constant/definition/allbits.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


#include "tpch_kit.hpp"
#include "common.hpp"

#include <fstream>
#include <cassert>
#include <tuple>
#include <ctime>

template<typename ...Types>
class TableReader {
	static constexpr size_t NUM_COLS = sizeof...(Types);

	size_t m_start_pos;

	char* m_word_starts[NUM_COLS];
	int64_t m_lengths[NUM_COLS];

	static constexpr char kTerminator = '\0';
	static constexpr char kSeperator = '|';
	static constexpr char kNewline = '\n';

	static inline bool terminator(char c) {
		return (c == kTerminator) | (c == kNewline);
	}

	static char* tokenize(char* line, int64_t max_words, char** words, int64_t* lengths,
			int64_t& out_num_words) {
		int64_t num_words = 0;
		int64_t word_start = 0;
		int64_t i = 0;

		while (1) {
			char c = line[i];
			if ((terminator(c)) | (c == kSeperator)) {
				words[num_words] = &line[word_start];
				lengths[num_words] = i - word_start;

				// replace kSeperator with kTerminator
				line[i] = kTerminator;

				// saves one register for the iteration variables
				word_start = i + 1;

				num_words++;

				if (terminator(c)) {
					break;
				}
				if (num_words == max_words) {
					out_num_words = num_words;
					return &line[word_start];
				}
			}
			i++;
		}

		out_num_words = num_words;
		return nullptr;
	}

	template <typename T>
	std::tuple<T> parse(int64_t idx) const
	{
		T t(m_word_starts[idx], m_lengths[idx]);
		return std::tuple<T>(std::move(t));
	}

	template <typename T, typename Arg, typename... Args>
	std::tuple<T, Arg, Args...> parse(int64_t idx)  const
	{
		T t(m_word_starts[idx], m_lengths[idx]);
		return std::tuple_cat(std::tuple<T>(std::move(t)),
			parse<Arg, Args...>(idx+1));
	}

public:
	template<typename PushFun>
	void DoLine(char* str, PushFun&& push) {
		int64_t num = 0;
		tokenize(str, NUM_COLS, m_word_starts, m_lengths, num);
		assert(num >= NUM_COLS);

		push(parse<Types...>(0));
	}

	template<typename PushFun>
	void DoFile(const std::string& file, PushFun&& push) {
		std::ifstream f;

		f.open(file, std::ifstream::in);
		assert(f.is_open());
		const auto N = 1024*1024;
		char buf[N];
		while (f.getline(buf, N)) {
			DoLine(buf, std::forward<PushFun>(push));
		}
	}
};

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>

bool
BaseDecimal::parse(const char* first, const char* last, int& intg, int& frac)
{
    using boost::spirit::qi::int_;
    using boost::spirit::qi::_1;
    using boost::spirit::qi::phrase_parse;
    using boost::spirit::ascii::space;
    using boost::phoenix::ref;

    bool r = phrase_parse(first, last,
        ( int_[ref(intg) = _1] >> '.' >> int_[ref(frac) = _1] | int_[ref(intg) = _1] ),
        space);

    if (!r || first != last) // fail if we did not get a full match
        return false;
    return r;
}

bool
Date::parse(const char* first, const char* last, int& day, int& month, int& year)
{
    using boost::spirit::qi::int_;
    using boost::spirit::qi::_1;
    using boost::spirit::qi::phrase_parse;
    using boost::spirit::ascii::space;
    using boost::phoenix::ref;

    bool r = phrase_parse(first, last,
        ( int_[ref(year) = _1] >> '-' >> int_[ref(month) = _1] >> '-' >> int_[ref(day) = _1]),
        space);

    if (!r || first != last) // fail if we did not get a full match
        return false;

    return r;
}

Date::Date(const char* v, int64_t len, int plus_days)
{
	if (len < 0) {
		assert(len == -1);
		len = strlen(v);
	}
	int day = 0, month = 0, year = 0;
	if (!parse(v, v + len, day, month, year)) {
		assert(false);
	}
	dte_val = todate(day, month, year);
	dte_val += plus_days;
}

void
lineitem::FromFile(const std::string& file)
{
	using Decimal = Decimal<15, 2>;

	const clock_t begin = clock();
	TableReader<SkipCol, SkipCol, SkipCol, SkipCol, Decimal, Decimal, Decimal, Decimal, Char, Char, Date> reader;
	reader.DoFile(file, [&] (auto t) {
		assert(std::get<8>(t).chr_val);
		assert(std::get<9>(t).chr_val);

		l_quantity.Push(std::get<4>(t).dec_val);
		l_extendedprice.Push(std::get<5>(t).dec_val);
		l_discount.Push(std::get<6>(t).dec_val);
		l_tax.Push(std::get<7>(t).dec_val);
		l_returnflag.Push(std::get<8>(t).chr_val);
		l_linestatus.Push(std::get<9>(t).chr_val);
		l_shipdate.Push(std::get<10>(t).dte_val);

	});

	const clock_t end = clock();
	const double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;

#ifdef PRINT_MINMAX
	printf("loaded %zu tuples in %.2fs i.e. %.2f kTuples/s\n", l_quantity.cardinality, elapsed_secs, (double)l_quantity.cardinality / elapsed_secs / 1000.0);

	#define mm(name) printf("%s min=%lld max=%lld diff=%lld\n", #name, l_##name.minmax.min, l_##name.minmax.max, l_##name.minmax.max - l_##name.minmax.min)

	mm(quantity);
	mm(extendedprice);
	mm(discount);
	mm(tax);
	mm(returnflag);
	mm(linestatus);
	mm(shipdate);
	#undef mm
#endif
}


#include <algorithm>
#include <fstream>
#include <sstream>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>

#include <iostream>

#include "Indices.hxx"
#include "Tensor.hxx"
#include "Epsilon.hxx"
#include "Eta.hxx"
#include "Scalar.hxx"
#include "LinearAlgebra.hxx"
#include "Algorithms.hxx"

std::string printTree (std::unique_ptr<Tree<Node>> const & tree) {
  if (tree->isEmpty()) {
    return printForest (tree->forest, 0);
  } else {
    std::stringstream ss;
    ss << tree->node->print();
    ss << "\n";
    ss << printForest (tree->forest, 1);
    return ss.str();
  }
}

std::string printTreeMaple (std::unique_ptr<Tree<Node>> const & tree) {
  std::stringstream ss;
  auto it = tree->firstLeaf();

  bool first = true;
  while (it != nullptr) {
    if (first) {
      first = false;
    } else {
      ss << "+ ";
    }
    ss << printBranchMaple (it->getBranch());
    ss << std::endl;

    it = it->nextLeaf();
  }

  return ss.str();
}

std::string printBranchMaple (std::vector <Node *> const & branch) {
  assert (branch.size() > 1);
  std::stringstream ss;

  ss << branch.back()->printMaple();

  for (size_t counter = 0; counter < branch.size() - 1; ++counter) {
    ss << " * ";
    ss << branch[counter]->printMaple();
  }

  return ss.str();
}

std::string printForest (Forest<Node> const & f, size_t depth) {
  std::stringstream ss;
  std::for_each (f.cbegin(), f.cend(),
    [depth, &ss] (auto const & t) {
      for (size_t counter = 0; counter < depth; ++counter) {
        ss << "\t";
      }
      ss << t->node->print();
      ss << "\n";
      ss << printForest (t->forest, depth + 1);
    });
  return ss.str();
}

std::unique_ptr<Tree<Node>> copyTree (std::unique_ptr<Tree<Node>> const & tree) {
  auto new_tree = std::make_unique<Tree<Node>>();
  if (!tree->isEmpty()) {
    new_tree->node = tree->node->clone();
  }
  new_tree->forest = Forest<Node>();
  new_tree->forest.reserve(tree->forest.size());

  std::for_each(tree->forest.cbegin(), tree->forest.cend(),
    [&new_tree,&tree] (auto const & t) {
      new_tree->forest.emplace_back(std::move(copyTree(t)));
      new_tree->forest.back()->parent = new_tree.get();
    });

  return new_tree;
}

void multiplyTree (std::unique_ptr<Tree<Node>> & tree, mpq_class const & factor) {
  auto it = tree->firstLeaf();

  while (it != nullptr) {
    it->node->multiply(factor);
    it = it->nextLeaf();
  }
}

void exchangeTensorIndices (std::unique_ptr<Tree<Node>> & tree, std::map<char, char> const & exchange_map) {
  if (!tree->isEmpty()) {
    tree->node->exchangeTensorIndices(exchange_map);
  }

  std::for_each(tree->forest.begin(), tree->forest.end(),
    [&exchange_map] (auto & t) {
      exchangeTensorIndices (t, exchange_map);
    });
}

void exchangeSymmetrizeTree (std::unique_ptr<Tree<Node>> & tree, std::map<char, char> const & exchange_map, int parity) {
  multiplyTree(tree, mpq_class(1, 2));

  auto new_tree = copyTree(tree);

  mpq_class factor = (parity < 0 ? -1 : 1);
  multiplyTree (new_tree, factor);

  exchangeTensorIndices (new_tree, exchange_map);

  applyTensorSymmetries (new_tree);
  removeEmptyBranches(new_tree);

  sortTreeAndMerge (tree, new_tree);
  removeEmptyBranches(tree);
}

void multiExchangeSymmetrizeTree (std::unique_ptr<Tree<Node>> & tree, std::vector<std::pair<std::map<char, char>, int>> const & exchange_map_set) {
  multiplyTree (tree, mpq_class(1, exchange_map_set.size() + 1));

  auto tree_copy = copyTree (tree);

  std::for_each (exchange_map_set.cbegin(), exchange_map_set.cend(),
    [&tree_copy,&tree] (auto const & p) {
      auto new_tree = copyTree (tree_copy);
      auto exchange_map = p.first;
      int parity = p.second;
      mpq_class factor = (parity < 0 ? -1 : 1);
      multiplyTree (new_tree, factor);
      exchangeTensorIndices (new_tree, exchange_map);
      applyTensorSymmetries (new_tree);
      removeEmptyBranches (new_tree);
      sortTreeAndMerge (tree, new_tree);
      removeEmptyBranches (tree);
    });
}

void redefineScalarsSym (std::unique_ptr<Tree<Node>> & tree) {
  auto leaf_it = tree->firstLeaf();
  std::map<size_t, std::pair<size_t, mpq_class>> substitution_map;
  size_t var_counter = 0;

  while (leaf_it != nullptr) {
    Scalar * scalar = static_cast<Scalar *>(leaf_it->node.get());
    auto first = scalar->getCoefficientMap()->begin();
    auto it = substitution_map.find(first->first);
    if (it == substitution_map.end()) {
      std::unique_ptr<Node> new_scalar = std::make_unique<Scalar>(++var_counter, first->second);
      std::swap(new_scalar, leaf_it->node);
      substitution_map[first->first] = std::make_pair(var_counter, first->second);
    } else {
      std::unique_ptr<Node> new_scalar = std::make_unique<Scalar>(it->second.first, first->second);
      std::swap(new_scalar, leaf_it->node);
    }

    leaf_it = leaf_it->nextLeaf();
  }
}

void sortBranch (std::vector<Node *> & branch) {
  std::sort (branch.begin(), branch.end(),
    [] (auto & n, auto & m) {
      return (*n) < m;
    });
}

void sortForest (Forest<Node> & forest) {
  std::sort (forest.begin(), forest.end(),
    [] (auto const & n, auto const & m) {
      return *(n->node) < m->node.get();
    });
}

void sortTree (std::unique_ptr<Tree<Node>> & tree) {
  sortForest (tree->forest);

  std::for_each(tree->forest.begin(), tree->forest.end(),
    [] (auto & t) {
      sortTree (t);
    });
}

void sortTreeAndMerge (std::unique_ptr<Tree<Node>> & dst, std::unique_ptr<Tree<Node>> const & src) {
  auto src_it = src->firstLeaf();

  while (src_it != nullptr) {
    auto branch = src_it->getBranch();
    sortBranch (branch);

    insertBranch (dst, branch);

    src_it = src_it->nextLeaf();
  }
}

void mergeTrees (std::unique_ptr<Tree<Node>> & dst, std::unique_ptr<Tree<Node>> const & src) {
  auto src_it = src->firstLeaf();

  while (src_it != nullptr) {
    auto branch = src_it->getBranch();

    insertBranch (dst, branch);

    src_it = src_it->nextLeaf();
  }
}

void canonicalizeTree (std::unique_ptr<Tree<Node>> & tree) {
  applyTensorSymmetries (tree);

  auto ret = std::make_unique<Tree<Node>>();

  sortTreeAndMerge (ret, tree);
  removeZeroScalars (ret);
  removeEmptyBranches (ret);
  sortTree (ret);

  std::swap (ret, tree);
}

void insertBranch (std::unique_ptr<Tree<Node>> & dst, std::vector<Node *> & branch, size_t const node_number) {
  if (branch.size() == node_number) {
    return;
  }

  bool const branch_node_is_scalar = (typeid(*(branch[node_number])) == typeid(Scalar));

  auto node_it = std::find_if (dst->forest.begin(), dst->forest.end(),
    [&branch,branch_node_is_scalar,node_number] (auto & t) {
      if (t->node == nullptr) {
        return false;
      } else if (branch_node_is_scalar && (typeid(*(t->node)) == typeid(Scalar))) {
        return true;
      } else if (t->node->equals(branch[node_number])){
        return true;
      } else {
        return false;
      }
    });

  if (node_it == dst->forest.end()) {
    dst->forest.push_back (std::make_unique<Tree<Node>>());
    dst->forest.back()->node = branch[node_number]->clone();
    dst->forest.back()->parent = dst.get();
    insertBranch(dst->forest.back(), branch, node_number + 1);
  } else if (branch_node_is_scalar && (typeid(*((*node_it)->node)) == typeid(Scalar))) {
    static_cast<Scalar *>((*node_it)->node.get())->addOther(static_cast<Scalar *>(branch[node_number]));
    insertBranch (*node_it, branch, node_number + 1);
  } else {
    insertBranch (*node_it, branch, node_number + 1);
  }
}

void removeEmptyBranches (std::unique_ptr<Tree<Node>> & tree) {
  std::for_each(tree->forest.begin(), tree->forest.end(),
    [] (auto & t) {
      removeEmptyBranches (t);
    });

  tree->forest.erase(std::remove_if(tree->forest.begin(), tree->forest.end(),
    [] (auto & t) {

      if (!t->isEmpty()) {
        auto is_node_scalar = (typeid(*(t->node)) == typeid(Scalar));
        if (is_node_scalar && static_cast<Scalar *>(t->node.get())->isZero()) {
          return true;
        } else if (!is_node_scalar && t->isLeaf()) {
          return true;
        }
      } else {
        if (t->isLeaf()) {
          return true;
        }
      }

  return false;

  }), tree->forest.end());
}

void removeZeroScalars (std::unique_ptr<Tree<Node>> & tree) {
  auto it = tree->firstLeaf();

  while (it != nullptr) {
    static_cast<Scalar *>(it->node.get())->removeZeros();
    it = it->nextLeaf();
  }
}

bool isTreeSorted (std::unique_ptr<Tree<Node>> const & tree) {
  return (std::is_sorted(tree->forest.cbegin(), tree->forest.cend(),
    [] (auto const & a, auto const & b) {
      return *(a->node) < b->node.get();
    }) && std::all_of(tree->forest.cbegin(), tree->forest.cend(),
      [] (auto const & t) {
        return isTreeSorted (t);
      }));
}

std::map<size_t, mpq_class> evaluateTree (std::unique_ptr<Tree<Node>> const & tree, std::map<char, char> const & eval_map, mpq_class prefactor) {
  std::map<size_t, mpq_class> ret;

  if (tree->isLeaf()) {
    std::map<size_t, mpq_class> const * leaf_map = tree->node->getCoefficientMap();
    std::for_each(leaf_map->cbegin(), leaf_map->cend(),
      [&ret,&prefactor] (auto const & p) {
        ret.insert(std::make_pair(p.first, prefactor * p.second));
      });
  } else {
    if (!tree->isRoot()) {
      prefactor *= static_cast<Tensor const *>(tree->node.get())->evaluate(eval_map);
      if (prefactor == 0) {
        return ret;
      }
    }

    std::for_each(tree->forest.cbegin(), tree->forest.cend(),
      [&ret, &eval_map, &prefactor] (auto const & t) {
        auto _map = evaluateTree (t, eval_map, prefactor);
        std::for_each(_map.cbegin(), _map.cend(),
          [&ret] (auto const & a) {
            auto it = ret.find(a.first);
            if (it == ret.end()) {
              ret.insert(std::make_pair(a.first, a.second));
            } else {
              it->second += a.second;
              if (it->second == 0) {
                ret.erase(it);
              }
            }
          });
      });
  }

  return ret;
}

void applyTensorSymmetries(std::unique_ptr<Tree<Node>> & tree, int parity) {
  std::vector<Tree<Node>*> erase_vec;
  std::for_each(tree->forest.begin(), tree->forest.end(),
    [parity,&erase_vec] (auto & t) {
      int new_parity = t->node->applyTensorSymmetries(parity);
      if (new_parity == 0) {
        erase_vec.push_back(t.get());
      } else {
        applyTensorSymmetries (t, new_parity);
      }
    });

  tree->forest.erase(std::remove_if(tree->forest.begin(), tree->forest.end(),
    [&erase_vec] (auto & t) {
      return (std::find(erase_vec.begin(), erase_vec.end(), t.get()) != erase_vec.end());
    }), tree->forest.end());
}

std::map<size_t, size_t> getVariableMap (std::unique_ptr<Tree<Node>> const & tree) {
  auto variables = getVariableSet(tree);
  std::map<size_t, size_t> ret;
  std::for_each(variables.cbegin(), variables.cend(),
    [&ret,n=0] (auto const & v) mutable {
      ret[v] = n++;
    });
  return ret;
}

std::set<size_t> getVariableSet (std::unique_ptr<Tree<Node>> const & tree) {
  std::set<size_t> ret;

  auto leaf_it = tree->firstLeaf();

  while (leaf_it != nullptr) {
    ret.merge (leaf_it->node->getVariableSet());
    leaf_it = leaf_it->nextLeaf();
  }

  return ret;
}

void shrinkForest (Forest<Node> & forest) {
  forest.shrink_to_fit();
  std::for_each(forest.begin(), forest.end(),
    [] (auto & t) {
      t->forest.shrink_to_fit();
    });
}

void substituteVariables (std::unique_ptr<Tree<Node>> & tree, std::map<size_t, size_t> const & subs_map) {
  auto leaf_it = tree->firstLeaf();

  while (leaf_it != nullptr) {
    leaf_it->node->substituteVariables (subs_map);
    leaf_it = leaf_it->nextLeaf();
  }
}

void setVariablesToZero (std::unique_ptr<Tree<Node>> & tree, std::set<size_t> const & variables) {
  auto leaf_it = tree->firstLeaf();

  while (leaf_it != nullptr) {
    leaf_it->node->removeVariables(variables);
    leaf_it = leaf_it->nextLeaf();
  }
}

void solveNumerical (std::vector<std::pair<std::unique_ptr<Tree<Node>> &, std::function< void (std::unique_ptr<Tree<Node>> const &, std::set<std::map<size_t, mpq_class>> &)>>> const & equations) {
  std::set<std::map<size_t, mpq_class>> eval_res_set;
  std::set<size_t> var_set;

  std::for_each (equations.cbegin(), equations.cend(),
      [&eval_res_set,&var_set] (auto const & eq) {
        auto & fun = eq.second;
        auto & tree = eq.first;

        var_set.merge (getVariableSet (tree));
        fun (tree, eval_res_set);
      });

    std::cout << "number of equations : " << eval_res_set.size() << std::endl;

    std::cout << "number of coefficients : " << var_set.size() << std::endl;

    std::map<size_t, size_t> var_map;
    std::set<std::pair<std::pair<size_t, size_t>, mpq_class>> eval_res_mat;
    std::set<std::pair<std::pair<size_t, size_t>, mpq_class>> eval_res_mat_mapped;

    std::for_each(eval_res_set.cbegin(), eval_res_set.cend(),
      [row_counter=0,&eval_res_mat] (auto const & m) mutable {
        std::for_each(m.cbegin(), m.cend(),
          [&row_counter,&eval_res_mat] (auto const & p) {
            eval_res_mat.insert(std::make_pair(std::make_pair(row_counter, p.first), p.second));
          });
        ++row_counter;
      });

    eval_res_set.clear();

    std::for_each(eval_res_mat.cbegin(), eval_res_mat.cend(),
      [&eval_res_mat_mapped,&var_map,var=0] (auto const & v) mutable {
        auto it = var_map.find(v.first.second);
        if (it == var_map.end()) {
          var_map.insert(std::make_pair(v.first.second, var));
          ++var;
        }
        eval_res_mat_mapped.insert(std::make_pair(std::make_pair(v.first.first, var_map.at(v.first.second)), v.second));
      });

    eval_res_mat.clear();

  solveLinearSystem (eval_res_mat_mapped, eval_res_mat_mapped.rbegin()->first.first + 1, var_map.size());
}

void reduceNumerical (std::unique_ptr<Tree<Node>> & tree, std::function< void (std::unique_ptr<Tree<Node>> const &, std::set<std::map<size_t, mpq_class>> &)> fun) {
  std::set<std::map<size_t, mpq_class>> eval_res_set;

  fun (tree, eval_res_set);

  std::set<size_t> var_set = getVariableSet (tree);
  std::map<size_t, size_t> var_map;
  std::set<size_t> erase_set = var_set;
  std::set<std::pair<std::pair<size_t, size_t>, mpq_class>> eval_res_mat;
  std::set<std::pair<std::pair<size_t, size_t>, mpq_class>> eval_res_mat_mapped;

  std::for_each(eval_res_set.cbegin(), eval_res_set.cend(),
    [row_counter=0,&eval_res_mat] (auto const & m) mutable {
      std::for_each(m.cbegin(), m.cend(),
        [&row_counter,&eval_res_mat] (auto const & p) {
          eval_res_mat.insert(std::make_pair(std::make_pair(row_counter, p.first), p.second));
        });
      ++row_counter;
    });

  eval_res_set.clear();

  std::for_each(eval_res_mat.cbegin(), eval_res_mat.cend(),
    [&eval_res_mat_mapped,&var_map,&erase_set,var=0] (auto const & v) mutable {
      erase_set.erase(v.first.second);
      auto it = var_map.find(v.first.second);
      if (it == var_map.end()) {
        var_map.insert(std::make_pair(v.first.second, var));
        ++var;
      }
      eval_res_mat_mapped.insert(std::make_pair(std::make_pair(v.first.first, var_map.at(v.first.second)), v.second));
    });

  eval_res_mat.clear();

  std::cout << "These equations contain " << var_map.size() << " variables." << std::endl;
  std::cout << "That is, " << erase_set.size() << " variables do not contribute at all." << std::endl;
  std::cout << std::endl;
  std::cout << "Computing LU decomposition of the linear system using Eigen subroutines ..." << std::endl;

  auto erase_set_2_mapped = findDependentVariables (eval_res_mat_mapped, eval_res_mat_mapped.rbegin()->first.first + 1, var_map.size());
  std::cout << "... done! " << erase_set_2_mapped.size() << " variables are dependent." << std::endl;

  eval_res_mat_mapped.clear();

  std::map<size_t, size_t> var_rmap;
  std::transform (var_map.cbegin(), var_map.cend(), std::inserter(var_rmap, var_rmap.begin()),
    [] (auto const & p) {
      return std::make_pair(p.second, p.first);
    });

  std::set<size_t> erase_set_2;
  std::transform(erase_set_2_mapped.cbegin(), erase_set_2_mapped.cend(), std::inserter(erase_set_2, erase_set_2.begin()),
    [&var_rmap] (auto const & v) {
      return var_rmap.at(v);
    });

  erase_set_2_mapped.clear();

  erase_set.merge(erase_set_2);
  erase_set_2.clear();

  std::cout << "Removing all " << erase_set.size() << " variables from the ansatz." << std::endl;

  setVariablesToZero (tree, erase_set);
  removeEmptyBranches (tree);

  var_set = getVariableSet (tree);

  var_map.clear();

  std::transform (var_set.cbegin(), var_set.cend(), std::inserter (var_map, var_map.begin()),
    [counter=0] (auto const & v) mutable {
      return std::make_pair (v, ++counter);
    });

  substituteVariables (tree, var_map);
}

bool compareTrees (std::unique_ptr<Tree<Node>> const & tree1, std::unique_ptr<Tree<Node>> const & tree2) {
  bool nodes_equal = false;
  if (tree1->isEmpty() != tree2->isEmpty()) {
    nodes_equal = false;
  } else if (tree1->isEmpty() && tree2->isEmpty()) {
    nodes_equal = true;
  } else {
    nodes_equal = tree1->node->equals(tree2->node.get());
  }

  return (nodes_equal && std::equal(tree1->forest.cbegin(), tree1->forest.cend(),
                                    tree2->forest.cbegin(), tree2->forest.cend(),
                                    [] (auto const & t1, auto const & t2) {
                                      return compareTrees (t1, t2);
                                    }));
}

void contractTreeWithEta (std::unique_ptr<Tree<Node>> & tree, char i1, char i2) {
  std::vector<std::unique_ptr<Tree<Node>>> new_forest;
  std::for_each (tree->forest.begin(), tree->forest.end(),
    [i1, i2, &new_forest] (auto & t) {
      auto parent = t->parent;
      auto forest = contractTreeWithEtaInner (t, i1, i2);
      std::for_each (forest.begin(), forest.end(),
        [&t,parent] (auto & t2) {
          t2->parent = parent;
        });
      new_forest.insert (new_forest.end(), std::make_move_iterator (forest.begin()), std::make_move_iterator (forest.end()));
    });
  std::swap (tree->forest, new_forest);
}

Forest<Node> contractTreeWithEtaInner (std::unique_ptr<Tree<Node>> & tree, char i1, char i2) {
  tree->parent = nullptr;
  Forest<Node> ret;
  auto & node_type = typeid (*(tree->node));
  if (node_type == typeid (Epsilon)) {
    bool const contains_i1 = static_cast<Epsilon *>(tree->node.get())->containsIndex (i1);
    bool const contains_i2 = static_cast<Epsilon *>(tree->node.get())->containsIndex (i2);
    if (contains_i1) {
      if (contains_i2) {
        return ret;
      } else {
        auto _tree = std::make_unique<Tree<Node>>();
        _tree->forest.emplace_back (std::move(tree));
        return std::move(eliminateSecondEta(_tree, i1, i2)->forest);
      }
    } else {
      if (contains_i2) {
        auto _tree = std::make_unique<Tree<Node>>();
        _tree->forest.emplace_back (std::move(tree));
        return std::move(eliminateSecondEta(_tree, i2, i1)->forest);
      } else {
        contractTreeWithEta (tree, i1, i2);
        ret.emplace_back (std::move(tree));
        return ret;
      }
    }
  } else if (node_type == typeid (Eta)) {
    bool const contains_i1 = static_cast<Eta *>(tree->node.get())->containsIndex (i1);
    bool const contains_i2 = static_cast<Eta *>(tree->node.get())->containsIndex (i2);
    if (contains_i1) {
      if (contains_i2) {
        multiplyTree (tree, mpq_class (4, 1));
        std::for_each (tree->forest.begin(), tree->forest.end(),
          [&ret] (auto & t) {
            ret.emplace_back(std::move(t));
          });
        return ret;
      } else {
        auto _tree = std::make_unique<Tree<Node>>();
        _tree->forest.emplace_back (std::move(tree));
        return std::move(eliminateSecondEta(_tree, i1, i2)->forest);
      }
    } else {
      if (contains_i2) {
        auto _tree = std::make_unique<Tree<Node>>();
        _tree->forest.emplace_back (std::move(tree));
        return std::move(eliminateSecondEta(_tree, i2, i1)->forest);
      } else {
        contractTreeWithEta (tree, i1, i2);
        ret.emplace_back (std::move(tree));
        return ret;
      }
    }
  } else {
    contractTreeWithEta (tree, i1, i2);
    ret.emplace_back (std::move(tree));
    return ret;
  }
}

std::unique_ptr<Tree<Node>> eliminateSecondEta (std::unique_ptr<Tree<Node>> & tree, char i1, char i2) {
  auto ret = std::make_unique<Tree<Node>>();
  auto it = tree->firstLeaf ();

  while (it != nullptr) {
    auto branch = it->getBranch();
    assert (branch[0] != nullptr);

    auto eta_it = std::find_if (branch.cbegin(), branch.cend(),
      [i2] (auto const & n) {
        return (n != nullptr &&
                typeid (*n) == typeid (Eta) &&
                static_cast<Eta *>(n)->containsIndex (i2));
      });
    char i_new = static_cast<Eta *>(*eta_it)->getOther(i2);

    branch.erase (eta_it);
    auto node_cpy = branch[0]->clone();
    branch[0] = node_cpy.get();
    static_cast<Tensor *>(branch[0])->exchangeTensorIndices({{i1, i_new}});

    sortBranch (branch);
    insertBranch (ret, branch);

    it = it->nextLeaf();
  }

  return ret;
}

void contractTreeWithEpsilon3 (std::unique_ptr<Tree<Node>> & tree, char m, char i1, char i2, char i3) {
  auto new_tree = std::make_unique<Tree<Node>> ();
  auto it = tree->firstLeaf ();

  while (it != nullptr) {
    auto branch = it->getBranch ();
    auto & type = typeid(*(branch[0]));
    if (type == typeid(Eta)) {
      if (std::any_of (branch.cbegin(), branch.cend(),
           [i1, i2, i3] (auto const & n) {
             if (typeid (*n) == typeid (Eta)) {
               auto eta = static_cast<Eta *>(n);
               int count = 0;
               if (eta->containsIndex(i1)) { ++count; }
               if (eta->containsIndex(i2)) { ++count; }
               if (eta->containsIndex(i3)) { ++count; }
               if (count == 2) {
                 return true;
               } else {
                 assert (count < 2);
                 return false;
               }
             } else {
               return false;
             }
           })) {
        // do nothing
      } else {
        std::vector<Node *> new_branch;
        char j1 = 0;
        char j2 = 0;
        char j3 = 0;
        new_branch.push_back (nullptr);
        std::for_each (branch.begin(), branch.end(),
            [&j1,&j2,&j3,i1,i2,i3,&new_branch] (auto n) {
              if (typeid (*n) == typeid (Eta)) {
                auto eta = static_cast<Eta *>(n);
                if (eta->containsIndex(i1)) {
                  assert (j1 == 0);
                  j1 = eta->getOther (i1);
                }
                else if (eta->containsIndex(i2)) {
                  assert (j2 == 0);
                  j2 = eta->getOther (i2);
                }
                else if (eta->containsIndex(i3)) {
                  assert (j3 == 0);
                  j3 = eta->getOther(i3);
                } else {
                  new_branch.push_back (n);
                }
              } else {
                new_branch.push_back (n);
              }
            });
        assert (j1 != 0 && j2 != 0 && j3 != 0);
        assert (branch.size() == new_branch.size() + 2);
        std::unique_ptr<Node> epsilon = std::make_unique<Epsilon> (m, j1, j2, j3);
        new_branch[0] = epsilon.get();
        insertBranch (new_tree, new_branch);
      }
    } else if (type == typeid(Epsilon)) {
      auto epsilon = static_cast<Epsilon *> (branch[0]);
      auto mult_res = epsilon->multiplyWithOther3 (i1, i2, i3);
      auto new_tree_2 = std::make_unique<Tree<Node>>();
      new_tree_2->forest.emplace_back (std::make_unique<Tree<Node>>());
      new_tree_2->forest[0]->node = std::make_unique<Eta>(m, std::get<1>(mult_res));
      new_tree_2->forest[0]->parent = new_tree_2.get();
      std::for_each (branch.cbegin() + 1, branch.cend(),
          [&mult_res,&new_tree_2] (auto const & n) {
            auto leaf = new_tree_2->firstLeaf();
            if (typeid(*n) == typeid(Scalar)) {
              auto scalar_clone = n->clone();
              scalar_clone->multiply (std::get<0>(mult_res));
              leaf->forest.emplace_back (std::make_unique<Tree<Node>>());
              std::swap (leaf->forest[0]->node, scalar_clone);
              leaf->forest[0]->parent = leaf;
            } else if (typeid(*n) == typeid(Eta)) {
              auto eta_clone = n->clone();
              eta_clone->exchangeTensorIndices (std::get<2>(mult_res));
              leaf->forest.emplace_back (std::make_unique<Tree<Node>>());
              std::swap (leaf->forest[0]->node, eta_clone);
              leaf->forest[0]->parent = leaf;
            } else {
              assert (false);
            }
          });
      std::vector<char> indices_to_symmetrize;
      indices_to_symmetrize.push_back (std::get<1>(mult_res));
      std::transform (std::get<2>(mult_res).cbegin(), std::get<2>(mult_res).cend(),
          std::back_inserter (indices_to_symmetrize), [] (auto const & p) { return p.second; });
      auto to_sym_size = indices_to_symmetrize.size();
      assert (to_sym_size > 0);
      assert (to_sym_size < 5);
      if (to_sym_size == 2) {
        exchangeSymmetrizeTree (new_tree_2, {{indices_to_symmetrize[0], indices_to_symmetrize[1]},
                                          {indices_to_symmetrize[1], indices_to_symmetrize[0]}}, -1);
      } else if (to_sym_size == 3) {
        auto & iv = indices_to_symmetrize;
        multiExchangeSymmetrizeTree (new_tree_2,
            {{{{iv[1], iv[2]}, {iv[2], iv[1]}}, -1},
             {{{iv[0], iv[1]}, {iv[1], iv[0]}}, -1},
             {{{iv[0], iv[1]}, {iv[1], iv[2]}, {iv[2], iv[0]}}, 1},
             {{{iv[0], iv[2]}, {iv[1], iv[0]}, {iv[2], iv[1]}}, 1},
             {{{iv[0], iv[2]}, {iv[2], iv[0]}}, -1}});
      } else if (to_sym_size == 4) {
        auto & iv = indices_to_symmetrize;
        multiExchangeSymmetrizeTree (new_tree_2,
            {{{{iv[2], iv[3]}, {iv[3], iv[2]}}, -1},                                  // 0 1 3 2
             {{{iv[1], iv[2]}, {iv[2], iv[1]}}, -1},                                  // 0 2 1 3
             {{{iv[1], iv[2]}, {iv[2], iv[3]}, {iv[3], iv[1]}}, 1},                   // 0 2 3 1
             {{{iv[1], iv[3]}, {iv[2], iv[1]}, {iv[3], iv[2]}}, 1},                   // 0 3 1 2
             {{{iv[1], iv[3]}, {iv[3], iv[1]}}, -1},                                  // 0 3 2 1
             {{{iv[0], iv[1]}, {iv[1], iv[0]}}, -1},                                  // 1 0 2 3
             {{{iv[0], iv[1]}, {iv[1], iv[0]}, {iv[2], iv[3]}, {iv[3], iv[2]}}, 1},   // 1 0 3 2
             {{{iv[0], iv[1]}, {iv[1], iv[2]}, {iv[2], iv[0]}}, 1},                   // 1 2 0 3
             {{{iv[0], iv[1]}, {iv[1], iv[2]}, {iv[2], iv[3]}, {iv[3], iv[0]}}, -1},  // 1 2 3 0
             {{{iv[0], iv[1]}, {iv[1], iv[3]}, {iv[2], iv[0]}, {iv[3], iv[2]}}, -1},  // 1 3 0 2
             {{{iv[0], iv[1]}, {iv[1], iv[3]}, {iv[3], iv[0]}}, 1},                   // 1 3 2 0
             {{{iv[0], iv[2]}, {iv[1], iv[0]}, {iv[2], iv[1]}}, 1},                   // 2 0 1 3
             {{{iv[0], iv[2]}, {iv[1], iv[0]}, {iv[2], iv[3]}, {iv[3], iv[1]}}, -1},  // 2 0 3 1
             {{{iv[0], iv[2]}, {iv[2], iv[0]}}, -1},                                  // 2 1 0 3
             {{{iv[0], iv[2]}, {iv[2], iv[3]}, {iv[3], iv[0]}}, 1},                   // 2 1 3 0
             {{{iv[0], iv[2]}, {iv[1], iv[3]}, {iv[2], iv[0]}, {iv[3], iv[1]}}, 1},   // 2 3 0 1
             {{{iv[0], iv[2]}, {iv[1], iv[3]}, {iv[2], iv[1]}, {iv[3], iv[0]}}, -1},  // 2 3 1 0
             {{{iv[0], iv[3]}, {iv[1], iv[0]}, {iv[2], iv[1]}, {iv[3], iv[2]}}, -1},  // 3 0 1 2
             {{{iv[0], iv[3]}, {iv[1], iv[0]}, {iv[3], iv[1]}}, 1},                   // 3 0 2 1
             {{{iv[0], iv[3]}, {iv[2], iv[0]}, {iv[3], iv[2]}}, 1},                   // 3 1 0 2
             {{{iv[0], iv[3]}, {iv[3], iv[0]}}, -1},                                  // 3 1 2 0
             {{{iv[0], iv[3]}, {iv[1], iv[2]}, {iv[2], iv[0]}, {iv[3], iv[1]}}, -1},  // 3 2 0 1
             {{{iv[0], iv[3]}, {iv[1], iv[2]}, {iv[2], iv[1]}, {iv[3], iv[0]}}, 1}}); // 3 2 1 0
      }
      mergeTrees (new_tree, new_tree_2);
    } else {
      assert(false);
    }
    it = it->nextLeaf();
  }

  std::swap (tree, new_tree);
}

void saveTree (std::unique_ptr<Tree<Node>> const & tree, std::string const & filename) {
  std::ofstream file {filename};
  boost::archive::text_oarchive oa {file};
  oa << tree;
}

std::unique_ptr<Tree<Node>> loadTree (std::string const & filename) {
  std::ifstream file {filename};
  boost::archive::text_iarchive ia {file};
  std::unique_ptr<Tree<Node>> tree;
  ia >> tree;
  return tree;
}

bool checkSaveAndLoad (std::unique_ptr<Tree<Node>> const & tree) {
  saveTree (tree, "check.prs");
  auto tree2 = loadTree ("check.prs");
  return (compareTrees (tree, tree2));
}

void shiftVariables (std::unique_ptr<Tree<Node>> & tree, int i) {
  auto it = tree->firstLeaf();

  while (it != nullptr) {
    assert (typeid(*(it->node.get())) == typeid(Scalar));
    static_cast<Scalar *>(it->node.get())->shiftVariables (i);

    it = it->nextLeaf();
  }
}

void multiplyTreeWithEta (std::unique_ptr<Tree<Node>> & tree, char i1, char i2) {
  auto new_tree = std::make_unique<Tree<Node>>();

  new_tree->forest.emplace_back (std::make_unique<Tree<Node>>());
  new_tree->forest.back()->parent = new_tree.get();
  new_tree->forest.back()->node = std::make_unique<Eta> (i1, i2);

  std::transform (tree->forest.begin(), tree->forest.end(), std::back_inserter (new_tree->forest.back()->forest),
      [&new_tree] (auto & t) {
        auto new_subtree = std::make_unique<Tree<Node>>();
        std::swap (t, new_subtree);
        new_subtree->parent = new_tree->forest.back().get();
        return std::move (new_subtree);
      });

  std::swap (tree, new_tree);
}


// json parser
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <utility>
namespace pt = boost::property_tree;

#include "../MemoryKernel/MemoryKernelFactory.h"
#include "PermittivityLorentz.h"

PermittivityLorentz::PermittivityLorentz(
    double eps_inf, double omega_p, double omega_0,
    std::shared_ptr<MemoryKernel> memory_kernel)
    : eps_inf(eps_inf), omega_p(omega_p), omega_0(omega_0),
      memory_kernel(std::move(memory_kernel)) {}

// constructor for drude model from .json file
PermittivityLorentz::PermittivityLorentz(const std::string &input_file) {

  // Create a root
  pt::ptree root;

  // Load the json file in this ptree
  pt::read_json(input_file, root);

  // check if type is right
  std::string type = root.get<std::string>("Permittivity.type");
  assert(type == "lorentz");

  // read parameters
  this->eps_inf = root.get<double>("Permittivity.eps_inf");
  this->omega_p = root.get<double>("Permittivity.omega_p");
  this->omega_0 = root.get<double>("Permittivity.omega_0");

  this->memory_kernel =
      MemoryKernelFactory::create(input_file, "Permittivity.MemoryKernel");
}

// calculate the permittivity
std::complex<double> PermittivityLorentz::calculate(double omega) const {
  // dummies for result and complex unit
  std::complex<double> result;
  std::complex<double> I(0.0, 1.0);

  // calculate the result
  result = eps_inf - omega_p * omega_p /
                         (omega_0 * omega_0 - omega * omega -
                          I * omega * memory_kernel->calculate(omega));

  return result;
}

// calculate the permittivity scaled by omega
std::complex<double>
PermittivityLorentz::calculate_times_omega(double omega) const {
  // dummies for result and complex unit
  std::complex<double> result;
  std::complex<double> I(0.0, 1.0);

  // calculate the result
  result = eps_inf * omega - omega_p * omega_p * omega /
                                 (omega_0 * omega_0 - omega * omega -
                                  I * omega * memory_kernel->calculate(omega));

  return result;
}

void PermittivityLorentz::print_info(std::ostream &stream) const {
  stream << "# PermittivityLorentz\n#\n"
         << "# eps_inf = " << eps_inf << "\n"
         << "# omega_p = " << omega_p << "\n"
         << "# omega_0 = " << omega_0 << "\n";
  memory_kernel->print_info(stream);
}


/**
 *  carAbst_dba4.cpp
 *
 *  Abstraction-based control for vehicle motion planning.
 *  A different workspace setting from carAbst.cpp.
 *
 *  Created by Yinan Li on Aug 10, 2020.
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <cstdlib>
#include <boost/algorithm/string.hpp>

#include "src/abstraction.hpp"
#include "src/DBAparser.h"
#include "src/bsolver.hpp"

#include "src/hdf5io.h"

#include "car.hpp"

int main(int argc, char *argv[])
{
    /**
     * Default values
     **/
    std::string specfile;
    double eta[]{0.2, 0.2, 0.2}; /* partition precision */
    bool preprocess = false;

    /* Input arguments:
     * carAbst dbafile precision(e.g. 0.2 0.2 0.2) -preprocess
     */
    if (argc < 2 || argc > 6) {
	std::cout << "Improper number of arguments.\n";
	std::exit(1);
    }
    specfile = std::string(argv[1]);
    if (argc > 2 && argc < 5) {
	std::cout << "Input precision should be of 3-dim, e.g. 0.2 0.2 0.2.\n";
	std::exit(1);
    }
    if (argc >= 5) {
	for(int i = 2; i < 5; ++i)
	    eta[i-2] = std::atof(argv[i]);

    	if (argc > 5) {
    	    std::string param = std::string(argv[5]);
    	    if (param == "-preprocess")
    		preprocess = true;
    	    else {
    		std::cout << "Wrong argument for preprocessing.\n";
    		std::exit(1);
    	    }
    	}
    }
    std::cout << "Partition precision: " << eta[0] << ' '
	      << eta[1] << ' ' << eta[2] << '\n';

    clock_t tb, te;
    /* set the state space */
    const double theta = 3.5;
    double xlb[] = {0, 0, -theta};
    double xub[] = {10, 10, theta};
    /* set the control values */
    double ulb[] = {-1.0, -1.0};
    double uub[] = {1.0, 1.0};
    double mu[] = {0.3, 0.3};
    /* define the control system */
    rocs::DTCntlSys<carde> car("reach goal", h, carde::n, carde::m);
    car.init_workspace(xlb, xub);
    car.init_inputset(mu, ulb, uub);

    /**
     * Construct and save abstraction
     */
    // const double eta[] = {0.2, 0.2, 0.2}; /* set precision */
    rocs::abstraction<rocs::DTCntlSys<carde>> abst(&car);
    abst.init_state(eta, xlb, xub);
    std::cout << "The number of abstraction states: " << abst._x._nv << '\n';
    /* Assign the label of avoid area to -1 */
    rocs::UintSmall nAvoid = 3;
    double obs[3][4] = {
	{0.0, 3.2, 4.0, 5.0},
	{5.4, 6.0, 5.0, 10.0},
	{4.5, 5.2, 0.0, 2.5}
    };
    auto label_avoid = [&obs, &nAvoid, &abst, &eta](size_t i) {
    		     std::vector<double> x(abst._x._dim);
    		     abst._x.id_to_val(x, i);
    		     double c1= eta[0]/2.0+1e-10;
    		     double c2= eta[1]/2.0+1e-10;
    		     for(size_t i = 0; i < nAvoid; ++i) {
    			 if ((obs[i][0]-c1) <= x[0] && x[0] <= (obs[i][1]+c1) &&
    			     (obs[i][2]-c2) <= x[1] && x[1] <= (obs[i][3]+c2))
    			     return -1;
    		     }
    		     return 0;
    		 };
    abst.assign_labels(label_avoid);
    std::vector<size_t> obstacles;
    for (size_t i = 0; i < abst._x._nv; ++i) {
    	if (abst._labels[i] < 0)
    	    obstacles.push_back(i);
    }
    /* Compute abstraction */
    /* Robustness margins */
    double e1[] = {0,0,0};
    double e2[] = {0,0,0};
    tb = clock();
    abst.assign_transitions(e1, e2);
    // abst.assign_transitions();
    te = clock();
    float tabst = (float)(te - tb)/CLOCKS_PER_SEC;
    std::cout << "Time of computing abstraction: " << tabst << '\n';
    std::cout << "# of transitions: " << abst._ts._ntrans << '\n';
    /* Write abstraction to file */
    // std::string nts_file_name = std::string("nts") + std::to_string(0.2) + std::string(".txt");
    // rocs::txtWriter wtr(nts_file_name.c_str());
    // wtr.write_fts_info(abst._ts);
    // wtr.write_pre_transitions(abst._ts, abst._x, xlb, xub);
    // wtr.open();
    // wtr.close();

    /**
     * Read DBA from dba*.txt file
     */
    std::cout << "Reading the specification...\n";
    rocs::UintSmall nAP = 0, nNodes = 0, q0 = 0;
    std::vector<rocs::UintSmall> acc;
    std::vector<std::vector<rocs::UintSmall>> arrayM;
    if (!rocs::read_spec(specfile, nNodes, nAP, q0, arrayM, acc))
	std::exit(1);

    /*
     * Assign labels to states: has to be consistent with the dba file.
     */
    double goal[6][4] = {{1.0, 2.0, 0.5, 2.0},
			 {7.1, 9.1, 1.9, 2.9},
			 {0.0, 4.0, 6.0, 10.0},
			 {6.0, 10.0, 0.0, 4.0},
			 {0.5, 2.5, 7.5, 8.5},
			 {3.8, 4.6, 3.1, 4.5}};
    rocs::UintSmall nGoal = 6;
    auto label_target = [&goal, &nGoal, &abst, &eta](size_t i) {
		      std::vector<double> x(abst._x._dim);
		      abst._x.id_to_val(x, i);
		      double c1= eta[0]/2.0; //+1e-10;
		      double c2= eta[1]/2.0; //+1e-10;
		      boost::dynamic_bitset<> label(nGoal, false); // n is the number of goals
		      for(rocs::UintSmall i = 0; i < nGoal; ++i) {
			  label[nGoal-1-i] = (goal[i][0] <= (x[0]-c1) && (x[0]+c1) <= goal[i][1] &&
				      goal[i][2] <= (x[1]-c2) && (x[1]+c2) <= goal[i][3])
			      ? true: false;
		      }
		      return label.to_ulong(); // label[0] is the least significant digit
		  };
    abst.assign_labels(label_target);
    std::cout << "Specification assignment is done.\n";

    /**
     * Solve a Buchi game on the product of NTS and DBA.
     */
    // std::cout << "Start solving a Buchi game on the product of the abstraction and DBA...\n";
    // rocs::HEAD psolver; // memories will be allocated for psolver
    // rocs::initialization(&psolver);
    // rocs::construct_dba(&psolver, (int)nAP, (int)nNodes, (int)q0, acc, arrayM);
    // tb = clock();
    // rocs::solve_buchigame_on_product(&psolver, abst);
    // te = clock();
    // float tsyn = (float)(te - tb)/CLOCKS_PER_SEC;
    // std::cout << "Time of synthesizing controller: " << tsyn << '\n';
    std::cout << "Start solving a Buchi game on the product of the abstraction and DBA...\n";
    rocs::BSolver solver;
    solver.construct_dba((int)nAP, (int)nNodes, (int)q0, acc, arrayM);
    tb = clock();
    solver.load_abstraction(abst);
    solver.generate_product(abst);
    solver.solve_buchigame_on_product();
    te = clock();
    float tsyn = (float)(te - tb)/CLOCKS_PER_SEC;
    std::cout << "Time of synthesizing controller: " << tsyn << '\n';

    /**
     * Display and save memoryless controllers.
     */
    std::vector<std::string> tokens;
    boost::split(tokens, specfile, boost::is_any_of("."));
    std::string datafile = "controller_" + tokens[0] + "_";
    for(int i = 0; i < 3; ++i) {
	std::stringstream ss;
	ss << std::setprecision(1);
	ss << eta[i];
	datafile += ss.str();
	if (i < 2)
	    datafile += "-";
    }
    datafile += ".h5";

    std::cout << "Writing the controller...\n";
    // solver.write_controller_to_txt(const_cast<char*>(datafile.c_str()));
    rocs::h5FileHandler ctlrWtr(datafile, H5F_ACC_TRUNC);
    ctlrWtr.write_problem_setting< rocs::DTCntlSys<carde> >(car);
    ctlrWtr.write_array<double>(eta, carde::n, "eta");
    ctlrWtr.write_2d_array<double>(abst._x._data, "xgrid");
    ctlrWtr.write_discrete_controller(&(solver._sol));
    std::cout << "Controller writing is done.\n";

    std::cout << "Total time of used (abstraction+synthesis): " << tabst+tsyn << '\n';

    return 0;
}


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014 Max-Planck-Institute for Intelligent Systems,
 *                     University of Southern California
 *    Jan Issac (jan.issac@gmail.com)
 *    Manuel Wuthrich (manuel.wuthrich@gmail.com)
 *
 *
 *
 *  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 Willow Garage, Inc. 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 OWNER 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.
 *
 */

/**
 * \file joint_distribution_iid_test.cpp
 * \date Febuary 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#include <gtest/gtest.h>

#include <Eigen/Dense>

#include <fl/distribution/gaussian.hpp>
#include <fl/distribution/joint_distribution_iid.hpp>

TEST(JointDistribution_IID_Tests, fixed_fixed_single_gaussian)
{
    constexpr int dim = 2;

    typedef Eigen::Matrix<double, dim, 1> FVariate;

    typedef fl::JointDistribution<
                fl::MultipleOf<fl::Gaussian<FVariate>, 1>
            > FSingeDistribution;

    FSingeDistribution joint_distr =
        FSingeDistribution(fl::Gaussian<FVariate>());

    typedef Eigen::Matrix<double, dim, 1> ExpectedMean;
    typedef Eigen::Matrix<double, dim, dim> ExpectedCovariance;

    EXPECT_EQ(joint_distr.dimension(), dim);
    EXPECT_TRUE(fl::are_similar(joint_distr.mean(), ExpectedMean::Zero(dim, 1)));
    EXPECT_TRUE(fl::are_similar(
                    joint_distr.covariance(),
                    ExpectedCovariance::Identity(dim, dim)));
}

TEST(JointDistribution_IID_Tests, fixed_dynamic_single_gaussian)
{
    typedef Eigen::Matrix<double, -1, 1> DVariate;

    typedef fl::JointDistribution<
                fl::MultipleOf<fl::Gaussian<DVariate>, 1>
            > FSingeDistribution;

    FSingeDistribution joint_distr =
        FSingeDistribution(fl::Gaussian<DVariate>(13));

    constexpr int dim = 13;

    typedef Eigen::Matrix<double, dim, 1> ExpectedMean;
    typedef Eigen::Matrix<double, dim, dim> ExpectedCovariance;

    EXPECT_EQ(joint_distr.dimension(), dim);
    EXPECT_TRUE(
        fl::are_similar(joint_distr.mean(), ExpectedMean::Zero(dim, 1)));
    EXPECT_TRUE(
        fl::are_similar(joint_distr.covariance(),
                        ExpectedCovariance::Identity(dim, dim)));
}

TEST(JointDistribution_IID_Tests, dynamic_dynamic_single_gaussian)
{
    typedef Eigen::Matrix<double, -1, 1> DVariate;

    typedef fl::JointDistribution<
                fl::MultipleOf<fl::Gaussian<DVariate>, -1>
            > FSingeDistribution;

    FSingeDistribution joint_distr =
        FSingeDistribution(fl::Gaussian<DVariate>(13), 1);

    constexpr int dim = 13;

    typedef Eigen::Matrix<double, dim, 1> ExpectedMean;
    typedef Eigen::Matrix<double, dim, dim> ExpectedCovariance;

    EXPECT_EQ(joint_distr.dimension(), dim);
    EXPECT_TRUE(
        fl::are_similar(joint_distr.mean(), ExpectedMean::Zero(dim, 1)));
    EXPECT_TRUE(
        fl::are_similar(joint_distr.covariance(),
                        ExpectedCovariance::Identity(dim, dim)));
}

TEST(JointDistribution_IID_Tests, fixed_fixed_three_gaussian)
{
    typedef Eigen::Matrix<double, 13, 1> FVariate;

    typedef fl::JointDistribution<
                fl::MultipleOf<fl::Gaussian<FVariate>, 3>
            > FSingeDistribution;

    FSingeDistribution joint_distr =
        FSingeDistribution(fl::Gaussian<FVariate>());

    constexpr int dim =  3 * 13;

    typedef Eigen::Matrix<double, dim, 1> ExpectedMean;
    typedef Eigen::Matrix<double, dim, dim> ExpectedCovariance;

    EXPECT_EQ(joint_distr.dimension(), dim);
    EXPECT_TRUE(
        fl::are_similar(joint_distr.mean(), ExpectedMean::Zero(dim, 1)));
    EXPECT_TRUE(
        fl::are_similar(joint_distr.covariance(),
                        ExpectedCovariance::Identity(dim, dim)));
}

TEST(JointDistribution_IID_Tests, fixed_dynamic_three_gaussian)
{
    typedef Eigen::Matrix<double, -1, 1> DVariate;

    typedef fl::JointDistribution<
                fl::MultipleOf<fl::Gaussian<DVariate>, 3>
            > FSingeDistribution;

    FSingeDistribution joint_distr =
        FSingeDistribution(fl::Gaussian<DVariate>(13));

    constexpr int dim =  3 * 13;

    typedef Eigen::Matrix<double, dim, 1> ExpectedMean;
    typedef Eigen::Matrix<double, dim, dim> ExpectedCovariance;

    EXPECT_EQ(joint_distr.dimension(), dim);
    EXPECT_TRUE(
        fl::are_similar(joint_distr.mean(), ExpectedMean::Zero(dim, 1)));
    EXPECT_TRUE(
        fl::are_similar(joint_distr.covariance(),
                        ExpectedCovariance::Identity(dim, dim)));
}

TEST(JointDistribution_IID_Tests, dynamic_dynamic_three_gaussian)
{
    typedef Eigen::Matrix<double, -1, 1> DVariate;

    typedef fl::JointDistribution<
                fl::MultipleOf<fl::Gaussian<DVariate>, -1>
            > FSingeDistribution;

    FSingeDistribution joint_distr =
        FSingeDistribution(fl::Gaussian<DVariate>(13), 3);

    constexpr int dim =  3 * 13;

    typedef Eigen::Matrix<double, dim, 1> ExpectedMean;
    typedef Eigen::Matrix<double, dim, dim> ExpectedCovariance;

    EXPECT_EQ(joint_distr.dimension(), dim);
    EXPECT_TRUE(
        fl::are_similar(joint_distr.mean(), ExpectedMean::Zero(dim, 1)));
    EXPECT_TRUE(
        fl::are_similar(joint_distr.covariance(),
                        ExpectedCovariance::Identity(dim, dim)));
}


#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>
#include <cstdio>

namespace gazebo
{
	class VelocityInfoPublisher : public ModelPlugin
	{
	public:
		void Load(physics::ModelPtr _parent, sdf::ElementPtr elem)
			{
				if(!ros::isInitialized())
				{
					printf("ROS is NOT intialized. Not using velocity info plugin\n");
					return;
				}

				this->model = _parent;

				vel_info_pub = nh.advertise<geometry_msgs::TwistStamped>("/cylbot/velocity" , 1);

				twist_msg.header.stamp = ros::Time::now();
				twist_msg.header.frame_id = "/base_link";

				vel_info_pub.publish(twist_msg);

				this->updateConnection = event::Events::ConnectWorldUpdateBegin(
					boost::bind(&VelocityInfoPublisher::OnUpdate, this, _1));
			}

		void OnUpdate(const common::UpdateInfo& info)
			{
				math::Vector3 linear_vel = this->model->GetRelativeLinearVel();
				math::Vector3 angular_vel = this->model->GetRelativeAngularVel();

				twist_msg.header.stamp = ros::Time::now();

				// get translational velocity in plane
				twist_msg.twist.linear.x = linear_vel.x;
				twist_msg.twist.linear.y = linear_vel.y;

				// get the yaw
				twist_msg.twist.angular.z = angular_vel.z;

				vel_info_pub.publish(twist_msg);
			}

	private:
		physics::ModelPtr model;
		event::ConnectionPtr updateConnection;

		ros::NodeHandle nh;
		ros::Publisher vel_info_pub;

		geometry_msgs::TwistStamped twist_msg;
	};

	GZ_REGISTER_MODEL_PLUGIN(VelocityInfoPublisher)
}


/*
Copyright (c) 2016 Bastien Durix

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

/**
 *  \brief 2D skeletonization
 *  \author Bastien Durix
 */

#include <boost/program_options.hpp>
#include <time.h>
#include <iostream>
#include <iomanip>
#include <chrono>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>

#include <shape/DiscreteShape.h>
#include <boundary/DiscreteBoundary.h>
#include <skeleton/Skeletons.h>

#include <algorithm/extractboundary/NaiveBoundary.h>
#include <algorithm/skeletonization/propagation/SpherePropagation2D.h>
#include <algorithm/skeletonization/voronoi/VoronoiSkeleton2D.h>
#include <algorithm/skinning/Filling.h>
#include <algorithm/evaluation/ReprojError.h>
#include <algorithm/pruning/ScaleAxisTransform.h>
#include <algorithm/pruning/LambdaMedialAxis.h>
#include <algorithm/pruning/ThetaMedialAxis.h>

#include <displayopencv/DisplayShapeOCV.h>
#include <displayopencv/DisplayBoundaryOCV.h>
#include <displayopencv/DisplaySkeletonOCV.h>

std::tuple<double,double,int,int> EvalSkel(const shape::DiscreteShape<2>::Ptr dissh,
									   const boundary::DiscreteBoundary<2>::Ptr disbnd,
									   const skeleton::GraphSkel2d::Ptr skel)
{
	shape::DiscreteShape<2>::Ptr shp(new shape::DiscreteShape<2>(dissh->getWidth(),dissh->getHeight()));
	algorithm::skinning::Filling(shp,skel);

	double res = algorithm::evaluation::SymDiffArea(dissh,shp);
	double res2 = algorithm::evaluation::HausDist(skel,disbnd,dissh->getFrame());

	std::list<unsigned int> lnod;
	skel->getAllNodes(lnod);
	unsigned int nbbr = 0;
	for(std::list<unsigned int>::iterator it = lnod.begin(); it != lnod.end(); it++)
	{
		unsigned int deg = skel->getNodeDegree(*it);
		if(deg != 2)
			nbbr += deg;
	}
	nbbr /= 2;
	std::tuple<double,double,int,int> result = std::make_tuple(res*100.0,res2,skel->getNbNodes(),nbbr);

	return result;
}

int main(int argc, char** argv)
{
	std::string imgfile, fileskl, fileimg, filebnd;
	bool output = false;
	bool compare = false;
	bool eval = false;
	double alpha, noise;

	boost::program_options::options_description desc("OPTIONS");

	desc.add_options()
		("help", "Help message")
		("imgfile", boost::program_options::value<std::string>(&imgfile)->default_value("mask"), "Binary image file (*.png)")
		("output", boost::program_options::value<bool>(&output)->implicit_value(true), "Returns output images")
		("sigma", boost::program_options::value<double>(&noise)->default_value(1.0), "Shape noise (sigma parameter)")
		("alpha", boost::program_options::value<double>(&alpha)->default_value(2.1), "Skeleton precision (alpha parameter)")
		("fileimg", boost::program_options::value<std::string>(&fileimg)->default_value("skelpropagortho"), "Skeleton img file")
		("compare", boost::program_options::value<bool>(&compare)->implicit_value(true), "Compare result with pruning methods")
		;

	boost::program_options::variables_map vm;
	boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
	boost::program_options::notify(vm);

	if (vm.count("help")) {
		std::cout << desc << std::endl;
		return 0;
	}

	time_t start,end;
	double diff;

	cv::Mat shpimggray = cv::imread(imgfile,cv::ImreadModes::IMREAD_GRAYSCALE);
	cv::Mat shpimg;
	cv::threshold(shpimggray,shpimg,125,255,cv::THRESH_BINARY);

	// topological closure
	cv::Mat shpdil;
	cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT,cv::Size(3,3),cv::Point(1,1));
	cv::dilate(shpimg,shpdil,element);
	cv::erode(shpdil,shpimg,element);
	shape::DiscreteShape<2>::Ptr dissh = shape::DiscreteShape<2>::Ptr(new shape::DiscreteShape<2>(shpimg.cols,shpimg.rows));
	cv::Mat cpymat(shpimg.rows,shpimg.cols,CV_8U,&dissh->getContainer()[0]);
	shpimg.copyTo(cpymat);
	cv::Mat image(shpimg.rows,shpimg.cols,CV_8UC3,cv::Scalar(255,255,255));

	displayopencv::DisplayDiscreteShape(dissh,image,dissh->getFrame(),cv::Scalar(0,0,255));

	boundary::DiscreteBoundary<2>::Ptr disbnd = algorithm::extractboundary::NaiveBoundary(dissh);

	auto start0 = std::chrono::steady_clock::now();
	algorithm::skeletonization::OptionsSphProp options(noise,alpha);
	skeleton::GraphSkel2d::Ptr grskelpropag = algorithm::skeletonization::SpherePropagation2D(disbnd,dissh,options);
	auto duration0 = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start0);
	std::tuple<double,double,int,int> respropag = EvalSkel(dissh,disbnd,grskelpropag);
	int t0 = duration0.count();
	double A0 = std::get<0>(respropag); // sym area diff
	double H0 = std::get<1>(respropag); // Hausdorff dist
	int N0 = std::get<2>(respropag); // nb nodes
	int B0 = std::get<3>(respropag); // nb branches

	if(H0 > alpha)
		std::cerr << "Problem while computing propagation" << std::endl;

	std::cout << "Propagation skeleton computation: " << duration0.count() << "ms." << std::endl;

	shape::DiscreteShape<2>::Ptr shppropag(new shape::DiscreteShape<2>(dissh->getWidth(),dissh->getHeight()));
	algorithm::skinning::Filling(shppropag,grskelpropag);

	cv::Mat imagepropag;
	image.copyTo(imagepropag);
	displayopencv::DisplayDiscreteShape(shppropag,imagepropag,shppropag->getFrame(),cv::Scalar(0,255,0));
	displayopencv::DisplayDiscreteBoundary(disbnd,imagepropag,dissh->getFrame(),cv::Scalar(0,0,0));
	displayopencv::DisplayGraphSkeleton(grskelpropag,imagepropag,dissh->getFrame(),cv::Scalar(255,0,0));

	if(output)
	{
		std::ostringstream oss;
		oss.setf( std::ios::fixed, std:: ios::floatfield );
		oss.precision(1);
		oss << fileimg << ".png";
		cv::imwrite(oss.str(), imagepropag);
	}

	if(compare)
	{
		std::cout.setf(std::ios_base::fixed, std::ios_base::floatfield);
		std::cout.precision(2);

		auto startv = std::chrono::steady_clock::now();
		skeleton::GraphSkel2d::Ptr grskelvoro = algorithm::skeletonization::VoronoiSkeleton2d(disbnd);
		auto durationv = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - startv);
		std::tuple<double,double,int,int> resv = EvalSkel(dissh,disbnd,grskelvoro);
		int tv = durationv.count();
		double Av = std::get<0>(resv); // sym area diff
		double Hv = std::get<1>(resv); // Hausdorff dist
		int Nv = std::get<2>(resv); // nb nodes
		int Bv = std::get<3>(resv); // nb branches

		int t1;
		double A1;
		double H1;
		int N1;
		int B1;
		double sat = 1.2;
		auto start1 = std::chrono::steady_clock::now();
		skeleton::GraphSkel2d::Ptr grskelsat = algorithm::pruning::ScaleAxisTransform(grskelvoro,sat);
		auto duration1 = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start1);
		std::tuple<double,double,int,int> res1 = EvalSkel(dissh,disbnd,grskelsat);
		t1 = tv + duration1.count();
		A1 = std::get<0>(res1); // sym area diff
		H1 = std::get<1>(res1); // Hausdorff dist
		N1 = std::get<2>(res1); // nb nodes
		B1 = std::get<3>(res1); // nb branches

		if(output)
		{
			shape::DiscreteShape<2>::Ptr shpskel(new shape::DiscreteShape<2>(dissh->getWidth(),dissh->getHeight()));
			algorithm::skinning::Filling(shpskel,grskelsat);

			cv::Mat imagerec;
			image.copyTo(imagerec);
			displayopencv::DisplayDiscreteShape(shpskel,imagerec,shpskel->getFrame(),cv::Scalar(0,255,0));
			displayopencv::DisplayDiscreteBoundary(disbnd,imagerec,dissh->getFrame(),cv::Scalar(0,0,0));
			displayopencv::DisplayGraphSkeleton(grskelsat,imagerec,dissh->getFrame(),cv::Scalar(255,0,0));

			std::ostringstream oss;
			oss.setf( std::ios::fixed, std:: ios::floatfield );
			oss.precision(1);
			oss << "sat.png";
			cv::imwrite(oss.str(), imagerec);
		}

		int t2;
		double A2;
		double H2;
		int N2;
		int B2;
		double lambda = 2.0;
		auto start2 = std::chrono::steady_clock::now();
		skeleton::GraphSkel2d::Ptr grskellambda = algorithm::pruning::LambdaMedialAxis(grskelvoro,disbnd,lambda);
		auto duration2 = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start2);
		std::tuple<double,double,int,int> res2 = EvalSkel(dissh,disbnd,grskellambda);
		t2 = tv + duration2.count();
		A2 = std::get<0>(res2); // sym area diff
		H2 = std::get<1>(res2); // Hausdorff dist
		N2 = std::get<2>(res2); // nb nodes
		B2 = std::get<3>(res2); // nb branches

		if(output)
		{
			shape::DiscreteShape<2>::Ptr shpskel(new shape::DiscreteShape<2>(dissh->getWidth(),dissh->getHeight()));
			algorithm::skinning::Filling(shpskel,grskellambda);

			cv::Mat imagerec;
			image.copyTo(imagerec);
			displayopencv::DisplayDiscreteShape(shpskel,imagerec,shpskel->getFrame(),cv::Scalar(0,255,0));
			displayopencv::DisplayDiscreteBoundary(disbnd,imagerec,dissh->getFrame(),cv::Scalar(0,0,0));
			displayopencv::DisplayGraphSkeleton(grskellambda,imagerec,dissh->getFrame(),cv::Scalar(255,0,0));

			std::ostringstream oss;
			oss.setf( std::ios::fixed, std:: ios::floatfield );
			oss.precision(0);
			oss << "lambda.png";
			cv::imwrite(oss.str(), imagerec);
		}

		int t3;
		double A3;
		double H3;
		int N3;
		int B3;
		double theta = 100*M_PI/180.0;
		auto start3 = std::chrono::steady_clock::now();
		skeleton::GraphSkel2d::Ptr grskeltheta = algorithm::pruning::ThetaMedialAxis(grskelvoro,theta);
		auto duration3 = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start3);
		std::tuple<double,double,int,int> res3 = EvalSkel(dissh,disbnd,grskeltheta);
		t3 = tv + duration3.count();
		A3 = std::get<0>(res3); // sym area diff
		H3 = std::get<1>(res3); // Hausdorff dist
		N3 = std::get<2>(res3); // nb nodes
		B3 = std::get<3>(res3); // nb branches

		if(output)
		{
			shape::DiscreteShape<2>::Ptr shpskel(new shape::DiscreteShape<2>(dissh->getWidth(),dissh->getHeight()));
			algorithm::skinning::Filling(shpskel,grskeltheta);

			cv::Mat imagerec;
			image.copyTo(imagerec);
			displayopencv::DisplayDiscreteShape(shpskel,imagerec,shpskel->getFrame(),cv::Scalar(0,255,0));
			displayopencv::DisplayDiscreteBoundary(disbnd,imagerec,dissh->getFrame(),cv::Scalar(0,0,0));
			displayopencv::DisplayGraphSkeleton(grskeltheta,imagerec,dissh->getFrame(),cv::Scalar(255,0,0));

			std::ostringstream oss;
			oss.setf( std::ios::fixed, std:: ios::floatfield );
			oss.precision(2);
			oss << "theta.png";
			cv::imwrite(oss.str(), imagerec);
		}

		std::cout << "\t Time \t SymArea  Hausdorff  Branches \t Nodes" << std::endl;
		std::cout << "Propag : " << t0 << " \t " << A0 << " \t  " << H0 << " \t     " << B0 << " \t " << N0 << std::endl;
		std::cout << "Voro   : " << tv << " \t " << Av << " \t  " << Hv << " \t     " << Bv << " \t " << Nv << std::endl;
		std::cout << "SAT    : " << t1 << " \t " << A1 << " \t  " << H1 << " \t     " << B1 << " \t " << N1 << std::endl;
		std::cout << "Lambda : " << t2 << " \t " << A2 << " \t  " << H2 << " \t     " << B2 << " \t " << N2 << std::endl;
		std::cout << "Theta  : " << t3 << " \t " << A3 << " \t  " << H3 << " \t     " << B3 << " \t " << N3 << std::endl;
	}

	return 0;
}


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

#include <nt2/include/functor.hpp>
#include <nt2/include/constants/logeps.hpp>
#include <nt2/sdk/meta/generative_hierarchy.hpp>
#include <nt2/core/container/dsl/generative.hpp>
#include <nt2/core/functions/common/generative.hpp>

#include <nt2/sdk/parameters.hpp>
#include <boost/preprocessor/arithmetic/inc.hpp>
#include <boost/preprocessor/repetition/repeat_from_to.hpp>

namespace nt2
{
  #if defined(DOXYGEN_ONLY)
  /*!
    @brief Logeps generator

    Create an array full of log(eps).

    @par Semantic:

    logeps() semantic depends of its parameters type and quantity:

    - The following code:
      @code
      double x = logeps();
      @endcode
      is equivalent to
      @code
      double x = log(eps);
      @endcode

    - For any Integer @c n, the following code:
      @code
      auto x = logeps(n);
      @endcode
      generates an expression that evaluates as a @size2d{n,n} table filled with
      @c double log(eps)

    - For any Integer @c sz1,...,szn , the following code:
      @code
      auto x = logeps(sz1,...,szn);
      @endcode
      generates an expression that evaluates as a @sizes{sz1,szn} table filled
      @c double log(eps)

    - For any Expression @c dims evaluating as a row vector of @c N elements,
      the following code:
      @code
      auto x = logeps(dims);
      @endcode
      generates an expression that evaluates as a @sizes{dims(1),dims(N)}
      table filled with @c double log(eps)

    - For any Fusion Sequence @c dims of @c N elements, the following code:
      @code
      auto x = logeps(dims);
      @endcode
      generates an expression that evaluates as a @sizes{at_c<0>(dims),at_c<N-1>(dims)}
      table filled with @c double log(eps)

    - For any type @c T, the following code:
      @code
      T x = logeps( as_<T>() );
      @endcode
      is equivalent to
      @code
      T x = T(2);
      @endcode

    - For any Integer @c n and any type @c T, the following code:
      @code
      auto x = logeps(n, as_<T>());
      @endcode
      generates an expression that evaluates as a @size2d{n,n} table filled with
      type @c T logepss.

    - For any Integer @c sz1,...,szN and any type @c T, the following code:
      @code
      auto x = logeps(sz1,...,szn, as_<T>());
      @endcode
      generates an expression that evaluates as a @sizes{sz1,szn} table filled
      with type @c T logepss.

    - For any Expression @c dims evaluating as a row vector of @c N elements
      and any type @c T, the following code:
      @code
      auto x = logeps(dims, as_<T>());
      @endcode
      generates an expression that evaluates as a @sizes{dims(1),dims(N)}
      table filled with type @c T logepss.

    - For any Fusion Sequence @c dims of @c N elements and any type @c T, the
      following code:
      @code
      auto x = logeps(dims, as_<T>());
      @endcode
      generates an expression that evaluates as a @sizes{at_c<0>(dims),at_c<N-1>(dims)}
      table filled with type @c T logepss.

    @param dims Size of each dimension, specified as one or more integer values
                or as a row vector of integer values. If any @c dims is lesser
                or equal to 0, then the resulting expression is empty.

    @param classname  Type specifier of the output. If left unspecified, the
                      resulting expression behaves as an array of double.

    @return An Expression evaluating as an array of a given type and dimensions
            filled with the @c Logeps constant.

    @sa Logeps
  **/
  template<typename... Args, typename ClassName>
  details::unspecified logeps(Args const&... dims, ClassName const& classname);

  /// @overload
  template<typename... Args> details::unspecified logeps(Args const&... dims);

  /// @overload
  template<typename ClassName> ClassName::type logeps(ClassName const& classname);

  /// @overload
  double logeps();

  #else

  #define M0(z,n,t)                                                            \
  NT2_FUNCTION_IMPLEMENTATION(nt2::tag::Logeps,logeps, n)                            \
  /**/

  BOOST_PP_REPEAT_FROM_TO(1,BOOST_PP_INC(BOOST_PP_INC(NT2_MAX_DIMENSIONS)),M0,~)

  #undef M0

  #endif
}

namespace nt2 { namespace ext
{
  /// INTERNAL ONLY
  template<typename Domain, typename Expr, int N>
  struct  value_type<tag::Logeps,Domain,N,Expr>
        : meta::generative_value<Expr>
  {};

  /// INTERNAL ONLY
  template<typename Domain, typename Expr, int N>
  struct  size_of<tag::Logeps,Domain,N,Expr>
        : meta::generative_size<Expr>
  {};
} }
#endif


// Boost.Geometry
// Unit Test

// Copyright (c) 2016-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to 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 BOOST_GEOMETRY_TEST_STRATEGIES_SEGMENT_INTERSECTION_SPH_HPP
#define BOOST_GEOMETRY_TEST_STRATEGIES_SEGMENT_INTERSECTION_SPH_HPP

#include <geometry_test_common.hpp>

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

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

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

#include <boost/geometry/policies/relate/direction.hpp>
#include <boost/geometry/policies/relate/intersection_points.hpp>
#include <boost/geometry/policies/relate/tupled.hpp>

template <typename T>
bool equals_relaxed_val(T const& v1, T const& v2, T const& eps_scale)
{
    T const c1 = 1;
    T relaxed_eps = std::numeric_limits<T>::epsilon()
        * bg::math::detail::greatest(c1, bg::math::abs(v1), bg::math::abs(v2))
        * eps_scale;
    return bg::math::abs(v1 - v2) <= relaxed_eps;
}

template <typename P1, typename P2, typename T>
bool equals_relaxed(P1 const& p1, P2 const& p2, T const& eps_scale)
{
    typedef typename bg::select_coordinate_type<P1, P2>::type calc_t;
    calc_t c1 = 1;
    calc_t p10 = bg::get<0>(p1);
    calc_t p11 = bg::get<1>(p1);
    calc_t p20 = bg::get<0>(p2);
    calc_t p21 = bg::get<1>(p2);
    calc_t relaxed_eps = std::numeric_limits<calc_t>::epsilon()
        * bg::math::detail::greatest(c1, bg::math::abs(p10), bg::math::abs(p11), bg::math::abs(p20), bg::math::abs(p21))
        * eps_scale;
    calc_t diff0 = p10 - p20;
    // needed e.g. for -179.999999 - 180.0
    if (diff0 < -180)
        diff0 += 360;
    return bg::math::abs(diff0) <= relaxed_eps
        && bg::math::abs(p11 - p21) <= relaxed_eps;
}

template <typename S1, typename S2, typename Strategy, typename P>
void test_strategy_one(S1 const& s1, S2 const& s2,
                       Strategy const& strategy,
                       char m, std::size_t expected_count,
                       P const& ip0 = P(), P const& ip1 = P(),
                       int opposite_id = -1)
{
    typedef typename bg::coordinate_type<P>::type coord_t;
    typedef bg::policies::relate::segments_tupled
                <
                    bg::policies::relate::segments_intersection_points
                        <
                            bg::segment_intersection_points<P, bg::segment_ratio<coord_t> >
                        >,
                    bg::policies::relate::segments_direction
                > policy_t;

    typedef typename policy_t::return_type return_type;

    // NOTE: robust policy is currently ignored
    return_type res = strategy.apply(s1, s2, policy_t(), 0);

    size_t const res_count = boost::get<0>(res).count;
    char const res_method = boost::get<1>(res).how;

    BOOST_CHECK_MESSAGE(res_method == m,
                        "IP method: " << res_method << " different than expected: " << m
                            << " for " << bg::wkt(s1) << " and " << bg::wkt(s2));

    BOOST_CHECK_MESSAGE(res_count == expected_count,
                        "IP count: " << res_count << " different than expected: " << expected_count
                            << " for " << bg::wkt(s1) << " and " << bg::wkt(s2));

    // The EPS is scaled because during the conversion various angles may be not converted
    // to cartesian 3d the same way which results in a different intersection point
    // For more information read the info in spherical intersection strategy file.

    // Plus in geographic CS the result strongly depends on the compiler,
    // probably due to differences in FP trigonometric function implementations

    int eps_scale = 1;
    bool is_geographic = boost::is_same<typename bg::cs_tag<S1>::type, bg::geographic_tag>::value;
    if (is_geographic)
    {
        eps_scale = 100000;
    }
    else
    {
        eps_scale = res_method != 'i' ? 1 : 1000;
    }

    if (res_count > 0 && expected_count > 0)
    {
        P const& res_i0 = boost::get<0>(res).intersections[0];
        coord_t denom_a0 = boost::get<0>(res).fractions[0].robust_ra.denominator();
        coord_t denom_b0 = boost::get<0>(res).fractions[0].robust_rb.denominator();
        BOOST_CHECK_MESSAGE(equals_relaxed(res_i0, ip0, eps_scale),
                            "IP0: " << std::setprecision(16) << bg::wkt(res_i0) << " different than expected: " << bg::wkt(ip0)
                                << " for " << bg::wkt(s1) << " and " << bg::wkt(s2));
        BOOST_CHECK_MESSAGE(denom_a0 > coord_t(0),
                            "IP0 fraction A denominator: " << std::setprecision(16) << denom_a0 << " is incorrect");
        BOOST_CHECK_MESSAGE(denom_b0 > coord_t(0),
                            "IP0 fraction B denominator: " << std::setprecision(16) << denom_b0 << " is incorrect");
    }
    if (res_count > 1 && expected_count > 1)
    {
        P const& res_i1 = boost::get<0>(res).intersections[1];
        coord_t denom_a1 = boost::get<0>(res).fractions[1].robust_ra.denominator();
        coord_t denom_b1 = boost::get<0>(res).fractions[1].robust_rb.denominator();
        BOOST_CHECK_MESSAGE(equals_relaxed(res_i1, ip1, eps_scale),
                            "IP1: " << std::setprecision(16) << bg::wkt(res_i1) << " different than expected: " << bg::wkt(ip1)
                                << " for " << bg::wkt(s1) << " and " << bg::wkt(s2));
        BOOST_CHECK_MESSAGE(denom_a1 > coord_t(0),
                            "IP1 fraction A denominator: " << std::setprecision(16) << denom_a1 << " is incorrect");
        BOOST_CHECK_MESSAGE(denom_b1 > coord_t(0),
                            "IP1 fraction B denominator: " << std::setprecision(16) << denom_b1 << " is incorrect");
    }

    if (opposite_id >= 0)
    {
        bool opposite = opposite_id != 0;
        BOOST_CHECK_MESSAGE(opposite == boost::get<1>(res).opposite,
                            bg::wkt(s1) << " and " << bg::wkt(s2) << (opposite_id == 0 ? " are not " : " are ") << "opposite" );
    }
}

template <typename T>
T translated(T v, double t)
{
    v += T(t);
    if (v > 180)
        v -= 360;
    return v;
}

template <typename S1, typename S2, typename Strategy, typename P>
void test_strategy(S1 const& s1, S2 const& s2,
                   Strategy const& strategy,
                   char m, std::size_t expected_count,
                   P const& ip0 = P(), P const& ip1 = P(),
                   int opposite_id = -1)
{
    S1 s1t = s1;
    S2 s2t = s2;
    P ip0t = ip0;
    P ip1t = ip1;

#ifndef BOOST_GEOMETRY_TEST_GEO_INTERSECTION_TEST_SIMILAR
    test_strategy_one(s1t, s2t, strategy, m, expected_count, ip0t, ip1t, opposite_id);
#else
    double t = 0;
    for (int i = 0; i < 4; ++i, t += 90)
    {
        bg::set<0, 0>(s1t, translated(bg::get<0, 0>(s1), t));
        bg::set<1, 0>(s1t, translated(bg::get<1, 0>(s1), t));
        bg::set<0, 0>(s2t, translated(bg::get<0, 0>(s2), t));
        bg::set<1, 0>(s2t, translated(bg::get<1, 0>(s2), t));
        if (expected_count > 0)
            bg::set<0>(ip0t, translated(bg::get<0>(ip0), t));
        if (expected_count > 1)
            bg::set<0>(ip1t, translated(bg::get<0>(ip1), t));

        test_strategy_one(s1t, s2t, strategy, m, expected_count, ip0t, ip1t, opposite_id);
    }
#endif
}

template <typename S1, typename S2, typename P, typename Strategy>
void test_strategy(std::string const& s1_wkt, std::string const& s2_wkt,
                   Strategy const& strategy,
                   char m, std::size_t expected_count,
                   std::string const& ip0_wkt = "", std::string const& ip1_wkt = "",
                   int opposite_id = -1)
{
    S1 s1;
    S2 s2;
    P ip0, ip1;

    bg::read_wkt(s1_wkt, s1);
    bg::read_wkt(s2_wkt, s2);
    if (! ip0_wkt.empty())
        bg::read_wkt(ip0_wkt, ip0);
    if (!ip1_wkt.empty())
        bg::read_wkt(ip1_wkt, ip1);

    test_strategy(s1, s2, strategy, m, expected_count, ip0, ip1, opposite_id);
}

template <typename S, typename P, typename Strategy>
void test_strategy(std::string const& s1_wkt, std::string const& s2_wkt,
                   Strategy const& strategy,
                   char m, std::size_t expected_count,
                   std::string const& ip0_wkt = "", std::string const& ip1_wkt = "",
                   int opposite_id = -1)
{
    test_strategy<S, S, P>(s1_wkt, s2_wkt, strategy, m, expected_count, ip0_wkt, ip1_wkt, opposite_id);
}

#endif // BOOST_GEOMETRY_TEST_STRATEGIES_SEGMENT_INTERSECTION_SPH_HPP