/*******************************************************************************
 *
 * Data structures for the symbolic manipulation of linear constraints.
 *
 * Author: Arnaud J. Venet (arnaud.j.venet@nasa.gov)
 * Contributor: Jorge A. Navas (jorge.navas@sri.com)
 *
 * Notices:
 *
 * Copyright (c) 2011 United States Government as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All Rights Reserved.
 *
 * Disclaimers:
 *
 * No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF
 * ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS,
 * ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE,
 * OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE
 * ERROR FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO
 * THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
 * ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS,
 * RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS
 * RESULTING FROM USE OF THE SUBJECT SOFTWARE.  FURTHER, GOVERNMENT AGENCY
 * DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING THIRD-PARTY SOFTWARE,
 * IF PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."
 *
 * Waiver and Indemnity:  RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST
 * THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL
 * AS ANY PRIOR RECIPIENT.  IF RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS
 * IN ANY LIABILITIES, DEMANDS, DAMAGES, EXPENSES OR LOSSES ARISING FROM SUCH
 * USE, INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING FROM,
 * RECIPIENT'S USE OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD
 * HARMLESS THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS,
 * AS WELL AS ANY PRIOR RECIPIENT, TO THE EXTENT PERMITTED BY LAW.
 * RECIPIENT'S SOLE REMEDY FOR ANY SUCH MATTER SHALL BE THE IMMEDIATE,
 * UNILATERAL TERMINATION OF THIS AGREEMENT.
 *
 ******************************************************************************/

#pragma once

#include <crab/common/types.hpp>
#include <crab/numbers/bignums.hpp>
#include <crab/domains/patricia_trees.hpp>

#include <boost/container/flat_map.hpp>
#include <boost/functional/hash_fwd.hpp> // for hash_combine
#include <boost/iterator/transform_iterator.hpp>
#include <boost/optional.hpp>

#include <functional>
#include <memory>
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include <ostream>

namespace ikos {

template <typename Number, typename VariableName> class linear_expression {

public:
  typedef Number number_t;
  typedef VariableName varname_t;
  typedef variable<Number, VariableName> variable_t;
  typedef linear_expression<Number, VariableName> linear_expression_t;
  typedef std::pair<Number, variable_t> component_t;
  typedef patricia_tree_set<variable_t> variable_set_t;

private:
  typedef boost::container::flat_map<variable_t, Number> map_t;
  typedef std::shared_ptr<map_t> map_ptr;
  typedef typename map_t::value_type pair_t;

  map_ptr _map;
  Number _cst;

  linear_expression(map_ptr map, Number cst) : _map(map), _cst(cst) {}

  linear_expression(const map_t &map, Number cst)
      : _map(std::make_shared<map_t>()), _cst(cst) {
    *this->_map = map;
  }

  void add(variable_t x, Number n) {
    typename map_t::iterator it = this->_map->find(x);
    if (it != this->_map->end()) {
      Number r = it->second + n;
      if (r == 0) {
        this->_map->erase(it);
      } else {
        it->second = r;
      }
    } else {
      if (n != 0) {
        this->_map->insert(pair_t(x, n));
      }
    }
  }

  struct tr_value_ty
      : public std::unary_function<typename map_t::value_type, component_t> {
    tr_value_ty() {}
    component_t operator()(const typename map_t::value_type &kv) const {
      return {kv.second, kv.first};
    }
  };

public:
  typedef boost::transform_iterator<tr_value_ty, typename map_t::iterator>
      iterator;
  typedef boost::transform_iterator<tr_value_ty, typename map_t::const_iterator>
      const_iterator;

  linear_expression() : _map(std::make_shared<map_t>()), _cst(0) {}

  linear_expression(Number n) : _map(std::make_shared<map_t>()), _cst(n) {}

  linear_expression(int64_t n)
      : _map(std::make_shared<map_t>()), _cst(Number(n)) {}

  linear_expression(variable_t x) : _map(std::make_shared<map_t>()), _cst(0) {
    this->_map->insert(pair_t(x, Number(1)));
  }

  linear_expression(Number n, variable_t x)
      : _map(std::make_shared<map_t>()), _cst(0) {
    this->_map->insert(pair_t(x, n));
  }

  linear_expression(const linear_expression_t &e) = default;

  linear_expression(linear_expression_t &&e) = default;

  linear_expression_t &operator=(const linear_expression_t &e) = default;

  linear_expression_t &operator=(linear_expression_t &&e) = default;

  const_iterator begin() const {
    return boost::make_transform_iterator(_map->begin(), tr_value_ty());
  }

  const_iterator end() const {
    return boost::make_transform_iterator(_map->end(), tr_value_ty());
  }

  iterator begin() {
    return boost::make_transform_iterator(_map->begin(), tr_value_ty());
  }

  iterator end() {
    return boost::make_transform_iterator(_map->end(), tr_value_ty());
  }

  size_t hash() const {
    size_t res = 0;
    for (const_iterator it = begin(), et = end(); it != et; ++it) {
      boost::hash_combine(res, std::make_pair((*it).second, (*it).first));
    }
    boost::hash_combine(res, _cst);
    return res;
  }

  // syntactic equality
  bool equal(const linear_expression_t &o) const {
    if (is_constant()) {
      if (!o.is_constant()) {
        return false;
      } else {
        return (constant() == o.constant());
      }
    } else {
      if (constant() != o.constant()) {
        return false;
      }

      if (size() != o.size()) {
        return false;
      } else {
        for (const_iterator it = begin(), jt = o.begin(), et = end(); it != et;
             ++it, ++jt) {
          if (((*it).first != (*jt).first) || ((*it).second != (*jt).second)) {
            return false;
          }
        }
        return true;
      }
    }
  }

  bool is_constant() const { return (this->_map->size() == 0); }

  Number constant() const { return this->_cst; }

  std::size_t size() const { return this->_map->size(); }

  Number operator[](variable_t x) const {
    typename map_t::const_iterator it = this->_map->find(x);
    if (it != this->_map->end()) {
      return it->second;
    } else {
      return 0;
    }
  }

  template <typename RenamingMap>
  linear_expression_t rename(const RenamingMap &map) const {
    Number cst(this->_cst);
    linear_expression_t new_exp(cst);
    for (auto v : this->variables()) {
      auto const it = map.find(v);
      if (it != map.end()) {
        variable_t v_out((*it).second);
        new_exp = new_exp + this->operator[](v) * v_out;
      } else {
        new_exp = new_exp + this->operator[](v) * v;
      }
    }
    return new_exp;
  }

  linear_expression_t operator+(Number n) const {
    linear_expression_t r(this->_map, this->_cst + n);
    return r;
  }

  linear_expression_t operator+(int64_t n) const {
    return this->operator+(Number(n));
  }

  linear_expression_t operator+(variable_t x) const {
    linear_expression_t r(*this->_map, this->_cst);
    r.add(x, Number(1));
    return r;
  }

  linear_expression_t operator+(const linear_expression_t &e) const {
    linear_expression_t r(*this->_map, this->_cst + e._cst);
    for (typename map_t::const_iterator it = e._map->begin();
         it != e._map->end(); ++it) {
      r.add(it->first, it->second);
    }
    return r;
  }

  linear_expression_t operator-(Number n) const { return this->operator+(-n); }

  linear_expression_t operator-(int64_t n) const {
    return this->operator+(-Number(n));
  }

  linear_expression_t operator-(variable_t x) const {
    linear_expression_t r(*this->_map, this->_cst);
    r.add(x, Number(-1));
    return r;
  }

  linear_expression_t operator-() const { return this->operator*(Number(-1)); }

  linear_expression_t operator-(const linear_expression_t &e) const {
    linear_expression_t r(*this->_map, this->_cst - e._cst);
    for (typename map_t::const_iterator it = e._map->begin();
         it != e._map->end(); ++it) {
      r.add(it->first, -it->second);
    }
    return r;
  }

  linear_expression_t operator*(Number n) const {
    if (n == 0) {
      return linear_expression_t();
    } else {
      map_ptr map = std::make_shared<map_t>();
      for (typename map_t::const_iterator it = this->_map->begin();
           it != this->_map->end(); ++it) {
        Number c = n * it->second;
        if (c != 0) {
          map->insert(pair_t(it->first, c));
        }
      }
      return linear_expression_t(map, n * this->_cst);
    }
  }

  linear_expression_t operator*(int64_t n) const {
    return operator*(Number(n));
  }

  variable_set_t variables() const {
    variable_set_t variables;
    for (const_iterator it = this->begin(); it != this->end(); ++it) {
      variables += it->second;
    }
    return variables;
  }

  bool is_well_typed() const {
    typename variable_t::bitwidth_t b;
    crab::variable_type type;
    for (const_iterator it = begin(), et = end(); it != et; ++it) {
      variable_t v = it->second;
      if (it == begin()) {
        b = v.get_bitwidth();
        type = v.get_type();
      } else {
        if (v.get_bitwidth() != b || v.get_type() != type) {
          return false;
        }
      }
    }
    return true;
  }

  boost::optional<variable_t> get_variable() const {
    if (this->is_constant())
      return boost::optional<variable_t>();
    else {
      if ((this->constant() == 0) && (this->size() == 1)) {
        const_iterator it = this->begin();
        Number coeff = it->first;
        if (coeff == 1)
          return boost::optional<variable_t>(it->second);
      }
      return boost::optional<variable_t>();
    }
  }

  void write(crab::crab_os &o) const {
    for (typename map_t::const_iterator it = this->_map->begin();
         it != this->_map->end(); ++it) {
      Number n = it->second;
      variable_t v = it->first;
      if (n > 0 && it != this->_map->begin()) {
        o << "+";
      }
      if (n == -1) {
        o << "-";
      } else if (n != 1) {
        o << n << "*";
      }
      o << v;
    }
    if (this->_cst > 0 && this->_map->size() > 0) {
      o << "+";
    }
    if (this->_cst != 0 || this->_map->size() == 0) {
      o << this->_cst;
    }
  }

  // for dgb
  void dump() { write(crab::outs()); }

}; // class linear_expression

template <typename Number, typename VariableName>
inline crab::crab_os &
operator<<(crab::crab_os &o, const linear_expression<Number, VariableName> &e) {
  e.write(o);
  return o;
}

/* used by boost::hash_combine */
template <typename Number, typename VariableName>
inline std::size_t
hash_value(const linear_expression<Number, VariableName> &e) {
  return e.hash();
}

template <typename Number, typename VariableName>
struct linear_expression_hasher {
  size_t operator()(const linear_expression<Number, VariableName> &e) const {
    return e.hash();
  }
};

template <typename Number, typename VariableName>
struct linear_expression_equal {
  bool operator()(const linear_expression<Number, VariableName> &e1,
                  const linear_expression<Number, VariableName> &e2) const {
    return e1.equal(e2);
  }
};

template <typename Number, typename VariableName>
using linear_expression_unordered_set =
    std::unordered_set<linear_expression<Number, VariableName>,
                       linear_expression_hasher<Number, VariableName>,
                       linear_expression_equal<Number, VariableName>>;

template <typename Number, typename VariableName, typename Value>
using linear_expression_unordered_map =
    std::unordered_map<linear_expression<Number, VariableName>, Value,
                       linear_expression_hasher<Number, VariableName>,
                       linear_expression_equal<Number, VariableName>>;

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator*(Number n, variable<Number, VariableName> x) {
  return linear_expression<Number, VariableName>(n, x);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator*(int64_t n, variable<Number, VariableName> x) {
  return linear_expression<Number, VariableName>(Number(n), x);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator*(variable<Number, VariableName> x, Number n) {
  return linear_expression<Number, VariableName>(n, x);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator*(variable<Number, VariableName> x, int64_t n) {
  return linear_expression<Number, VariableName>(Number(n), x);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator*(Number n, const linear_expression<Number, VariableName> &e) {
  return e.operator*(n);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator*(int64_t n, const linear_expression<Number, VariableName> &e) {
  return e.operator*(n);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator+(variable<Number, VariableName> x, Number n) {
  return linear_expression<Number, VariableName>(x).operator+(n);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator+(variable<Number, VariableName> x, int64_t n) {
  return linear_expression<Number, VariableName>(x).operator+(n);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator+(Number n, variable<Number, VariableName> x) {
  return linear_expression<Number, VariableName>(x).operator+(n);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator+(int64_t n, variable<Number, VariableName> x) {
  return linear_expression<Number, VariableName>(x).operator+(n);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator+(variable<Number, VariableName> x, variable<Number, VariableName> y) {
  return linear_expression<Number, VariableName>(x).operator+(y);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator+(Number n, const linear_expression<Number, VariableName> &e) {
  return e.operator+(n);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator+(int64_t n, const linear_expression<Number, VariableName> &e) {
  return e.operator+(n);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator+(variable<Number, VariableName> x,
          const linear_expression<Number, VariableName> &e) {
  return e.operator+(x);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator-(variable<Number, VariableName> x, Number n) {
  return linear_expression<Number, VariableName>(x).operator-(n);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator-(variable<Number, VariableName> x, int64_t n) {
  return linear_expression<Number, VariableName>(x).operator-(n);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator-(Number n, variable<Number, VariableName> x) {
  return linear_expression<Number, VariableName>(Number(-1), x).operator+(n);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator-(int64_t n, variable<Number, VariableName> x) {
  return linear_expression<Number, VariableName>(Number(-1), x).operator+(n);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator-(variable<Number, VariableName> x, variable<Number, VariableName> y) {
  return linear_expression<Number, VariableName>(x).operator-(y);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator-(Number n, const linear_expression<Number, VariableName> &e) {
  return linear_expression<Number, VariableName>(n).operator-(e);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator-(int64_t n, const linear_expression<Number, VariableName> &e) {
  return linear_expression<Number, VariableName>(Number(n)).operator-(e);
}

template <typename Number, typename VariableName>
inline linear_expression<Number, VariableName>
operator-(variable<Number, VariableName> x,
          const linear_expression<Number, VariableName> &e) {
  return linear_expression<Number, VariableName>(Number(1), x).operator-(e);
}

template <typename Number, typename VariableName> class linear_constraint {

public:
  typedef Number number_t;
  typedef VariableName varname_t;
  typedef linear_constraint<Number, VariableName> linear_constraint_t;
  typedef variable<Number, VariableName> variable_t;
  typedef linear_expression<Number, VariableName> linear_expression_t;
  typedef patricia_tree_set<variable_t> variable_set_t;
  typedef enum { EQUALITY, DISEQUATION, INEQUALITY, STRICT_INEQUALITY } kind_t;
  typedef typename linear_expression_t::iterator iterator;
  typedef typename linear_expression_t::const_iterator const_iterator;

private:
  kind_t _kind;
  linear_expression_t _expr;
  // This flag has meaning only if _kind == INEQUALITY or STRICT_INEQUALITY.
  // If true the inequality is signed otherwise unsigned.
  // By default all constraints are signed.
  bool _signedness;

public:
  linear_constraint() : _kind(EQUALITY), _signedness(true) {}

  linear_constraint(const linear_expression_t &expr, kind_t kind)
      : _kind(kind), _expr(expr), _signedness(true) {}

  linear_constraint(const linear_expression_t &expr, kind_t kind,
                    bool signedness)
      : _kind(kind), _expr(expr), _signedness(signedness) {
    if (_kind != INEQUALITY && _kind != STRICT_INEQUALITY) {
      CRAB_ERROR("Only inequalities can have signedness information");
    }
  }

  linear_constraint(const linear_constraint_t &c) = default;

  linear_constraint(linear_constraint_t &&c) = default;

  linear_constraint_t &operator=(const linear_constraint_t &c) = default;

  linear_constraint_t &operator=(linear_constraint_t &&c) = default;

  static linear_constraint_t get_true() {
    linear_constraint_t res(linear_expression_t(Number(0)), EQUALITY);
    return res;
  }

  static linear_constraint_t get_false() {
    linear_constraint_t res(linear_expression_t(Number(0)), DISEQUATION);
    return res;
  }

  bool is_tautology() const {
    switch (this->_kind) {
    case DISEQUATION:
      return (this->_expr.is_constant() && this->_expr.constant() != 0);
    case EQUALITY:
      return (this->_expr.is_constant() && this->_expr.constant() == 0);
    case INEQUALITY:
      return (this->_expr.is_constant() && this->_expr.constant() <= 0);
    case STRICT_INEQUALITY:
      return (this->_expr.is_constant() && this->_expr.constant() < 0);
    default:
      CRAB_ERROR("Unreachable");
    }
  }

  bool is_contradiction() const {
    switch (this->_kind) {
    case DISEQUATION:
      return (this->_expr.is_constant() && this->_expr.constant() == 0);
    case EQUALITY:
      return (this->_expr.is_constant() && this->_expr.constant() != 0);
    case INEQUALITY:
      return (this->_expr.is_constant() && this->_expr.constant() > 0);
    case STRICT_INEQUALITY:
      return (this->_expr.is_constant() && this->_expr.constant() >= 0);
    default:
      CRAB_ERROR("Unreachable");
    }
  }

  bool is_inequality() const { return (this->_kind == INEQUALITY); }

  bool is_strict_inequality() const {
    return (this->_kind == STRICT_INEQUALITY);
  }

  bool is_equality() const { return (this->_kind == EQUALITY); }

  bool is_disequation() const { return (this->_kind == DISEQUATION); }

  const linear_expression_t &expression() const { return this->_expr; }

  kind_t kind() const { return this->_kind; }

  bool is_signed() const {
    if (_kind != INEQUALITY && _kind != STRICT_INEQUALITY) {
      CRAB_WARN("Only inequalities have signedness");
    }
    return _signedness;
  }

  bool is_unsigned() const { return (!is_signed()); }

  void set_signed() {
    if (_kind == INEQUALITY || _kind == STRICT_INEQUALITY) {
      _signedness = true;
    } else {
      CRAB_WARN("Only inequalities have signedness");
    }
  }

  void set_unsigned() {
    if (_kind == INEQUALITY || _kind == STRICT_INEQUALITY) {
      _signedness = false;
    } else {
      CRAB_WARN("Only inequalities have signedness");
    }
  }

  const_iterator begin() const { return this->_expr.begin(); }

  const_iterator end() const { return this->_expr.end(); }

  iterator begin() { return this->_expr.begin(); }

  iterator end() { return this->_expr.end(); }

  Number constant() const { return -this->_expr.constant(); }

  std::size_t size() const { return this->_expr.size(); }

  // syntactic equality
  bool equal(const linear_constraint_t &o) const {
    return (_kind == o._kind && _signedness == o._signedness &&
            _expr.equal(o._expr));
  }

  size_t hash() const {
    size_t res = 0;
    boost::hash_combine(res, _expr);
    boost::hash_combine(res, _kind);
    if (_kind == INEQUALITY || _kind == STRICT_INEQUALITY) {
      boost::hash_combine(res, _signedness);
    }
    return res;
  }

  index_t index() const {
    // XXX: to store linear constraints in patricia trees
    // Ufff, the indexes may not be unique. Check that patricia
    // trees are ok with that.
    return (index_t)hash();
  }

  Number operator[](variable_t x) const { return this->_expr.operator[](x); }

  variable_set_t variables() const { return this->_expr.variables(); }

  bool is_well_typed() const { return _expr.is_well_typed(); }

  linear_constraint_t negate() const;

  template <typename RenamingMap>
  linear_constraint_t rename(const RenamingMap &map) const {
    linear_expression_t e = this->_expr.rename(map);
    return linear_constraint_t(e, this->_kind, is_signed());
  }

  void write(crab::crab_os &o) const {
    if (this->is_contradiction()) {
      o << "false";
    } else if (this->is_tautology()) {
      o << "true";
    } else {
      linear_expression_t e = this->_expr - this->_expr.constant();
      o << e;
      switch (this->_kind) {
      case INEQUALITY: {
        if (is_signed()) {
          o << " <= ";
        } else {
          o << " <=_u ";
        }
        break;
      }
      case STRICT_INEQUALITY: {
        if (is_signed()) {
          o << " < ";
        } else {
          o << " <_u ";
        }
        break;
      }
      case EQUALITY: {
        o << " = ";
        break;
      }
      case DISEQUATION: {
        o << " != ";
        break;
      }
      }
      Number c = -this->_expr.constant();
      o << c;
    }
  }

  // for dgb
  void dump() { write(crab::outs()); }

}; // class linear_constraint

template <typename Number, typename VariableName>
inline crab::crab_os &
operator<<(crab::crab_os &o, const linear_constraint<Number, VariableName> &c) {
  c.write(o);
  return o;
}

namespace linear_constraint_impl {

template <typename Number, typename VariableName>
linear_constraint<Number, VariableName>
negate_inequality(const linear_constraint<Number, VariableName> &c) {
  typedef linear_expression<Number, VariableName> linear_expression_t;
  typedef linear_constraint<Number, VariableName> linear_constraint_t;
  assert(c.is_inequality());
  // default implementation: negate(e <= 0) = e > 0
  linear_expression_t e(-c.expression());
  return linear_constraint_t(e, linear_constraint_t::kind_t::STRICT_INEQUALITY,
                             c.is_signed());
}

// Specialized version for z_number
template <typename VariableName>
linear_constraint<z_number, VariableName>
negate_inequality(const linear_constraint<z_number, VariableName> &c) {
  typedef linear_expression<z_number, VariableName> linear_expression_t;
  typedef linear_constraint<z_number, VariableName> linear_constraint_t;
  assert(c.is_inequality());
  // negate(e <= 0) = e >= 1
  linear_expression_t e(-(c.expression() - 1));
  return linear_constraint_t(e, linear_constraint_t::kind_t::INEQUALITY,
                             c.is_signed());
}

template <typename Number, typename VariableName>
linear_constraint<Number, VariableName> strict_to_non_strict_inequality(
    const linear_constraint<Number, VariableName> &c) {
  typedef linear_constraint<Number, VariableName> linear_constraint_t;
  assert(c.is_strict_inequality());
  // Default implementation: do nothing
  // Given constraint e < 0 we could return two linear constraints: e <= 0 and e
  // != 0. The linear interval solver lowers strict inequalities in that way.
  return c;
}

// Specialized version for z_number
template <typename VariableName>
linear_constraint<z_number, VariableName> strict_to_non_strict_inequality(
    const linear_constraint<z_number, VariableName> &c) {
  typedef linear_expression<z_number, VariableName> linear_expression_t;
  typedef linear_constraint<z_number, VariableName> linear_constraint_t;
  assert(c.is_strict_inequality());
  // e < 0 --> e <= -1
  linear_expression_t e(c.expression() + 1);
  return linear_constraint_t(e, linear_constraint_t::kind_t::INEQUALITY,
                             c.is_signed());
}
} // end namespace linear_constraint_impl

template <typename Number, typename VariableName>
linear_constraint<Number, VariableName>
linear_constraint<Number, VariableName>::negate() const {
  typedef linear_constraint<Number, VariableName> linear_constraint_t;
  typedef linear_expression<Number, VariableName> linear_expression_t;

  if (is_tautology()) {
    return get_false();
  } else if (is_contradiction()) {
    return get_true();
  } else {
    switch (kind()) {
    case INEQUALITY: {
      // negate_inequality tries to take advantage if we use z_number.
      return linear_constraint_impl::negate_inequality(*this);
    }
    case STRICT_INEQUALITY: {
      // negate(x + y < 0)  <-->  x + y >= 0 <--> -x -y <= 0
      linear_expression_t e = -this->_expr;
      return linear_constraint_t(e, INEQUALITY, is_signed());
    }
    case EQUALITY:
      return linear_constraint_t(this->_expr, DISEQUATION);
    case DISEQUATION:
      return linear_constraint_t(this->_expr, EQUALITY);
    default:
      CRAB_ERROR("Cannot negate linear constraint");
    }
  }
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<=(const linear_expression<Number, VariableName> &e, Number n) {
  return linear_constraint<Number, VariableName>(
      e - n, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<=(const linear_expression<Number, VariableName> &e, int64_t n) {
  return linear_constraint<Number, VariableName>(
      e - n, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<=(Number n, const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      n - e, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<=(int64_t n, const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      n - e, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<=(const linear_expression<Number, VariableName> &e,
           variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      e - x, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<=(variable<Number, VariableName> x,
           const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      x - e, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<=(variable<Number, VariableName> x, Number n) {
  return linear_constraint<Number, VariableName>(
      x - n, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<=(variable<Number, VariableName> x, int64_t n) {
  return linear_constraint<Number, VariableName>(
      x - n, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<=(Number n, variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      n - x, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<=(int64_t n, variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      n - x, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<=(variable<Number, VariableName> x, variable<Number, VariableName> y) {
  return linear_constraint<Number, VariableName>(
      x - y, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<=(const linear_expression<Number, VariableName> &e1,
           const linear_expression<Number, VariableName> &e2) {
  return linear_constraint<Number, VariableName>(
      e1 - e2, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>=(const linear_expression<Number, VariableName> &e, Number n) {
  return linear_constraint<Number, VariableName>(
      n - e, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>=(const linear_expression<Number, VariableName> &e, int64_t n) {
  return linear_constraint<Number, VariableName>(
      n - e, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>=(Number n, const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      e - n, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>=(int64_t n, const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      e - n, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>=(const linear_expression<Number, VariableName> &e,
           variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      x - e, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>=(variable<Number, VariableName> x,
           const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      e - x, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>=(variable<Number, VariableName> x, Number n) {
  return linear_constraint<Number, VariableName>(
      n - x, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>=(variable<Number, VariableName> x, int64_t n) {
  return linear_constraint<Number, VariableName>(
      n - x, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>=(Number n, variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      x - n, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>=(int64_t n, variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      x - n, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>=(variable<Number, VariableName> x, variable<Number, VariableName> y) {
  return linear_constraint<Number, VariableName>(
      y - x, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>=(const linear_expression<Number, VariableName> &e1,
           const linear_expression<Number, VariableName> &e2) {
  return linear_constraint<Number, VariableName>(
      e2 - e1, linear_constraint<Number, VariableName>::INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<(const linear_expression<Number, VariableName> &e, Number n) {
  return linear_constraint<Number, VariableName>(
      e - n, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<(const linear_expression<Number, VariableName> &e, int64_t n) {
  return linear_constraint<Number, VariableName>(
      e - n, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<(Number n, const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      n - e, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<(int64_t n, const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      n - e, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<(const linear_expression<Number, VariableName> &e,
          variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      e - x, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<(variable<Number, VariableName> x,
          const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      x - e, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<(variable<Number, VariableName> x, Number n) {
  return linear_constraint<Number, VariableName>(
      x - n, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<(variable<Number, VariableName> x, int64_t n) {
  return linear_constraint<Number, VariableName>(
      x - n, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<(Number n, variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      n - x, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<(int64_t n, variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      n - x, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<(variable<Number, VariableName> x, variable<Number, VariableName> y) {
  return linear_constraint<Number, VariableName>(
      x - y, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator<(const linear_expression<Number, VariableName> &e1,
          const linear_expression<Number, VariableName> &e2) {
  return linear_constraint<Number, VariableName>(
      e1 - e2, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>(const linear_expression<Number, VariableName> &e, Number n) {
  return linear_constraint<Number, VariableName>(
      n - e, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>(const linear_expression<Number, VariableName> &e, int64_t n) {
  return linear_constraint<Number, VariableName>(
      n - e, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>(Number n, const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      e - n, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>(int64_t n, const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      e - n, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>(const linear_expression<Number, VariableName> &e,
          variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      x - e, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>(variable<Number, VariableName> x,
          const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      e - x, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>(variable<Number, VariableName> x, Number n) {
  return linear_constraint<Number, VariableName>(
      n - x, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>(variable<Number, VariableName> x, int64_t n) {
  return linear_constraint<Number, VariableName>(
      n - x, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>(Number n, variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      x - n, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>(int64_t n, variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      x - n, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>(variable<Number, VariableName> x, variable<Number, VariableName> y) {
  return linear_constraint<Number, VariableName>(
      y - x, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator>(const linear_expression<Number, VariableName> &e1,
          const linear_expression<Number, VariableName> &e2) {
  return linear_constraint<Number, VariableName>(
      e2 - e1, linear_constraint<Number, VariableName>::STRICT_INEQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator==(const linear_expression<Number, VariableName> &e, Number n) {
  return linear_constraint<Number, VariableName>(
      e - n, linear_constraint<Number, VariableName>::EQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator==(const linear_expression<Number, VariableName> &e, int64_t n) {
  return linear_constraint<Number, VariableName>(
      e - n, linear_constraint<Number, VariableName>::EQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator==(Number n, const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      e - n, linear_constraint<Number, VariableName>::EQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator==(int64_t n, const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      e - n, linear_constraint<Number, VariableName>::EQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator==(const linear_expression<Number, VariableName> &e,
           variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      e - x, linear_constraint<Number, VariableName>::EQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator==(variable<Number, VariableName> x,
           const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      e - x, linear_constraint<Number, VariableName>::EQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator==(variable<Number, VariableName> x, Number n) {
  return linear_constraint<Number, VariableName>(
      x - n, linear_constraint<Number, VariableName>::EQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator==(variable<Number, VariableName> x, int64_t n) {
  return linear_constraint<Number, VariableName>(
      x - n, linear_constraint<Number, VariableName>::EQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator==(Number n, variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      x - n, linear_constraint<Number, VariableName>::EQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator==(int64_t n, variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      x - n, linear_constraint<Number, VariableName>::EQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator==(variable<Number, VariableName> x, variable<Number, VariableName> y) {
  return linear_constraint<Number, VariableName>(
      x - y, linear_constraint<Number, VariableName>::EQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator==(const linear_expression<Number, VariableName> &e1,
           const linear_expression<Number, VariableName> &e2) {
  return linear_constraint<Number, VariableName>(
      e1 - e2, linear_constraint<Number, VariableName>::EQUALITY);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator!=(const linear_expression<Number, VariableName> &e, Number n) {
  return linear_constraint<Number, VariableName>(
      e - n, linear_constraint<Number, VariableName>::DISEQUATION);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator!=(const linear_expression<Number, VariableName> &e, int64_t n) {
  return linear_constraint<Number, VariableName>(
      e - n, linear_constraint<Number, VariableName>::DISEQUATION);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator!=(Number n, const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      e - n, linear_constraint<Number, VariableName>::DISEQUATION);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator!=(int64_t n, const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      e - n, linear_constraint<Number, VariableName>::DISEQUATION);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator!=(const linear_expression<Number, VariableName> &e,
           variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      e - x, linear_constraint<Number, VariableName>::DISEQUATION);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator!=(variable<Number, VariableName> x,
           const linear_expression<Number, VariableName> &e) {
  return linear_constraint<Number, VariableName>(
      e - x, linear_constraint<Number, VariableName>::DISEQUATION);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator!=(variable<Number, VariableName> x, Number n) {
  return linear_constraint<Number, VariableName>(
      x - n, linear_constraint<Number, VariableName>::DISEQUATION);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator!=(variable<Number, VariableName> x, int64_t n) {
  return linear_constraint<Number, VariableName>(
      x - n, linear_constraint<Number, VariableName>::DISEQUATION);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator!=(Number n, variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      x - n, linear_constraint<Number, VariableName>::DISEQUATION);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator!=(int64_t n, variable<Number, VariableName> x) {
  return linear_constraint<Number, VariableName>(
      x - n, linear_constraint<Number, VariableName>::DISEQUATION);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator!=(variable<Number, VariableName> x, variable<Number, VariableName> y) {
  return linear_constraint<Number, VariableName>(
      x - y, linear_constraint<Number, VariableName>::DISEQUATION);
}

template <typename Number, typename VariableName>
inline linear_constraint<Number, VariableName>
operator!=(const linear_expression<Number, VariableName> &e1,
           const linear_expression<Number, VariableName> &e2) {
  return linear_constraint<Number, VariableName>(
      e1 - e2, linear_constraint<Number, VariableName>::DISEQUATION);
}

/* used by boost::hash_combine */
template <typename Number, typename VariableName>
inline std::size_t
hash_value(const linear_constraint<Number, VariableName> &c) {
  return c.hash();
}

template <typename Number, typename VariableName>
struct linear_constraint_hasher {
  size_t operator()(const linear_constraint<Number, VariableName> &c) const {
    return c.hash();
  }
};

template <typename Number, typename VariableName>
struct linear_constraint_equal {
  bool operator()(const linear_constraint<Number, VariableName> &c1,
                  const linear_constraint<Number, VariableName> &c2) const {
    return c1.equal(c2);
  }
};

template <typename Number, typename VariableName>
using linear_constraint_unordered_set =
    std::unordered_set<linear_constraint<Number, VariableName>,
                       linear_constraint_hasher<Number, VariableName>,
                       linear_constraint_equal<Number, VariableName>>;

template <typename Number, typename VariableName, typename Value>
using linear_constraint_unordered_map =
    std::unordered_map<linear_constraint<Number, VariableName>, Value,
                       linear_constraint_hasher<Number, VariableName>,
                       linear_constraint_equal<Number, VariableName>>;

template <typename Number, typename VariableName>
class linear_constraint_system {

public:
  typedef Number number_t;
  typedef VariableName varname_t;
  typedef linear_expression<Number, VariableName> linear_expression_t;
  typedef linear_constraint<Number, VariableName> linear_constraint_t;
  typedef linear_constraint_system<Number, VariableName>
      linear_constraint_system_t;
  typedef variable<Number, VariableName> variable_t;
  typedef patricia_tree_set<variable_t> variable_set_t;

private:
  typedef std::vector<linear_constraint_t> cst_collection_t;

public:
  typedef typename cst_collection_t::iterator iterator;
  typedef typename cst_collection_t::const_iterator const_iterator;

private:
  cst_collection_t _csts;

public:
  linear_constraint_system() {}

  linear_constraint_system(const linear_constraint_t &cst) {
    _csts.push_back(cst);
  }

  linear_constraint_system(const linear_constraint_system_t &o) = default;

  linear_constraint_system(linear_constraint_system_t &&o) = default;

  linear_constraint_system_t &operator=(const linear_constraint_system_t &o) = default;

  linear_constraint_system_t &operator=(linear_constraint_system_t &&o) = default;

  linear_constraint_system_t &operator+=(const linear_constraint_t &c) {
    if (!std::any_of(
            _csts.begin(), _csts.end(),
            [c](const linear_constraint_t &c1) { return c1.equal(c); })) {
      _csts.push_back(c);
    }
    return *this;
  }

  linear_constraint_system_t &operator+=(const linear_constraint_system_t &s) {
    for (auto c : s) {
      if (!std::any_of(
              _csts.begin(), _csts.end(),
              [c](const linear_constraint_t &c1) { return c1.equal(c); })) {
        _csts.push_back(c);
      }
    }
    return *this;
  }

  linear_constraint_system_t
  operator+(const linear_constraint_system_t &s) const {
    linear_constraint_system_t r;
    r.operator+=(s);
    r.operator+=(*this);
    return r;
  }

  /**
     Replace pairs e<=0 and -e<=0 with e==0
  **/
  linear_constraint_system_t normalize() const {
    linear_expression_unordered_set<number_t, varname_t> expr_set;
    linear_expression_unordered_map<number_t, varname_t, unsigned> index_map;
    std::vector<bool> toremove(_csts.size(), false); // indexes to be removed
    linear_constraint_system_t out;

    for (unsigned i = 0, e = _csts.size(); i < e; ++i) {
      if (_csts[i].is_inequality()) {
        linear_expression_t exp = _csts[i].expression();
        if (expr_set.find(-exp) == expr_set.end()) {
          // remember the index and the expression
          index_map.insert({exp, i});
          expr_set.insert(exp);
        } else {
          // we found exp<=0 and -exp<= 0
          unsigned j = index_map[-exp];
          if (_csts[i].is_signed() == _csts[j].is_signed()) {
            toremove[i] = true;
            toremove[j] = true;
            bool insert_pos = true;
            if (exp.size() == 1 && (*(exp.begin())).first < 0) {
              // unary equality: we choose the one with the positive position.
              insert_pos = false;
            }
            if (!insert_pos) {
              out += linear_constraint_t(-exp, linear_constraint_t::EQUALITY);
            } else {
              out += linear_constraint_t(exp, linear_constraint_t::EQUALITY);
            }
          }
        }
      }
    }

    for (unsigned i = 0, e = _csts.size(); i < e; ++i) {
      if (!toremove[i]) {
        out += _csts[i];
      }
    }

    return out;
  }

  const_iterator begin() const { return _csts.begin(); }

  const_iterator end() const { return _csts.end(); }

  iterator begin() { return _csts.begin(); }

  iterator end() { return _csts.end(); }

  variable_set_t variables() const {
    variable_set_t variables;
    for (auto c : *this)
      variables |= c.variables();
    return variables;
  }

  // TODO: expensive linear operation.
  // XXX: We can keep track of whether the system is false in an
  // incremental manner.
  bool is_false() const {
    if (_csts.empty())
      return false; // empty is considered true

    for (auto it = this->begin(); it != this->end(); ++it) {
      auto c = *it;
      if (!c.is_contradiction()) {
        return false;
      }
    }
    return true; // all constraints are false
  }

  bool is_true() const { return _csts.empty(); }

  std::size_t size() const { return _csts.size(); }

  void write(crab::crab_os &o) const {
    o << "{";
    for (const_iterator it = this->begin(); it != this->end();) {
      auto c = *it;
      o << c;
      ++it;
      if (it != end()) {
        o << "; ";
      }
    }
    o << "}";
  }

  // for dgb
  void dump() { write(crab::outs()); }

  std::string get_string(){
    crab::crab_string_os ss;
    write(ss);
    return ss.str();
  }

}; // class linear_constraint_system

template <typename Number, typename VariableName>
inline crab::crab_os &
operator<<(crab::crab_os &o,
           const linear_constraint_system<Number, VariableName> &sys) {
  sys.write(o);
  return o;
}

// This class contains a disjunction of linear constraints (i.e., DNF form)
template <typename Number, typename VariableName>
class disjunctive_linear_constraint_system {

public:
  typedef Number number_t;
  typedef VariableName varname_t;
  typedef linear_constraint<Number, VariableName> linear_constraint_t;
  typedef linear_constraint_system<Number, VariableName>
      linear_constraint_system_t;
  typedef disjunctive_linear_constraint_system<Number, VariableName> this_type;

private:
  typedef std::vector<linear_constraint_system_t> cst_collection_t;
  cst_collection_t _csts;
  bool _is_false;

public:
  typedef typename cst_collection_t::iterator iterator;
  typedef typename cst_collection_t::const_iterator const_iterator;

  disjunctive_linear_constraint_system(bool is_false = false)
      : _is_false(is_false) {}

  explicit disjunctive_linear_constraint_system(
      const linear_constraint_system_t &cst)
      : _is_false(false) {
    if (cst.is_false()) {
      _is_false = true;
    } else {
      _csts.push_back(cst);
    }
  }

  disjunctive_linear_constraint_system(const this_type &o) = default;

  disjunctive_linear_constraint_system(this_type &&o) = default;

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

  this_type &operator=(this_type &&o) = default;

  bool is_false() const { return _is_false; }

  bool is_true() const { return (!is_false() && _csts.empty()); }

  // make true
  void clear() {
    _is_false = false;
    _csts.clear();
  }

  /*
    c1 or ... or cn  += true  ==> error
    c1 or ... or cn  += false ==> c1 or .. or cn
    c1 or ... or cn  += c     ==> c1 or ... or cn or c
  */
  this_type &operator+=(const linear_constraint_system_t &cst) {
    if (cst.is_true()) {
      // adding true should make the whole thing true
      // but we prefer to raise an error for now.
      CRAB_ERROR("Disjunctive linear constraint: cannot add true");
    }

    if (!cst.is_false()) {
      _csts.push_back(cst);
      _is_false = false;
    }
    return *this;
  }

  /*
    c1 or ... or cn  += true  ==> error
    c1 or ... or cn  += false ==> c1 or .. or cn
    c1 or ... or cn  += d1 or ... or dn   ==> c1 or ... or cn or d1 or ... or dn
  */
  this_type &operator+=(const this_type &s) {
    if (s.is_true()) {
      // adding true should make the whole thing true
      // but we prefer to raise an error for now.
      CRAB_ERROR("Disjunctive linear constraint: cannot add true");
    }
    if (!s.is_false()) {
      for (const linear_constraint_system_t &c : s) {
        this->operator+=(c);
      }
    }
    return *this;
  }

  this_type operator+(const this_type &s) const {
    this_type r;
    r.operator+=(s);
    r.operator+=(*this);
    return r;
  }

  // To enumerate all the conjunctions
  const_iterator begin() const {
    if (is_false())
      CRAB_ERROR(
          "Disjunctive Linear constraint: trying to call begin() when false");
    return _csts.begin();
  }

  const_iterator end() const {
    if (is_false())
      CRAB_ERROR(
          "Disjunctive Linear constraint: trying to call end() when false");
    return _csts.end();
  }

  iterator begin() {
    if (is_false())
      CRAB_ERROR(
          "Disjunctive Linear constraint: trying to call begin() when false");
    return _csts.begin();
  }

  iterator end() {
    if (is_false())
      CRAB_ERROR(
          "Disjunctive Linear constraint: trying to call end() when false");
    return _csts.end();
  }

  // Return the number of conjunctions
  std::size_t size() const { return _csts.size(); }

  void write(crab::crab_os &o) const {
    if (is_false()) {
      o << "_|_";
    } else if (is_true()) {
      o << "{}";
    } else if (size() == 1) {
      o << _csts[0];
    } else {
      assert(size() > 1);
      for (const_iterator it = this->begin(); it != this->end();) {
        auto c = *it;
        o << c;
        ++it;
        if (it != end()) {
          o << " or \n";
        }
      }
    }
  }

  std::string get_string(){
    crab::crab_string_os ss;
    write(ss);
    return ss.str();
  }
};

template <typename Number, typename VariableName>
inline crab::crab_os &operator<<(
    crab::crab_os &o,
    const disjunctive_linear_constraint_system<Number, VariableName> &sys) {
  sys.write(o);
  return o;
}

} // namespace ikos

namespace std {
/**  specialization of std::hash for linear expressions and constraints **/
template <typename Number, typename VariableName>
struct hash<ikos::linear_expression<Number, VariableName>> {
  using linear_expression_t = ikos::linear_expression<Number, VariableName>;
  size_t operator()(const linear_expression_t &e) const { return e.hash(); }
};

template <typename Number, typename VariableName>
struct hash<ikos::linear_constraint<Number, VariableName>> {
  using linear_constraint_t = ikos::linear_constraint<Number, VariableName>;
  size_t operator()(const linear_constraint_t &c) const { return c.hash(); }
};

} // end namespace std


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

#ifndef MTL_SMAT_DMAT_MULT_INCLUDE
#define MTL_SMAT_DMAT_MULT_INCLUDE

#include <boost/numeric/mtl/operation/set_to_zero.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/flatcat.hpp>
#include <boost/numeric/meta_math/loop1.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace functor {

template <typename Assign= assign::assign_sum,
	  typename Backup= no_op>     // To allow 2nd parameter, is ignored
struct gen_smat_dmat_mult
{
    template <typename MatrixA, typename MatrixB, typename MatrixC>
    void operator()(MatrixA const& a, MatrixB const& b, MatrixC& c)
    {
	vampir_trace<4018> tracer;
	apply(a, b, c, typename OrientedCollection<MatrixA>::orientation());
    }

private:
    template <typename MatrixA, typename MatrixB, typename MatrixC>
    void apply(MatrixA const& a, MatrixB const& b, MatrixC& c, tag::row_major)
    {
	using namespace tag;
	using traits::range_generator;
        typedef typename range_generator<row, MatrixA>::type       a_cur_type;
        typedef typename range_generator<row, MatrixC>::type       c_cur_type;
	typedef typename range_generator<col, MatrixB>::type       b_cur_type;

        typedef typename range_generator<nz, a_cur_type>::type     a_icur_type;
        typedef typename range_generator<all, b_cur_type>::type    b_icur_type;
        typedef typename range_generator<iter::all, c_cur_type>::type    c_icur_type;

	typename traits::col<MatrixA>::type             col_a(a);
	typename traits::const_value<MatrixA>::type     value_a(a);
	typename traits::const_value<MatrixB>::type     value_b(b);

	if (Assign::init_to_zero) set_to_zero(c);

	a_cur_type ac= begin<row>(a), aend= end<row>(a);
	for (c_cur_type cc= begin<row>(c); ac != aend; ++ac, ++cc) {

	    b_cur_type bc= begin<col>(b), bend= end<col>(b);
	    for (c_icur_type cic= begin<iter::all>(cc); bc != bend; ++bc, ++cic) {

		typename MatrixC::value_type c_tmp(*cic);
		for (a_icur_type aic= begin<nz>(ac), aiend= end<nz>(ac); aic != aiend; ++aic) {

		    typename Collection<MatrixA>::size_type     ca= col_a(*aic);   // column of non-zero

		    b_icur_type bic= begin<all>(bc);
		    bic+= ca;
		    Assign::update(c_tmp, value_a(*aic) * value_b(*bic));
		}
		*cic= c_tmp;
	    }
	}
    }

    template <typename MatrixA, typename MatrixB, typename MatrixC>
    void apply(MatrixA const& a, MatrixB const& b, MatrixC& c, tag::col_major)
    {
	using namespace tag;
	using traits::range_generator;
        typedef typename range_generator<col, MatrixA>::type       a_cur_type;
        typedef typename range_generator<nz, a_cur_type>::type     a_icur_type;

	typename traits::row<MatrixA>::type             row_a(a);
	typename traits::const_value<MatrixA>::type     value_a(a);

	if (Assign::init_to_zero) set_to_zero(c);

	unsigned rb= 0; // traverse all rows of b
	for (a_cur_type ac= begin<col>(a), aend= end<col>(a); ac != aend; ++ac, ++rb)
	    for (a_icur_type aic= begin<nz>(ac), aiend= end<nz>(ac); aic != aiend; ++aic) {
		typename Collection<MatrixA>::size_type     ra= row_a(*aic);   // row in A and C
		typename Collection<MatrixA>::value_type    va= value_a(*aic); // value of non-zero

		for (unsigned cb= 0; cb < num_cols(b); ++cb) // column in B and C
		    Assign::update(c(ra, cb), va * b(rb, cb));
	    }
    }
};

// =======================
// Unrolled
// required has_2D_layout
// =======================

// Define defaults if not yet given as Compiler flag
#ifndef MTL_SMAT_DMAT_MULT_TILING1
#  define MTL_SMAT_DMAT_MULT_TILING1 8
#endif

template <unsigned long Index0, unsigned long Max0, typename Assign>
struct gen_tiling_smat_dmat_mult_block
    : public meta_math::loop1<Index0, Max0>
{
    typedef meta_math::loop1<Index0, Max0>                                    base;
    typedef gen_tiling_smat_dmat_mult_block<base::next_index0, Max0, Assign>  next_t;

    template <typename Value, typename ValueA, typename ValueB, typename Size>
    static inline void apply(Value& tmp00, Value& tmp01, Value& tmp02, Value& tmp03, Value& tmp04,
			     Value& tmp05, Value& tmp06, Value& tmp07, Value& tmp08, Value& tmp09,
			     Value& tmp10, Value& tmp11, Value& tmp12, Value& tmp13, Value& tmp14, Value& tmp15,
			     const ValueA& va, ValueB *begin_b, const Size& bci)
    {
	tmp00+= va * *(begin_b + base::index0 * bci);
	next_t::apply(tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07, tmp08, tmp09,
		      tmp10, tmp11, tmp12, tmp13, tmp14, tmp15, tmp00,
		      va, begin_b, bci);
    }

    template <typename Value, typename MatrixC, typename SizeC>
    static inline void update(Value& tmp00, Value& tmp01, Value& tmp02, Value& tmp03, Value& tmp04,
			      Value& tmp05, Value& tmp06, Value& tmp07, Value& tmp08, Value& tmp09,
			      Value& tmp10, Value& tmp11, Value& tmp12, Value& tmp13, Value& tmp14, Value& tmp15,
			      MatrixC& c, SizeC i, SizeC k)
    {
	Assign::update(c(i, k + base::index0), tmp00);
	next_t::update(tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07, tmp08, tmp09,
		       tmp10, tmp11, tmp12, tmp13, tmp14, tmp15, tmp00,
		       c, i, k);
    }
};

template <unsigned long Max0, typename Assign>
struct gen_tiling_smat_dmat_mult_block<Max0, Max0, Assign>
    : public meta_math::loop1<Max0, Max0>
{
    typedef meta_math::loop1<Max0, Max0>                                    base;

    template <typename Value, typename ValueA, typename ValueB, typename Size>
    static inline void apply(Value& tmp00, Value&, Value&, Value&, Value&,
			     Value&, Value&, Value&, Value&, Value&,
			     Value&, Value&, Value&, Value&, Value&, Value&,
			     const ValueA& va, ValueB *begin_b, const Size& bci)
    {
	tmp00+= va * *(begin_b + base::index0 * bci);
    }

    template <typename Value, typename MatrixC, typename SizeC>
    static inline void update(Value& tmp00, Value&, Value&, Value&, Value&,
			      Value&, Value&, Value&, Value&, Value&,
			      Value&, Value&, Value&, Value&, Value&, Value&,
			      MatrixC& c, SizeC i, SizeC k)
    {
	Assign::update(c(i, k + base::index0), tmp00);
    }
};

template <unsigned long Tiling1= MTL_SMAT_DMAT_MULT_TILING1,
	  typename Assign= assign::assign_sum,
	  typename Backup= gen_smat_dmat_mult<Assign> >
struct gen_tiling_smat_dmat_mult
{
    template <typename MatrixA, typename MatrixB, typename MatrixC>
    void operator()(MatrixA const& a, MatrixB const& b, MatrixC& c)
    {
    vampir_trace<4019> tracer;
	apply(a, b, c, traits::layout_flatcat<MatrixC>());
    }

private:
    template <typename MatrixA, typename MatrixB, typename MatrixC>
    void apply(MatrixA const& a, MatrixB const& b, MatrixC& c, tag::universe)
    {
	Backup()(a, b, c);
    }

    template <typename MatrixA, typename MatrixB, typename MatrixC>
    void apply(MatrixA const& a, MatrixB const& b, MatrixC& c, tag::flat<tag::has_2D_layout>)
    {
	apply2(a, b, c, typename OrientedCollection<MatrixA>::orientation());
    }

    template <typename MatrixA, typename MatrixB, typename MatrixC>
    void apply2(MatrixA const& a, MatrixB const& b, MatrixC& c, tag::col_major)
    {
	// may be I'll write an optimized version later
	Backup()(a, b, c);
    }

    template <typename MatrixA, typename MatrixB, typename MatrixC>
    void apply2(MatrixA const& a, MatrixB const& b, MatrixC& c, tag::row_major)
    {
	using namespace tag;
	using traits::range_generator;

	typedef gen_tiling_smat_dmat_mult_block<1, Tiling1, Assign>  block;
	typedef typename Collection<MatrixA>::size_type              size_type;
	typedef typename Collection<MatrixC>::value_type             value_type;
	const value_type z= math::zero(c[0][0]);    // if this are matrices we need their size

        typedef typename range_generator<row, MatrixA>::type         a_cur_type;
        typedef typename range_generator<nz, a_cur_type>::type       a_icur_type;

	typename traits::col<MatrixA>::type             col_a(a);
	typename traits::const_value<MatrixA>::type     value_a(a);

	if (Assign::init_to_zero) set_to_zero(c);

	size_type i_max= num_cols(b), i_block= Tiling1 * (i_max / Tiling1);
	size_t bci= i_max > 1 ? &b(0, 1) - &b(0, 0) : 1; // offset of incrementing B's column if more than 1 column

	size_type rc= 0; // start in row 0
	for (a_cur_type ac= begin<row>(a), aend= end<row>(a); ac != aend; ++ac, ++rc) {

	    for (size_type i= 0; i < i_block; i+= Tiling1) {

		value_type tmp00= z, tmp01= z, tmp02= z, tmp03= z, tmp04= z,
                           tmp05= z, tmp06= z, tmp07= z, tmp08= z, tmp09= z,
 		           tmp10= z, tmp11= z, tmp12= z, tmp13= z, tmp14= z, tmp15= z;

		for (a_icur_type aic= begin<nz>(ac), aiend= end<nz>(ac); aic != aiend; ++aic) {
		    typename Collection<MatrixA>::size_type     ca= col_a(*aic);   // column of non-zero
		    typename Collection<MatrixA>::value_type    va= value_a(*aic); // value of non-zero

		    // Element in first vector in block to be multiplied with va; rb==ca
		    const typename MatrixB::value_type *begin_b= &b(ca, i);
		    block::apply(tmp00, tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07, tmp08, tmp09,
				 tmp10, tmp11, tmp12, tmp13, tmp14, tmp15,
				 va, begin_b, bci);
		}
		block::update(tmp00, tmp01, tmp02, tmp03, tmp04, tmp05, tmp06, tmp07, tmp08, tmp09,
			      tmp10, tmp11, tmp12, tmp13, tmp14, tmp15,
			      c, rc, i);
	    }

	    for (size_type i= i_block; i < i_max; i++) {
		value_type tmp00= z;
		for (a_icur_type aic= begin<nz>(ac), aiend= end<nz>(ac); aic != aiend; ++aic) {
		    typename Collection<MatrixA>::size_type     ca= col_a(aic);   // column of non-zero
		    tmp00+= value_a(*aic) * b(ca, i);
		}
		Assign::update(c(rc, i), tmp00);
	    }
	}
    }
};

}} // namespace mtl::functor

#endif // MTL_SMAT_DMAT_MULT_INCLUDE


//=============================================================================
//
//  CLASS NConstraintInterfaceADOLC
//
//=============================================================================

#ifndef COMISO_NCONSTRAINTINTERFACEADOLC_HH
#define COMISO_NCONSTRAINTINTERFACEADOLC_HH

//== COMPILE-TIME PACKAGE REQUIREMENTS ========================================
#include <CoMISo/Config/config.hh>
#if COMISO_ADOLC_AVAILABLE
#if COMISO_EIGEN3_AVAILABLE

//== INCLUDES =================================================================

#include <CoMISo/Config/CoMISoDefines.hh>
#include "SuperSparseMatrixT.hh"

#include <adolc/adolc.h>
#include <adolc/adouble.h>
#include <adolc/drivers/drivers.h>
#include <adolc/sparse/sparsedrivers.h>
#include <adolc/taping.h>

#include "NConstraintInterface.hh"

#include "TapeIDSingleton.hh"

#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Sparse>

//== FORWARDDECLARATIONS ======================================================

//== NAMESPACES ===============================================================

namespace COMISO {

//== CLASS DEFINITION =========================================================

/** \class NProblemInterfaceADOLC NProblemInterfaceADOLC.hpp

    The problem interface using automatic differentiation.
 */
class COMISODLLEXPORT NConstraintInterfaceADOLC : public NConstraintInterface {
public:

    // Define Sparse Datatypes
    typedef NConstraintInterface::SVectorNC SVectorNC;
    typedef NConstraintInterface::SMatrixNC SMatrixNC;

    typedef NConstraintInterface::ConstraintType ConstraintType;

    /// Default constructor
    NConstraintInterfaceADOLC(int _n_unknowns,
    		const ConstraintType _type = NC_EQUAL,
    		double _eps = 1e-6) :
        NConstraintInterface(_type, _eps),
        n_unknowns_(_n_unknowns),
        tape_available_(false),
        use_tape_(true),
        tape_(static_cast<short int>(TapeIDSingleton::Instance()->requestId())),
        dense_hessian_(NULL),
        sparse_nz_(0),
		sparse_r_ind_p_(NULL),
		sparse_c_ind_p_(NULL),
		sparse_val_p_(NULL) {

    }

    /// Destructor
    virtual ~NConstraintInterfaceADOLC() {

    	cleanup_sparse_hessian();

        TapeIDSingleton::Instance()->releaseId(static_cast<size_t>(tape_));
    }

    /**
     * \brief Only override this function
     */
    virtual adouble eval_c_adouble(const adouble* _x) = 0;

public:

    virtual int n_unknowns() {
        return n_unknowns_;
    }

    virtual double eval_constraint(const double* _x) {

        double y = 0.0;

        if(!tape_available_ || !use_tape_) {

        	adouble y_d = 0.0;

//        	if(active_vars_.get() == NULL)
//        		active_vars_.reset(new adouble[n_unknowns_]);

        	adouble* xa = new adouble[n_unknowns_];

			trace_on(tape_); // Start taping

			// Fill data vector
			for(int i = 0; i < n_unknowns_; ++i)
				xa[i] <<= _x[i];

			// Call virtual function to compute
			// functional value
			y_d = eval_c_adouble(xa);

			y_d >>= y;

			trace_off();

#ifdef ADOLC_STATS
			print_stats();
#endif

			delete[] xa;

			tape_available_ = true;

        } else {

        	int ec = function(tape_, 1, n_unknowns_, const_cast<double*>(_x), &y);

#ifdef ADOLC_RET_CODES
			std::cout << "Info: function() returned code " << ec << std::endl;
#endif

			// tape not valid anymore? retape and evaluate again
			if(ec < 0)
			{
			  tape_available_ = false;
			  return eval_constraint(_x);
			}
        }

        return y;
    }

    virtual void eval_gradient(const double* _x, SVectorNC& _g) {

        if(!tape_available_ || !use_tape_) {

        	// Evaluate original functional
            eval_constraint(_x);
        }

//        if(gradient_.get() == NULL)
//        	gradient_.reset(new double[n_unknowns_]);

        double* grad_p = new double[n_unknowns_];

        _g.resize(n_unknowns_);
        _g.setZero();

        // Evaluate gradient
		int ec = gradient(tape_, n_unknowns_, _x, grad_p);

		// Check if retaping is required
		if(ec < 0)
		{
#ifdef ADOLC_RET_CODES
			std::cout << __FUNCTION__ << " invokes retaping of function due to discontinuity! Return code: " << ec << std::endl;
#endif

			tape_available_ = false;
			eval_gradient(_x,_g);
		}

#ifdef ADOLC_RET_CODES
        std::cout << "Info: gradient() returned code " << ec << std::endl;
#endif

        for(int i = 0; i < n_unknowns_; ++i) {
            _g.coeffRef(i) += grad_p[i];
        }

        delete[] grad_p;
    }

    virtual void eval_hessian(const double* _x, SMatrixNC& _H) {

        _H.resize(n_unknowns_, n_unknowns_);

        // tape update required?
		if (!tape_available_ || !use_tape_)
			eval_constraint(_x);

		/*
		 * Hessian matrix is sparse
		 */
		if(sparse_hessian()) {

			int opt[2] = {0, 0};

			bool sparsity_pattern_available = bool(sparse_r_ind_p_);

			int ec = sparse_hess(tape_, n_unknowns_,
					sparsity_pattern_available, _x, &sparse_nz_,
					&sparse_r_ind_p_, &sparse_c_ind_p_, &sparse_val_p_, opt);

			if (ec < 0) {
#ifdef ADOLC_RET_CODES
				std::cout << __FUNCTION__ << " invokes retaping of function due to discontinuity! Return code: " << ec << std::endl;
#endif
				// Retape function if return code indicates discontinuity
				tape_available_ = false;
				eval_constraint(_x);
				ec = sparse_hess(tape_, n_unknowns_,
						sparsity_pattern_available, _x, &sparse_nz_,
						&sparse_r_ind_p_, &sparse_c_ind_p_, &sparse_val_p_,	opt);
			}

			// data should be available now
			assert(sparse_r_ind_p_ != NULL);
			assert(sparse_c_ind_p_ != NULL);
			assert(sparse_val_p_ != NULL);

#ifdef ADOLC_RET_CODES
			std::cout << "Info: sparse_hessian() returned code " << ec << std::endl;
#endif

			// Store data
			for (int i = 0; i < sparse_nz_; ++i) {

				_H(sparse_r_ind_p_[i], sparse_c_ind_p_[i]) = sparse_val_p_[i];

				// Provide also upper diagonal part?
				if (sparse_r_ind_p_[i] != sparse_c_ind_p_[i])
					_H(sparse_c_ind_p_[i], sparse_r_ind_p_[i]) = sparse_val_p_[i];
			}

		} else {
			/*
			 * Hessian matrix is dense
			 */
			allocate_dense_hessian();

			int ec = hessian(tape_, n_unknowns_, const_cast<double*>(_x),
					dense_hessian_);

			if (ec < 0) {
#ifdef ADOLC_RET_CODES
				std::cout << __FUNCTION__ << " invokes retaping of function due to discontinuity! Return code: " << ec << std::endl;
#endif
				// Retape function if return code indicates discontinuity
				tape_available_ = false;
				eval_constraint(_x);
				ec = hessian(tape_, n_unknowns_, const_cast<double*>(_x),
						dense_hessian_);
			}

#ifdef ADOLC_RET_CODES
			std::cout << "Info: hessian() returned code " << ec << std::endl;
#endif

			for (int i = 0; i < n_unknowns_; ++i) {
				// Diagonal
				_H(i, i) = dense_hessian_[i][i];
				for (int j = 0; j < i; ++j) {
					_H(i, j) = dense_hessian_[i][j];
					// Also store upper diagonal part
					_H(j, i) = dense_hessian_[i][j];
				}
			}
		}
    }

    /** \brief Use tape
     * Set this to false if the energy functional
     * is discontinuous (so that the operator tree
     * has to be re-established at each evaluation)
     */
    bool use_tape() const {
        return use_tape_;
    }

    void use_tape(bool _b) {
        use_tape_ = _b;
    }

    /**
     * \brief Provide information to make computations
     * as efficient as possible.
     */
    virtual bool is_linear()         const { return false; }
    virtual bool constant_gradient() const { return false; }

    /**
     * \brief Indicate whether the hessian is sparse.
     * If so, the computations (as well as the memory
     * consumption) can be performed more efficiently.
     */
    virtual bool constant_hessian () const { return false;}

private:

	void allocate_dense_hessian() {
		if (!dense_hessian_) {
			dense_hessian_ = new double*[n_unknowns_];
			for (int i = 0; i < n_unknowns_; ++i)
				dense_hessian_[i] = new double[i + 1];
		}

		// store #unknowns to access in destructor
		sparse_nz_ = n_unknowns_;
	}

	void cleanup_dense_hessian() {
		if (dense_hessian_) {
			for (int i = 0; i < sparse_nz_; ++i)
				delete[] dense_hessian_[i];

			delete[] dense_hessian_;
		}
	}

	void cleanup_sparse_hessian() {

		if (sparse_r_ind_p_ != NULL)
			delete[] sparse_r_ind_p_;
		if (sparse_c_ind_p_ != NULL)
			delete[] sparse_c_ind_p_;
		if (sparse_val_p_ != NULL)
			delete[] sparse_val_p_;
	}

    void print_stats() {

    	size_t tape_stats[11];
    	tapestats(tape_, tape_stats);
		std::cout << "Status values for tape " << tape_ << std::endl;
		std::cout << "===============================================" << std::endl;
		std::cout << "Number of independent variables:\t" << tape_stats[0] << std::endl;
		std::cout << "Number of dependent variables:\t\t" << tape_stats[1] << std::endl;
		std::cout << "Max. number of live active variables:\t" << tape_stats[2] << std::endl;
		std::cout << "Size of value stack:\t\t\t" << tape_stats[3] << std::endl;
		std::cout << "Buffer size:\t\t\t\t" << tape_stats[4] << std::endl;
		std::cout << "Total number of operations recorded:\t" << tape_stats[5] << std::endl;
		std::cout << "Other stats [6]:\t\t\t" << tape_stats[6] << std::endl;
		std::cout << "Other stats [7]:\t\t\t" << tape_stats[7] << std::endl;
		std::cout << "Other stats [8]:\t\t\t" << tape_stats[8] << std::endl;
		std::cout << "Other stats [9]:\t\t\t" << tape_stats[9] << std::endl;
		std::cout << "Other stats [10]:\t\t\t" << tape_stats[10] << std::endl;
		std::cout << "===============================================" << std::endl;
    }

    // Number of unknowns
    int n_unknowns_;

    bool tape_available_;
    bool use_tape_;

    SMatrixNC constant_hessian_;

    const short int tape_;

//    std::auto_ptr<adouble> active_vars_;
//    std::auto_ptr<double> gradient_;

    // dense hessian (if required)
    double** dense_hessian_;

    // Sparse hessian data
	int sparse_nz_;
	unsigned int* sparse_r_ind_p_;
	unsigned int* sparse_c_ind_p_;
	double* sparse_val_p_ ;
};

//=============================================================================
}// namespace COMISO
//=============================================================================
#endif // COMISO_ADOLC_AVAILABLE
//=============================================================================
#endif // COMISO_EIGEN3_AVAILABLE
//=============================================================================
#endif // ACG_NCONSTRAINTINTERFACEAD_HH defined
//=============================================================================


//
// Created by david on 6/7/2018.
//

#include <random>
#include "ImageGraph.h"
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#include "graph_cut.h"

namespace {
    using namespace Gadgetron;
    static std::mt19937 rng_state(4242);

    template<unsigned int D>
    void update_regularization_edge(ImageGraph<D> &graph, const hoNDArray<uint16_t> &field_map,
                                    const hoNDArray<uint16_t> &proposed_field_map,
                                    const hoNDArray<float> &second_deriv, const size_t idx, const size_t idx2,
                                    const size_t edge_idx, float scaling) {

        int f_value1 = field_map[idx];
        int pf_value1 = proposed_field_map[idx];
        int f_value2 = field_map[idx2];
        int pf_value2 = proposed_field_map[idx2];
        int a = std::norm(f_value1 - f_value2);
        int b = std::norm(f_value1 - pf_value2);
        int c = std::norm(pf_value1 - f_value2);
        int d = std::norm(pf_value1 - pf_value2);

        float weight = b + c - a - d;

        assert(weight >= 0);
        float lambda = std::max(std::min(second_deriv[idx], second_deriv[idx2]), 0.0f) * scaling;
        weight *= lambda;

        assert(lambda >= 0);

        auto &capacity_map = graph.edge_capacity_map;

        capacity_map[edge_idx] += weight;
        {
            float aq = lambda * (c - a);

            if (aq > 0) {
                capacity_map[graph.edge_from_source(idx)] += aq;

            } else {
                capacity_map[graph.edge_to_sink(idx)] -= aq;
            }
        }

        {
            float aj = lambda * (d - c);
            if (aj > 0) {
                capacity_map[graph.edge_from_source(idx2)] += aj;

            } else {
                capacity_map[graph.edge_to_sink(idx2)] -= aj;
            }
        }

    }

    template<unsigned int D>
    ImageGraph<D> make_graph(const hoNDArray<uint16_t> &field_map, const hoNDArray<uint16_t> &proposed_field_map,
                             const hoNDArray<float> &residual_diff_map, const hoNDArray<float> &second_deriv) {

        const auto dims = vector_td<int,3>(field_map.get_size(0),field_map.get_size(1),field_map.get_size(2));

        vector_td<int,D> graph_dims;
        for (int i = 0; i < D; i++) graph_dims[i] = dims[i];

        ImageGraph<D> graph = ImageGraph<D>(graph_dims);

        auto &capacity_map = graph.edge_capacity_map;
        //Add regularization edges

        for (size_t kz = 0; kz < dims[2]; kz++) {
            for (size_t ky = 0; ky < dims[1]; ky++) {
                for (size_t kx = 0; kx < dims[0]; kx++) {
                    size_t idx = kz*dims[1]*dims[0]+ky * dims[0] + kx;

                    if (kx < (dims[0] - 1)) {
                        size_t idx2 = idx + 1;

                        update_regularization_edge(graph, field_map, proposed_field_map, second_deriv, idx, idx2,
                                                   graph.edge(idx, idx2).first, 1);
                    }

                    if (ky < (dims[1] - 1)) {
                        size_t idx2 = idx + dims[0];
                        update_regularization_edge(graph, field_map, proposed_field_map, second_deriv, idx, idx2,
                                                   graph.edge(idx, idx2).first, 1);
                    }

                    if (kz < (dims[2] - 1)) {
                        size_t idx2 = idx + dims[0]*dims[1];
                        update_regularization_edge(graph, field_map, proposed_field_map, second_deriv, idx, idx2,
                                                   graph.edge(idx, idx2).first, 1);
                    }

                    float residual_diff = residual_diff_map[idx];

                    if (residual_diff > 0) {
                        capacity_map[graph.edge_to_sink(idx)] += int(residual_diff);

                    } else {
                        capacity_map[graph.edge_from_source(idx)] -= int(residual_diff);
                    }

                }
            }
        }

        return graph;
    }

    template<unsigned int DIMS>
    std::vector<boost::default_color_type>
    graph_cut(const hoNDArray<uint16_t> &field_map_index, const hoNDArray<uint16_t> &proposed_field_map_index,
              const hoNDArray<float> &lambda_map, const hoNDArray<float> &residual_diff_map) {

        ImageGraph<DIMS> graph = make_graph<DIMS>(field_map_index, proposed_field_map_index, residual_diff_map,
                                                  lambda_map);

        float flow = boost::boykov_kolmogorov_max_flow(graph, graph.source_vertex, graph.sink_vertex);

        return std::move(graph.color_map);
    }

}
namespace Gadgetron {

    hoNDArray<uint16_t>
    update_field_map(const hoNDArray<uint16_t> &field_map_index, const hoNDArray<uint16_t> &proposed_field_map_index,
                     const hoNDArray<float> &residuals_map, const hoNDArray<float> &lambda_map) {

        hoNDArray<float> residual_diff_map(field_map_index.dimensions());
        const auto X = field_map_index.get_size(0);
        const auto Y = field_map_index.get_size(1);
        const auto Z = field_map_index.get_size(2);

        for (size_t kz = 0; kz < Z; kz++) {
            for (size_t ky = 0; ky < Y; ky++) {
                for (size_t kx = 0; kx < X; kx++) {
                    residual_diff_map(kx, ky,kz) = residuals_map(field_map_index(kx, ky,kz), kx, ky,kz) -
                                                residuals_map(proposed_field_map_index(kx, ky,kz), kx, ky,kz);

                }
            }
        }

        std::vector<boost::default_color_type> color_map;
        if (Z == 1) {
            color_map = graph_cut<2>(field_map_index, proposed_field_map_index, lambda_map,
                                     residual_diff_map);
        } else {
            color_map = graph_cut<3>(field_map_index, proposed_field_map_index, lambda_map, residual_diff_map);
        }

        auto result = field_map_index;
        size_t updated_voxels = 0;
        for (size_t i = 0; i < field_map_index.get_number_of_elements(); i++) {
            if (color_map[i] != boost::default_color_type::black_color) {
                updated_voxels++;
                result[i] = proposed_field_map_index[i];
            }
        }

        return result;

    }

}