#pragma once

/**************************************************************************
 * A wrapper for a disjunctive domain of intervals from "Boxes: A
 * Symbolic Abstract Domain of Boxes" by A. Gurfinkel and S. Chaki
 * published in SAS'10.
 **************************************************************************/
#include <crab/config.h>

#include <crab/domains/abstract_domain.hpp>
#include <crab/domains/intervals.hpp>
#include <crab/support/debug.hpp>
#include <crab/support/stats.hpp>

#ifndef HAVE_LDD
/*
 * Dummy implementation if ldd not found
 */
#include "crab/domains/abstract_domain.def"
namespace crab {
namespace domains {
template <typename N, typename V>
class boxes_domain final
    : public abstract_domain_api<boxes_domain<N, V>> {
public:
  using this_type = boxes_domain<N, V>;
  boxes_domain() {}
  UNAVAILABLE_DOMAIN("No LDD. Run cmake with -DCRAB_USE_LDD=ON")
};
} // namespace domains
} // namespace crab
#else
/*
 *  Real implementation starts here
 */

#include <crab/domains/abstract_domain_params.hpp>
#include <crab/domains/abstract_domain_specialized_traits.hpp>
#include <crab/domains/backward_assign_operations.hpp>
#include <crab/domains/ldd/ldd.hpp>
#include <crab/domains/ldd/ldd_print.hpp>

#include <algorithm>
#include <boost/bimap.hpp>
#include <boost/optional.hpp>

namespace crab {

namespace domains {

using namespace crab::domains::ldd;

/*
 * The wrapper has two global datastructures:
 * 1) a ldd manager and
 * 2) a map from VariableName to ldd dimension.
 *
 * FIXME: since the ldd manager is shared we need to fix a
 * single size for all ldds. Since ldds are sparse we can fix a
 * size big enough for our programs.
 *
 * FIXME: Ldd_TermReplace seems to leak memory sometimes.
 */
template <typename Number, typename VariableName>
class boxes_domain final
    : public abstract_domain_api<boxes_domain<Number, VariableName>> {

  using interval_domain_t = ikos::interval_domain<Number, VariableName>;
  using boxes_domain_t = boxes_domain<Number, VariableName>;
  using abstract_domain_t = abstract_domain_api<boxes_domain_t>;

public:
  using number_t = Number;
  using varname_t = VariableName;
  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_t;
  using typename abstract_domain_t::variable_vector_t;
  using typename abstract_domain_t::variable_or_constant_vector_t;

private:
  using kind_t = typename linear_constraint_t::kind_t;

  // --- map from crab variable index to ldd term index
  using var_map_t = boost::bimap<variable_t, int>;
  using binding_t = typename var_map_t::value_type;

  LddNodePtr m_ldd;
  static LddManager *s_ldd_man;
  static var_map_t s_var_map;

  // -- bool reasoning is mostly based on disjunctions so for
  //    efficiency we might want to disable it if precision
  //    gains do not pay off.
  const bool m_bool_reasoning = true;

  static LddManager *get_ldd_man() {
    if (!s_ldd_man) {
      DdManager *cudd = Cudd_Init(0, 0, CUDD_UNIQUE_SLOTS, 127, 0);
      unsigned ldd_size = crab_domain_params_man::get().boxes_ldd_size();
      bool dynamic_reordering = crab_domain_params_man::get().boxes_dynamic_reordering();
      theory_t *theory = ldd::create_box_theory<number_t>(ldd_size);
      CRAB_LOG("boxes", crab::outs() << "Created a ldd of size "
                                     << ldd_size << "\n";);
      s_ldd_man = Ldd_Init(cudd, theory);
      if (dynamic_reordering) {
	Cudd_AutodynEnable(cudd, CUDD_REORDER_GROUP_SIFT);
      }
      Ldd_SanityCheck(get_ldd_man());
    }
    return s_ldd_man;
  }

  theory_t *get_theory() { return Ldd_GetTheory(get_ldd_man()); }

  const theory_t *get_theory() const { return Ldd_GetTheory(get_ldd_man()); }

  static int num_of_vars() { return s_var_map.left.size(); }

  int get_var_dim(variable_t v) const {
    auto it = s_var_map.left.find(v);
    if (it != s_var_map.left.end()) {
      return it->second;
    } else {
      // Reserved dim 0 for SPECIAL use (as a temporary var)
      unsigned int id = s_var_map.size() + 1;
      unsigned ldd_size = crab_domain_params_man::get().boxes_ldd_size();
      if (id >= ldd_size) {
        CRAB_ERROR("The Ldd size of ", ldd_size, " needs to be larger");
      }
      s_var_map.insert(binding_t(v, id));
      return id;
    }
  }

  inline constant_t mk_cst(ikos::z_number k) {
    ikos::q_number qk(k);
    return (constant_t)tvpi_create_cst(qk.get_mpq_t());
  }

  inline constant_t mk_cst(ikos::q_number k) {
    return (constant_t)tvpi_create_cst(k.get_mpq_t());
  }

  // convex approximation
  void convex_approx() {
    m_ldd = lddPtr(get_ldd_man(), Ldd_TermMinmaxApprox(get_ldd_man(), &*m_ldd));
  }

  LddNodePtr convex_approx(LddNodePtr ldd) const {
    return lddPtr(get_ldd_man(), Ldd_TermMinmaxApprox(get_ldd_man(), &*ldd));
  }

  LddNodePtr project(variable_t v) const {
    crab::CrabStats::count(domain_name() + ".count.project");
    crab::ScopedCrabStats __st__(domain_name() + ".project");

    std::vector<int> qvars;
    // num_of_vars is shared by all ldd's
    qvars.reserve(num_of_vars() - 1);
    for (auto p : s_var_map.left) {
      if (!(p.first == v)) {
        qvars.push_back(get_var_dim(p.first));
      }
    }
    return lddPtr(get_ldd_man(), Ldd_MvExistAbstract(get_ldd_man(), &*m_ldd,
						     &qvars[0], qvars.size()));
  }

  LddNodePtr join(LddNodePtr v1, LddNodePtr v2) const {
    return lddPtr(get_ldd_man(), Ldd_Or(get_ldd_man(), &*v1, &*v2));
  }

  /** return term for variable v, neg for negation of variable */
  linterm_t term_from_var(variable_t v, bool neg = false) {
    int dim = get_var_dim(v);
    int sgn = neg ? -1 : 1;
    linterm_t term =
        Ldd_GetTheory(get_ldd_man())->create_linterm_sparse_si(&dim, &sgn, 1);
    return term;
  }

  /** return term from SPECIAL variable $0 */
  linterm_t term_from_special_var(bool neg = false) {
    int dim = 0; // reserved for SPECIAL variable
    int sgn = neg ? -1 : 1;
    return Ldd_GetTheory(get_ldd_man())
        ->create_linterm_sparse_si(&dim, &sgn, 1);
  }

  void copy_term(variable_t v, boost::optional<variable_t> x) {
    if (is_top() || is_bottom()) {
      return;
    }

    this->operator-=(v); // remove v before assigning new term

    linterm_t lhs = term_from_var(v);
    linterm_t rhs = (x ? term_from_var(*x) : term_from_special_var());

    m_ldd =
        lddPtr(get_ldd_man(), Ldd_TermCopy(get_ldd_man(), &(*m_ldd), lhs, rhs));
    Ldd_GetTheory(get_ldd_man())->destroy_term(lhs);
    Ldd_GetTheory(get_ldd_man())->destroy_term(rhs);

    if (!x) {
      // XXX: if we copy the SPECIAL var to v we forget the SPECIAL
      //      var after we copy.
      crab::CrabStats::count(domain_name() + ".count.forget");
      crab::ScopedCrabStats __st__(domain_name() + ".forget");
      int dim = 0; // SPECIAL variable is dim 0
      m_ldd = lddPtr(get_ldd_man(),
                     Ldd_ExistsAbstract(get_ldd_man(), &*m_ldd, dim));
    }
  }

  /**
   **  All the expressiveness about numerical operations with boxes
   **  is limited to:
   **     v := a * x + [k.lb(),k.ub()],
   **     where a is a constant, k an interval and x variable
   **  Each numerical operation is reduced to this form.
   */
  void apply_ldd(const variable_t &v, const variable_t &x, number_t a, interval_t k) {
    if (is_top() || is_bottom())
      return;

    linterm_t t = term_from_var(v);
    linterm_t r = term_from_var(x);

    constant_t c = mk_cst(a);

    constant_t kmin = NULL;
    constant_t kmax = NULL;
    if (k.lb().is_finite())
      kmin = mk_cst(*(k.lb().number()));
    if (k.ub().is_finite())
      kmax = mk_cst(*(k.ub().number()));

    m_ldd = lddPtr(get_ldd_man(), Ldd_TermReplace(get_ldd_man(), &(*m_ldd), t,
                                                  r, c, kmin, kmax));

    Ldd_GetTheory(get_ldd_man())->destroy_term(t);
    Ldd_GetTheory(get_ldd_man())->destroy_term(r);
    Ldd_GetTheory(get_ldd_man())->destroy_cst(c);
    if (kmin)
      Ldd_GetTheory(get_ldd_man())->destroy_cst(kmin);
    if (kmax)
      Ldd_GetTheory(get_ldd_man())->destroy_cst(kmax);
  }

  /* Special case of apply_ldd : v := [lb,ub] */
  LddNodePtr apply_interval(const variable_t &v, interval_t ival) {
    assert(!is_bottom());

    constant_t kmin = NULL, kmax = NULL;
    if (boost::optional<number_t> l = ival.lb().number())
      kmin = mk_cst(*l);
    if (boost::optional<number_t> u = ival.ub().number())
      kmax = mk_cst(*u);

    linterm_t t = term_from_var(v);
    LddNodePtr res =
        lddPtr(get_ldd_man(), Ldd_TermReplace(get_ldd_man(), &(*m_ldd), t, NULL,
                                              NULL, kmin, kmax));
    if (kmin)
      get_theory()->destroy_cst(kmin);
    if (kmax)
      get_theory()->destroy_cst(kmax);
    get_theory()->destroy_term(t);
    return res;
  }

  void num_from_ldd_cst(constant_t cst, ikos::z_number &res) const {
    // XXX We know that the theory is tvpi, use its method direclty.
    ikos::q_number q;
    tvpi_cst_set_mpq(q.get_mpq_t(), (tvpi_cst_t)cst);
    res = q.round_to_lower();
  }

  void num_from_ldd_cst(constant_t cst, ikos::q_number &res) const {
    // XXX We know that the theory is tvpi, use its method direclty.
    tvpi_cst_set_mpq(res.get_mpq_t(), (tvpi_cst_t)cst);
  }

  linear_expression_t expr_from_ldd_term(linterm_t term) const {
    linear_expression_t e(0);
    for (size_t i = 0; i < (size_t)get_theory()->term_size(term); i++) {
      number_t k(0); // any value
      num_from_ldd_cst(get_theory()->term_get_coeff(term, i), k);
      variable_t v(getVarName(get_theory()->term_get_var(term, i)));
      e = e + (k * linear_expression_t(v));
    }
    return e;
  }

  ikos::linear_constraint<ikos::z_number, varname_t> cst_from_ldd_strict_cons(
      const ikos::linear_expression<ikos::z_number, varname_t> &l,
      const ikos::linear_expression<ikos::z_number, varname_t> &r) const {
    // l < r <-> l+1 <= r
    ikos::linear_expression<ikos::z_number, varname_t> e = l + 1;
    e = e - r;
    return ikos::linear_constraint<ikos::z_number, varname_t>(
        e, linear_constraint_t::INEQUALITY);
  }

  ikos::linear_constraint<ikos::q_number, varname_t> cst_from_ldd_strict_cons(
      const ikos::linear_expression<ikos::q_number, varname_t> &l,
      const ikos::linear_expression<ikos::q_number, varname_t> &r) const {
    return ikos::linear_constraint<ikos::q_number, varname_t>(
        l - r, linear_constraint_t::STRICT_INEQUALITY);
  }

  linear_constraint_t cst_from_ldd_cons(lincons_t lincons) const {

    linear_expression_t lhs =
        expr_from_ldd_term(get_theory()->get_term(lincons));
    number_t rhs(0); // any value
    num_from_ldd_cst(get_theory()->get_constant(lincons), rhs);

    if (get_theory()->is_strict(lincons)) {
      // lhs < rhs
      return cst_from_ldd_strict_cons(lhs, rhs);
    } else {
      // lhs <= rhs
      linear_expression_t e = lhs - rhs;
      return linear_constraint_t(e, linear_constraint_t::INEQUALITY);
    }
  }

  // Create a new ldd representing the constraint e
  // where e can be one of
  //    c*x <= k | c*x == k | c*x != k
  //    and c is 1 or -1 and k is an integer constant
  LddNodePtr make_unit_constraint(number_t coef, linterm_t term, kind_t kind,
                                  number_t k) {

    assert(coef == 1 || coef == -1);

    if (kind == kind_t::EQUALITY) { // x == k <-> x<=k and !(x<k)
      // x<=k
      constant_t c1 = mk_cst(k);
      // XXX: make copy of term so that we can free memory using
      // destroy_lincons without having double free
      linterm_t copy_term = get_theory()->dup_term(term);
      lincons_t cons1 =
          get_theory()->create_cons(copy_term, 0 /*non-strict*/, c1);
      LddNodePtr n1 =
          lddPtr(get_ldd_man(), get_theory()->to_ldd(get_ldd_man(), cons1));
      // !(x<k)
      constant_t c2 = mk_cst(k);
      lincons_t cons2 = get_theory()->create_cons(term, 1 /*strict*/, c2);
      LddNodePtr n2 = lddPtr(
          get_ldd_man(), Ldd_Not(get_theory()->to_ldd(get_ldd_man(), cons2)));
      get_theory()->destroy_lincons(cons1);
      get_theory()->destroy_lincons(cons2);
      return lddPtr(get_ldd_man(), Ldd_And(get_ldd_man(), &*n1, &*n2));
    } else if (kind == kind_t::INEQUALITY) {
      // x <= k
      constant_t c = mk_cst(k);
      lincons_t cons = get_theory()->create_cons(term, 0 /*non-strict*/, c);
      LddNodePtr n =
          lddPtr(get_ldd_man(), get_theory()->to_ldd(get_ldd_man(), cons));
      get_theory()->destroy_lincons(cons);
      return n;
    } else if (kind == kind_t::STRICT_INEQUALITY) {
      // x < k
      constant_t c = mk_cst(k);
      lincons_t cons = get_theory()->create_cons(term, 1 /*strict*/, c);
      LddNodePtr n =
          lddPtr(get_ldd_man(), get_theory()->to_ldd(get_ldd_man(), cons));
      get_theory()->destroy_lincons(cons);
      return n;
    } else {
      assert(kind == kind_t::DISEQUATION);
      return lddPtr(get_ldd_man(), Ldd_Not(&*make_unit_constraint(
                                       coef, term, kind_t::EQUALITY, k)));
    }
  }

  LddNodePtr make_unit_constraint(number_t coef, variable_t var, kind_t kind,
                                  number_t k) {
    linterm_t term = term_from_var(var, (coef == 1 ? false : true));
    return make_unit_constraint(coef, term, kind, k);
  }

  LddNodePtr make_unit_constraint(number_t coef, kind_t kind, number_t k) {
    linterm_t term = term_from_special_var((coef == 1 ? false : true));
    return make_unit_constraint(coef, term, kind, k);
  }

  void apply_unit_constraint(number_t coef, variable_t x, kind_t kind,
                             number_t k) {
    LddNodePtr n = make_unit_constraint(coef, x, kind, k);
    m_ldd = lddPtr(get_ldd_man(), Ldd_And(get_ldd_man(), &*m_ldd, &*n));
  }

  // Extract interval constraints from linear expression exp. This
  // operation uses operator[] which loses precision. It should be
  // only used with non-unit expressions.
  void unitcsts_of_exp(const linear_expression_t &exp,
                       std::vector<std::pair<variable_t, number_t>> &lbs,
                       std::vector<std::pair<variable_t, number_t>> &ubs) {

    number_t unbounded_lbcoeff;
    number_t unbounded_ubcoeff;
    boost::optional<variable_t> unbounded_lbvar;
    boost::optional<variable_t> unbounded_ubvar;
    number_t exp_ub = -(exp.constant());
    std::vector<std::pair<std::pair<number_t, variable_t>, number_t>> pos_terms;
    std::vector<std::pair<std::pair<number_t, variable_t>, number_t>> neg_terms;
    for (auto p : exp) {
      number_t coeff(p.first);
      if (coeff > number_t(0)) {
        variable_t y(p.second);
        // evaluate the variable in the domain
        auto y_lb = this->operator[](y).lb();
        if (y_lb.is_infinite()) {
          if (unbounded_lbvar)
            return;
          unbounded_lbvar = y;
          unbounded_lbcoeff = coeff;
        } else {
          number_t ymin(*(y_lb.number()));
          exp_ub -= ymin * coeff;
          pos_terms.push_back({{coeff, y}, ymin});
        }
      } else {
        variable_t y(p.second);
        // evaluate the variable in the domain
        auto y_ub = this->operator[](y).ub();
        if (y_ub.is_infinite()) {
          if (unbounded_ubvar)
            return;
          unbounded_ubvar = y;
          unbounded_ubcoeff = -(coeff);
        } else {
          number_t ymax(*(y_ub.number()));
          exp_ub -= ymax * coeff;
          neg_terms.push_back({{-coeff, y}, ymax});
        }
      }
    }

    if (unbounded_lbvar) {
      if (!unbounded_ubvar) {
        // Add bounds for x
        variable_t x(*unbounded_lbvar);
        ubs.push_back({x, exp_ub / unbounded_lbcoeff});
      }
    } else {
      if (unbounded_ubvar) {
        // Bounds for y
        variable_t y(*unbounded_ubvar);
        lbs.push_back({y, -exp_ub / unbounded_ubcoeff});
      } else {
        for (auto pl : neg_terms)
          lbs.push_back(
              {pl.first.second, -exp_ub / pl.first.first + pl.second});
        for (auto pu : pos_terms)
          ubs.push_back({pu.first.second, exp_ub / pu.first.first + pu.second});
      }
    }
  }

  interval_t eval_interval(const linear_expression_t &e) {
    interval_t r = e.constant();
    for (auto p : e)
      r += p.first * operator[](p.second);
    return r;
  }

  // Given a constraint a1*x1 + ... + an*xn <= k and pivot xi,
  // it computes the interval:
  //  k - intv (a1*x1 + ... + ai-1*xi-1 + ai+1*xi+1 + ... an*xn)
  interval_t compute_residual(const linear_expression_t &e, variable_t pivot) {
    interval_t residual(-e.constant());
    for (typename linear_expression_t::const_iterator it = e.begin();
         it != e.end(); ++it) {
      variable_t v = it->second;
      if (!(v == pivot)) {
        residual = residual - (interval_t(it->first) * this->operator[](v));
      }
    }
    return residual;
  }

  bool add_linear_leq(const linear_expression_t &e) {
    std::vector<std::pair<variable_t, number_t>> lbs;
    std::vector<std::pair<variable_t, number_t>> ubs;

    unitcsts_of_exp(e, lbs, ubs);
    CRAB_LOG("boxes-extract-leq",
             for (auto p
                  : lbs) {
               crab::outs() << "\t" << p.first << ">=" << p.second << "\n";
             } for (auto p
                    : ubs) {
               crab::outs() << "\t" << p.first << "<=" << p.second << "\n";
             });

    for (auto p : lbs) {
      apply_unit_constraint(-number_t(1) /*coef*/, p.first, kind_t::INEQUALITY,
                            -p.second);
      if (is_bottom()) {
        return false;
      }
    }
    for (auto p : ubs) {
      apply_unit_constraint(number_t(1) /*coef*/, p.first, kind_t::INEQUALITY,
                            p.second);
      if (is_bottom()) {
        return false;
      }
    }
    return true;
  }

  bool add_linear_diseq(const linear_expression_t &e) {
    assert(!is_bottom());

    for (typename linear_expression_t::const_iterator it = e.begin();
         it != e.end(); ++it) {
      variable_t v = it->second;
      interval_t res_i = compute_residual(e, v) / interval_t(it->first);
      boost::optional<number_t> k = res_i.singleton();
      if (k) {
        linear_constraint_t cst(v != *k);
        operator+=(cst);
        if (is_bottom())
          return false;
      } else {
        // XXX: we lose precision here because res_i is an
        // interval which already over-approximates the possible
        // values of the residual.
      }
    }
    return true;
  }

  boxes_domain(LddNodePtr ldd) : m_ldd(ldd) {
    if (crab_domain_params_man::get().boxes_convexify_threshold() > 0) {
      // XXX: the value of convexify_threshold is quite arbitrary. A
      // good value seems around 1000000
      unsigned threshold = num_of_vars() *
	crab_domain_params_man::get().boxes_convexify_threshold();
      // unsigned num_paths = Ldd_PathSize(NULL, &*m_ldd);
      unsigned num_paths = (unsigned)Cudd_CountPath(&*m_ldd);
      if (threshold > 0 && num_paths > threshold) {
        convex_approx();
        CRAB_WARN("ldd size was too large: ", num_paths, ". Made ldd convex.");
      }
    }
  }

  interval_domain_t to_intervals(LddNodePtr &ldd) {
    crab::CrabStats::count(domain_name() + ".count.to_intervals");
    crab::ScopedCrabStats __st__(domain_name() + ".to_intervals");

    interval_domain_t res; // top

    if (&*ldd == Ldd_GetFalse(get_ldd_man())) {
      res.set_to_bottom();
      return res;
    }
    if (&*ldd == Ldd_GetTrue(get_ldd_man())) {
      return res;
    }

    LddNodePtr ldd_copy(ldd);
    ldd_copy = convex_approx(ldd_copy);

    auto disjs = to_disjunctive_linear_constraint_system(ldd_copy);
    if (disjs.is_true()) {
      return res;
    } else if (disjs.is_false()) {
      res.set_to_bottom();
      return res;
    } else {
      if (disjs.size() != 1) {
        CRAB_ERROR("Boxes::to_intervals: it should not be disjunctive ", disjs);
      }
      for (auto c : *(disjs.begin())) {
        res += c;
      }
      return res;
    }
  }

  void to_disjunctive_linear_constraint_system_aux(
      LddManager *ldd, LddNode *n, disjunctive_linear_constraint_system_t &e,
      std::vector<int> &list) const {
    /**
     * the latest negative constraint to be printed.
     * is NULL if there isn't one
     */
    lincons_t negc = nullptr;

    DdNode *N = Cudd_Regular(n);

    if (cuddIsConstant(N)) {
      /* n == N here implies that n is one */
      if (n == N) {

        linear_constraint_system_t csts;

        /* for each level */
        for (int i = 0; i < ldd->cudd->size; i++) {
          /* let p be the index of level i */
          int p = ldd->cudd->invperm[i];
          /* let v be the value of p */
          int v = list[p];

          /* skip don't care */
          if (v == 2)
            continue;

          if (v == 0 && ldd->ddVars[p] != nullptr) {
            lincons_t c;
            c = ldd->theory->negate_cons(ldd->ddVars[p]);

            if (negc != nullptr) {
              /* consider negative constraint if it is not implied
                 by c
              */
              if (!ldd->theory->is_stronger_cons(c, negc)) {
                csts += cst_from_ldd_cons(negc);
              }
              ldd->theory->destroy_lincons(negc);
            }

            /* store the current constraint to be considered later */
            negc = c;
            continue;
          }

          /* if there is a negative constraint waiting to be
             considered, conjoin it now
          */
          if (negc != nullptr) {
            csts += cst_from_ldd_cons(negc);
            ldd->theory->destroy_lincons(negc);
            negc = nullptr;
          }

          /* if v is not a don't care but p does not correspond to a
           * constraint, consider it as a Boolean variable */
          if (v != 2 && ldd->ddVars[p] == nullptr) {
            // XXX: I don't know what to do here
            // fprintf(stderr, "%sb%d",(v == 0 ? "!" : " "), p);
          }
          /* v is true */
          else if (v == 1) {
            csts += cst_from_ldd_cons(ldd->ddVars[p]);
          }
        } // end for

        /* if there is a constraint waiting to be considered, do it
           now */
        if (negc != nullptr) {
          csts += cst_from_ldd_cons(negc);
          ldd->theory->destroy_lincons(negc);
          negc = nullptr;
        }

        if (csts.is_true()) {
          // FIXME: if csts is true then e should become true
        } else {
          e += csts;
        }
      }
    } else {
      DdNode *Nv = Cudd_NotCond(cuddT(N), N != n);
      DdNode *Nnv = Cudd_NotCond(cuddE(N), N != n);
      int index = N->index;
      list[index] = 0;
      to_disjunctive_linear_constraint_system_aux(ldd, Nnv, e, list);
      list[index] = 1;
      to_disjunctive_linear_constraint_system_aux(ldd, Nv, e, list);
      list[index] = 2;
    }
    return;
  }

  inline LddNodePtr mk_false(boost::optional<variable_t> x) {
    return (x ? make_unit_constraint(number_t(1), variable_t(*x),
				     linear_constraint_t::EQUALITY, number_t(0)):
	    make_unit_constraint(number_t(1),
				 linear_constraint_t::EQUALITY, number_t(0)));
  }

  inline linear_constraint_t mk_false_cst(const variable_t &x) {
    return linear_constraint_t(x == 0);
  }

  // Important: Crab follows LLVM semantics where TRUE is 1. Note that
  // in C, TRUE is any value different from 0. Following LLVM
  // semantics avoids many disjunctions in the boolean operations.
  inline LddNodePtr mk_true(boost::optional<variable_t> x) {
    return (x ? make_unit_constraint(number_t(1), variable_t(*x),
				     linear_constraint_t::EQUALITY, number_t(1)):
	        make_unit_constraint(number_t(1),
				     linear_constraint_t::EQUALITY, number_t(1)));
    // return (x ? make_unit_constraint(number_t(1), variable_t(*x),
    // 				     linear_constraint_t::DISEQUATION, number_t(0)):
    // 	        make_unit_constraint(number_t(1),
    // 				     linear_constraint_t::DISEQUATION, number_t(0)));
  }

  inline linear_constraint_t mk_true_cst(const variable_t &x) {
    return linear_constraint_t(x == 1);
    //return linear_constraint_t(x != 0);
  }

  // Return a node of the form ITE(y Op z, x=TRUE, x=FALSE)
  LddNodePtr gen_binary_bool(bool_operation_t op, boost::optional<variable_t> x,
                             const variable_t &y, const variable_t &z) {
    switch (op) {
    case OP_BAND: {
      LddNodePtr c = lddPtr(get_ldd_man(),
                            Ldd_And(get_ldd_man(), &*mk_true(y), &*mk_true(z)));
      return lddPtr(get_ldd_man(),
                    Ldd_Ite(get_ldd_man(), &*c, &*mk_true(x), &*mk_false(x)));
    }
    case OP_BOR: {
      LddNodePtr c = lddPtr(get_ldd_man(),
                            Ldd_Or(get_ldd_man(), &*mk_true(y), &*mk_true(z)));
      return lddPtr(get_ldd_man(),
                    Ldd_Ite(get_ldd_man(), &*c, &*mk_true(x), &*mk_false(x)));
    }
    case OP_BXOR: {
      LddNodePtr c = lddPtr(get_ldd_man(),
                            Ldd_Xor(get_ldd_man(), &*mk_true(y), &*mk_true(z)));
      return lddPtr(get_ldd_man(),
                    Ldd_Ite(get_ldd_man(), &*c, &*mk_true(x), &*mk_false(x)));

    }
    default:
      CRAB_ERROR("Unknown boolean operator");
    }
  }

  variable_t getVarName(int v) const {
    auto it = s_var_map.right.find(v);
    if (it != s_var_map.right.end())
      return it->second;
    else {
      CRAB_ERROR("Index ", v, " cannot be mapped back to a variable name");
    }
  }

public:
  static void clear_global_state() { s_var_map.clear(); }

  boxes_domain() : m_ldd(lddPtr(get_ldd_man(), Ldd_GetTrue(get_ldd_man()))) {}

  ~boxes_domain() {
    // DdManager *cudd = nullptr;
    // theory_t *theory = nullptr;
    // if (s_ldd_man)  {
    //   cudd = Ldd_GetCudd(s_ldd_man);
    //   theory = Ldd_GetTheory(s_ldd_man);
    //   Ldd_Quit(s_ldd_man);
    // }
    // if (theory) tvpi_destroy_theory(theory);
    // if (cudd) Cudd_Quit(cudd);
  }

  boxes_domain_t make_top() const override {
    boxes_domain_t out(lddPtr(get_ldd_man(), Ldd_GetTrue(get_ldd_man())));
    return out;
  }

  boxes_domain_t make_bottom() const override {
    boxes_domain_t out(lddPtr(get_ldd_man(), Ldd_GetFalse(get_ldd_man())));
    return out;
  }

  void set_to_top() override {
    boxes_domain_t abs(lddPtr(get_ldd_man(), Ldd_GetTrue(get_ldd_man())));
    std::swap(*this, abs);
  }

  void set_to_bottom() override {
    boxes_domain_t abs(lddPtr(get_ldd_man(), Ldd_GetFalse(get_ldd_man())));
    std::swap(*this, abs);
  }

  boxes_domain(const boxes_domain_t &other)
      : // m_ldd(other.m_ldd)
        m_ldd(lddPtr(get_ldd_man(), &(*other.m_ldd))) {
    crab::CrabStats::count(domain_name() + ".count.copy");
    crab::ScopedCrabStats __st__(domain_name() + ".copy");
  }

  boxes_domain(boxes_domain_t &&other) : m_ldd(std::move(other.m_ldd)) {}

  boxes_domain_t &operator=(const boxes_domain_t &other) {
    crab::CrabStats::count(domain_name() + ".count.copy");
    crab::ScopedCrabStats __st__(domain_name() + ".copy");
    if (this != &other) {
      // m_ldd = other.m_ldd;
      m_ldd = lddPtr(get_ldd_man(), &(*other.m_ldd));
    }
    return *this;
  }

  bool is_bottom() const override {
    return &*m_ldd == Ldd_GetFalse(get_ldd_man());
  }

  bool is_top() const override { return &*m_ldd == Ldd_GetTrue(get_ldd_man()); }

  bool operator<=(const boxes_domain_t &other) const override {
    crab::CrabStats::count(domain_name() + ".count.leq");
    crab::ScopedCrabStats __st__(domain_name() + ".leq");

    bool res = Ldd_TermLeq(get_ldd_man(), &(*m_ldd), &(*other.m_ldd));

    CRAB_LOG("boxes", boxes_domain_t left(*this); boxes_domain_t right(other);
             crab::outs() << "Check if " << left << " <= " << right << " ---> "
                          << res << "\n";);
    return res;
  }

  void operator|=(const boxes_domain_t &other) override {
    *this = *this | other;
  }

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

    return boxes_domain_t(join(m_ldd, other.m_ldd));
  }

  boxes_domain_t operator&(const boxes_domain_t &other) const override {
    crab::CrabStats::count(domain_name() + ".count.meet");
    crab::ScopedCrabStats __st__(domain_name() + ".meet");

    return boxes_domain_t(
        lddPtr(get_ldd_man(), Ldd_And(get_ldd_man(), &*m_ldd, &*other.m_ldd)));
  }

  boxes_domain_t operator||(const boxes_domain_t &other) const override {
    crab::CrabStats::count(domain_name() + ".count.widening");
    crab::ScopedCrabStats __st__(domain_name() + ".widening");

    // It is not necessarily true that the new value is bigger
    // than the old value so we apply
    // widen(old, new) = widen(old,(join(old,new)))
    LddNodePtr v = join(m_ldd, other.m_ldd);
    LddNodePtr w =
        lddPtr(get_ldd_man(), Ldd_BoxWiden2(get_ldd_man(), &*m_ldd, &*v));
    boxes_domain_t res(w);
    CRAB_LOG("boxes", crab::outs() << "Performed widening \n"
                                   << "**" << *this << "\n"
                                   << "** " << other << "\n"
                                   << "= " << res << "\n";);
    return res;
  }

  boxes_domain_t widening_thresholds(
      const boxes_domain_t &other,
      const iterators::thresholds<number_t> & /*ts*/) const override {
    // CRAB_WARN(" boxes widening operator with thresholds not implemented");
    return (*this || other);
  }

  boxes_domain_t operator&&(const boxes_domain_t &other) const override {
    crab::CrabStats::count(domain_name() + ".count.narrowing");
    crab::ScopedCrabStats __st__(domain_name() + ".narrowing");

    boxes_domain_t res(*this & other);
    // CRAB_WARN(" boxes narrowing operator replaced with meet");
    CRAB_LOG("boxes", crab::outs() << "Performed narrowing \n"
                                   << "**" << *this << "\n"
                                   << "** " << other << "\n"
                                   << "= " << res << "\n";);
    return res;
  }

  boxes_domain_t complement() const {
    crab::CrabStats::count(domain_name() + ".count.complement");
    crab::ScopedCrabStats __st__(domain_name() + ".complement");
    LddNodePtr w = lddPtr(get_ldd_man(), Ldd_Not(&*m_ldd));
    boxes_domain_t res(w);
    CRAB_LOG("boxes", boxes_domain_t tmp(*this); crab::outs()
                                                 << "Performed complement \n"
                                                 << "**" << tmp << "\n"
                                                 << "= " << res << "\n";);

    return res;
  }

  void operator-=(const variable_t &var) override {
    crab::CrabStats::count(domain_name() + ".count.forget");
    crab::ScopedCrabStats __st__(domain_name() + ".forget");

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

    int id = get_var_dim(var);
    m_ldd =
        lddPtr(get_ldd_man(), Ldd_ExistsAbstract(get_ldd_man(), &*m_ldd, id));
  }

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

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

    std::vector<int> qvars;
    qvars.reserve(variables.size());
    for (variable_t v : variables) {
      qvars.push_back(get_var_dim(v));
    }

    m_ldd = lddPtr(get_ldd_man(), Ldd_MvExistAbstract(get_ldd_man(), &*m_ldd,
                                                      &qvars[0], qvars.size()));
  }

  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 : s_var_map.left)
      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 &v, const variable_t &new_v) override {
    crab::CrabStats::count(domain_name() + ".count.expand");
    crab::ScopedCrabStats __st__(domain_name() + ".expand");

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

    // new_v should be completely unconstrained
    this->operator-=(new_v);

    linterm_t lnew_v = term_from_var(new_v);
    linterm_t lv = term_from_var(v);

    m_ldd = lddPtr(get_ldd_man(),
                   Ldd_TermCopy(get_ldd_man(), &(*m_ldd), lnew_v, lv));
    Ldd_GetTheory(get_ldd_man())->destroy_term(lnew_v);
    Ldd_GetTheory(get_ldd_man())->destroy_term(lv);
  }

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

    CRAB_LOG("boxes", crab::outs() << "--- assume(" << cst << ") --> ";);

    if (is_bottom() || cst.is_tautology()) {
      return;
    }

    if (cst.is_contradiction()) {
      set_to_bottom();
      return;
    }

    // XXX: we do nothing with unsigned linear inequalities
    // TODO: we can express these constraints with more disjunctions.
    if (cst.is_inequality() && cst.is_unsigned()) {
      CRAB_WARN("unsigned inequality skipped in boxes domain");
      return;
    }

    linear_expression_t exp = cst.expression();
    unsigned int size = exp.size();
    if (size == 1) {
      // Handle precisely unit constraints
      auto it = exp.begin();
      number_t cx = it->first;
      if (cx == number_t(1) || cx == number_t(-1)) {
        number_t k = -exp.constant();
        variable_t x = it->second;
        apply_unit_constraint(cx, x, cst.kind(), k);
      } else {
        CRAB_WARN("non-unit coefficients not implemented in boxes");
      }
    }
    // Lose precision with non-unit constraints
    else if (size >= 2) {
      if (cst.is_disequation()) {
        if (!add_linear_diseq(cst.expression())) {
          CRAB_LOG("boxes", crab::outs() << "_|_\n";);
          return;
        }
      } else if (cst.is_inequality()) {
        if (!add_linear_leq(cst.expression())) {
          CRAB_LOG("boxes", crab::outs() << "_|_\n";);
          return;
        }
      } else if (cst.is_strict_inequality()) {
        // We try to convert a strict to non-strict.
        auto nc =
            ikos::linear_constraint_impl::strict_to_non_strict_inequality(cst);
        if (nc.is_inequality()) {
          // here we succeed
          if (!add_linear_leq(nc.expression())) {
            CRAB_LOG("boxes", crab::outs() << "_|_\n";);
            return;
          }
        }
      } else if (cst.is_equality()) {
        linear_expression_t exp = cst.expression();
        // split equality into two inequalities
        if (!add_linear_leq(exp) || !add_linear_leq(-exp)) {
          CRAB_LOG("boxes", crab::outs() << "_|_\n";);
          return;
        }
      }
    }

    CRAB_LOG("boxes", crab::outs() << *this << "\n";);
  }

  void normalize() override {}

  void minimize() override {}

  /* 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 boxes_domain_t &invariant) override {
    CRAB_WARN("Intrinsics ", name, " not implemented by ", domain_name());
  }
  /* end intrinsics operations */

  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");

    assert(from.size() == to.size());

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

    CRAB_WARN(domain_name(), "::rename not implemented");
  }

  void operator+=(const linear_constraint_system_t &csts) override {
    if (!is_bottom()) {
      for (auto cst : csts) {
	operator+=(cst);
      }
    }
  }

  void set(const variable_t &v, interval_t ival) {
    crab::CrabStats::count(domain_name() + ".count.assign");
    crab::ScopedCrabStats __st__(domain_name() + ".assign");

    if (!is_bottom()) {
      m_ldd = apply_interval(v, ival);
    }
  }

  virtual interval_t operator[](const variable_t &v) override {
    crab::CrabStats::count(domain_name() + ".count.to_intervals");
    crab::ScopedCrabStats __st__(domain_name() + ".to_intervals");

    if (is_bottom())
      return interval_t::bottom();
    if (is_top())
      return interval_t::top();

    LddNodePtr ldd = project(v);
    interval_domain_t intv = to_intervals(ldd);
    interval_t i = intv[v];
    CRAB_LOG("boxes-project", crab::outs() << "Before projecting on " << v
                                           << ": " << *this << "\n"
                                           << "Projection " << i << "\n";);
    return i;
  }

  // x := e
  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()) {
      return;
    }

    if (e.is_constant()) {
      // Handle precisely if e is constant
      constant_t c = mk_cst(e.constant());
      linterm_t t = term_from_var(x);
      m_ldd = lddPtr(get_ldd_man(), Ldd_TermReplace(get_ldd_man(), &(*m_ldd), t,
                                                    NULL, NULL, c, c));
      get_theory()->destroy_cst(c);
      get_theory()->destroy_term(t);
    } else if (e.size() == 1) {
      // Handle precise if e is a*x + k
      auto it = e.begin();
      apply_ldd(x, it->second, it->first, e.constant());
    } else {
      crab::ScopedCrabStats __st__(domain_name() + ".assign.converting_to_intervals");
      // Lose precision in the general case
      // XXX: we can probably do a bit better here
      m_ldd = apply_interval(x, eval_interval(e));
    }

    CRAB_LOG("boxes", crab::outs() << "--- " << x << ":=" << e << "\n"
                                   << *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()) {
      return;
    }

    if (op >= OP_ADDITION && op <= OP_SUBTRACTION /*OP_MULTIPLICATION*/) {
      // FIXME: we skip multiplication because it seems imprecise.  It
      // produces imprecise lower bounds in unittests-boxes.
      switch (op) {
      case OP_ADDITION:
        apply_ldd(x, y, 1, k);
        break;
      case OP_SUBTRACTION:
        apply_ldd(x, y, 1, -k);
        break;
      case OP_MULTIPLICATION:
        apply_ldd(x, y, k, number_t(0));
        break;
      default:
        CRAB_ERROR("Unexpected operator ", op);
      }
      CRAB_LOG("boxes", crab::outs() << "--- " << x << " := " << y << " " << op
                                     << " " << k << "\n"
                                     << *this << "\n";);
    } else {
      // Convert to intervals
      interval_t yi(this->operator[](y));
      interval_t zi(k);
      interval_t xi(interval_t::bottom());
      switch (op) {
      case OP_MULTIPLICATION:
        xi = yi * zi;
        break;
      case OP_SDIV:
        xi = yi / zi;
        break;
      case OP_UDIV:
        xi = yi.UDiv(zi);
        break;
      case OP_SREM:
        xi = yi.SRem(zi);
        break;
      case OP_UREM:
        xi = yi.URem(zi);
        break;
      default:
        CRAB_ERROR("Unexpected operator ", op);
      }
      set(x, xi);
    }
  }

  // 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");

    if (is_bottom()) {
      return;
    }

    // --- if z is a singleton we do not lose precision
    interval_t zi = this->operator[](z);
    if (auto k = zi.singleton()) {
      apply(op, x, y, *k);
      return;
    }

    // --- if y or z is top then we give up
    interval_t yi = this->operator[](y);
    if (yi.is_top() || zi.is_top()) {
      this->operator-=(x);
      goto apply_end;
    }

    // --- if y is a singleton we do not lose precision
    if (auto k = yi.singleton()) {
      if (op == OP_ADDITION || op == OP_MULTIPLICATION) {
        apply(op, x, z, *k);
        return;
      } else if (op == OP_SUBTRACTION) {
        // x = yi - z <--> x = -z + [yi.lb,yi.ub]
        apply_ldd(x, z, -1, *k);
        goto apply_end;
      }
    }

    // XXX: ideally we would like to extract all the interval
    // constraints from x:= y op z, forget x and, then add all
    // the interval constraints. However, we would need to
    // project on y and z while keeping all the disjunctive
    // information about these variables. Note that the method
    // operator[] loses all the disjunctive information.

    // -- We need to abstract either y or z and lose some
    // precision. We abstract the one with the smaller interval.
    if (op == OP_ADDITION) {
      if (yi <= zi) { // abstract y
        // x = yi + z <--> x = z + yi
        apply_ldd(x, z, 1, yi);
      } else { // abstract z
        // x = y + zi
        apply_ldd(x, y, 1, zi);
      }
      goto apply_end;
    }

    // -- We need to abstract either y or z and lose some
    // precision. We abstract the one with the smaller interval.
    if (op == OP_SUBTRACTION) {
      if (yi <= zi) { // abstract y
        // x = yi - z <-->  x = -z + yi
        apply_ldd(x, z, -1, yi);
      } else { // abstract z
        // x = y - zi
        apply_ldd(x, y, 1, zi * number_t(-1));
      }
      goto apply_end;
    }

    // --- we go to intervals and lose precision
    switch (op) {
    case OP_MULTIPLICATION:
      set(x, yi * zi);
      break;
    case OP_SDIV:
      set(x, yi / zi);
      break;
    case OP_UDIV:
      set(x, yi.UDiv(zi));
      break;
    case OP_SREM:
      set(x, yi.SRem(zi));
      break;
    case OP_UREM:
      set(x, yi.URem(zi));
      break;
    default:
      CRAB_ERROR("Unexpected operator ", op);
    }

  apply_end:
    CRAB_LOG("boxes", crab::outs() << "--- " << x << " := " << y << " " << op
                                   << " " << z << "\n"
                                   << *this << "\n";);
  }

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

    CRAB_LOG("boxes", crab::outs() << "Backward " << x << ":=" << e
                                   << "\n\tPOST=" << *this << "\n");
    BackwardAssignOps<boxes_domain_t>::assign(*this, x, e, invariant);
    CRAB_LOG("boxes", crab::outs() << "\tPRE=" << *this << "\n");
  }

  void backward_apply(arith_operation_t op, const variable_t &x,
                      const variable_t &y, number_t z,
                      const boxes_domain_t &invariant) override {
    crab::CrabStats::count(domain_name() + ".count.backward_apply");
    crab::ScopedCrabStats __st__(domain_name() + ".backward_apply");

    CRAB_LOG("boxes", crab::outs() << "Backward " << x << ":=" << y << op << z
                                   << "\n\tPOST=" << *this << "\n");
    BackwardAssignOps<boxes_domain_t>::apply(*this, op, x, y, z, invariant);
    CRAB_LOG("boxes", crab::outs() << "\tPRE=" << *this << "\n");
  }

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

    CRAB_LOG("boxes", crab::outs() << "Backward " << x << ":=" << y << op << z
                                   << "\n\tPOST=" << *this << "\n");
    BackwardAssignOps<boxes_domain_t>::apply(*this, op, x, y, z, invariant);
    CRAB_LOG("boxes", crab::outs() << "\tPRE=" << *this << "\n");
  }

  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())
      return;

    // Convert to intervals and perform the operation
    interval_t yi = this->operator[](y);
    interval_t zi = this->operator[](z);
    interval_t xi = interval_t::bottom();
    switch (op) {
    case OP_AND:
      xi = yi.And(zi);
      break;
    case OP_OR:
      xi = yi.Or(zi);
      break;
    case OP_XOR:
      xi = yi.Xor(zi);
      break;
    case OP_SHL:
      xi = yi.Shl(zi);
      break;
    case OP_LSHR:
      xi = yi.LShr(zi);
      break;
    case OP_ASHR:
      xi = yi.AShr(zi);
      break;
    }
    set(x, xi);
  }

  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())
      return;

    // Convert to intervals and perform the operation
    interval_t yi = operator[](y);
    interval_t zi(k);
    interval_t xi = interval_t::bottom();
    switch (op) {
    case OP_AND:
      xi = yi.And(zi);
      break;
    case OP_OR:
      xi = yi.Or(zi);
      break;
    case OP_XOR:
      xi = yi.Xor(zi);
      break;
    case OP_SHL:
      xi = yi.Shl(zi);
      break;
    case OP_LSHR:
      xi = yi.LShr(zi);
      break;
    case OP_ASHR:
      xi = yi.AShr(zi);
      break;
    }
    set(x, xi);
  }

  ////////
  //// boolean operations
  ////////
  void assign_bool_cst(const variable_t &lhs,
                       const linear_constraint_t &cst) override {
    if (!m_bool_reasoning) {
      return;
    }

    crab::CrabStats::count(domain_name() + ".count.assign_bool_cst");
    crab::ScopedCrabStats __st__(domain_name() + ".assign_bool_cst");

    auto is_interval_cst = [](const linear_constraint_t &c) {
    	   return (c.expression().size() == 1 &&
    		   (c.expression().begin()->first == number_t(1) ||
    		    c.expression().begin()->first == number_t(-1))); };

    if (is_bottom()) {
      return;
    }

    // Important to forget first lhs
    this->operator-=(lhs);

    if (cst.is_tautology()) {
      this->operator+=(mk_true_cst(lhs));
    } else if (cst.is_contradiction()) {
      this->operator+=(mk_false_cst(lhs));
    } else {

      if (is_interval_cst(cst)) {
	crab::CrabStats::count(domain_name() + ".count.assign_bool_cst.interval");
	crab::ScopedCrabStats __st__(domain_name() + ".assign_bool_cst.interval");
      	/// Interval constraint
      	if (cst.is_inequality() && cst.is_unsigned()) {
      	  CRAB_WARN("unsigned inequality skipped in boxes domain");
      	  return;
     	}
      	linear_expression_t exp = cst.expression();
        number_t cx = exp.begin()->first;
      	variable_t vx = exp.begin()->second;
      	number_t k = -exp.constant();
      	// m_ldd &= ite (cst, lhs = TRUE, lhs = FALSE);
      	LddNodePtr ldd_cst = make_unit_constraint(cx, vx, cst.kind(), k);
      	LddNodePtr ldd_rhs = lddPtr(get_ldd_man(), Ldd_Ite(get_ldd_man(), &*(ldd_cst),
      							   &*mk_true(lhs), &*mk_false(lhs)));
      	m_ldd = lddPtr(get_ldd_man(), Ldd_And(get_ldd_man(), &*m_ldd, &*ldd_rhs));
      } else {
      	/// Non-interval constraint
	crab::CrabStats::count(domain_name() + ".count.assign_bool_cst.non-interval");
	crab::ScopedCrabStats __st__(domain_name() + ".assign_bool_cst.non-interval");
	// Note that we could project away irrelevant variables before
	// performing the join but this would lose precision.
        boxes_domain_t tt(*this);
        boxes_domain_t ff(*this);
        tt += cst;
        tt += mk_true_cst(lhs);
        ff += cst.negate();
        ff += mk_false_cst(lhs);
        if (::crab::CrabSanityCheckFlag) {
          if (tt.is_bottom() && ff.is_bottom()) {
            CRAB_ERROR("Something went wrong in assign_bool_cst ", lhs,
                       " := ", cst, " with ", *this);
          }
        }
        *this = (tt | ff);
      }
    }

    CRAB_LOG("boxes", crab::outs() << lhs << ":= "
                                   << "(" << cst << ")\n"
                                   << *this << "\n");
  }

  void assign_bool_ref_cst(const variable_t &lhs,
                           const reference_constraint_t &cst) override {
    if (is_bottom())
      return;

    this->operator-=(lhs);
    CRAB_WARN(
        "boxes boolean assignment of reference constraints not implemented");
  }

  void assign_bool_var(const variable_t &x, const variable_t &y,
                       bool is_not_y) override {
    if (!m_bool_reasoning)
      return;

    crab::CrabStats::count(domain_name() + ".count.assign_bool_var");
    crab::ScopedCrabStats __st__(domain_name() + ".assign_bool_var");

    if (is_bottom()) {
      return;
    }

    if (is_not_y) {
      // forget left-hand side
      operator-=(x);

      if (x == y) {
	// TODO
	CRAB_WARN("boxes skipped ", x, " not(", y, ")");
	return;
      }

      // Note that we could project away irrelevant variables before
      // performing the join but this would lose precision.
      boxes_domain_t inv1(*this);
      boxes_domain_t inv2(*this);
      inv1 += mk_true_cst(y);
      inv1 += mk_false_cst(x);
      inv2 += mk_false_cst(y);
      inv2 += mk_true_cst(x);
      *this = (inv1 | inv2);
    } else {
      apply_ldd(x, y, number_t(1), number_t(0));
    }

    CRAB_LOG("boxes", crab::outs() << x << ":=";
             if (is_not_y) crab::outs() << "not(" << y << ")";
             else crab::outs() << y; crab::outs() << "\n"
                                                  << *this << "\n");
  }

  void apply_binary_bool(bool_operation_t op, const variable_t &x,
                         const variable_t &y, const variable_t &z) override {

    if (!m_bool_reasoning)
      return;

    crab::CrabStats::count(domain_name() + ".count.apply_bin_bool");
    crab::ScopedCrabStats __st__(domain_name() + ".apply_bin_bool");

    if (is_bottom()) {
      return;
    }

    // XXX: if *lhs is null then it represents the SPECIAL
    // variable $0.
    boost::optional<variable_t> lhs;

    if (!(x == y) && !(x == z)) {
      lhs = boost::optional<variable_t>(x);
      // XXX: x does not appear on the rhs so we can remove it
      // without losing precision.
      this->operator-=(x);
    }

    auto add_assumptions = [&y,&z, this]() {
			     // the Ldd does not know that y and z can
			     // only be either 0 or 1.  The Ite term
			     // needs to negate the values of x and
			     // y. So this negation can add a
			     // disjunction to express != 0 or != 1.
			     this->operator+=(y >= 0);
			     this->operator+=(y <= 1);
			     this->operator+=(z >= 0);
			     this->operator+=(z <= 1);
			   };

    m_ldd = lddPtr(get_ldd_man(),
		   Ldd_And(get_ldd_man(), &*m_ldd,
			   &*gen_binary_bool(op, lhs, y, z)));
    add_assumptions();

    if ((x == y) || (x == z)) {
      // XXX: if we are here we added ite(y op z, $0 >= 1, $0 <= 0);
      // so we still need to assign $0 to x:
      copy_term(x, boost::optional<variable_t>());
    }

    CRAB_LOG("boxes", crab::outs()
                          << x << ":= " << y << " " << op << " " << z << "\n"
                          << *this << "\n");
  }

  void assume_bool(const variable_t &x, bool is_negated) override {
    if (!m_bool_reasoning)
      return;

    crab::CrabStats::count(domain_name() + ".count.assume_bool");
    crab::ScopedCrabStats __st__(domain_name() + ".assume_bool");

    if (is_bottom()) {
      return;
    }

    m_ldd = lddPtr(get_ldd_man(),
                   Ldd_And(get_ldd_man(), &*m_ldd,
                           ((is_negated) ? &*mk_false(x) : &*mk_true(x))));

    CRAB_LOG("boxes",
             if (!is_negated) {
               crab::outs() << "--- bool_assume(" << x << ")"
                            << "\n"
                            << *this << "\n";
             } else {
               crab::outs() << "--- bool_assume(not(" << x << "))"
                            << "\n"
                            << *this << "\n";
             });
  }

  // Backward boolean operations
  void backward_assign_bool_cst(const variable_t &lhs,
                                const linear_constraint_t &rhs,
                                const boxes_domain_t &inv) override {
    if (is_bottom()) {
      return;
    }

    this->operator-=(lhs);
    CRAB_WARN("boxes backward boolean assignment not implemented");
  }

  void backward_assign_bool_ref_cst(const variable_t &lhs,
                                    const reference_constraint_t &rhs,
                                    const boxes_domain_t &inv) override {
    if (is_bottom()) {
      return;
    }

    this->operator-=(lhs);
    CRAB_WARN("boxes backward boolean assignment of reference constraints not "
              "implemented");
  }

  void backward_assign_bool_var(const variable_t &lhs, const variable_t &rhs,
                                bool is_not_rhs,
                                const boxes_domain_t &inv) override {
    if (is_bottom()) {
      return;
    }

    this->operator-=(lhs);
    CRAB_WARN("boxes backward boolean assignment not implemented");
  }

  void backward_apply_binary_bool(bool_operation_t op, const variable_t &x,
                                  const variable_t &y, const variable_t &z,
                                  const boxes_domain_t &inv) override {
    if (is_bottom()) {
      return;
    }

    this->operator-=(x);
    CRAB_WARN("boxes backward boolean apply not implemented");
  }

  /// Boxes domain implements only standard abstract operations of a
  /// numerical domain plus boolean operations. It is intended to be
  /// used as a leaf domain in the hierarchy of domains.
  ARRAY_OPERATIONS_NOT_IMPLEMENTED(boxes_domain_t)
  REGION_AND_REFERENCE_OPERATIONS_NOT_IMPLEMENTED(boxes_domain_t)

  DEFAULT_SELECT(boxes_domain_t)
  DEFAULT_SELECT_BOOL(boxes_domain_t)

  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 csts;

    if (is_bottom()) {
      csts += linear_constraint_t::get_false();
    } else if (is_top()) {
      csts += linear_constraint_t::get_true();
    } else {
      // --- produce convex approximation
      LddNodePtr ldd(m_ldd);
      ldd = convex_approx(ldd);
      // --- extract linear inequalities from the convex ldd
      auto disjs = to_disjunctive_linear_constraint_system(ldd);
      if (disjs.is_false()) {
        csts += linear_constraint_t::get_false();
      } else if (disjs.is_true()) {
        csts += linear_constraint_t::get_true();
      } else {
        if (disjs.size() != 1) {
          CRAB_ERROR("Boxes::to_linear_constraint_system: it should not be "
                     "disjunctive ",
                     disjs);
        }
        for (auto c : *(disjs.begin())) {
          csts += c;
        }
      }
    }

    return csts;
  }

  disjunctive_linear_constraint_system_t
  to_disjunctive_linear_constraint_system(const LddNodePtr &ldd) const {
    LddManager *ldd_man = get_ldd_man();

    std::vector<int> list;
    list.reserve(ldd_man->cudd->size);
    for (int i = 0; i < ldd_man->cudd->size; i++)
      list.push_back(2);

    disjunctive_linear_constraint_system_t r;
    to_disjunctive_linear_constraint_system_aux(ldd_man, ldd.get(), r, list);
    return r;
  }

  disjunctive_linear_constraint_system_t
  to_disjunctive_linear_constraint_system() const override {
    return to_disjunctive_linear_constraint_system(m_ldd);
  }

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

    if (is_top()) {
      o << "{}";
    } else if (is_bottom()) {
      o << "_|_";
    } else {
#if 0
      auto r = to_linear_constraint_system();
#else
      auto r = to_disjunctive_linear_constraint_system();
#endif
      o << r;
    }
  }

  std::string domain_name() const override { return "Boxes"; }
};

template <typename N, typename V>
LddManager *boxes_domain<N, V>::s_ldd_man = nullptr;

template <typename N, typename V>
typename boxes_domain<N, V>::var_map_t boxes_domain<N, V>::s_var_map;

template <typename Number, typename VariableName>
class checker_domain_traits<boxes_domain<Number, VariableName>> {
public:
  using this_type = boxes_domain<Number, VariableName>;
  using linear_constraint_t = typename this_type::linear_constraint_t;
  using disjunctive_linear_constraint_system_t =
      typename this_type::disjunctive_linear_constraint_system_t;

  static bool entail(this_type &lhs,
                     const disjunctive_linear_constraint_system_t &rhs) {
    // -- trivial cases first
    if (rhs.is_false()) {
      return false;
    } else if (rhs.is_true()) {
      return true;
    } else if (lhs.is_bottom()) {
      return true;
    } else if (lhs.is_top()) {
      return false;
    }
    this_type inv;
    inv.set_to_bottom();
    for (auto const &csts : rhs) {
      this_type conj;
      conj += csts;
      inv |= conj;
    }
    return (lhs & inv.complement()).is_bottom();
  }

  static bool entail(const disjunctive_linear_constraint_system_t &lhs,
                     this_type &rhs) {
    // -- trivial cases first
    if (rhs.is_bottom()) {
      return false;
    } else if (rhs.is_top()) {
      return true;
    } else if (lhs.is_false()) {
      return true;
    } else if (lhs.is_true()) {
      return false;
    }
    this_type inv;
    inv.set_to_bottom();
    for (auto const &csts : lhs) {
      this_type conj;
      conj += csts;
      inv |= conj;
    }
    return (inv & rhs.complement()).is_bottom();
  }

  static bool entail(this_type &lhs, const linear_constraint_t &rhs) {
    // -- trivial cases first
    if (lhs.is_bottom())
      return true;
    if (rhs.is_tautology())
      return true;
    if (rhs.is_contradiction())
      return false;

    this_type inv(lhs);
    inv += rhs.negate();
    return inv.is_bottom();
  }

  static bool intersect(this_type &inv, const linear_constraint_t &cst) {
    // default code

    // -- trivial cases first
    if (inv.is_bottom() || cst.is_contradiction())
      return false;
    if (inv.is_top() || cst.is_tautology())
      return true;

    this_type cst_inv;
    cst_inv += cst;
    return (!(cst_inv & inv).is_bottom());
  }
};

template <typename Number, typename VariableName>
class special_domain_traits<boxes_domain<Number, VariableName>> {
public:
  static void clear_global_state(void) {
    boxes_domain<Number, VariableName>::clear_global_state();
  }
};

} // namespace domains
} // namespace crab
#endif /* HAVE_LDD */

namespace crab {
namespace domains {
template <typename Number, typename VariableName>
struct abstract_domain_traits<boxes_domain<Number, VariableName>> {
  using number_t = Number;
  using varname_t = VariableName;
};
} // namespace domains
} // namespace crab


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Ioan Sucan */

#include <tf/transform_listener.h>
#include <moveit/ompl_interface/ompl_interface.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/profiler/profiler.h>

#include <boost/math/constants/constants.hpp>
#include <sstream>

static const std::string ROBOT_DESCRIPTION = "robot_description";
static const double pi = boost::math::constants::pi<double>();
std::map<std::string,float> create_tolerances_map()
{
	std::map<std::string,float> m;
	m["low"] = 0.15;
	m["mid"] = 0.3;
	m["high"] = 0.6;
	return m;
}
std::map<std::string, float> tolerances = create_tolerances_map();

moveit_msgs::Constraints getConstraints(int states, std::string tolerance_id)
{
	//constraint variants:
	//s_model_tool0:upright:<states>:<tolerance>
	//where <states> is the number of states (e.g 1000, 5000, ...)
	//<tolerance> is the level of axis tolerance:
	//low    mid    high
	//0.15   0.3    0.6
  moveit_msgs::OrientationConstraint ocm;
  ocm.link_name = "s_model_tool0";
  ocm.header.frame_id = "table_top";
  ocm.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0);
  ocm.absolute_x_axis_tolerance = tolerances[tolerance_id];
  ocm.absolute_y_axis_tolerance = tolerances[tolerance_id];
  ocm.absolute_z_axis_tolerance = pi;
  ocm.weight = 1.0;
  moveit_msgs::Constraints cmsg;
  cmsg.orientation_constraints.resize(1, ocm);
	std::stringstream ss;
	ss << ocm.link_name << ":upright:" << states << ":" << tolerance_id;
  cmsg.name = ss.str();
  return cmsg;
}

void computeDB(const robot_model::RobotModelPtr& robot_model, unsigned int ns, unsigned int ne, std::string tolerance_id)
{
  planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(robot_model));
  ompl_interface::OMPLInterface ompl_interface(robot_model);
  moveit_msgs::Constraints c = getConstraints(ns, tolerance_id);
  ompl_interface::ConstraintApproximationConstructionOptions opt;
  opt.state_space_parameterization = "JointModel";
  opt.samples = ns;
  opt.edges_per_sample = ne;
  opt.explicit_motions = true; //true
  opt.max_edge_length = 2 * pi; //0.2
  opt.explicit_points_resolution = 0.05; //0.05
  opt.max_explicit_points = 200; //10

 /*
  * Needed to call update() on complete_initial_robot_state_ in constructor of ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext
  * file: moveit/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp
  */

  ompl_interface.getConstraintsLibrary().addConstraintApproximation(c, "arm", ps, opt);
  ompl_interface.getConstraintsLibrary().saveConstraintApproximations("cadb");
  ROS_INFO("Done");
}

/*
bool constructConstraintApproximation(moveit_msgs::ConstructConstraintApproximation::Request &req,
moveit_msgs::ConstructConstraintApproximation::Response &res)
{
  planning_scene::PlanningScenePtr diff_scene = psm_.getPlanningScene()->diff();
  robot_state::robotStateMsgToRobotState(*psm_.getPlanningScene()->getTransforms(), req.start_state,
diff_scene->getCurrentStateNonConst());
  ompl_interface::ConstraintApproximationConstructionResults ca_res =
    ompl_interface_.getConstraintsLibrary().addConstraintApproximation(req.constraint, req.group,
req.state_space_parameterization,
                                                                       diff_scene, req.samples, req.edges_per_sample);
  if (ca_res.approx)
  {
    res.sampling_success_rate = ca_res.sampling_success_rate;
    res.state_sampling_time = ca_res.state_sampling_time;
    res.state_connection_time = ca_res.state_connection_time;
    res.filename = ca_res.approx->getFilename();
    return ompl_interface_.saveConstraintApproximations();
  }
  else
    return false;
}
*/

int main(int argc, char** argv)
{
	ros::init(argc, argv, "construct_ompl_state_database", ros::init_options::AnonymousName);

	ros::AsyncSpinner spinner(1);
	spinner.start();

	unsigned int nstates = 1000;
	unsigned int nedges = 0;

	if (argc > 1)
		try
		{
			nstates = boost::lexical_cast<unsigned int>(argv[1]);
		}
	catch (...)
	{
	}

	if (argc > 2)
		try
		{
			nedges = boost::lexical_cast<unsigned int>(argv[2]);
		}
	catch (...)
	{
	}

	robot_model_loader::RobotModelLoader rml(ROBOT_DESCRIPTION);
	/*
	for (std::map<std::string,float>::iterator it=tolerances.begin(); it!=tolerances.end(); ++it){
		computeDB(rml.getModel(), 1000, nedges, it->first);
		computeDB(rml.getModel(), 5000, nedges, it->first);
		computeDB(rml.getModel(), 10000, nedges, it->first);
	}
	*/

	computeDB(rml.getModel(), 20000, nedges, "high");

	ros::shutdown();
	return 0;
}


#include <occupancy_grid_utils/ray_tracer.h>
#include <occupancy_grid_utils/exceptions.h>
#include <tf/transform_datatypes.h>
#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
#include <algorithm>
#include <ros/assert.h>

namespace occupancy_grid_utils
{

namespace nm=nav_msgs;
namespace gm=geometry_msgs;
namespace sm=sensor_msgs;

using boost::bind;
using boost::ref;
using boost::optional;

RayTraceIterRange rayTrace (const Cell& c1, const Cell& c2)
{
  return RayTraceIterRange(RayTraceIterator(c1,c2), RayTraceIterator(c2,c2,true));
}

inline
bool cellWithinBounds (const nm::MapMetaData& info, const Cell& c)
{
  return withinBounds(info, c);
}

optional<Cell> rayTraceOntoGrid (const nm::MapMetaData& info, const Cell& c1, const Cell& c2)
{
  RayTraceIterRange r = rayTrace(c2, c1);
  RayTraceIterator pos = std::find_if (r.first, r.second, bind(cellWithinBounds, boost::ref(info), _1));
  optional<Cell> c;
  if (pos!=r.second)
    c = *pos;
  return c;
}

RayTraceIterRange rayTrace (const nm::MapMetaData& info, const gm::Point& p1, const gm::Point& p2,
                            bool project_onto_grid, bool project_source_onto_grid,
                            float max_range)
{

  gm::Point np2 = p2;
  if (max_range > 0) {
    double distance = euclideanDistance(p1,p2);
    if (distance > max_range) {
      np2.x = ((p2.x - p1.x) * max_range/distance) + p1.x;
      np2.y = ((p2.y - p1.y) * max_range/distance) + p1.y;
    }
  }

  Cell c1 = pointCell(info, p1);
  Cell c2 = pointCell(info, np2);
  ROS_DEBUG_STREAM_NAMED ("ray_trace", "Ray tracing between " << c1.x <<
                          ", " << c1.y << " and " << c2.x << ", " << c2.y);
  const RayTraceIterator done(c1, c1, true);
  const RayTraceIterRange empty_range(done, done);
  if (!withinBounds(info, c1)) {
    if (project_source_onto_grid) {
      const optional<Cell> c = rayTraceOntoGrid(info, c2, c1);
      if (c)
        c1 = *c;
      else
        return empty_range;
    }
    else
      throw PointOutOfBoundsException(p1);
  }

  if (!withinBounds(info, np2)) {
    if (project_onto_grid) {
      const optional<Cell> c = rayTraceOntoGrid(info, c1, c2);
      if (c)
        c2 = *c;
      else
        return empty_range;
    }
    else {
      throw PointOutOfBoundsException(np2);
    }
  }

  ROS_DEBUG_STREAM_NAMED ("ray_trace", "Projected ray trace endpoints to " << c1.x <<
                          ", " << c1.y << " and " << c2.x << ", " << c2.y);
  return rayTrace(c1, c2);
}

gm::Point rayEndPoint (const gm::Point& p0, const double theta, const double d)
{
  gm::Point p;
  p.x = p0.x + cos(theta)*d;
  p.y = p0.y + sin(theta)*d;
  return p;
}

sm::LaserScan::Ptr
simulateRangeScan (const nm::OccupancyGrid& grid, const gm::Pose& sensor_pose,
                   const sm::LaserScan& scanner_info, const bool unknown_obstacles)
{
  sm::LaserScan::Ptr result(new sm::LaserScan(scanner_info));

  const double angle_range = scanner_info.angle_max - scanner_info.angle_min;
  const unsigned n =
    (unsigned) round(1+angle_range/scanner_info.angle_increment);
  const gm::Point& p0 = sensor_pose.position;
  const Cell c0 = pointCell(grid.info, p0);
  const double theta0 = tf::getYaw(sensor_pose.orientation);
  result->ranges.resize(n);

  for (unsigned i=0; i<n; i++)
  {
    const double theta = scanner_info.angle_min+i*scanner_info.angle_increment;
    const gm::Point scan_max =
      rayEndPoint(p0, theta0 + theta, scanner_info.range_max+1);

    result->ranges[i] = scanner_info.range_max+1; // Default if loop terminates
    BOOST_FOREACH (const Cell& c, rayTrace(grid.info, p0, scan_max, true))
    {
      const gm::Point p = cellCenter(grid.info, c);
      const double d = sqrt(pow(p.x-p0.x, 2) + pow(p.y-p0.y, 2));
      char data = grid.data[cellIndex(grid.info, c)];
      if (d > scanner_info.range_max)
        break;
      else if (data == OCCUPIED && !(c==c0))
      {
        result->ranges[i] = d;
        break;
      }
      else if (data == UNKNOWN && !(c==c0))
      {
        result->ranges[i] = unknown_obstacles ? d : scanner_info.range_max+1;
        break;
      }
    }
  }

  return result;
}

} // namespace occupancy_grid_utils


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <tudat/simulation/estimation.h>
#include "tudat/simulation/propagation_setup/propagationLambertTargeterFullProblem.h"
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>
#include <Eigen/Core>
#include "tudat/basics/testMacros.h"

#include "tudat/astro/ephemerides/approximatePlanetPositions.h"
#include "tudat/astro/trajectory_design/trajectory.h"
#include "tudat/astro/trajectory_design/exportTrajectory.h"
#include "tudat/astro/trajectory_design/planetTrajectory.h"

using namespace tudat;
using namespace tudat::propagators;

namespace tudat
{

namespace unit_tests
{

//! Function to setup a body map corresponding to the assumptions of the Lambert targeter,
//! using default ephemerides for the central body only, while the positions of departure and arrival bodies are provided as inputs.
/*!
 * Function to setup a Lambert targeter map. The body map contains the central, departure and arrival bodies and the body to be propagated.
 * The positions of the departure and arrival bodies are defined by the user and provided as inputs.
 * \param nameCentralBody Name of the central body.
 * \param nameBodyToPropagate Name of the body to be propagated.
 * \param departureAndArrivalBodies Vector containing the names of the departure and arrival bodies.
 * \param cartesianPositionAtDeparture Vector containing the position coordinates of the departure body [m].
 * \param cartesianPositionAtArrival Vector containing the position coordinates of the arrival body [m].
 * \return Body map for the Lambert targeter.
 */
simulation_setup::NamedBodyMap setupBodyMapFromUserDefinedStatesForLambertTargeter(
        const std::string& nameCentralBody,
        const std::string& nameBodyToPropagate,
        const std::pair< std::string, std::string >&  departureAndArrivalBodies,
        const Eigen::Vector3d& cartesianPositionAtDeparture,
        const Eigen::Vector3d& cartesianPositionAtArrival )
{
    spice_interface::loadStandardSpiceKernels( );

    std::string frameOrigin = "SSB";
    std::string frameOrientation = "ECLIPJ2000";

    // Create ephemeris vector for departure and arrival bodies.
    std::vector< ephemerides::EphemerisPointer > ephemerisVectorDepartureAndArrivalBodies(2);
    ephemerisVectorDepartureAndArrivalBodies[ 0 ] = std::make_shared< ephemerides::ConstantEphemeris > (
                ( Eigen::Vector6d( ) << cartesianPositionAtDeparture,
                  0.0, 0.0, 0.0 ).finished( ), frameOrigin, frameOrientation );
    ephemerisVectorDepartureAndArrivalBodies[ 1 ] = std::make_shared< ephemerides::ConstantEphemeris >(
                ( Eigen::Vector6d( ) << cartesianPositionAtArrival,
                  0.0, 0.0, 0.0 ).finished( ), frameOrigin, frameOrientation );

    // Create body map.
    simulation_setup::NamedBodyMap bodyMap =
            propagators::setupBodyMapFromUserDefinedEphemeridesForLambertTargeter(
                nameCentralBody, nameBodyToPropagate, departureAndArrivalBodies, ephemerisVectorDepartureAndArrivalBodies );

    return bodyMap;

}

BOOST_AUTO_TEST_SUITE( testFullPropagationLambertTargeter )

//! Test if the difference between the Lambert targeter solution and the full dynamics problem is computed correctly.
BOOST_AUTO_TEST_CASE( testFullPropagationLambertTargeterBasic )
{

    std::cout.precision(20);

    double initialTime = 0.0;

    Eigen::Vector3d cartesianPositionAtDeparture ( 2.0 * 6.378136e6, 0.0, 0.0 );
    Eigen::Vector3d cartesianPositionAtArrival ( 2.0 * 6.378136e6, 2.0 * std::sqrt( 3.0 ) * 6.378136e6, 0.0 );

    double timeOfFlight = 806.78 * 5.0;
    double fixedStepSize = timeOfFlight / 10000.0;

    std::string bodyToPropagate = "spacecraft" ;
    std::string centralBody = "Earth";

    // Define integrator settings.
    std::shared_ptr< numerical_integrators::IntegratorSettings< double > > integratorSettings =
            std::make_shared < numerical_integrators::IntegratorSettings < > >
            ( numerical_integrators::rungeKutta4, initialTime, fixedStepSize);

    std::pair< std::string, std::string > departureAndArrivalBodies =
            std::make_pair( "departure", "arrival" );

<<<<<<< HEAD
    // Define the system of bodies.
    simulation_setup::SystemOfBodies bodies = propagators::setupBodyMapFromUserDefinedStatesForLambertTargeter("Earth", "spacecraft", departureAndArrivalBodies,
                                                                                cartesianPositionAtDeparture, cartesianPositionAtArrival);

    basic_astrodynamics::AccelerationMap accelerationModelMap = propagators::setupAccelerationMapLambertTargeter(
                "Earth", "spacecraft", bodies);

   // Compute the difference in state between the full problem and the Lambert targeter solution at departure and at arrival
   std::pair< Eigen::Vector6d, Eigen::Vector6d > differenceState =
            propagators::getDifferenceFullPropagationWrtLambertTargeterAtDepartureAndArrival(cartesianPositionAtDeparture,
             cartesianPositionAtArrival, timeOfFlight, initialTime, bodies, accelerationModelMap, bodyToPropagate,
             centralBody, integratorSettings, departureAndArrivalBodies, false);
=======
    // Define the body map.
    simulation_setup::NamedBodyMap bodyMap = setupBodyMapFromUserDefinedStatesForLambertTargeter(
                "Earth", "spacecraft", departureAndArrivalBodies, cartesianPositionAtDeparture, cartesianPositionAtArrival );

    basic_astrodynamics::AccelerationMap accelerationModelMap = propagators::setupAccelerationMapLambertTargeter(
                "Earth", "spacecraft", bodyMap );
>>>>>>> dominic-origin/features/mission_segments_refactor

    std::map< double, Eigen::Vector6d > lambertTargeterResult;
    std::map< double, Eigen::Vector6d > fullProblemResult;
    std::map< double, Eigen::VectorXd > dependentVariableResult;
    propagateLambertTargeterAndFullProblem(
                timeOfFlight, initialTime, bodyMap, accelerationModelMap, bodyToPropagate,
                centralBody, departureAndArrivalBodies, integratorSettings, lambertTargeterResult, fullProblemResult,
                dependentVariableResult, false );

    for( auto stateIterator : lambertTargeterResult )
    {
        for( int i = 0; i < 3; i++ )
        {
            BOOST_CHECK_SMALL( std::fabs( lambertTargeterResult.at( stateIterator.first )( i ) -
                                          fullProblemResult.at( stateIterator.first )( i ) ), 1.0E-5 );
            BOOST_CHECK_SMALL( std::fabs( lambertTargeterResult.at( stateIterator.first )( i + 3 ) -
                                          fullProblemResult.at( stateIterator.first )( i + 3 ) ), 1.0E-9 );
        }
    }
}

}

}


#ifndef STAN_MATH_PRIM_SCAL_PROB_BINOMIAL_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_BINOMIAL_RNG_HPP

#include <boost/random/binomial_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_greater_or_equal.hpp>
#include <stan/math/prim/scal/err/check_less_or_equal.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/inv_logit.hpp>
#include <stan/math/prim/scal/fun/log1m.hpp>
#include <stan/math/prim/scal/fun/log_inv_logit.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/binomial_coefficient_log.hpp>
#include <stan/math/prim/scal/fun/lbeta.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/fun/inc_beta.hpp>

namespace stan {
  namespace math {

    /**
     * Return a pseudorandom Binomial random variable for the given population
     * size and chance of success parameters using the specified random number
     * generator.
     *
     * @tparam RNG class of rng
     * @param N population size parameter
     * @param theta chance of success parameter
     * @param rng random number generator
     * @return Binomial random variate
     * @throw std::domain_error if N is negative
     * @throw std::domain_error if theta is not a valid probability
     */
    template <class RNG>
    inline int
    binomial_rng(int N,
                 double theta,
                 RNG& rng) {
      using boost::variate_generator;
      using boost::binomial_distribution;

      static const char* function("binomial_rng");

      check_nonnegative(function, "Population size parameter", N);
      check_finite(function, "Probability parameter", theta);
      check_less_or_equal(function, "Probability parameter", theta, 1.0);
      check_greater_or_equal(function, "Probability parameter", theta, 0.0);

      variate_generator<RNG&, binomial_distribution<> >
        binomial_rng(rng, binomial_distribution<>(N, theta));
      return binomial_rng();
    }

  }
}
#endif


#include "safety_module/SafetyModuleWrapper.hpp"

#include <std_msgs/Empty.h>

#include <Eigen/Geometry>
#include <vector>

namespace safety_module
{

SafetyModuleWrapper::SafetyModuleWrapper()
    : _safetyModule(0.18, 0.01, 0.0, 20.0), _nh("~"), _laserSub(_nh, "/base_scan", 1),
      _cmdSub(_nh, "/deep_planner/cmd_vel_stamped", 1), _sync(_laserSub, _cmdSub, 10)
{
  _safetyInterruptPub = _nh.advertise<std_msgs::Empty>("/safety_interrupt", 1);
  _cmdPub             = _nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
  _sync.registerCallback(&SafetyModuleWrapper::syncCallback, this);
}

void SafetyModuleWrapper::syncCallback(const sensor_msgs::LaserScan::ConstPtr& laser,
                                       const geometry_msgs::TwistStamped::ConstPtr& cmd)
{
  // Get the transformation between robot base and laser base
  tf::StampedTransform transform;
  _tfListener.lookupTransform("/base_link", "/base_laser_link", ros::Time(0), transform);
  Eigen::Vector2d currentPosition(transform.getOrigin().getX(), transform.getOrigin().getY());
  Eigen::Quaterniond currentOrientation(
      transform.getRotation().getW(), transform.getRotation().getX(),
      transform.getRotation().getY(), transform.getRotation().getZ());

  // Convert the laser measurements to 2D points in the laser frame
  std::vector<Eigen::Vector2d> pointsList;
  for (size_t i = 0; i < laser->ranges.size(); ++i) {
    double alpha = laser->angle_min + static_cast<double>(i) * laser->angle_increment;
    Eigen::Vector2d laserMeas(cos(alpha) * laser->ranges.at(i), sin(alpha) * laser->ranges.at(i));
    pointsList.push_back(laserMeas);
  }

  if (_safetyModule.motionIsSafe(currentPosition, currentOrientation, cmd->twist.linear.x,
                                 cmd->twist.angular.z, pointsList)) {
    geometry_msgs::Twist output;
    output = cmd->twist;
    _cmdPub.publish(output);
  } else {
    std_msgs::Empty msg;
    _safetyInterruptPub.publish(msg);

    // Publish a message with zeros
    geometry_msgs::Twist output;
    _cmdPub.publish(output);
  }
  // TODO Handle reset more carefully
  _safetyModule.reset();
}

} /* safety_module */


/*
 * Copyright (c) 2015-2016, Luca Fulchir<luca@fulchir.it>, All rights reserved.
 *
 * This file is part of "libRaptorQ".
 *
 * libRaptorQ is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as
 * published by the Free Software Foundation, either version 3
 * of the License, or (at your option) any later version.
 *
 * libRaptorQ is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * and a copy of the GNU Lesser General Public License
 * along with libRaptorQ.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once

#include "libRaptorQ/common.hpp"
#include "libRaptorQ/Parameters.hpp"
#include "libRaptorQ/Octet.hpp"
#include <Eigen/Dense>

namespace RaptorQ__v1 {
namespace Impl {

using DenseMtx = Eigen::Matrix<Octet, Eigen::Dynamic, Eigen::Dynamic,
                                                            Eigen::RowMajor>;

class RAPTORQ_LOCAL Operation
{
public:
    enum class _t : uint8_t {
        NONE = 0x00,
        SWAP = 0x01,
        ADD_MUL = 0x02,
        DIV = 0x03,
        BLOCK = 0x04,
        REORDER = 0x05
    };
    Operation() = delete;
    Operation (const Operation &rhs)
        :_type (rhs._type)
    {
        switch (rhs._type)
        {
        case _t::SWAP:
            swap = rhs.swap;
            break;
        case _t::ADD_MUL:
            add_mul = rhs.add_mul;
            break;
        case _t::DIV:
            div = rhs.div;
            break;
        case _t::BLOCK:
            block = rhs.block;
            break;
        case _t::REORDER:
            reorder = rhs.reorder;
            break;
        case _t::NONE:
            break;
        }
    }
    Operation& operator= (const Operation &rhs)
    {
        assert (_type == rhs._type && "Operation types do not correspond.");
        switch (rhs._type)
        {
        case _t::SWAP:
            swap = rhs.swap;
            break;
        case _t::ADD_MUL:
            add_mul = rhs.add_mul;
            break;
        case _t::DIV:
            div = rhs.div;
            break;
        case _t::BLOCK:
            block = rhs.block;
            break;
        case _t::REORDER:
            reorder = rhs.reorder;
            break;
        case _t::NONE:
            break;
        }
        return *this;
    }

    Operation (Operation &&rhs)
        :_type (rhs._type)
    {
        switch (rhs._type)
        {
        case _t::SWAP:
            swap = std::move (rhs.swap);
            break;
        case _t::ADD_MUL:
            add_mul = std::move (rhs.add_mul);
            break;
        case _t::DIV:
            div = std::move (rhs.div);
            break;
        case _t::BLOCK:
            block = std::move (rhs.block);
            break;
        case _t::REORDER:
            reorder = std::move (rhs.reorder);
            break;
        case _t::NONE:
            break;
        }
    }

    Operation& operator= (Operation &&rhs)
    {
        assert (_type == rhs._type && "Operation types do not correspond.");
        switch (rhs._type)
        {
        case _t::SWAP:
            swap = std::move (rhs.swap);
            break;
        case _t::ADD_MUL:
            add_mul = std::move (rhs.add_mul);
            break;
        case _t::DIV:
            div = std::move (rhs.div);
            break;
        case _t::BLOCK:
            block = std::move (rhs.block);
            break;
        case _t::REORDER:
            reorder = std::move (rhs.reorder);
            break;
        case _t::NONE:
            break;
        }
        return *this;
    }

    Operation (const _t type, const uint16_t row_1, const uint16_t row_2)
        : _type (type), swap (row_1, row_2) { assert (type == _t::SWAP); }
    Operation (const _t type, const uint16_t row_1, const uint16_t row_2,
                                                            const Octet scalar)
        : _type (type), add_mul (row_1, row_2, scalar)
                                            { assert (type == _t::ADD_MUL); }
    Operation (const _t type, const uint16_t row, const Octet scalar)
        : _type (type), div (row, scalar) { assert (type == _t::DIV); }
    Operation (const _t type, const DenseMtx &mtx)
        : _type (type), block (mtx) { assert (type == _t::BLOCK); }
    Operation (const _t type, const std::vector<uint16_t> &order)
        : _type (type), reorder (order) { assert (type == _t::REORDER); }

    ~Operation ()
    {
        if (_type == _t::BLOCK)
            block.clear();
        if (_type == _t::REORDER)
            reorder.clear();
    }

    void build_mtx (DenseMtx &mtx) const
    {
        switch (_type)
        {
        case _t::SWAP:
            return swap.build_mtx (mtx);
        case _t::ADD_MUL:
            return add_mul.build_mtx (mtx);
        case _t::DIV:
            return div.build_mtx (mtx);
        case _t::BLOCK:
            return block.build_mtx (mtx);
        case _t::REORDER:
            return reorder.build_mtx (mtx);
        case _t::NONE:
            break;
        }
    }
private:
    Operation (const _t type)
        :_type (type) {}
    class RAPTORQ_LOCAL Swap
    {
    public:
        Swap (const uint16_t row_1, const uint16_t row_2)
            : _row_1 (row_1), _row_2 (row_2) {}
        Swap (const Swap&) = default;
        Swap& operator= (const Swap&) = default;
        Swap (Swap &&) = default;
        Swap& operator= (Swap &&) = default;
        ~Swap() {}
        void build_mtx (DenseMtx &mtx) const
            { mtx.row(_row_1).swap (mtx.row(_row_2)); }
    private:
        uint16_t _row_1, _row_2;
    };

    class RAPTORQ_LOCAL Add_Mul
    {
    public:
        Add_Mul (const uint16_t row_1, const uint16_t row_2, const Octet scalar)
            : _row_1 (row_1), _row_2 (row_2), _scalar (scalar) {}
        Add_Mul (const Add_Mul&) = default;
        Add_Mul& operator= (const Add_Mul&) = default;
        Add_Mul (Add_Mul&&) = default;
        Add_Mul& operator= (Add_Mul&&) = default;
        ~Add_Mul() {}
        void build_mtx (DenseMtx &mtx) const
        {
            const auto row = mtx.row (_row_2) * _scalar;
            mtx.row (_row_1) += row;
        }
    private:
        uint16_t _row_1, _row_2;
        Octet _scalar;
    };

    class RAPTORQ_LOCAL Div
    {
    public:
        Div (const uint16_t row_1, const Octet scalar)
            : _row_1 (row_1), _scalar (scalar) {}
        Div (const Div&) = default;
        Div& operator= (const Div&) = default;
        Div (Div&&) = default;
        Div& operator= (Div&&) = default;
        ~Div() {}
        void build_mtx (DenseMtx &mtx) const
            { mtx.row (_row_1) /= _scalar; }
    private:
        uint16_t _row_1;
        Octet _scalar;
    };

    class RAPTORQ_LOCAL Block
    {
    public:
        Block (const DenseMtx &block)
            : _block (block) {}
        Block (const Block&) = default;
        Block& operator= (const Block&) = default;
        Block (Block&&) = default;
        Block& operator= (Block&&) = default;
        ~Block() {}
        void build_mtx (DenseMtx &mtx) const
        {
            const auto orig = mtx.block (0,0, _block.cols(), mtx.cols());
            mtx.block (0, 0, _block.cols(), mtx.cols()) = _block * orig;
        }
        void clear()
            { _block = DenseMtx(); }
    private:
        DenseMtx _block;
    };

    class RAPTORQ_LOCAL Reorder
    {
    public:
        Reorder (const std::vector<uint16_t> &order)
            : _order (order) {}
        Reorder (const Reorder&) = default;
        Reorder& operator= (const Reorder&) = default;
        Reorder (Reorder&&) = default;
        Reorder& operator= (Reorder&&) = default;
        ~Reorder() {}
        void build_mtx (DenseMtx &mtx) const
        {
            uint16_t overhead = static_cast<uint16_t> (
                                static_cast<uint16_t> (mtx.rows()) - _order.size());
            DenseMtx ret = DenseMtx (mtx.rows() - overhead , mtx.cols());

            // reorder some of the lines as requested by the _order vector
            uint16_t row = 0;
            for (const uint16_t pos : _order)
                ret.row (pos) = mtx.row (row++);
            mtx.swap (ret);
            // other lines will not influence the computation, ignore them
        }
        void clear()
            { _order = std::vector<uint16_t>(); }
    private:
        std::vector<uint16_t> _order;
    };

    const _t _type;
    union {
        Swap swap;
        Add_Mul add_mul;
        Div div;
        Block block;
        Reorder reorder;
    };
};

}   // namespace Impl
}   // namespace RaptorQ