/*******************************************************************************
 * Abstract domain described in Section 4 from the paper "An Abstract
 * Domain of Uninterpreted Functions" by Gange, Navas, Schachte,
 * Sondergaard, and Stuckey published in VMCAI'16.
 *
 * Each program variable is mapped to a syntactic term (aka
 * uninterpreted function). The join is antiunification and the meet
 * is a pseudo-meet based on the classical congruence closure
 * algorithm. The domain is suitable to infer equalities between
 * variables.
 ******************************************************************************/

#pragma once

#include <crab/domains/abstract_domain.hpp>
#include <crab/domains/abstract_domain_specialized_traits.hpp>
#include <crab/domains/backward_assign_operations.hpp>
#include <crab/domains/term/term_expr.hpp>
#include <crab/domains/term/term_operators.hpp>
#include <crab/support/debug.hpp>
#include <crab/support/stats.hpp>

#include <algorithm>
#include <map>
#include <set>
#include <utility>
#include <vector>

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

namespace crab {
namespace domains {

// TODO: factorize code. uf_domain and term_domain share a lot of
// code.
template <typename Number, typename VariableName>
class uf_domain final
    : public abstract_domain_api<uf_domain<Number, VariableName>> {

  using uf_domain_t = uf_domain<Number, VariableName>;
  using abstract_domain_t = abstract_domain_api<uf_domain_t>;

public:
  using typename abstract_domain_t::disjunctive_linear_constraint_system_t;
  using typename abstract_domain_t::interval_t;
  using typename abstract_domain_t::linear_constraint_system_t;
  using typename abstract_domain_t::linear_constraint_t;
  using typename abstract_domain_t::linear_expression_t;
  using typename abstract_domain_t::reference_constraint_t;
  using typename abstract_domain_t::variable_or_constant_t;
  using typename abstract_domain_t::variable_or_constant_vector_t;
  using typename abstract_domain_t::variable_t;
  using typename abstract_domain_t::variable_vector_t;
  using number_t = Number;
  using varname_t = VariableName;

private:
  using ttbl_t = term::term_table<number_t, term::term_operator_t>;
  using term_id_t = typename ttbl_t::term_id_t;
  using term_t = typename ttbl_t::term_t;
  using const_term_t = typename ttbl_t::const_term_t;
  using var_term_t = typename ttbl_t::var_term_t;
  using ftor_term_t = typename ttbl_t::ftor_term_t;

  using var_map_t = boost::container::flat_map<variable_t, term_id_t>;
  using var_set_t = std::set<variable_t>;
  using rev_var_map_t = boost::container::flat_map<term_id_t, var_set_t>;
  using linterm_t = typename linear_expression_t::component_t;

  bool m_is_bottom;
  ttbl_t m_ttbl;
  var_map_t m_var_map;
  rev_var_map_t m_rev_var_map;

  uf_domain(bool is_top) : m_is_bottom(!is_top) {}

  uf_domain(ttbl_t &&tbl, var_map_t &&vm, rev_var_map_t &&rvm)
      : m_is_bottom(false), m_ttbl(std::move(tbl)), m_var_map(std::move(vm)),
        m_rev_var_map(std::move(rvm)) {
    check_terms(__LINE__);
  }

  void check_terms(int line) const {
    CRAB_LOG(
        "uf-check-terms",
        for (auto const &p
             : m_var_map) {
          if (!(p.second < m_ttbl.size())) {
            CRAB_ERROR("term_equiv.hpp at line=", line, ": ",
                       "term id is not the table term");
          }
        }

        for (auto kv
             : m_rev_var_map) {
          for (auto v : kv.second) {
            auto it = m_var_map.find(v);
            if (it->second != kv.first) {
              CRAB_ERROR("term_equiv.hpp at line=", line, ": ", v,
                         " is mapped to t", it->second,
                         " but the reverse map says that should be t",
                         kv.first);
            }
          }
        });
  }

  void deref(term_id_t t) {
    std::vector<term_id_t> forgotten /*unused*/;
    m_ttbl.deref(t, forgotten);
  }

  /* Begin manipulate the reverse variable map */
  void add_rev_var_map(rev_var_map_t &rvmap, term_id_t t, variable_t v) const {
    auto it = rvmap.find(t);
    if (it != rvmap.end()) {
      it->second.insert(v);
    } else {
      var_set_t varset;
      varset.insert(v);
      rvmap.insert(std::make_pair(t, varset));
    }
  }

  void remove_rev_var_map(term_id_t t, const variable_t &v) {
    auto it = m_rev_var_map.find(t);
    if (it != m_rev_var_map.end()) {
      it->second.erase(v);
      if (it->second.empty()) {
        m_rev_var_map.erase(it);
      }
    }
  }
  /* End manipulate the reverse variable map */

  const term_t &get_term(term_id_t t) const {
    const term_t *ptr_t = m_ttbl.get_term_ptr(t);
    assert(ptr_t);
    return *ptr_t;
  }

  void rebind_var(const variable_t &x, term_id_t tx) {
    m_ttbl.add_ref(tx);

    auto it(m_var_map.find(x));
    if (it != m_var_map.end()) {
      remove_rev_var_map((*it).second, x);
      deref((*it).second);
      m_var_map.erase(it);
    }
    m_var_map.insert(std::make_pair(x, tx));
    add_rev_var_map(m_rev_var_map, tx, x);
  }

  term_id_t term_of_const(const number_t &n) {
    boost::optional<term_id_t> opt_tn(m_ttbl.find_const(n));
    if (opt_tn) {
      return *opt_tn;
    } else {
      return m_ttbl.make_const(n);
    }
  }

  term_id_t term_of_var(variable_t v, var_map_t &var_map,
                        rev_var_map_t &rvar_map, ttbl_t &ttbl) {
    auto it(var_map.find(v));
    if (it != var_map.end()) {
      // assert((*it).first == v);
      assert(ttbl.size() > (*it).second);
      return (*it).second;
    } else {
      // Allocate a fresh term
      term_id_t id(ttbl.fresh_var());
      var_map[v] = id;
      add_rev_var_map(rvar_map, id, v);
      ttbl.add_ref(id);
      return id;
    }
  }

  term_id_t term_of_var(variable_t v) {
    return term_of_var(v, m_var_map, m_rev_var_map, m_ttbl);
  }

  term_id_t term_of_linterm(linterm_t term) {
    if (term.first == 1) {
      return term_of_var(term.second);
    } else {
      return build_term(term::conv2termop(OP_MULTIPLICATION),
                        term_of_const(term.first), term_of_var(term.second));
    }
  }

  term_id_t build_term(term::term_operator_t op, term_id_t tx) {
    std::vector<term_id_t> ids = {tx};
    return build_term(op, ids);
  }

  term_id_t build_term(term::term_operator_t op, term_id_t tx, term_id_t ty) {
    std::vector<term_id_t> ids = {tx,ty};
    return build_term(op, ids);
  }

  term_id_t build_term(term::term_operator_t op, const std::vector<term_id_t> &ids) {
    boost::optional<term_id_t> eopt(m_ttbl.find_ftor(op, ids));
    if (eopt) {
      return *eopt;
    } else {
      term_id_t tx = m_ttbl.apply_ftor(op, ids);
      return tx;
    }
  }

  term_id_t build_linexpr(const linear_expression_t &e) {
    number_t cst = e.constant();
    typename linear_expression_t::const_iterator it(e.begin());
    if (it == e.end()) {
      return term_of_const(cst);
    }

    term_id_t t;
    if (cst == 0) {
      t = term_of_linterm(*it);
      ++it;
    } else {
      t = term_of_const(cst);
    }
    for (; it != e.end(); ++it) {
      t = build_term(term::conv2termop(OP_ADDITION), t, term_of_linterm(*it));
    }

    return t;
  }

  boost::optional<std::pair<variable_t, variable_t>>
  get_eq_or_diseq(linear_constraint_t cst) {
    if (cst.is_equality() || cst.is_disequation()) {
      if (cst.size() == 2 && cst.constant() == 0) {
        auto it = cst.begin();
        auto nx = it->first;
        auto vx = it->second;
        ++it;
        assert(it != cst.end());
        auto ny = it->first;
        auto vy = it->second;
        if (nx == (ny * -1)) {
          return std::make_pair(vx, vy);
        }
      }
    }
    return boost::optional<std::pair<variable_t, variable_t>>();
  }

  // helper for pseudo-meet: choose one non-var term from the
  // equivalence class associated with t.
  template <typename Range>
  boost::optional<term_id_t> choose_non_var(ttbl_t &ttbl,
                                            const Range &terms) const {
    std::vector<term_id_t> non_var_terms(terms.size());
    auto it = std::copy_if(terms.begin(), terms.end(), non_var_terms.begin(),
                           [&ttbl](term_id_t t) {
                             term_t *t_ptr = ttbl.get_term_ptr(t);
                             return (t_ptr && t_ptr->kind() == term::TERM_APP);
                           });
    non_var_terms.resize(std::distance(non_var_terms.begin(), it));
    if (non_var_terms.empty()) {
      return boost::optional<term_id_t>();
    } else {
      // TODO: the heuristics as described in the VMCAI'16 paper
      // that chooses the one that has more references each class.
      return *(non_var_terms.begin());
    }
  }

  // helper for pseudo-meet
  term_id_t build_dag_term(ttbl_t &ttbl, int t,
                           term::congruence_closure_solver<ttbl_t> &solver,
                           ttbl_t &out_ttbl, std::vector<int> &stack,
                           std::map<int, term_id_t> &cache) const {

    // already processed
    auto it = cache.find(t);
    if (it != cache.end()) {
      CRAB_LOG("uf-meet", crab::outs() << "build_dag_term. Found in cache: ";
               crab::outs() << "t" << t << " --> "
                            << "t" << it->second << "\n";);
      return it->second;
    }

    // break the cycle with a fresh variable
    if (std::find(stack.begin(), stack.end(), t) != stack.end()) {
      term_id_t v = out_ttbl.fresh_var();
      CRAB_LOG("uf-meet", crab::outs() << "build_dag_term. Detected cycle: ";
               crab::outs() << "t" << t << " --> "
                            << "t" << v << "\n";);
      return v;
    }

    stack.push_back(t);
    auto membs = solver.get_members(t);
    boost::optional<term_id_t> f = choose_non_var(ttbl, membs);

    if (!f) {
      // no concrete definition exists return a fresh variable
      term_id_t v = out_ttbl.fresh_var();
      auto res = cache.insert(std::make_pair(t, v));
      stack.pop_back();
      CRAB_LOG("uf-meet", crab::outs()
                              << "build_dag_term. No concrete definition: ";
               crab::outs() << "t" << t << " --> "
                            << "t" << (res.first)->second << "\n";);
      return (res.first)->second;
    } else {
      // traverse recursively the term
      term_t *f_ptr = ttbl.get_term_ptr(*f);
      CRAB_LOG("uf-meet",
               crab::outs()
                   << "build_dag_term. Traversing recursively the term "
                   << "t" << *f << ":";
               crab::outs() << *f_ptr << "\n";);
      const std::vector<term_id_t> &args(term::term_args(f_ptr));
      std::vector<term_id_t> res_args;
      res_args.reserve(args.size());
      for (term_id_t c : args) {
        res_args.push_back(build_dag_term(ttbl, solver.get_class(c), solver,
                                          out_ttbl, stack, cache));
      }
      auto res = cache.insert(std::make_pair(
          t, out_ttbl.apply_ftor(term::term_ftor(f_ptr), res_args)));
      stack.pop_back();
      CRAB_LOG("uf-meet", crab::outs()
                              << "build_dag_term. Finished recursive case: ";
               crab::outs() << "t" << t << " --> "
                            << "t" << (res.first)->second << "\n";);
      return (res.first)->second;
    }
  }

  void print_term(const term_t &t, crab_os &o) const {
    if (t.kind() == term::TERM_CONST) {
      const const_term_t *ct = static_cast<const const_term_t *>(&t);
      o << ct->val;
    } else if (t.kind() == term::TERM_VAR) {
      const var_term_t *vt = static_cast<const var_term_t *>(&t);
      o << "$VAR_" << vt->var;
    } else {
      assert(t.kind() == term::TERM_APP && "term should be a function");
      const ftor_term_t *ft = static_cast<const ftor_term_t *>(&t);
      o << ft->ftor << "(";
      for (unsigned i = 0, sz = ft->args.size(); i < sz;) {
        print_term(get_term(ft->args[i]), o);
        ++i;
        if (i < sz) {
          o << ",";
        }
      }
      o << ")";
    }
  }

public:
  uf_domain_t make_top() const override { return uf_domain_t(true); }

  uf_domain_t make_bottom() const override { return uf_domain_t(false); }

  void set_to_top() override {
    uf_domain abs(true);
    std::swap(*this, abs);
  }

  void set_to_bottom() override {
    uf_domain abs(false);
    std::swap(*this, abs);
  }

  uf_domain() : m_is_bottom(false) {}

  uf_domain(const uf_domain_t &o)
      : m_is_bottom(o.m_is_bottom), m_ttbl(o.m_ttbl), m_var_map(o.m_var_map),
        m_rev_var_map(o.m_rev_var_map) {
    crab::CrabStats::count(domain_name() + ".count.copy");
    crab::ScopedCrabStats __st__(domain_name() + ".copy");
    check_terms(__LINE__);
  }

  uf_domain_t &operator=(const uf_domain_t &o) {
    crab::CrabStats::count(domain_name() + ".count.copy");
    crab::ScopedCrabStats __st__(domain_name() + ".copy");

    o.check_terms(__LINE__);
    if (this != &o) {
      m_is_bottom = o.m_is_bottom;
      m_ttbl = o.m_ttbl;
      m_var_map = o.m_var_map;
      m_rev_var_map = o.m_rev_var_map;
    }
    check_terms(__LINE__);
    return *this;
  }

  bool is_bottom() const override { return m_is_bottom; }

  bool is_top() const override { return !m_var_map.size() && !is_bottom(); }

  // Lattice operations
  bool operator<=(const uf_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.leq");
    crab::ScopedCrabStats __st__(domain_name() + ".leq");

    if (is_bottom()) {
      return true;
    } else if (o.is_bottom()) {
      return false;
    } else {
      // FIXME: avoid this copy
      uf_domain_t left(*this);
      uf_domain_t right(o);
      typename ttbl_t::term_map_t gen_map /*unused*/;

      // Build up the mapping of right onto left, variable by variable.
      // Assumption: the set of variables in left & right are common.
      for (auto p : left.m_var_map) {
        if (!left.m_ttbl.map_leq(right.m_ttbl, left.term_of_var(p.first),
                                 right.term_of_var(p.first), gen_map))
          return false;
      }
      return true;
    }
  }

  void operator|=(const uf_domain_t &o) override {
    crab::CrabStats::count(domain_name() + ".count.join");
    crab::ScopedCrabStats __st__(domain_name() + ".join");

    if (is_bottom() || o.is_top()) {
      *this = o;
    } else if (o.is_bottom() || is_top()) {
      return;
    } else {
      uf_domain_t right(o);
      ttbl_t out_tbl;
      var_map_t out_vmap;
      rev_var_map_t out_rvmap;
      typename ttbl_t::gener_map_t gener_map /*unused*/;

      for (auto p : m_var_map) {
        const variable_t &v = p.first;
        term_id_t tx = p.second;
        term_id_t ty(right.term_of_var(v));
        term_id_t tz =
            m_ttbl.generalize(right.m_ttbl, tx, ty, out_tbl, gener_map);
        assert(tz < out_tbl.size());
        out_vmap[v] = tz;
        add_rev_var_map(out_rvmap, tz, v);
      }

      for (auto p : out_vmap) {
        out_tbl.add_ref(p.second);
      }

      m_is_bottom = false;
      std::swap(m_ttbl, out_tbl);
      std::swap(m_var_map, out_vmap);
      std::swap(m_rev_var_map, out_rvmap);
    }
  }

  uf_domain_t operator|(const uf_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.join");
    crab::ScopedCrabStats __st__(domain_name() + ".join");

    if (is_bottom() || o.is_top()) {
      return o;
    } else if (o.is_bottom() || is_top()) {
      return *this;
    } else {
      // FIXME: avoid this copy
      uf_domain_t left(*this);
      uf_domain_t right(o);
      ttbl_t out_tbl;
      typename ttbl_t::gener_map_t gener_map /*unused*/;
      var_map_t out_vmap;
      rev_var_map_t out_rvmap;

      // For each program variable in state, compute a generalization.
      for (auto p : left.m_var_map) {
        const variable_t &v = p.first;
        term_id_t tx = p.second;
        term_id_t ty = right.term_of_var(v);
        term_id_t tz =
            left.m_ttbl.generalize(right.m_ttbl, tx, ty, out_tbl, gener_map);
        assert(tz < out_tbl.size());
        out_vmap[v] = tz;
        add_rev_var_map(out_rvmap, tz, v);
      }

      for (auto p : out_vmap) {
        out_tbl.add_ref(p.second);
      }

      uf_domain_t res(std::move(out_tbl), std::move(out_vmap),
                      std::move(out_rvmap));

      CRAB_LOG("uf", crab::outs() << "============ JOIN ==================";
               crab::outs() << *this << "\n----------------";
               crab::outs() << o << "\n----------------";
               crab::outs() << res << "\n================"
                            << "\n");

      return res;
    }
  }

  uf_domain_t operator||(const uf_domain_t &other) const override {
    return *this | other;
  }

  uf_domain_t widening_thresholds(
      const uf_domain_t &other,
      const iterators::thresholds<number_t> &ts) const override {
    return *this | other;
  }

  // Meet
  uf_domain_t operator&(const uf_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.meet");
    crab::ScopedCrabStats __st__(domain_name() + ".meet");

    if (is_bottom() || o.is_top()) {
      return *this;
    } else if (is_top() || o.is_bottom()) {
      return o;
    } else {
      ttbl_t out_ttbl(m_ttbl);
      std::map<term_id_t, term_id_t> copy_map;
      std::vector<int> stack;
      std::map<int, term_id_t> cache;
      var_map_t out_vmap;
      rev_var_map_t out_rvmap;

      // bring all terms to one ttbl
      for (auto p : o.m_var_map) {
        term_id_t tx = p.second;
        out_ttbl.copy_term(o.m_ttbl, tx, copy_map);
      }

      // build unifications between terms from this and o
      std::vector<std::pair<term_id_t, term_id_t>> eqs;
      for (auto p : m_var_map) {
        variable_t v(p.first);
        auto it = o.m_var_map.find(v);
        if (it != o.m_var_map.end()) {
          term_id_t tx = p.second;
          eqs.push_back(std::make_pair(tx, copy_map[it->second]));
        }
      }

      // compute equivalence classes
      term::congruence_closure_solver<ttbl_t> solver(&out_ttbl);
      solver.run(eqs);

      // new map from variable to an acyclic term
      for (auto p : m_var_map) {
        const variable_t &v = p.first;
        term_id_t t_old = p.second;
        term_id_t t_new = build_dag_term(out_ttbl, solver.get_class(t_old),
                                         solver, out_ttbl, stack, cache);
        out_vmap[v] = t_new;
        add_rev_var_map(out_rvmap, t_new, v);
      }
      for (auto p : o.m_var_map) {
        variable_t v(p.first);
        if (out_vmap.find(v) != out_vmap.end())
          continue;
        term_id_t t_old(copy_map[p.second]);
        term_id_t t_new = build_dag_term(out_ttbl, solver.get_class(t_old),
                                         solver, out_ttbl, stack, cache);
        out_vmap[v] = t_new;
        add_rev_var_map(out_rvmap, t_new, v);
      }

      for (auto p : out_vmap) {
        out_ttbl.add_ref(p.second);
      }

      uf_domain_t res(std::move(out_ttbl), std::move(out_vmap),
                      std::move(out_rvmap));

      CRAB_LOG("uf", crab::outs() << "============ MEET ==================";
               crab::outs() << *this << "\n----------------";
               crab::outs() << o << "\n----------------";
               crab::outs() << res << "\n================"
                            << "\n");
      return res;
    }
  }

  uf_domain_t operator&&(const uf_domain_t &o) const override {
    return *this & o;
  }

  // Remove a variable from the scope
  void operator-=(const variable_t &v) override {
    crab::CrabStats::count(domain_name() + ".count.forget");
    crab::ScopedCrabStats __st__(domain_name() + ".forget");

    auto it(m_var_map.find(v));
    if (it != m_var_map.end()) {
      term_id_t t = (*it).second;
      m_var_map.erase(it);
      remove_rev_var_map(t, v);
      deref(t);
    }
    CRAB_LOG("uf", crab::outs()
                       << "After removing " << v << ": " << *this << "\n";);
  }

  void assign(const variable_t &x, const linear_expression_t &e) override {
    crab::CrabStats::count(domain_name() + ".count.assign");
    crab::ScopedCrabStats __st__(domain_name() + ".assign");

    if (!is_bottom()) {
      term_id_t tx(build_linexpr(e));
      rebind_var(x, tx);
      check_terms(__LINE__);
      CRAB_LOG("uf", crab::outs() << "*** Assign " << x << ":=" << e << ":"
                                  << *this << "\n");
    }
  }

  // Apply operations to variables.

  // x = y op z
  void apply(arith_operation_t op, const variable_t &x, const variable_t &y,
             const variable_t &z) override {
    crab::CrabStats::count(domain_name() + ".count.apply");
    crab::ScopedCrabStats __st__(domain_name() + ".apply");
    check_terms(__LINE__);

    if (!is_bottom()) {
      term_id_t tx(
          build_term(term::conv2termop(op), term_of_var(y), term_of_var(z)));
      rebind_var(x, tx);
      check_terms(__LINE__);
      CRAB_LOG("uf", crab::outs() << "*** " << x << ":=" << y << " " << op
                                  << " " << z << ":" << *this << "\n");
    }
  }

  // x = y op k
  void apply(arith_operation_t op, const variable_t &x, const variable_t &y,
             number_t k) override {
    crab::CrabStats::count(domain_name() + ".count.apply");
    crab::ScopedCrabStats __st__(domain_name() + ".apply");

    if (!is_bottom()) {
      term_id_t tx(
          build_term(term::conv2termop(op), term_of_var(y), term_of_const(k)));
      rebind_var(x, tx);
      check_terms(__LINE__);
      CRAB_LOG("uf", crab::outs() << "*** " << x << ":=" << y << " " << op
                                  << " " << k << ":" << *this << "\n");
    }
  }

  void backward_assign(const variable_t &x, const linear_expression_t &e,
                       const uf_domain_t &inv) override {
    crab::CrabStats::count(domain_name() + ".count.backward_assign");
    crab::ScopedCrabStats __st__(domain_name() + ".backward_assign");
    if (!is_bottom()) {
      CRAB_WARN("backward_assign not implemented by ", domain_name());
    }
  }

  void backward_apply(arith_operation_t op, const variable_t &x,
                      const variable_t &y, number_t z,
                      const uf_domain_t &inv) override {
    crab::CrabStats::count(domain_name() + ".count.backward_apply");
    crab::ScopedCrabStats __st__(domain_name() + ".backward_apply");
    if (!is_bottom()) {
      CRAB_WARN("backward_apply not implemented by ", domain_name());
    }
  }

  void backward_apply(arith_operation_t op, const variable_t &x,
                      const variable_t &y, const variable_t &z,
                      const uf_domain_t &inv) override {
    crab::CrabStats::count(domain_name() + ".count.backward_apply");
    crab::ScopedCrabStats __st__(domain_name() + ".backward_apply");
    if (!is_bottom()) {
      CRAB_WARN("backward_apply not implemented by ", domain_name());
    }
  }

  void operator+=(const linear_constraint_t &cst) {
    crab::CrabStats::count(domain_name() + ".count.add_constraints");
    crab::ScopedCrabStats __st__(domain_name() + ".add_constraints");

    CRAB_LOG("uf", crab::outs()
                       << "*** Before assume " << cst << ":" << *this << "\n");

    if (is_bottom()) {
      return;
    }

    using pair_var_t = std::pair<variable_t, variable_t>;

    if (boost::optional<pair_var_t> eq = get_eq_or_diseq(cst)) {
      term_id_t tx(term_of_var((*eq).first));
      term_id_t ty(term_of_var((*eq).second));
      if (cst.is_disequation()) {
        if (tx == ty) {
          set_to_bottom();
          CRAB_LOG("uf", crab::outs() << "*** After assume " << cst << ":"
                                      << *this << "\n");
          return;
        }
      } else {
        // not bother if they are already equal
        if (tx == ty) {
          return;
        }

        std::vector<int> stack;
        std::map<int, term_id_t> cache;
        // congruence closure to compute equivalence classes
        term::congruence_closure_solver<ttbl_t> solver(&m_ttbl);
        std::vector<std::pair<term_id_t, term_id_t>> eqs = {{tx, ty}};
        solver.run(eqs);

        // new map from variable to an acyclic term
        for (auto p : m_var_map) {
          const variable_t &v = p.first;
          term_id_t t_old(term_of_var(v));
          term_id_t t_new = build_dag_term(m_ttbl, solver.get_class(t_old),
                                           solver, m_ttbl, stack, cache);
          rebind_var(v, t_new);
        }
      }
    }

    CRAB_LOG("uf", crab::outs()
                       << "*** After assume " << cst << ":" << *this << "\n");
  }

  void operator+=(const linear_constraint_system_t &csts) override {
    for (auto cst : csts) {
      this->operator+=(cst);
    }
  }

  interval_t operator[](const variable_t &x) override {
    crab::CrabStats::count(domain_name() + ".count.to_intervals");
    crab::ScopedCrabStats __st__(domain_name() + ".to_intervals");
    if (is_bottom()) {
      return interval_t::bottom();
    } else {
      return interval_t::top();
    }
  }

  void apply(int_conv_operation_t /*op*/, const variable_t &dst,
             const variable_t &src) override {
    // since reasoning about infinite precision we simply assign and
    // ignore the widths.
    assign(dst, src);
  }

  void apply(bitwise_operation_t op, const variable_t &x, const variable_t &y,
             const variable_t &z) override {
    crab::CrabStats::count(domain_name() + ".count.apply");
    crab::ScopedCrabStats __st__(domain_name() + ".apply");

    if (!is_bottom()) {
      term_id_t tx(
          build_term(term::conv2termop(op), term_of_var(y), term_of_var(z)));
      rebind_var(x, tx);
      check_terms(__LINE__);
      CRAB_LOG("uf", crab::outs() << "*** " << x << ":=" << y << " " << op
                                  << " " << z << ":" << *this << "\n");
    }
  }

  void apply(bitwise_operation_t op, const variable_t &x, const variable_t &y,
             number_t k) override {
    crab::CrabStats::count(domain_name() + ".count.apply");
    crab::ScopedCrabStats __st__(domain_name() + ".apply");

    if (!is_bottom()) {
      term_id_t tx(
          build_term(term::conv2termop(op), term_of_var(y), term_of_const(k)));
      rebind_var(x, tx);
      check_terms(__LINE__);
      CRAB_LOG("uf", crab::outs() << "*** " << x << ":=" << y << " " << op
                                  << " " << k << ":" << *this << "\n");
    }
  }

  /* Array operations */

  virtual void array_init(const variable_t & /*a*/,
                          const linear_expression_t & /*elem_size*/,
                          const linear_expression_t & /*lb_idx*/,
                          const linear_expression_t & /*ub_idx*/,
                          const linear_expression_t & /*val*/) override {
    // do nothing
  }

  virtual void array_load(const variable_t &lhs, const variable_t &a,
                          const linear_expression_t & /*elem_size*/,
                          const linear_expression_t &i) override {
    crab::CrabStats::count(domain_name() + ".count.array_read");
    crab::ScopedCrabStats __st__(domain_name() + ".array_read");

    if (!is_bottom()) {
      /**
       *  We treat the array load as an uninterpreted function
       *  lhs := array_load(a, i) -->  lhs := f(a,i)
       */
      term_id_t t_uf(
          build_term(term::TERM_OP_FUNCTION, term_of_var(a), build_linexpr(i)));
      rebind_var(lhs, t_uf);
      check_terms(__LINE__);
      CRAB_LOG("uf", crab::outs() << lhs << ":=" << a << "[" << i << "]  -- "
                                  << *this << "\n";);
    }
  }

  virtual void array_store(const variable_t &a,
                           const linear_expression_t & /*elem_size*/,
                           const linear_expression_t &i,
                           const linear_expression_t &val,
                           bool /*is_strong_update*/) override {
    // do nothing
  }

  virtual void array_store_range(const variable_t &a,
                                 const linear_expression_t &elem_size,
                                 const linear_expression_t &i,
                                 const linear_expression_t &j,
                                 const linear_expression_t &v) override {
    // do nothing
  }

  virtual void array_assign(const variable_t &lhs,
                            const variable_t &rhs) override {
    // do nothing
  }

  // backward array operations
  void backward_array_init(const variable_t &a,
                           const linear_expression_t &elem_size,
                           const linear_expression_t &lb_idx,
                           const linear_expression_t &ub_idx,
                           const linear_expression_t &val,
                           const uf_domain_t &invariant) override {
    CRAB_WARN(domain_name(), " does not implement backward operations");
  }
  void backward_array_load(const variable_t &lhs, const variable_t &a,
                           const linear_expression_t &elem_size,
                           const linear_expression_t &i,
                           const uf_domain_t &invariant) override {
    *this -= lhs;
    CRAB_WARN(domain_name(), " does not implement backward operations");
  }
  void backward_array_store(const variable_t &a,
                            const linear_expression_t &elem_size,
                            const linear_expression_t &i,
                            const linear_expression_t &v, bool is_strong_update,
                            const uf_domain_t &invariant) override {
    CRAB_WARN(domain_name(), " does not implement backward operations");
  }
  void backward_array_store_range(const variable_t &a,
                                  const linear_expression_t &elem_size,
                                  const linear_expression_t &i,
                                  const linear_expression_t &j,
                                  const linear_expression_t &v,
                                  const uf_domain_t &invariant) override {
    CRAB_WARN(domain_name(), " does not implement backward operations");
  }
  void backward_array_assign(const variable_t &lhs, const variable_t &rhs,
                             const uf_domain_t &invariant) override {
    CRAB_WARN(domain_name(), " does not implement backward operations");
  }

  DEFAULT_SELECT(uf_domain_t)

  // boolean operators
  virtual void assign_bool_cst(const variable_t &lhs,
                               const linear_constraint_t &rhs) override {
    // TODO
    operator-=(lhs);
  }

  virtual void assign_bool_ref_cst(const variable_t &lhs,
                                   const reference_constraint_t &rhs) override {
    // TODO
    operator-=(lhs);
  }

  virtual void assign_bool_var(const variable_t &lhs, const variable_t &rhs,
                               bool is_not_rhs) override {
    crab::CrabStats::count(domain_name() + ".count.assign_bool_var");
    crab::ScopedCrabStats __st__(domain_name() + ".assign_bool_var");

    if (!is_bottom()) {
      check_terms(__LINE__);
      if (is_not_rhs) {
        term_id_t tx(build_term(term::TERM_OP_NOT, term_of_var(rhs)));
        rebind_var(lhs, tx);
      } else {
        term_id_t tx(term_of_var(rhs));
        rebind_var(lhs, tx);
      }
      check_terms(__LINE__);

      CRAB_LOG(
          "uf", crab::outs() << "*** " << lhs << ":="; if (is_not_rhs) {
            crab::outs() << "not(" << rhs << ")";
          } else { crab::outs() << rhs; } crab::outs() << ":"
                                                       << *this << "\n");
    }
  }

  virtual void apply_binary_bool(bool_operation_t op, const variable_t &x,
                                 const variable_t &y,
                                 const variable_t &z) override {
    crab::CrabStats::count(domain_name() + ".count.apply_binary_bool");
    crab::ScopedCrabStats __st__(domain_name() + ".apply_binary_bool");

    if (!is_bottom()) {
      check_terms(__LINE__);
      term_id_t tx(
          build_term(term::conv2termop(op), term_of_var(y), term_of_var(z)));
      rebind_var(x, tx);
      check_terms(__LINE__);
      CRAB_LOG("uf", crab::outs() << "*** " << x << ":=" << y << " " << op
                                  << " " << z << ":" << *this << "\n");
    }
  }

  virtual void assume_bool(const variable_t &v, bool is_negated) override {
    // do nothing
  }

  void select_bool(const variable_t &lhs, const variable_t &cond,
                   const variable_t &b1, const variable_t &b2) override {
    operator-=(lhs);
  }

  // backward boolean operators
  virtual void backward_assign_bool_cst(const variable_t &lhs,
                                        const linear_constraint_t &rhs,
                                        const uf_domain_t &inv) override {
    CRAB_WARN(domain_name(), " does not implement backward operations");
  }

  virtual void backward_assign_bool_ref_cst(const variable_t &lhs,
                                            const reference_constraint_t &rhs,
                                            const uf_domain_t &inv) override {
    CRAB_WARN(domain_name(), " does not implement backward operations");
  }

  virtual void backward_assign_bool_var(const variable_t &lhs,
                                        const variable_t &rhs, bool is_not_rhs,
                                        const uf_domain_t &inv) override {
    CRAB_WARN(domain_name(), " does not implement backward operations");
  }

  virtual void backward_apply_binary_bool(bool_operation_t op,
                                          const variable_t &x,
                                          const variable_t &y,
                                          const variable_t &z,
                                          const uf_domain_t &inv) override {
    CRAB_WARN(domain_name(), " does not implement backward operations");
  }

  // Region operations
  virtual void region_init(const variable_t &reg) override {
    // do nothing
  }

  virtual void region_copy(const variable_t &lhs_reg,
                           const variable_t &rhs_reg) override {
    // do nothing
  }

  virtual void region_cast(const variable_t &src_reg,
                           const variable_t &dst_reg) override {
    // do nothing
  }

  virtual void ref_make(const variable_t &ref, const variable_t &reg,
                        const variable_or_constant_t &size,
                        const allocation_site &as) override {
    // do nothing
  }

  virtual void ref_free(const variable_t &reg, const variable_t &ref) override {
    // do nothing
  }

  virtual void ref_load(const variable_t &ref, const variable_t &reg,
                        const variable_t &res) override {
    crab::CrabStats::count(domain_name() + ".count.ref_load");
    crab::ScopedCrabStats __st__(domain_name() + ".ref_load");

    if (!is_bottom()) {
      /**
       *  We treat the load as an uninterpreted function:
       *  res := ref_load(reg, ref) -->  res := f(reg,ref)
       */
      term_id_t t_uf(build_term(term::TERM_OP_FUNCTION, term_of_var(reg),
                                build_linexpr(ref)));
      rebind_var(res, t_uf);
      check_terms(__LINE__);
      CRAB_LOG("uf", crab::outs() << res << ":=ref_load(" << reg << "," << ref
                                  << ")  -- " << *this << "\n";);
    }
  }

  virtual void ref_store(const variable_t &ref, const variable_t &reg,
                         const variable_or_constant_t &val) override {
    // do nothing
  }

  virtual void ref_gep(const variable_t &ref1, const variable_t &reg1,
                       const variable_t &ref2, const variable_t &reg2,
                       const linear_expression_t &offset) override {
    // do nothing
  }

  virtual void
  ref_load_from_array(const variable_t &lhs, const variable_t &ref,
                      const variable_t &region,
                      const linear_expression_t &index,
                      const linear_expression_t &elem_size) override {
    // TODO
    // do nothing
  }

  virtual void ref_store_to_array(const variable_t &ref,
                                  const variable_t &region,
                                  const linear_expression_t &index,
                                  const linear_expression_t &elem_size,
                                  const linear_expression_t &val) override {
    // do nothing
  }

  virtual void ref_assume(const reference_constraint_t &cst) override {
    // do nothing
  }

  void ref_to_int(const variable_t &reg, const variable_t &ref_var,
                  const variable_t &int_var) override {
    // do nothing
  }

  void int_to_ref(const variable_t &int_var, const variable_t &reg,
                  const variable_t &ref_var) override {
    // do nothing
  }

  void select_ref(const variable_t &lhs_ref, const variable_t &lhs_rgn,
                  const variable_t &cond, const variable_or_constant_t &ref1,
                  const boost::optional<variable_t> &rgn1,
                  const variable_or_constant_t &ref2,
                  const boost::optional<variable_t> &rgn2) override {
    // do nothing
  }

  boolean_value is_null_ref(const variable_t &ref) override {
    // do nothing
    return boolean_value();
  }
  bool
  get_allocation_sites(const variable_t &ref,
                       std::vector<allocation_site> &alloc_sites) override {
    // do nothing
    return false;
  }

  bool get_tags(const variable_t &rgn, const variable_t &ref,
                std::vector<uint64_t> &tags) override {
    // do nothing
    return false;
  }

  // Miscellaneous
  void rename(const variable_vector_t &from,
              const variable_vector_t &to) override {
    crab::CrabStats::count(domain_name() + ".count.rename");
    crab::ScopedCrabStats __st__(domain_name() + ".rename");

    if (is_top() || is_bottom()) {
      return;
    }

    CRAB_LOG(
        "uf", crab::outs() << "Renaming {"; for (auto v
                                                 : from) {
          crab::outs() << v << ";";
        } crab::outs() << "} with ";
        for (auto v
             : to) { crab::outs() << v << ";"; } crab::outs()
        << "}:\n";
        crab::outs() << *this << "\n";);

    auto error_if_found = [this](const variable_t &v) {
      auto it = m_var_map.find(v);
      if (it != m_var_map.end()) {
        CRAB_ERROR(domain_name() + "::rename assumes that ", v,
                   " does not exist");
      }
    };

    for (unsigned i = 0, sz = from.size(); i < sz; ++i) {
      const variable_t &v = from[i];
      const variable_t &new_v = to[i];
      if (v == new_v) { // nothing to rename
        continue;
      }

      error_if_found(new_v);

      auto it = m_var_map.find(v);
      if (it != m_var_map.end()) {
        term_id_t id = it->second;
        m_var_map.erase(it);
        m_var_map.insert(std::make_pair(new_v, id));
        remove_rev_var_map(id, v);
        add_rev_var_map(m_rev_var_map, id, new_v);
      }
    }
    CRAB_LOG("uf", crab::outs() << "RESULT=" << *this << "\n");
  }

  void forget(const variable_vector_t &variables) override {
    if (is_bottom() || is_top()) {
      return;
    }

    for (auto v : variables) {
      *this -= v;
    }
  }

  void project(const variable_vector_t &variables) override {
    crab::CrabStats::count(domain_name() + ".count.project");
    crab::ScopedCrabStats __st__(domain_name() + ".project");

    if (is_bottom() || is_top()) {
      return;
    }

    if (variables.empty()) {
      set_to_top();
      return;
    }

    std::set<variable_t> s1, s2;
    variable_vector_t s3;
    for (auto p : m_var_map) {
      s1.insert(p.first);
    }
    s2.insert(variables.begin(), variables.end());
    std::set_difference(s1.begin(), s1.end(), s2.begin(), s2.end(),
                        std::back_inserter(s3));
    forget(s3);
  }

  void expand(const variable_t &x, const variable_t &y) override {
    crab::CrabStats::count(domain_name() + ".count.expand");
    crab::ScopedCrabStats __st__(domain_name() + ".expand");

    if (is_bottom() || is_top()) {
      return;
    }

    linear_expression_t e(x);
    term_id_t tx(build_linexpr(e));
    rebind_var(y, tx);
    check_terms(__LINE__);
  }

  /* begin intrinsics operations */
  void intrinsic(std::string name, const variable_or_constant_vector_t &inputs,
                 const variable_vector_t &outputs) override {
    CRAB_WARN("Intrinsics ", name, " not implemented by ", domain_name());
  }

  void backward_intrinsic(std::string name,
                          const variable_or_constant_vector_t &inputs,
                          const variable_vector_t &outputs,
                          const uf_domain_t &invariant) override {
    CRAB_WARN("Intrinsics ", name, " not implemented by ", domain_name());
  }
  /* end intrinsics operations */

  void normalize() override {}
  void minimize() override {}

  // Output function
  void write(crab_os &o) const override {
    crab::CrabStats::count(domain_name() + ".count.write");
    crab::ScopedCrabStats __st__(domain_name() + ".write");

    if (is_bottom()) {
      o << "_|_";
      return;
    }
    if (m_var_map.empty()) {
      o << "{}";
      return;
    }

    bool first = true;
    o << "{";
    for (auto p : m_var_map) {
      if (first) {
        first = false;
      } else {
        o << ", ";
      }
      o << p.first << " -> ";
      print_term(get_term(p.second), o);
    }
    o << "}";

    CRAB_LOG("ufo-print-ttbl",
             /// For debugging purposes
             o << " ttbl={" << m_ttbl << "}\n";);
  }

  linear_constraint_system_t to_linear_constraint_system() const override {
    crab::CrabStats::count(domain_name() +
                           ".count.to_linear_constraint_system");
    crab::ScopedCrabStats __st__(domain_name() +
                                 ".to_linear_constraint_system");

    linear_constraint_system_t out_csts;
    if (is_bottom()) {
      out_csts += linear_constraint_t::get_false();
    } else if (!is_top()) {
      // Extract equalities

      // Seen equalities to avoid adding twice the same.
      std::set<std::pair<variable_t, variable_t>> seen;
      for (auto &kv : m_var_map) {
        const variable_t &x = kv.first;
        term_id_t tx = kv.second;
        auto it = m_rev_var_map.find(tx);
        if (it == m_rev_var_map.end()) {
          // this shouldn't happen
          continue;
        }
        for (auto var : it->second) {
          if (var.index() != x.index()) {
            if (seen.count(std::make_pair(var, x)) <= 0) {
              seen.insert(std::make_pair(x, var));
              out_csts += linear_constraint_t(linear_expression_t(x) ==
                                              linear_expression_t(var));
            }
          }
        }
      }
    }
    return out_csts;
  }

  disjunctive_linear_constraint_system_t
  to_disjunctive_linear_constraint_system() const override {
    auto lin_csts = to_linear_constraint_system();
    if (lin_csts.is_false()) {
      return disjunctive_linear_constraint_system_t(true /*is_false*/);
    } else if (lin_csts.is_true()) {
      return disjunctive_linear_constraint_system_t(false /*is_false*/);
    } else {
      return disjunctive_linear_constraint_system_t(lin_csts);
    }
  }

  std::string domain_name() const override { return "UFDomain"; }
}; // class uf_domain

template <typename Number, typename VariableName>
struct abstract_domain_traits<uf_domain<Number, VariableName>> {
  using number_t = Number;
  using varname_t = VariableName;
}; // end uf_domain

} // namespace domains
} // namespace crab


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_FUNCTION_TRUNC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_TRUNC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing trunc capabilities

    Computes the truncation toward @ref Zero of its parameter.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = trunc(x);
    @endcode

    The code is similar to:

    @code
    T r = sign(x)*floor(abs(x));
    @endcode

    @par Note:

      - For floating point number it is also one of the two ouputs of
        the @ref modf function.
        And we have:
        @code
        trunc(x) + frac(x) == x;
        @endcode
        except for nans

      - If large numbers correct behaviour is not needed the fast_ decorator can be used,
        but 'fast_' means that no provisions are taken for floating values  too large to fit
         in the same size integer type.

    @par Alias:
    fix

    @par Decorators

    std_, fast_ for floating entries

    @see abs, frac, floor, sign, modf

  **/
  const boost::dispatch::functor<tag::trunc_> trunc = {};
} }
#endif

#include <boost/simd/function/scalar/trunc.hpp>
#include <boost/simd/function/simd/trunc.hpp>

#endif


/*=============================================================================
    Copyright (c) 2001-2003 Daniel Nuffer
    http://spirit.sourceforge.net/

    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)
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  Demonstrates parse trees. This is discussed in the
//  "Trees" chapter in the Spirit User's Guide.
//
///////////////////////////////////////////////////////////////////////////////
#define BOOST_SPIRIT_DUMP_PARSETREE_AS_XML

#include <boost/spirit/include/classic_core.hpp>
#include <boost/spirit/include/classic_parse_tree.hpp>
#include <boost/assert.hpp>

#include <iostream>
#include <stack>
#include <functional>
#include <string>

#ifdef BOOST_SPIRIT_DUMP_PARSETREE_AS_XML
#include <boost/spirit/include/classic_tree_to_xml.hpp>
#include <map>
#endif

////////////////////////////////////////////////////////////////////////////
// This example shows how to use a parse tree
using namespace std;
using namespace BOOST_SPIRIT_CLASSIC_NS;

// Here's some typedefs to simplify things
typedef char const*         iterator_t;
typedef tree_match<iterator_t> parse_tree_match_t;
typedef parse_tree_match_t::const_tree_iterator iter_t;

typedef pt_match_policy<iterator_t> match_policy_t;
typedef scanner_policies<iteration_policy, match_policy_t, action_policy> scanner_policy_t;
typedef scanner<iterator_t, scanner_policy_t> scanner_t;
typedef rule<scanner_t> rule_t;

//  grammar rules
rule_t expression, term, factor, integer;

////////////////////////////////////////////////////////////////////////////
// Here's the function prototypes that we'll use.  One function for each
// grammar rule.
long evaluate(const tree_parse_info<>& info);
long eval_expression(iter_t const& i);
long eval_term(iter_t const& i);
long eval_factor(iter_t const& i);
long eval_integer(iter_t const& i);

long evaluate(const tree_parse_info<>& info)
{
    return eval_expression(info.trees.begin());
}

// i should be pointing to a node created by the expression rule
long eval_expression(iter_t const& i)
{
    parser_id id = i->value.id();
    BOOST_ASSERT(id == expression.id()); // check the id

    // first child points to a term, so call eval_term on it
    iter_t chi = i->children.begin();
    long lhs = eval_term(chi);
    for (++chi; chi != i->children.end(); ++chi)
    {
        // next node points to the operator.  The text of the operator is
        // stored in value (a vector<char>)
        char op = *(chi->value.begin());
        ++chi;
        long rhs = eval_term(chi);
        if (op == '+')
            lhs += rhs;
        else if (op == '-')
            lhs -= rhs;
        else
            BOOST_ASSERT(0);
    }
    return lhs;
}

long eval_term(iter_t const& i)
{
    parser_id id = i->value.id();
    BOOST_ASSERT(id == term.id());

    iter_t chi = i->children.begin();
    long lhs = eval_factor(chi);
    for (++chi; chi != i->children.end(); ++chi)
    {
        char op = *(chi->value.begin());
        ++chi;
        long rhs = eval_factor(chi);
        if (op == '*')
            lhs *= rhs;
        else if (op == '/')
            lhs /= rhs;
        else
            BOOST_ASSERT(0);
    }
    return lhs;
}

long eval_factor(iter_t const& i)
{
    parser_id id = i->value.id();
    BOOST_ASSERT(id == factor.id());

    iter_t chi = i->children.begin();
    id = chi->value.id();
    if (id == integer.id())
        return eval_integer(chi->children.begin());
    else if (*(chi->value.begin()) == '(')
    {
        ++chi;
        return eval_expression(chi);
    }
    else if (*(chi->value.begin()) == '-')
    {
        ++chi;
        return -eval_factor(chi);
    }
    else
    {
        BOOST_ASSERT(0);
        return 0;
    }
}

long eval_integer(iter_t const& i)
{
    // extract integer (not always delimited by '\0')
    string integer(i->value.begin(), i->value.end());

    return strtol(integer.c_str(), 0, 10);
}

////////////////////////////////////////////////////////////////////////////
int
main()
{

    //  Start grammar definition
    integer     =   lexeme_d[ token_node_d[ (!ch_p('-') >> +digit_p) ] ];
    factor      =   integer
                |   '(' >> expression >> ')'
                |   ('-' >> factor);
    term        =   factor >>
                    *(  ('*' >> factor)
                      | ('/' >> factor)
                    );
    expression  =   term >>
                    *(  ('+' >> term)
                      | ('-' >> term)
                    );
    //  End grammar definition

    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "\t\tThe simplest working calculator...\n\n";
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "Type an expression...or [q or Q] to quit\n\n";

    string str;
    while (getline(cin, str))
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        const char* first = str.c_str();

        tree_parse_info<> info = pt_parse(first, expression);

        if (info.full)
        {
#if defined(BOOST_SPIRIT_DUMP_PARSETREE_AS_XML)
            // dump parse tree as XML
            std::map<parser_id, std::string> rule_names;
            rule_names[integer.id()] = "integer";
            rule_names[factor.id()] = "factor";
            rule_names[term.id()] = "term";
            rule_names[expression.id()] = "expression";
            tree_to_xml(cout, info.trees, first, rule_names);
#endif

            // print the result
            cout << "parsing succeeded\n";
            cout << "result = " << evaluate(info) << "\n\n";
        }
        else
        {
            cout << "parsing failed\n";
        }
    }

    cout << "Bye... :-) \n\n";
    return 0;
}


#include <iostream>
#include <string>
#include <vector>
#include <cctype>
#include <sstream>
#include <memory>

using namespace std;

#include <boost/lexical_cast.hpp>

struct Token {
    enum Type {
        integer, plus, minus, lparen, rparen
    } type;
    string text;

    explicit Token(Type type, const string &text) :
            type{type}, text{text} {}

    friend ostream &operator<<(ostream &os, const Token &obj) {
        return os << "`" << obj.text << "`";
    }
};

vector<Token> lex(const string &input) {
    vector<Token> result;

    for (int i = 0; i < input.size(); ++i) {
        switch (input[i]) {
            case '+':
                result.push_back(Token{Token::plus, "+"});
                break;
            case '-':
                result.push_back(Token{Token::minus, "-"});
                break;
            case '(':
                result.push_back(Token{Token::lparen, "("});
                break;
            case ')':
                result.push_back(Token{Token::rparen, ")"});
                break;
            default:
                // number
                ostringstream buffer;
                buffer << input[i];
                for (int j = i + 1; j < input.size(); ++j) {
                    if (isdigit(input[j])) {
                        buffer << input[j];
                        ++i;
                    } else {
                        result.push_back(Token{Token::integer, buffer.str()});
                        break;
                    }
                }
        }
    }

    return result;
}

// parsing =====================================================

struct Element {
    virtual ~Element() = default;

    virtual int eval() const = 0;
};

struct Integer : Element {
    int value;

    explicit Integer(const int value)
            : value(value) {
    }

    int eval() const override { return value; }
};

struct BinaryOperation : Element {
    enum Type {
        addition, subtraction
    } type;
    shared_ptr<Element> lhs, rhs;

    int eval() const override {
        if (type == addition)
            return lhs->eval() + rhs->eval();
        return lhs->eval() - rhs->eval();
    }
};

shared_ptr<Element> parse(const vector<Token> &tokens) {
    auto result = make_unique<BinaryOperation>();
    bool have_lhs = false;
    for (size_t i = 0; i < tokens.size(); i++) {
        auto token = tokens[i];
        switch (token.type) {
            case Token::integer: {
                int value = boost::lexical_cast<int>(token.text);
                auto integer = make_shared<Integer>(value);
                if (!have_lhs) {
                    result->lhs = integer;
                    have_lhs = true;
                } else result->rhs = integer;
            }
                break;
            case Token::plus:
                result->type = BinaryOperation::addition;
                break;
            case Token::minus:
                result->type = BinaryOperation::subtraction;
                break;
            case Token::lparen: {
                int j = i;
                for (; j < tokens.size(); ++j)
                    if (tokens[j].type == Token::rparen)
                        break; // found it!

                vector<Token> subexpression(&tokens[i + 1], &tokens[j]);
                auto element = parse(subexpression);
                if (!have_lhs) {
                    result->lhs = element;
                    have_lhs = true;
                } else result->rhs = element;
                i = j; // advance
            }
                break;
        }
    }
    return result;
}

int main() {
    string input{"(13-4)-(12+1)"}; // see if you can make nested braces work
    auto tokens = lex(input);

    // let's see the tokens
    for (auto &t : tokens)
        cout << t << "   ";
    cout << endl;

    try {
        auto parsed = parse(tokens);
        cout << input << " = " << parsed->eval() << endl;
    }
    catch (const exception &e) {
        cout << e.what() << endl;
    }

    return 0;
}


#ifndef Eigen_Dense_Solver_hh
#define Eigen_Dense_Solver_hh

#include <vector>

#include <Eigen/Dense>

#include "Dense_Solver.hh"

template<class Scalar>
class Eigen_Dense_Solver : public Dense_Solver<Scalar>
{
public:

    // Matrices and vectors
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> EMatrix;
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> EMatrixR;
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> EVector;

    // Mapped types
    typedef Eigen::Map<EVector> EMVector;
    typedef Eigen::Map<EMatrix> EMMatrix;
    typedef Eigen::Map<EMatrixR> EMMatrixR;

    // LU decomposition
    typedef Eigen::FullPivLU<EMatrixR> ELU;

    // Constructor
    Eigen_Dense_Solver(int size):
        initialized_(false),
        size_(size)
    {
    }

    // Rank of matrix
    virtual int size() const override
    {
        return size_;
    }

    // Check whether data has been initialized
    virtual bool initialized() const override
    {
        return initialized_;
    }

    // Set matrix and perform decomposition
    virtual void initialize(std::vector<Scalar> &a_data) override
    {
        Check(a_data.size() == size_ * size_);

        a_ = EMMatrixR(&a_data[0], size_, size_);
        lu_ = a_.fullPivLu();
        initialized_ = true;
    }

    // Solve problem Ax=b using temporary data (no initialization needed)
    virtual void solve(std::vector<Scalar> &a_data,
                       std::vector<Scalar> &b_data,
                       std::vector<Scalar> &x_data) override
    {
        Check(a_data.size() == size_ * size_);
        Check(b_data.size() == size_);
        Check(x_data.size() == size_);

        EMMatrixR a(&a_data[0], size_, size_);
        EMVector b(&b_data[0], size_);
        EMVector x(&x_data[0], size_);

        x = a.fullPivLu().solve(b);
    }

    // Apply to one vector
    virtual void solve(std::vector<double> &b_data,
                       std::vector<double> &x_data) override
    {
        Assert(initialized_);
        Check(b_data.size() == size_);
        Check(x_data.size() == size_);

        EMVector b(&b_data[0], size_);
        EMVector x(&x_data[0], size_);

        x = lu_.solve(b);
    }

    // Apply to multiple vectors (possibly a matrix)
    virtual void multi_solve(int number_of_vectors,
                             std::vector<double> &b_data,
                             std::vector<double> &x_data) override
    {
        Assert(initialized_);
        Check(b_data.size() == size_ * number_of_vectors);
        Check(x_data.size() == size_ * number_of_vectors);

        EMMatrix b(&b_data[0], size_, number_of_vectors);
        EMMatrix x(&x_data[0], size_, number_of_vectors);

        x = lu_.solve(b);
    }

    // Get inverse
    virtual void inverse(std::vector<double> &ainv_data) override
    {
        Assert(initialized_);
        Check(ainv_data.size() == size_ * size_);

        EMMatrixR ainv(&ainv_data[0], size_, size_);

        ainv = lu_.inverse();
    }

    virtual void inverse(std::vector<double> &a_data,
                         std::vector<double> &ainv_data) override
    {
        Check(a_data.size() == size_ * size_);
        Check(ainv_data.size() == size_ * size_);

        EMMatrixR a(&a_data[0], size_, size_);
        EMMatrixR ainv(&ainv_data[0], size_, size_);

        ainv = a.fullPivLu().inverse();
    }

    // Get determinant
    virtual Scalar determinant() override
    {
        Assert(initialized_);

        return lu_.determinant();
    }

private:

    bool initialized_;
    int size_;
    EMatrixR a_;
    ELU lu_;
};

#endif


//  (C) Copyright Pieter Bastiaan Ober 2014.
//  Use, modification and distribution are 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)

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>

#include <boost/accumulators/statistics/rolling_variance.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

template<typename T>
void assert_is_double(T const &)
{
    BOOST_MPL_ASSERT((is_same<T, double>));
}

/*
REFERENCE VALUES PROVIDED BY OCTAVE:

x=[1.2 2.3 3.4 4.5 0.4 2.2 7.1 4.0]

v1_2 = var(x(1:2))
v1_3 = var(x(1:3))
v1_4 = var(x(1:4))
v2_5 = var(x(2:5))
v3_6 = var(x(3:6))
v4_7 = var(x(4:7))
v5_8 = var(x(5:8))

GIVES:

v1_2 =  0.605000000000000
v1_3 =  1.21000000000000
v1_4 =  2.01666666666667
v2_5 =  3.05666666666667
v3_6 =  3.08250000000000
v4_7 =  8.41666666666667
v5_8 =  8.16250000000000
*/

///////////////////////////////////////////////////////////////////////////////
// rolling_variance_test_impl
// implements a test for window_size = 4

size_t window_size = 4;

template<typename accumulator_set_type>
void rolling_variance_test_impl(accumulator_set_type& acc)
{
    // Window contains x(1), value is zero
    acc(1.2);
    BOOST_CHECK_CLOSE(rolling_variance(acc),0.0,1e-10);

    // Window contains x(1)...x(2)
    acc(2.3);
    BOOST_CHECK_CLOSE(rolling_variance(acc),0.605,1e-10);

    // Window contains x(1)...x(3)
    acc(3.4);
    BOOST_CHECK_CLOSE(rolling_variance(acc),1.21,1e-10);

    // Window contains x(1)...x(4)
    acc(4.5);
    BOOST_CHECK_CLOSE(rolling_variance(acc),2.01666666666667,1e-10);

    // Window contains x(2)...x(5)
    acc(0.4);
    BOOST_CHECK_CLOSE(rolling_variance(acc),3.05666666666667,1e-10);

    // Window contains x(3)...x(6)
    acc(2.2);
    BOOST_CHECK_CLOSE(rolling_variance(acc),3.08250000000000,1e-10);

    // Window contains x(4)...x(7)
    acc(7.1);
    BOOST_CHECK_CLOSE(rolling_variance(acc),8.41666666666667,1e-10);

    // Window contains x(5)...x(8)
    acc(4.0);
    BOOST_CHECK_CLOSE(rolling_variance(acc),8.16250000000000,1e-10);

    assert_is_double(rolling_variance(acc));
}

///////////////////////////////////////////////////////////////////////////////
// test_rolling_variance
//
void test_rolling_variance()
{
    // tag::rolling_window::window_size
    accumulator_set<double, stats<tag::immediate_rolling_variance> >
        acc_immediate_rolling_variance(tag::immediate_rolling_variance::window_size = window_size);

    accumulator_set<double, stats<tag::immediate_rolling_variance, tag::rolling_mean> >
        acc_immediate_rolling_variance2(tag::immediate_rolling_variance::window_size = window_size);

    accumulator_set<double, stats<tag::rolling_variance(immediate)> >
        acc_immediate_rolling_variance3(tag::immediate_rolling_variance::window_size = window_size);

    accumulator_set<double, stats<tag::lazy_rolling_variance> >
        acc_lazy_rolling_variance(tag::lazy_rolling_variance::window_size = window_size);

    accumulator_set<double, stats<tag::rolling_variance(lazy)> >
       acc_lazy_rolling_variance2(tag::immediate_rolling_variance::window_size = window_size);

    accumulator_set<double, stats<tag::rolling_variance> >
        acc_default_rolling_variance(tag::rolling_variance::window_size = window_size);

    //// test the different implementations
    rolling_variance_test_impl(acc_immediate_rolling_variance);
    rolling_variance_test_impl(acc_immediate_rolling_variance2);
    rolling_variance_test_impl(acc_immediate_rolling_variance3);
    rolling_variance_test_impl(acc_lazy_rolling_variance);
    rolling_variance_test_impl(acc_lazy_rolling_variance2);
    rolling_variance_test_impl(acc_default_rolling_variance);

    //// test that the default implementation is the 'immediate' computation
    BOOST_REQUIRE(sizeof(acc_lazy_rolling_variance) != sizeof(acc_immediate_rolling_variance));
    BOOST_CHECK  (sizeof(acc_default_rolling_variance) == sizeof(acc_immediate_rolling_variance));

    //// test the equivalence of the different ways to indicate a feature
    BOOST_CHECK  (sizeof(acc_immediate_rolling_variance) == sizeof(acc_immediate_rolling_variance2));
    BOOST_CHECK  (sizeof(acc_immediate_rolling_variance) == sizeof(acc_immediate_rolling_variance3));
    BOOST_CHECK  (sizeof(acc_lazy_rolling_variance) == sizeof(acc_lazy_rolling_variance2));
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("rolling variance test");

    test->add(BOOST_TEST_CASE(&test_rolling_variance));

    return test;
}


/*
 * proc/rng.cpp --
 *
 * This file is part of nettcl2d application.
 *
 * Copyright (c) 2012 Andrey V. Nakin <andrey.nakin@gmail.com>
 * All rights reserved.
 *
 * See the file "COPYING" for information on usage and redistribution
 * of this file, and for a DISCLAIMER OF ALL WARRANTIES.
 *
 */

#ifndef PROC_RNG_HPP_
#define PROC_RNG_HPP_

#include <tcl.h>
#include <string>

#include <boost/shared_ptr.hpp>
#include <phlib/tclutils.h>
#include "wrapper.hpp"
#include "../calc/abstract_rng.hpp"
#include "../rng/uniform_rng.hpp"
#include "../rng/uniform2_rng.hpp"
#include "../rng/const_rng.hpp"

namespace proc {

	namespace type {
		extern const char* rng;
	}

	class RngWrapper : public Wrapper<&type::rng> {

		typedef Wrapper<&type::rng> Base;

		explicit RngWrapper(AbstractRng* const engine) :
			engine(engine) {}

		virtual Base* clone() const {
			return new RngWrapper(dynamic_cast<AbstractRng*>(engine->clone()));
		}

		static int doMain(ClientData clientData, Tcl_Interp * interp, int objc, Tcl_Obj * CONST objv[]) {
			return process(clientData, interp, objc, objv, main);
		}

		static int main(ClientData clientData, Tcl_Interp * interp, int objc, Tcl_Obj * CONST objv[]) {
			if (objc < 2)
				throw WrongNumArgs(interp, 1, objv, "command");

			const std::string cmd = Tcl_GetStringFromObj(objv[1], NULL);

			try {
				if ("create" == cmd) {
					return create(interp, objc - 2, objv + 2);
				}

				else if ("exists" == cmd) {
					return exists(interp, objc - 2, objv + 2);
				}

				else if ("seed" == cmd) {
					return processInstance(clientData, interp, objc - 2, objv + 2, static_cast<InstanceHandler>(&RngWrapper::seed));
				}

				else if ("next" == cmd) {
					return processInstance(clientData, interp, objc - 2, objv + 2, static_cast<InstanceHandler>(&RngWrapper::next));
				}

				else
					throw WrongArgValue(interp, "create | exists | seed | next");
			} catch (WrongNumArgs& ex) {
				throw WrongNumArgs(interp, 2 + ex.objc, objv, ex.message);
			}

			return TCL_OK;
		}

		static int create(Tcl_Interp * interp, int objc, Tcl_Obj * CONST objv[]) {
			if (objc < 1)
				throw WrongNumArgs(interp, 0, objv, "rngType");

			const std::string methodName = Tcl_GetStringFromObj(objv[0], NULL);
			AbstractRng* rng;

			if ("uniform" == methodName) {
				if (objc != 3)
					throw WrongNumArgs(interp, 1, objv, "mean range");

				rng = new rng::Uniform(
						phlib::TclUtils::getDouble(interp, objv[1]),
						phlib::TclUtils::getDouble(interp, objv[2]));
			}

			else if ("uniform2" == methodName) {
				if (objc != 3)
					throw WrongNumArgs(interp, 1, objv, "min max");

				rng = new rng::Uniform2(
						phlib::TclUtils::getDouble(interp, objv[1]),
						phlib::TclUtils::getDouble(interp, objv[2]));
			}

			else if ("const" == methodName) {
				if (objc != 2)
					throw WrongNumArgs(interp, 1, objv, "value");

				rng = new rng::Const(phlib::TclUtils::getDouble(interp, objv[1]));
			}

			else
				throw WrongArgValue(interp, "const | uniform | uniform2");

			// instantiate new TCL object
			Tcl_Obj* const w = Tcl_NewObj();
			w->typePtr = RngWrapper::type();
			w->internalRep.otherValuePtr = new RngWrapper(rng);
			::Tcl_SetObjResult(interp, w);

			return TCL_OK;
		}

		int seed(ClientData /* clientData */, Tcl_Interp * interp, int objc, Tcl_Obj * CONST objv[]) {
			if (objc != 1)
				throw phlib::TclUtils::wrong_num_args_exception(interp, objc, objv);

			engine->seed(phlib::TclUtils::getLong(interp, objv[0]));
			return TCL_OK;
		}

		int next(ClientData /* clientData */, Tcl_Interp * interp, int objc, Tcl_Obj * CONST objv[]) {
			if (objc != 0)
				throw phlib::TclUtils::wrong_num_args_exception(interp, objc, objv);

			::Tcl_SetObjResult(interp, ::Tcl_NewDoubleObj(engine->generate()));
			return TCL_OK;
		}

	public:

		boost::shared_ptr<AbstractRng> engine;

		static RngWrapper* validateArg(Tcl_Interp *interp, const Tcl_Obj* arg) {
			return static_cast<RngWrapper*>(Base::validateArg(interp, arg));
		}

		static void registerCommands(Tcl_Interp * interp) {
			registerCommand(interp, doMain);
		}

	};

}

#endif // PROC_RNG_HPP_


/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

#include <cstdlib>
#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
#include <cmath>
#include <limits>
#include <vector>

#include <boost/numeric/odeint.hpp>

#include "NAOS/constants.hpp"
#include "NAOS/basicMath.hpp"
#include "NAOS/basicAstro.hpp"
#include "NAOS/misc.hpp"
#include "NAOS/ellipsoidGravitationalAcceleration.hpp"

namespace naos
{

//! equations of motion (for a particle around the asteroid modelled as an Ellipsoid)
/*!
 * first order differential equations describing the motion of a particle or spacecraft around a
 * central body modeled as an ellipsoid using ellipsoid gravitational model.
 */
class equationsOfMotionParticleAroundEllipsoid
{
    // declare parameters, gravitational parameter and the semi major axes of the ellipsoid
    const double gravParameter;
    const double alpha;
    const double beta;
    const double gamma;
    const double zRotation;

public:
    // Default constructor with member initializer list
    equationsOfMotionParticleAroundEllipsoid(
               const double aGravParameter,
               const double aAlpha,
               const double aBeta,
               const double aGamma,
               const double aZRotation )
            : gravParameter( aGravParameter ),
              alpha( aAlpha ),
              beta( aBeta ),
              gamma( aGamma ),
              zRotation( aZRotation )
    { }
    void operator() ( const std::vector< double > &stateVector,
                      std::vector< double > &dXdt,
                      const double currentTime )
    {
        // calculate the gravitational accelerations first
        std::vector< double > gravAcceleration( 3, 0.0 );

        computeEllipsoidGravitationalAcceleration( alpha,
                                                   beta,
                                                   gamma,
                                                   gravParameter,
                                                   stateVector[ xPositionIndex ],
                                                   stateVector[ yPositionIndex ],
                                                   stateVector[ zPositionIndex ],
                                                   gravAcceleration );

        // now calculate the derivatives
        dXdt[ xPositionIndex ] = stateVector[ xVelocityIndex ];
        dXdt[ yPositionIndex ] = stateVector[ yVelocityIndex ];
        dXdt[ zPositionIndex ] = stateVector[ zVelocityIndex ];

        dXdt[ xVelocityIndex ] = gravAcceleration[ xPositionIndex ]
                                + 2.0 * zRotation * stateVector[ yVelocityIndex ]
                                + zRotation * zRotation * stateVector[ xPositionIndex ];

        dXdt[ yVelocityIndex ] = gravAcceleration[ yPositionIndex ]
                                - 2.0 * zRotation * stateVector[ xVelocityIndex ]
                                + zRotation * zRotation * stateVector[ yPositionIndex ];

        dXdt[ zVelocityIndex ] = gravAcceleration[ zPositionIndex ];
    }
};

//! Store intermediate state values and time( if needed )
/*!
 * This structure contains members that will save all intermediate state values and times when
 * an object of this structure is passed as an argument to the integrator function.
 */
struct pushBackStateAndTime
{
    // declare containers to store state and time
    std::vector< std::vector< double > > &stateContainer;
    std::vector< double > &timeContainer;

    //member initializer list
    pushBackStateAndTime( std::vector< std::vector< double > > &aState,
                          std::vector< double > &aTime )
                : stateContainer( aState ),
                  timeContainer( aTime )
    { }

    void operator() ( const std::vector< double > &singleStateVector, const double singleTime )
    {
        // store the intermediate state and time values in the containers
        stateContainer.push_back( singleStateVector );
        timeContainer.push_back( singleTime );
    }
};

//! particle around ellipsoid integration
/*!
 * integrate the equations of motion for a particle around an ellipsoid. The gravitational accelerations
 * calculated using the ellipsoid gravitational potential model.
 */
void executeParticleAroundEllipsoid( const double alpha,
                                     const double beta,
                                     const double gamma,
                                     const double gravParameter,
                                     std::vector< double > asteroidRotationVector,
                                     std::vector< double > &initialOrbitalElements,
                                     const double initialStepSize,
                                     const double startTime,
                                     const double endTime,
                                     std::ostringstream &outputFilePath,
                                     const int dataSaveIntervals )
{
    //! open the output csv file to save data. Declare file headers.
    std::ofstream outputFile;
    outputFile.open( outputFilePath.str( ) );
    outputFile << "x" << ",";
    outputFile << "y" << ",";
    outputFile << "z" << ",";
    outputFile << "vx" << ",";
    outputFile << "vy" << ",";
    outputFile << "vz" << ",";
    outputFile << "t" << std::endl;
    outputFile.precision( 16 );

    //! convert the initial orbital elements to cartesian state
    std::vector< double > initialStateInertial( 6, 0.0 );
    initialStateInertial = convertKeplerianElementsToCartesianCoordinates( initialOrbitalElements,
                                                                           gravParameter );

    //! account for non-zero start time value and calculate the initial state in body frame
    double phi = asteroidRotationVector[ zPositionIndex ] * startTime;

    std::vector< double > initialState( 6, 0.0 );

    // get the initial body frame position
    initialState[ xPositionIndex ]
            = initialStateInertial[ xPositionIndex ] * std::cos( phi )
            + initialStateInertial[ yPositionIndex ] * std::sin( phi );

    initialState[ yPositionIndex ]
            = -1.0 * initialStateInertial[ xPositionIndex ] * std::sin( phi )
            + initialStateInertial[ yPositionIndex ] * std::cos( phi );

    initialState[ zPositionIndex ] = initialStateInertial[ zPositionIndex ];

    // get the initial body frame velocity
    std::vector< double > inertialPositionVector = { initialStateInertial[ xPositionIndex ],
                                                     initialStateInertial[ yPositionIndex ],
                                                     initialStateInertial[ zPositionIndex ] };

    std::vector< double > omegaCrossPosition( 3, 0.0 );
    omegaCrossPosition = crossProduct( asteroidRotationVector, inertialPositionVector );

    double xbodyFrameVelocityInertialCoordinates
                = initialStateInertial[ xVelocityIndex ] - omegaCrossPosition[ 0 ];

    double ybodyFrameVelocityInertialCoordinates
                = initialStateInertial[ yVelocityIndex ] - omegaCrossPosition[ 1 ];

    double zbodyFrameVelocityInertialCoordinates
                = initialStateInertial[ zVelocityIndex ] - omegaCrossPosition[ 2 ];

    initialState[ xVelocityIndex ]
            = xbodyFrameVelocityInertialCoordinates * std::cos( phi )
            + ybodyFrameVelocityInertialCoordinates * std::sin( phi );

    initialState[ yVelocityIndex ]
            = -1.0 * xbodyFrameVelocityInertialCoordinates * std::sin( phi )
            + ybodyFrameVelocityInertialCoordinates * std::cos( phi );

    initialState[ zVelocityIndex] = zbodyFrameVelocityInertialCoordinates;

    // set up boost odeint
    const double absoluteTolerance = 1.0e-15;
    const double relativeTolerance = 1.0e-15;
    typedef boost::numeric::odeint::runge_kutta_fehlberg78< std::vector< double > > stepperType;

    // state step size guess (at each step this initial guess will be used)
    double stepSizeGuess = initialStepSize;

    // initialize the ode system
    const double zRotation = asteroidRotationVector[ zPositionIndex ];
    equationsOfMotionParticleAroundEllipsoid particleAroundEllipsoidProblem( gravParameter,
                                                                             alpha,
                                                                             beta,
                                                                             gamma,
                                                                             zRotation );

    // initialize current state vector and time
    std::vector< double > currentStateVector = initialState;
    double currentTime = startTime;
    double intermediateEndTime = currentTime + dataSaveIntervals;

    // save the initial state vector
    outputFile << currentStateVector[ xPositionIndex ] << ",";
    outputFile << currentStateVector[ yPositionIndex ] << ",";
    outputFile << currentStateVector[ zPositionIndex ] << ",";
    outputFile << currentStateVector[ xVelocityIndex ] << ",";
    outputFile << currentStateVector[ yVelocityIndex ] << ",";
    outputFile << currentStateVector[ zVelocityIndex ] << ",";
    outputFile << currentTime << std::endl;

    // start the integration outer loop
    while( intermediateEndTime <= endTime )
    {
        // perform integration, integrated result stored in currentStateVector
        size_t steps = boost::numeric::odeint::integrate_adaptive(
                            make_controlled( absoluteTolerance, relativeTolerance, stepperType( ) ),
                            particleAroundEllipsoidProblem,
                            currentStateVector,
                            currentTime,
                            intermediateEndTime,
                            stepSizeGuess );

        // update the time variables
        currentTime = intermediateEndTime;
        intermediateEndTime = currentTime + dataSaveIntervals;

        // save data
        outputFile << currentStateVector[ xPositionIndex ] << ",";
        outputFile << currentStateVector[ yPositionIndex ] << ",";
        outputFile << currentStateVector[ zPositionIndex ] << ",";
        outputFile << currentStateVector[ xVelocityIndex ] << ",";
        outputFile << currentStateVector[ yVelocityIndex ] << ",";
        outputFile << currentStateVector[ zVelocityIndex ] << ",";
        outputFile << currentTime << std::endl;

    } // end of outer while loop for integration

    outputFile.close( );
}

//! Trajectory calculation for regolith around an asteroid (modelled as ellipsoid here)
/*!
 * Same as the previous function, except that the initial conditions are now given as a cartesian
 * state. The initial cartesian state should be given in body fixed frame of the asteroid.
 */
void singleRegolithTrajectoryCalculator( const double alpha,
                                         const double beta,
                                         const double gamma,
                                         const double gravParameter,
                                         std::vector< double > asteroidRotationVector,
                                         std::vector< double > &initialCartesianStateVector,
                                         const double initialStepSize,
                                         const double startTime,
                                         const double endTime,
                                         std::ostringstream &outputFilePath,
                                         const int dataSaveIntervals )
{
    //! open the output csv file to save data. Declare file headers.
    std::ofstream outputFile;
    outputFile.open( outputFilePath.str( ) );
    outputFile << "x" << ",";
    outputFile << "y" << ",";
    outputFile << "z" << ",";
    outputFile << "vx" << ",";
    outputFile << "vy" << ",";
    outputFile << "vz" << ",";
    outputFile << "t" << std::endl;
    outputFile.precision( 16 );

    //! get the initial cartesian state vector in a seperate container
    std::vector< double > initialState = initialCartesianStateVector;

    // set up boost odeint
    const double absoluteTolerance = 1.0e-15;
    const double relativeTolerance = 1.0e-15;
    typedef boost::numeric::odeint::runge_kutta_fehlberg78< std::vector< double > > stepperType;

    // state step size guess (at each step this initial guess will be used)
    double stepSizeGuess = initialStepSize;

    // initialize the ode system
    const double zRotation = asteroidRotationVector[ zPositionIndex ];
    equationsOfMotionParticleAroundEllipsoid particleAroundEllipsoidProblem( gravParameter,
                                                                             alpha,
                                                                             beta,
                                                                             gamma,
                                                                             zRotation );

    // initialize current state vector and time
    std::vector< double > currentStateVector = initialState;
    double currentTime = startTime;
    double intermediateEndTime = currentTime + dataSaveIntervals;

    // save the initial state vector
    outputFile << currentStateVector[ xPositionIndex ] << ",";
    outputFile << currentStateVector[ yPositionIndex ] << ",";
    outputFile << currentStateVector[ zPositionIndex ] << ",";
    outputFile << currentStateVector[ xVelocityIndex ] << ",";
    outputFile << currentStateVector[ yVelocityIndex ] << ",";
    outputFile << currentStateVector[ zVelocityIndex ] << ",";
    outputFile << currentTime << std::endl;

    // start the integration outer loop
    while( intermediateEndTime <= endTime )
    {
        // save the last know state vector for when the particle is outside the asteroid
        std::vector< double > lastStateVector = currentStateVector;

        // perform integration, integrated result stored in currentStateVector
        size_t steps = boost::numeric::odeint::integrate_adaptive(
                            make_controlled( absoluteTolerance, relativeTolerance, stepperType( ) ),
                            particleAroundEllipsoidProblem,
                            currentStateVector,
                            currentTime,
                            intermediateEndTime,
                            stepSizeGuess );

        //! check if the particle is inside the surface of the asteroid
        double xSquare = currentStateVector[ xPositionIndex ] * currentStateVector[ xPositionIndex ];
        double ySquare = currentStateVector[ yPositionIndex ] * currentStateVector[ yPositionIndex ];
        double zSquare = currentStateVector[ zPositionIndex ] * currentStateVector[ zPositionIndex ];

        double crashCheck = xSquare / ( alpha * alpha )
                            + ySquare / ( beta * beta )
                            + zSquare / ( gamma * gamma )
                            - 1.0;

        if( crashCheck == 0.0 )
        {
            // particle is on the surface of the asteroid, save data and stop integration
            // update the time variables
            currentTime = intermediateEndTime;

            // save data
            outputFile << currentStateVector[ xPositionIndex ] << ",";
            outputFile << currentStateVector[ yPositionIndex ] << ",";
            outputFile << currentStateVector[ zPositionIndex ] << ",";
            outputFile << currentStateVector[ xVelocityIndex ] << ",";
            outputFile << currentStateVector[ yVelocityIndex ] << ",";
            outputFile << currentStateVector[ zVelocityIndex ] << ",";
            outputFile << currentTime << std::endl;

            break;
        }

        if( crashCheck < 0.0 )
        {
            double stepSize = 1.0;
            const double machinePrecision = std::numeric_limits< double >::epsilon( );
            // if the particle is on the surface of the asteroid, then the while condition
            // will be false, for all other cases it will be true. Within the while loop, the
            // inside or outside differnetiation takes place.
            while( std::fabs( crashCheck ) > machinePrecision )
            {
                // std::cout << "crash check value = " << crashCheck << std::endl;
                if( crashCheck < 0.0 ) // particle is still inside the surface
                {
                    // std::cout << "particle inside the surface" << std::endl << std::endl;
                    // particle is inside the surface of the asteroid. restart the integration from last
                    // known state external to the asteroid
                    currentStateVector = lastStateVector;
                    stepSize = 0.5 * stepSize;
                }
                else // particle is outside the asteroid at the end of last integration step
                {
                    // std::cout << "particle outside the surface" << std::endl << std::endl;
                    lastStateVector = currentStateVector;
                    currentTime = intermediateEndTime;
                }

                typedef boost::numeric::odeint::runge_kutta_fehlberg78< std::vector< double > > errorStepperType;
                intermediateEndTime =  boost::numeric::odeint::integrate_n_steps(
                                                errorStepperType( ),
                                                particleAroundEllipsoidProblem,
                                                currentStateVector,
                                                currentTime,
                                                stepSize,
                                                1 );

                xSquare = currentStateVector[ xPositionIndex ] * currentStateVector[ xPositionIndex ];
                ySquare = currentStateVector[ yPositionIndex ] * currentStateVector[ yPositionIndex ];
                zSquare = currentStateVector[ zPositionIndex ] * currentStateVector[ zPositionIndex ];

                crashCheck = xSquare / ( alpha * alpha )
                            + ySquare / ( beta * beta )
                            + zSquare / ( gamma * gamma )
                            - 1.0;
            }

            // particle is on the surface of the asteroid, save data and stop integration
            // update the time variables
            currentTime = intermediateEndTime;

            // save data
            outputFile << currentStateVector[ xPositionIndex ] << ",";
            outputFile << currentStateVector[ yPositionIndex ] << ",";
            outputFile << currentStateVector[ zPositionIndex ] << ",";
            outputFile << currentStateVector[ xVelocityIndex ] << ",";
            outputFile << currentStateVector[ yVelocityIndex ] << ",";
            outputFile << currentStateVector[ zVelocityIndex ] << ",";
            outputFile << currentTime << std::endl;

            break;
        }

        // update the time variables
        currentTime = intermediateEndTime;
        intermediateEndTime = currentTime + dataSaveIntervals;

        // save data
        outputFile << currentStateVector[ xPositionIndex ] << ",";
        outputFile << currentStateVector[ yPositionIndex ] << ",";
        outputFile << currentStateVector[ zPositionIndex ] << ",";
        outputFile << currentStateVector[ xVelocityIndex ] << ",";
        outputFile << currentStateVector[ yVelocityIndex ] << ",";
        outputFile << currentStateVector[ zVelocityIndex ] << ",";
        outputFile << currentTime << std::endl;

    } // end of outer while loop for integration

    outputFile.close( );
}

} // namespace naos


// SPDX-License-Identifier: MIT
// SPDX-FileCopyrightText: Copyright 2019-2022 Heal Research

#include <numeric>
#include "operon/operators/non_dominated_sorter.hpp"
#include "operon/core/individual.hpp"
#include <Eigen/Core>

namespace Operon {
    using Vec = Eigen::Matrix<int64_t, -1, 1, Eigen::ColMajor>;
    using Mat = Eigen::Matrix<int64_t, -1, -1, Eigen::ColMajor>;

    inline auto ComputeComparisonMatrix(Operon::Span<Operon::Individual const> pop, Mat const& idx, Eigen::Index colIdx) noexcept
    {
        auto const n = static_cast<Eigen::Index>(pop.size());
        Mat c = Mat::Zero(n, n);
        Mat::ConstColXpr b = idx.col(colIdx);
        c.row(b(0)).fill(1); // NOLINT
        for (auto i = 1; i < n; ++i) {
            if (pop[b(i)][colIdx] == pop[b(i-1)][colIdx]) {
                c.row(b(i)) = c.row(b(i-1));
            } else {
                for (auto j = i; j < n; ++j) {
                    c(b(i), b(j)) = 1;
                }
            }
        }
        return c;
    }

    inline auto ComparisonMatrixSum(Operon::Span<Operon::Individual const> pop, Mat const& idx) noexcept {
        Mat d = ComputeComparisonMatrix(pop, idx, 0);
        for (int i = 1; i < idx.cols(); ++i) {
            d.noalias() += ComputeComparisonMatrix(pop, idx, i);
        }
        return d;
    }

    inline auto ComputeDegreeMatrix(Operon::Span<Operon::Individual const> pop, Mat const& idx) noexcept
    {
        auto const n = static_cast<Eigen::Index>(pop.size());
        auto const m = static_cast<Eigen::Index>(pop.front().Fitness.size());
        Mat d = ComparisonMatrixSum(pop, idx);
        for (auto i = 0; i < n; ++i) {
            for (auto j = i; j < n; ++j) {
                if (d(i, j) == m && d(j, i) == m) {
                    d(i, j) = d(j, i) = 0;
                }
            }
        }
        return d;
    }

    auto DominanceDegreeSorter::Sort(Operon::Span<Operon::Individual const> pop, Operon::Scalar eps) const -> NondominatedSorterBase::Result
    {
        auto const n = static_cast<Eigen::Index>(pop.size());
        auto const m = static_cast<Eigen::Index>(pop.front().Fitness.size());

        Operon::Less cmp;
        Mat idx = Vec::LinSpaced(n, 0, n-1).replicate(1, m);
        for (auto i = 0; i < m; ++i) {
            auto *data = idx.col(i).data();
            std::sort(data, data + n, [&](auto a, auto b) { return cmp(pop[a][i], pop[b][i], eps); });
        }
        Mat d = ComputeDegreeMatrix(pop, idx);
        auto count = 0L; // number of assigned solutions
        std::vector<std::vector<size_t>> fronts;
        std::vector<size_t> tmp(n);
        std::iota(tmp.begin(), tmp.end(), 0UL);

        std::vector<size_t> remaining;
        while (count < n) {
            std::vector<size_t> front;
            for (auto i : tmp) {
                if (std::all_of(tmp.begin(), tmp.end(), [&](auto j) { return d(j, i) < m; })) {
                    front.push_back(i);
                } else {
                    remaining.push_back(i);
                }
            }
            tmp.swap(remaining);
            remaining.clear();
            count += static_cast<int64_t>(front.size());
            fronts.push_back(front);
        }
        return fronts;
    }
} // namespace Operon


/*
 * Copyright 2020 Adobe. All rights reserved.
 * This file is licensed to you under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License. You may obtain a copy
 * of the License at http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software distributed under
 * the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR REPRESENTATIONS
 * OF ANY KIND, either express or implied. See the License for the specific language
 * governing permissions and limitations under the License.
 */
#include <cmath>
#include <iostream>

#include <lagrange/testing/common.h>

#include <Eigen/Geometry>

#include <lagrange/common.h>
#include <lagrange/compute_pointcloud_pca.h>
#include <lagrange/utils/safe_cast.h>

TEST_CASE("ComputePointcloudPCA", "[compute_pointcloud_pca][symmetry]")
{
    using namespace lagrange;

    using MatrixX3dRowMajor = Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>;
    using MatrixX3dColMajor = Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::ColMajor>;
    const double eps = 1e-10;

    // An arbitrary rotation
    const Eigen::Matrix3d rotation =
        Eigen::AngleAxisd(M_PI * 0.2657, Eigen::Vector3d(-1, 4, -7).normalized())
            .toRotationMatrix();

    // An arbitrary translation
    const Eigen::Vector3d translation(1.34, -5.214, 0.35654);

    // Some points on the x,y, and z axes
    const double a = 0.1;
    const double b = 0.4;
    const double c = 1.2;

    MatrixX3dRowMajor points(6, 3);
    points.row(0) << a, 0, 0;
    points.row(1) << -a, 0, 0;
    points.row(2) << 0, -b, 0;
    points.row(3) << 0, b, 0;
    points.row(4) << 0, 0, c;
    points.row(5) << 0, 0, -c;

    // Make sure that the pca was correct
    auto verify_pca = [eps, a, b, c](
                          const double mass,
                          const Eigen::MatrixXd& pts,
                          const Eigen::MatrixXd& weights,
                          const Eigen::MatrixXd& components,
                          const Eigen::MatrixXd& R,
                          const Eigen::VectorXd& t) {
        const double a2 = a * a;
        const double b2 = b * b;
        const double c2 = c * c;

        auto approx0 = Approx(0).margin(eps);

        REQUIRE(weights(0) == Approx(mass * 2 * a2));
        REQUIRE(weights(1) == Approx(mass * 2 * b2));
        REQUIRE(weights(2) == Approx(mass * 2 * c2));
        REQUIRE((components.col(0) - R * Eigen::Vector3d(1, 0, 0)).norm() == approx0);
        REQUIRE((components.col(1) - R * Eigen::Vector3d(0, 1, 0)).norm() == approx0);
        REQUIRE((components.col(2) - R * Eigen::Vector3d(0, 0, 1)).norm() == approx0);

        Eigen::MatrixXd pminustr = pts.rowwise() - t.transpose();
        REQUIRE(
            (mass * pminustr.transpose() * pminustr -
             components * weights.asDiagonal() * components.transpose())
                .norm() == approx0);
    };

    SECTION("Column Major")
    {
        using MatrixType = MatrixX3dColMajor;
        SECTION("Simple case")
        {
            MatrixType points_tr = points;
            auto out =
                compute_pointcloud_pca(points_tr, false /*shift_center*/, false /*normalize*/);
            verify_pca(
                1 /*mass*/,
                points_tr,
                out.weights,
                out.components,
                Eigen::Matrix3d::Identity(),
                Eigen::Vector3d::Zero());
        }

        SECTION("With rotation")
        {
            MatrixType points_tr = points * rotation.transpose();
            auto out =
                compute_pointcloud_pca(points_tr, false /*shift_center*/, false /*normalize*/);
            verify_pca(
                1 /* mass */,
                points_tr,
                out.weights,
                out.components,
                rotation,
                Eigen::Vector3d::Zero());
            REQUIRE(out.center.norm() == Approx(0.).margin(eps));
        }

        SECTION("With rotation and translation")
        {
            MatrixType points_tr =
                (points * rotation.transpose()).rowwise() + translation.transpose();
            auto out =
                compute_pointcloud_pca(points_tr, true /*shift_center*/, false /*normalize*/);
            verify_pca(1 /* mass */, points_tr, out.weights, out.components, rotation, translation);
            REQUIRE((out.center - translation).norm() == Approx(0.).margin(eps));
        }

        SECTION("With rotation and translation, also scale the covariance matrix")
        {
            MatrixType points_tr =
                (points * rotation.transpose()).rowwise() + translation.transpose();
            const double mass = safe_cast<double>(1.) / (points.rows());
            auto out = compute_pointcloud_pca(points_tr, true /*shift_center*/, true /*normalize*/);
            verify_pca(mass, points_tr, out.weights, out.components, rotation, translation);
            REQUIRE((out.center - translation).norm() == Approx(0.).margin(eps));
        }
    }

    SECTION("ROW MAJOR")
    {
        using MatrixType = MatrixX3dRowMajor;
        SECTION("Simple case")
        {
            MatrixType points_tr = points;
            auto out =
                compute_pointcloud_pca(points_tr, false /*shift_center*/, false /*normalize*/);
            verify_pca(
                1 /*mass*/,
                points_tr,
                out.weights,
                out.components,
                Eigen::Matrix3d::Identity(),
                Eigen::Vector3d::Zero());
        }

        SECTION("With rotation")
        {
            MatrixType points_tr = points * rotation.transpose();
            auto out =
                compute_pointcloud_pca(points_tr, false /*shift_center*/, false /*normalize*/);
            verify_pca(
                1 /* mass */,
                points_tr,
                out.weights,
                out.components,
                rotation,
                Eigen::Vector3d::Zero());
            REQUIRE(out.center.norm() == Approx(0.).margin(eps));
        }

        SECTION("With rotation and translation")
        {
            MatrixType points_tr =
                (points * rotation.transpose()).rowwise() + translation.transpose();
            auto out =
                compute_pointcloud_pca(points_tr, true /*shift_center*/, false /*normalize*/);
            verify_pca(1 /* mass */, points_tr, out.weights, out.components, rotation, translation);
            REQUIRE((out.center - translation).norm() == Approx(0.).margin(eps));
        }

        SECTION("With rotation and translation, also scale the covariance matrix")
        {
            MatrixType points_tr =
                (points * rotation.transpose()).rowwise() + translation.transpose();
            const double mass = safe_cast<double>(1.) / (points.rows());
            auto out = compute_pointcloud_pca(points_tr, true /*shift_center*/, true /*normalize*/);
            verify_pca(mass, points_tr, out.weights, out.components, rotation, translation);
            REQUIRE((out.center - translation).norm() == Approx(0.).margin(eps));
        }
    }

} // end of TEST


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/maximum.hpp>
#include <boost/hana/minimum.hpp>
#include <boost/hana/set.hpp>
#include <boost/hana/sum.hpp>
namespace hana = boost::hana;

int main() {
    constexpr auto xs = hana::make_set(hana::int_c<0>, hana::int_c<1>, hana::int_c<2>);
    static_assert(hana::minimum(xs) == hana::int_c<0>, "");
    static_assert(hana::maximum(xs) == hana::int_c<2>, "");
    static_assert(hana::sum<>(xs) == hana::int_c<3>, "");
}