#pragma once

/**
 *  Classical sign domain.
 *
 *  This is actually the extended sign domain defined in "Tutorial on
 *  Static Inference of Numeric Invariants by Abstract Interpretation"
 *  by A. Mine 2017.
 */

#include <crab/domains/abstract_domain.hpp>
#include <crab/domains/backward_assign_operations.hpp>
#include <crab/domains/interval.hpp>
#include <crab/domains/separate_domains.hpp>
#include <crab/support/stats.hpp>

#include <boost/optional.hpp>

#include <type_traits>

namespace crab {
namespace domains {

enum class sign_interval {
  BOT, // empty interval
  LTZ, // [-oo,0)
  GTZ, // (0, +oo]
  EQZ, // [0,0]
  NEZ, // [-oo,0) U (0, +oo]
  GEZ, // [0, +oo]
  LEZ, // [-oo,0]
  TOP, // [-oo,+oo]
       /*
                TOP
               / | \
              /  |  \
            LEZ NEZ  GEZ
             |\ /  \/|
             |/ EQZ \|
            LTZ  |  GTZ
              \  |  /
               \ | /
                BOT
        */
};

template <typename Number> class sign {
  static_assert(std::is_same<Number, ikos::z_number>::value,
                "Class sign only defined over ikos::z_number");

  using sign_t = sign<Number>;
  using interval_t = ikos::interval<Number>;
  using bound_t = ikos::bound<Number>;
  sign_interval m_sign;

  constexpr Number get_zero() const { return Number(0); }
  constexpr Number get_plus_one() const { return Number(1); }
  constexpr Number get_minus_one() const { return Number(-1); }

  sign_t shiftOp(const sign_t &o) const {
    // The shift operation is shl, lshr, or ashr
    // zero is special
    if (is_bottom() || o.is_bottom()) {
      return bottom();
    } else {
      if (equal_zero() || o.equal_zero()) {
        return *this;
      } else {
        return top();
      }
    }
  }

  sign_t defaultOp(const sign_t &o) const {
    if (is_bottom() || o.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

  explicit sign(sign_interval s) : m_sign(s) {}

public:
  explicit sign(bool is_bottom)
      : m_sign(is_bottom ? sign_interval::BOT : sign_interval::TOP) {}

  explicit sign(Number c) : m_sign(sign_interval::TOP) {
    Number zero(get_zero());
    if (c == zero) {
      m_sign = sign_interval::EQZ;
    } else if (c < zero) {
      m_sign = sign_interval::LTZ;
    } else /* (c > zero) */ {
      m_sign = sign_interval::GTZ;
    }
  }

  /* MODIFY HERE if not integers */
  sign_t from_interval(const interval_t &i) const {
    Number zero(get_zero());
    Number plus_one(get_plus_one());
    Number minus_one(get_minus_one());
    if (i.is_bottom()) {
      return sign_t::bottom();
    } else if (i.is_top()) {
      return sign_t::top();
    } else if (i <= interval_t(zero, zero)) {
      return sign_t(sign_interval::EQZ);
    } else if (i <= interval_t(bound_t::minus_infinity(), minus_one)) {
      return sign_t(sign_interval::LTZ);
    } else if (i <= interval_t(bound_t::minus_infinity(), zero)) {
      return sign_t(sign_interval::LEZ);
    } else if (i <= interval_t(plus_one, bound_t::plus_infinity())) {
      return sign_t(sign_interval::GTZ);
    } else if (i <= interval_t(zero, bound_t::plus_infinity())) {
      return sign_t(sign_interval::GEZ);
    } else {
      // unreachable
      return sign_t::top();
    }
  }

  /* MODIFY HERE if not integers */
  interval_t to_interval() const {
    Number zero(get_zero());
    Number plus_one(get_plus_one());
    Number minus_one(get_minus_one());
    if (is_bottom()) {
      return interval_t::bottom();
    } else if (is_top()) {
      return interval_t::top();
    } else if (equal_zero()) {
      return interval_t(zero);
    } else if (less_than_zero()) {
      return interval_t(bound_t::minus_infinity(), minus_one);
    } else if (greater_than_zero()) {
      return interval_t(plus_one, bound_t::plus_infinity());
    } else if (less_or_equal_than_zero()) {
      return interval_t(bound_t::minus_infinity(), zero);
    } else if (greater_or_equal_than_zero()) {
      return interval_t(zero, bound_t::plus_infinity());
    } else /* not_equal_zero*/ {
      // we cannot express not_equal_zero in an interval
      return interval_t::top();
    }
  }

  static sign_t bottom() { return sign(true); }

  static sign_t top() { return sign(false); }

  static sign_t mk_equal_zero() { return sign_t(sign_interval::EQZ); }

  static sign_t mk_less_than_zero() { return sign_t(sign_interval::LTZ); }

  static sign_t mk_greater_than_zero() { return sign_t(sign_interval::GTZ); }

  static sign_t mk_less_or_equal_than_zero() {
    return sign_t(sign_interval::LEZ);
  }

  static sign_t mk_greater_or_equal_than_zero() {
    return sign_t(sign_interval::GEZ);
  }

  static sign_t mk_not_equal_zero() { return sign_t(sign_interval::NEZ); }

  bool is_bottom() const { return m_sign == sign_interval::BOT; }

  bool is_top() const { return m_sign == sign_interval::TOP; }

  bool equal_zero() const { return m_sign == sign_interval::EQZ; }

  bool less_than_zero() const { return m_sign == sign_interval::LTZ; }

  bool greater_than_zero() const { return m_sign == sign_interval::GTZ; }

  bool less_or_equal_than_zero() const { return m_sign == sign_interval::LEZ; }

  bool greater_or_equal_than_zero() const {
    return m_sign == sign_interval::GEZ;
  }

  bool not_equal_zero() const { return m_sign == sign_interval::NEZ; }

  bool operator<=(const sign_t &o) const {
    if (is_bottom() || o.is_top()) {
      return true;
    } else if (o.is_bottom() || is_top()) {
      return false;
    } else {
      // operands are not either top or bottom
      if (m_sign == sign_interval::LTZ) {
        return o.m_sign == m_sign || o.m_sign == sign_interval::LEZ ||
               o.m_sign == sign_interval::NEZ;
      } else if (m_sign == sign_interval::GTZ) {
        return o.m_sign == m_sign || o.m_sign == sign_interval::GEZ ||
               o.m_sign == sign_interval::NEZ;
      } else if (m_sign == sign_interval::EQZ) {
        return o.m_sign == m_sign || o.m_sign == sign_interval::LEZ ||
               o.m_sign == sign_interval::GEZ;
      } else {
        /* m_sign == sign_interval::LEZ || m_sign == sign_interval::GEZ m_sign
         * == sign_interval::NEZ */
        return m_sign == o.m_sign;
      }
    }
  }

  bool operator==(const sign_t &o) const { return (m_sign == o.m_sign); }

  sign_t operator|(const sign_t &o) const {
    if (is_bottom() || o.is_top()) {
      return o;
    } else if (is_top() || o.is_bottom()) {
      return *this;
    } else {
      // operands are not either top or bottom
      if (m_sign == sign_interval::LTZ) {
        switch (o.m_sign) {
        case sign_interval::LTZ:
          return *this;
        case sign_interval::GTZ:
        case sign_interval::NEZ:
          return sign_t(sign_interval::NEZ);
        case sign_interval::EQZ:
        case sign_interval::LEZ:
          return sign_t(sign_interval::LEZ);
        case sign_interval::GEZ:
          return sign_t(sign_interval::TOP);
        default:
          CRAB_ERROR("sign::operator| unreachable 1");
        }
      } else if (m_sign == sign_interval::GTZ) {
        switch (o.m_sign) {
        case sign_interval::GTZ:
          return *this;
        case sign_interval::LTZ:
        case sign_interval::NEZ:
          return sign_t(sign_interval::NEZ);
        case sign_interval::EQZ:
        case sign_interval::GEZ:
          return sign_t(sign_interval::GEZ);
        case sign_interval::LEZ:
          return sign_t(sign_interval::TOP);
        default:
          CRAB_ERROR("sign::operator| unreachable 2");
        }
      } else if (m_sign == sign_interval::EQZ) {
        switch (o.m_sign) {
        case sign_interval::LTZ:
        case sign_interval::LEZ:
          return sign_t(sign_interval::LEZ);
        case sign_interval::GTZ:
        case sign_interval::GEZ:
          return sign_t(sign_interval::GEZ);
        case sign_interval::EQZ:
          return *this;
        case sign_interval::NEZ:
          return top();
        default:
          CRAB_ERROR("sign::operator| unreachable 3");
        }
      } else if (m_sign == sign_interval::LEZ) {
        switch (o.m_sign) {
        case sign_interval::LTZ:
        case sign_interval::EQZ:
        case sign_interval::LEZ:
          return *this;
        case sign_interval::GTZ:
        case sign_interval::NEZ:
        case sign_interval::GEZ:
          return top();
        default:
          CRAB_ERROR("sign::operator| unreachable 4");
        }
      } else if (m_sign == sign_interval::NEZ) {
        switch (o.m_sign) {
        case sign_interval::LTZ:
        case sign_interval::GTZ:
        case sign_interval::NEZ:
          return *this;
        case sign_interval::EQZ:
        case sign_interval::LEZ:
        case sign_interval::GEZ:
          return top();
        default:
          CRAB_ERROR("sign::operator| unreachable 5");
        }
      } else if (m_sign == sign_interval::GEZ) {
        switch (o.m_sign) {
        case sign_interval::EQZ:
        case sign_interval::GTZ:
        case sign_interval::GEZ:
          return *this;
        case sign_interval::LTZ:
        case sign_interval::LEZ:
        case sign_interval::NEZ:
          return top();
        default:
          CRAB_ERROR("sign::operator| unreachable 6");
        }
      }
    }
    CRAB_ERROR("sign::operator| unreachable 7");
  }

  sign_t operator&(const sign_t &o) const {
    if (is_bottom() || o.is_top())
      return *this;
    else if (is_top() || o.is_bottom()) {
      return o;
    } else {
      // operands are not either top or bottom
      if (m_sign == sign_interval::LTZ) {
        switch (o.m_sign) {
        case sign_interval::LTZ:
          return *this;
        case sign_interval::GTZ:
        case sign_interval::EQZ:
        case sign_interval::GEZ:
          return bottom();
        case sign_interval::LEZ:
        case sign_interval::NEZ:
          return sign_t(sign_interval::LTZ);
        default:
          CRAB_ERROR("sign::operator& unreachable 1");
        }
      } else if (m_sign == sign_interval::GTZ) {
        switch (o.m_sign) {
        case sign_interval::GTZ:
          return *this;
        case sign_interval::LTZ:
        case sign_interval::EQZ:
        case sign_interval::LEZ:
          return bottom();
        case sign_interval::GEZ:
        case sign_interval::NEZ:
          return sign_t(sign_interval::GTZ);
        default:
          CRAB_ERROR("sign::operator& unreachable 2");
        }
      } else if (m_sign == sign_interval::EQZ) {
        switch (o.m_sign) {
        case sign_interval::LTZ:
        case sign_interval::GTZ:
        case sign_interval::NEZ:
          return bottom();
        case sign_interval::GEZ:
        case sign_interval::LEZ:
        case sign_interval::EQZ:
          return *this;
        default:
          CRAB_ERROR("sign::operator& unreachable 3");
        }
      } else if (m_sign == sign_interval::LEZ) {
        switch (o.m_sign) {
        case sign_interval::LTZ:
        case sign_interval::EQZ:
        case sign_interval::LEZ:
          return o;
        case sign_interval::NEZ:
          return sign_t(sign_interval::LTZ);
        case sign_interval::GTZ:
          return bottom();
        case sign_interval::GEZ:
          return sign_t(sign_interval::EQZ);
        default:
          CRAB_ERROR("sign::operator& unreachable 4");
        }
      } else if (m_sign == sign_interval::GEZ) {
        switch (o.m_sign) {
        case sign_interval::EQZ:
        case sign_interval::GTZ:
        case sign_interval::GEZ:
          return o;
        case sign_interval::LTZ:
          return bottom();
        case sign_interval::LEZ:
          return sign_t(sign_interval::EQZ);
        case sign_interval::NEZ:
          return sign_t(sign_interval::GTZ);
        default:
          CRAB_ERROR("sign::operator& unreachable 5");
        }
      } else if (m_sign == sign_interval::NEZ) {
        switch (o.m_sign) {
        case sign_interval::LTZ:
        case sign_interval::GTZ:
        case sign_interval::NEZ:
          return o;
        case sign_interval::LEZ:
          return sign_t(sign_interval::LTZ);
        case sign_interval::GEZ:
          return sign_t(sign_interval::GTZ);
        case sign_interval::EQZ:
          return bottom();
        default:
          CRAB_ERROR("sign::operator& unreachable 6");
        }
      }
    }
    CRAB_ERROR("sign::operator& unreachable 7");
  }

  // addition
  sign_t operator+(const sign_t &o) const {
    if (is_bottom() || o.is_bottom()) {
      return bottom();
    } else if (is_top() || o.is_top()) {
      return top();
    } else {
      // operands are not either top or bottom
      if (m_sign == sign_interval::LTZ) {
        switch (o.m_sign) {
        case sign_interval::LTZ:
        case sign_interval::EQZ:
        case sign_interval::LEZ:
          return *this;
        case sign_interval::GTZ:
        case sign_interval::NEZ:
        case sign_interval::GEZ:
          return top();
        default:
          CRAB_ERROR("sign::operator+ unreachable 1");
        }
      } else if (m_sign == sign_interval::GTZ) {
        switch (o.m_sign) {
        case sign_interval::GTZ:
        case sign_interval::EQZ:
        case sign_interval::GEZ:
          return *this;
        case sign_interval::LTZ:
        case sign_interval::LEZ:
        case sign_interval::NEZ:
          return top();
        default:
          CRAB_ERROR("sign::operator+ unreachable 2");
        }
      } else if (m_sign == sign_interval::LEZ) {
        switch (o.m_sign) {
        case sign_interval::LTZ:
        case sign_interval::EQZ:
        case sign_interval::LEZ:
          return *this;
        case sign_interval::GTZ:
        case sign_interval::NEZ:
        case sign_interval::GEZ:
          return top();
        default:
          CRAB_ERROR("sign::operator+ unreachable 2");
        }
      } else if (m_sign == sign_interval::GEZ) {
        switch (o.m_sign) {
        case sign_interval::GTZ:
        case sign_interval::GEZ:
        case sign_interval::EQZ:
          return *this;
        case sign_interval::LTZ:
        case sign_interval::LEZ:
        case sign_interval::NEZ:
          return top();
        default:
          CRAB_ERROR("sign::operator+ unreachable 2");
        }
      } else if (m_sign == sign_interval::EQZ) {
        return o;
      } else /* (m_sign == sign_interval::NEZ)*/ {
        return top();
      }
    }
  }

  // subtraction
  sign_t operator-(const sign_t &o) const {
    if (is_bottom() || o.is_bottom()) {
      return bottom();
    } else if (is_top() || o.is_top()) {
      return top();
    } else {
      switch (o.m_sign) {
      case sign_interval::GTZ:
        return *this + sign_t(sign_interval::LTZ);
      case sign_interval::GEZ:
        return *this + sign_t(sign_interval::LEZ);
      case sign_interval::EQZ:
        return *this;
      case sign_interval::LTZ:
        return *this + sign_t(sign_interval::GTZ);
      case sign_interval::LEZ:
        return *this + sign_t(sign_interval::GEZ);
      default: /*sign_interval::NEZ*/
        return top();
      }
    }
  }

  // multiplication
  sign_t operator*(const sign_t &o) const {
    if (is_bottom() || o.is_bottom()) {
      return bottom();
    } else if (equal_zero() || o.equal_zero()) {
      return sign_t(sign_interval::EQZ);
    } else if (is_top() || o.is_top()) {
      return top();
    } else if (not_equal_zero() || o.not_equal_zero()) {
      return top();
    } else {
      // operands are not either top, bottom, zero, or non-zero
      if (m_sign == sign_interval::LTZ) {
        switch (o.m_sign) {
        case sign_interval::LTZ:
          return sign_t(sign_interval::GTZ);
        case sign_interval::LEZ:
          return sign_t(sign_interval::GEZ);
        case sign_interval::GTZ:
          return sign_t(sign_interval::LTZ);
        case sign_interval::GEZ:
          return sign_t(sign_interval::LEZ);
        default:
          CRAB_ERROR("sign::operator* unreachable 1");
        }
      } else if (m_sign == sign_interval::GTZ) {
        return o;
      } else if (m_sign == sign_interval::LEZ) {
        switch (o.m_sign) {
        case sign_interval::LTZ:
        case sign_interval::LEZ:
          return sign_t(sign_interval::GEZ);
        case sign_interval::GTZ:
        case sign_interval::GEZ:
          return sign_t(sign_interval::LEZ);
        default:
          CRAB_ERROR("sign::operator* unreachable 2");
        }
      } else if (m_sign == sign_interval::GEZ) {
        switch (o.m_sign) {
        case sign_interval::GTZ:
        case sign_interval::GEZ:
          return sign_t(sign_interval::GEZ);
        case sign_interval::LTZ:
        case sign_interval::LEZ:
          return sign_t(sign_interval::LEZ);
        default:
          CRAB_ERROR("sign::operator* unreachable 3");
        }
      }
    }
    CRAB_ERROR("sign::operator* unreachable 4");
  }

  // signed division
  sign_t operator/(const sign_t &o) const {
    if (is_bottom() || o.is_bottom()) {
      return bottom();
    } else if (o.equal_zero()) {
      return bottom();
    } else if (equal_zero()) {
      return *this;
    } else if (is_top() || o.is_top()) {
      return top();
    } else if (not_equal_zero() || o.not_equal_zero()) {
      return top();
    } else {
      // Once we exclude top, bottom, zero, and non-zero
      // signed division is like multiplication
      return (*this) * o;
    }
  }

  // division and remainder operations

  sign_t UDiv(const sign_t &o) const { return defaultOp(o); }

  sign_t SRem(const sign_t &o) const { return defaultOp(o); }

  sign_t URem(const sign_t &o) const { return defaultOp(o); }

  // bitwise operations
  sign_t And(const sign_t &o) const {
    // zero is special
    if (is_bottom() || o.is_bottom()) {
      return bottom();
    } else {
      if (equal_zero() || o.equal_zero()) {
        return sign_t(sign_interval::EQZ);
      } else {
        return top();
      }
    }
  }

  sign_t Or(const sign_t &o) const {
    // zero is special
    if (is_bottom() || o.is_bottom()) {
      return bottom();
    } else {
      if (equal_zero()) {
        return o;
      } else if (o.equal_zero()) {
        return *this;
      } else {
        return top();
      }
    }
  }

  sign_t Xor(const sign_t &o) const { return Or(o); }

  sign_t Shl(const sign_t &o) const { return shiftOp(o); }

  sign_t LShr(const sign_t &o) const { return shiftOp(o); }

  sign_t AShr(const sign_t &o) const { return shiftOp(o); }

  void write(crab::crab_os &o) const {
    switch (m_sign) {
    case sign_interval::BOT:
      o << "_|_";
      return;
    case sign_interval::LTZ:
      o << "[-oo,-1]";
      return;
    case sign_interval::GTZ:
      o << "[1,+oo]";
      return;
    case sign_interval::EQZ:
      o << "[0,0]";
      return;
    case sign_interval::NEZ:
      o << "[-oo,-1] U [1,+oo]";
      return;
    case sign_interval::GEZ:
      o << "[0,+oo]";
      return;
    case sign_interval::LEZ:
      o << "[-oo,0]";
      return;
    default: /*sign_interval::TOP*/
      o << "[-oo,+oo]";
    }
  }

  friend inline crab_os &operator<<(crab_os &o, const sign_t &c) {
    c.write(o);
    return o;
  }
};

template <typename Number, typename VariableName>
class sign_domain final : public crab::domains::abstract_domain_api<
                              sign_domain<Number, VariableName>> {
public:
  using sign_domain_t = sign_domain<Number, VariableName>;
  using abstract_domain_t = crab::domains::abstract_domain_api<sign_domain_t>;
  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;
  using sign_t = sign<Number>;
  using number_t = Number;
  using varname_t = VariableName;

private:
  using interval_domain_t = ikos::interval_domain<number_t, varname_t>;
  using separate_domain_t = ikos::separate_domain<variable_t, sign_t>;

private:
  separate_domain_t m_env;

  sign_domain(separate_domain_t &&env) : m_env(std::move(env)) {}

  void solve_constraints(const linear_constraint_system_t &csts) {

    // Solve < and > constraints
    auto solve_strict_inequality = [this](const variable_t &v, sign_t rhs, bool is_less_than) {
          sign_t lhs = m_env[v];
	  CRAB_LOG("sign-domain",
		   crab::outs() << v << "=" << lhs;
		   if (is_less_than) {
		     crab::outs() << " < ";
		   } else {
		     crab::outs() << " > ";
		   }
		   crab::outs() << rhs << "\n";);

	  if (rhs.equal_zero()) {
	    m_env.set(v, m_env[v] & (is_less_than ?
				     sign_t::mk_less_than_zero():
				     sign_t::mk_greater_than_zero()));
	    CRAB_LOG("sign-domain",
		     crab::outs() << "\t" << "Refined " << v  << "=" << m_env[v] << "\n";);
	    if (is_bottom()) return;
	  } else {
	    if (!is_less_than) {
	      std::swap(lhs, rhs);
	    }
	    if ((lhs.greater_than_zero() || lhs.greater_or_equal_than_zero()) &&
		(rhs.less_than_zero() || rhs.less_or_equal_than_zero())) {
	      set_to_bottom();
	      CRAB_LOG("sign-domain",
		       crab::outs() << "\t" << "Refined _|_\n";);
	      return;
	    }
	  }};

    // Solve <= and >= constraints
    auto solve_inequality = [this](const variable_t &v, sign_t rhs, bool is_less_equal) {
          sign_t lhs = m_env[v];
	  CRAB_LOG("sign-domain",
		   crab::outs() << v << "=" << lhs;
		   if (is_less_equal) {
		     crab::outs() << " <= ";
		   } else {
		     crab::outs() << " >= ";
		   }
		   crab::outs() << rhs << "\n";);

	  if (rhs.equal_zero()) {
	    m_env.set(v, m_env[v] & (is_less_equal ?
				     sign_t::mk_less_or_equal_than_zero():
				     sign_t::mk_greater_or_equal_than_zero()));
	    CRAB_LOG("sign-domain",
		     crab::outs() << "\t" << "Refined " << v  << "=" << m_env[v] << "\n";);
	    if (is_bottom()) return;
	  } else {
	    if (!is_less_equal) {
	      std::swap(lhs, rhs);
	    }
	    if (lhs.greater_or_equal_than_zero() && rhs.less_than_zero()) {
	      set_to_bottom();
	      CRAB_LOG("sign-domain",
		       crab::outs() << "\t" << "Refined _|_\n";);
	      return;
	    }
	    if (lhs.greater_than_zero() && (rhs.less_than_zero() || rhs.less_or_equal_than_zero())) {
	      set_to_bottom();
	      CRAB_LOG("sign-domain",
		       crab::outs() << "\t" << "Refined _|_\n";);
	      return;
	    }
	  }};

    if (!is_bottom()) {
      for (auto const &c : csts) {
        if (c.is_inequality() && c.is_unsigned()) {
          continue;
        }
        if (c.is_tautology()) {
          continue;
        }
        if (c.is_contradiction()) {
          set_to_bottom();
          return;
        }

	std::vector<std::pair<variable_t, sign_t>> less_than, less_equal;
	std::vector<std::pair<variable_t, sign_t>> greater_than, greater_equal;
	std::vector<std::pair<variable_t, sign_t>> equal, not_equal;

        extract_sign_constraints(c, less_than, less_equal,
				 greater_than, greater_equal, equal, not_equal);

	CRAB_LOG("sign-domain",
		 crab::outs() << "Adding " << c << "\n";);

	for (auto kv: less_than) {
	  solve_strict_inequality(kv.first, kv.second, true /*less_than*/);
	}
	for (auto kv: greater_than) {
	  solve_strict_inequality(kv.first, kv.second, false /*greater_than*/);
	}

	for (auto kv: less_equal) {
	  solve_inequality(kv.first, kv.second, true /*less_equal_than*/);
	}

	for (auto kv: greater_equal) {
	  solve_inequality(kv.first, kv.second, false /*greater_equal_than*/);
	}

	for (auto kv: equal) {
          sign_t lhs = m_env[kv.first];
          sign_t rhs = kv.second;
	  CRAB_LOG("sign-domain",
		   crab::outs() << kv.first << "=" << lhs << " == " << rhs << "\n";);
	  m_env.set(kv.first, m_env[kv.first] & rhs);
	  CRAB_LOG("sign-domain",
		   crab::outs() << "\t" << "Refined " << kv.first  << "=" << m_env[kv.first] << "\n";);
	  if (is_bottom()) return;
	}

	for (auto kv: not_equal) {
          sign_t lhs = m_env[kv.first];
          sign_t rhs = kv.second;
	  CRAB_LOG("sign-domain",
		   crab::outs() << kv.first << "=" << lhs << " != " << rhs << "\n";);
	  if (rhs.equal_zero()) {
	    m_env.set(kv.first,
		      m_env[kv.first] & sign_t::mk_not_equal_zero());
	    CRAB_LOG("sign-domain",
		     crab::outs() << "\t" << "Refined " << kv.first  << "=" << m_env[kv.first] << "\n";);
	    if (is_bottom()) return;
	  } else if (rhs.not_equal_zero()) {
	    m_env.set(kv.first, m_env[kv.first] & sign_t::mk_equal_zero());
	    CRAB_LOG("sign-domain",
		     crab::outs() << "\t" << "Refined " << kv.first  << "=" << m_env[kv.first] << "\n";);
	    if (is_bottom()) return;
	  }
	}
      }
    }
  }

  // Extract constraints of the form v OP sign where OP = {<, <=, >, >=, ==, != }
  void
  extract_sign_constraints(const linear_constraint_t &c,
			   std::vector<std::pair<variable_t, sign_t>> &less_than,
                           std::vector<std::pair<variable_t, sign_t>> &less_equal,
			   std::vector<std::pair<variable_t, sign_t>> &greater_than,
                           std::vector<std::pair<variable_t, sign_t>> &greater_equal,
			   std::vector<std::pair<variable_t, sign_t>> &equal,
			   std::vector<std::pair<variable_t, sign_t>> &not_equal) {
    auto e = c.expression();
    for (auto kv : e) {
      variable_t pivot = kv.second;
      sign_t res = compute_residual(e, pivot) / sign_t(kv.first);
      if (res.is_bottom()) {
	// this shouldn't happen
	continue;
      }
      if (!res.is_top()) {
	if (c.is_strict_inequality()) {
	  if (kv.first < number_t(0)) {
	    greater_than.push_back({pivot,res});
	  } else {
	    less_than.push_back({pivot,res});
	  }
	} else if (c.is_inequality()) {
	  if (kv.first < number_t(0)) {
	    greater_equal.push_back({pivot,res});
	  } else {
	    less_equal.push_back({pivot,res});
	  }
	} else if (c.is_equality()) {
	  equal.push_back({pivot,res});
	} else if (c.is_disequation()) {
	  not_equal.push_back({pivot,res});
	}
      }
    }
  }

  sign_t compute_residual(const linear_expression_t &e, variable_t pivot) {
    sign_t residual(-e.constant());
    for (auto kv : e) {
      const variable_t &v = kv.second;
      if (v.index() != pivot.index()) {
        residual = residual - (sign_t(kv.first) * m_env[v]);
      }
    }
    return residual;
  }

  sign_t eval_expr(const linear_expression_t &expr) const {
    if (is_bottom())
      return sign_t::bottom();

    sign_t r(expr.constant());
    for (auto kv : expr) {
      sign_t c(kv.first);
      r = r + (c * m_env[kv.second]);
    }
    return r;
  }

public:
  sign_domain_t make_top() const override {
    return sign_domain_t(separate_domain_t::top());
  }

  sign_domain_t make_bottom() const override {
    return sign_domain_t(separate_domain_t::bottom());
  }

  void set_to_top() override {
    sign_domain abs(separate_domain_t::top());
    std::swap(*this, abs);
  }

  void set_to_bottom() override {
    sign_domain abs(separate_domain_t::bottom());
    std::swap(*this, abs);
  }

  sign_t get_sign(const variable_t &v) const {
    return m_env[v];
  }

  void set_sign(const variable_t &v, sign_t s) {
    m_env.set(v, s);
  }

  sign_domain() : m_env(separate_domain_t::top()) {}

  sign_domain(const sign_domain_t &e) : m_env(e.m_env) {
    crab::CrabStats::count(domain_name() + ".count.copy");
    crab::ScopedCrabStats __st__(domain_name() + ".copy");
  }

  sign_domain(sign_domain_t &&e) : m_env(std::move(e.m_env)) {}

  sign_domain_t &operator=(const sign_domain_t &o) {
    crab::CrabStats::count(domain_name() + ".count.copy");
    crab::ScopedCrabStats __st__(domain_name() + ".copy");
    if (this != &o) {
      m_env = o.m_env;
    }
    return *this;
  }

  sign_domain_t &operator=(sign_domain_t &&o) {
    if (this != &o) {
      m_env = std::move(o.m_env);
    }
    return *this;
  }

  bool is_bottom() const override { return m_env.is_bottom(); }

  bool is_top() const override { return m_env.is_top(); }

  bool operator<=(const sign_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.leq");
    crab::ScopedCrabStats __st__(domain_name() + ".leq");
    return (m_env <= o.m_env);
  }

  void operator|=(const sign_domain_t &o) override {
    crab::CrabStats::count(domain_name() + ".count.join");
    crab::ScopedCrabStats __st__(domain_name() + ".join");
    CRAB_LOG("sign-domain",
             crab::outs() << "Join " << m_env << " and " << o.m_env << "\n";);
    m_env = m_env | o.m_env;
    CRAB_LOG("sign-domain", crab::outs() << "Res=" << m_env << "\n";);
  }

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

  sign_domain_t operator&(const sign_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.meet");
    crab::ScopedCrabStats __st__(domain_name() + ".meet");
    return (m_env & o.m_env);
  }

  sign_domain_t operator||(const sign_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.widening");
    crab::ScopedCrabStats __st__(domain_name() + ".widening");
    return (m_env | o.m_env);
  }

  sign_domain_t widening_thresholds(
      const sign_domain_t &o,
      const crab::iterators::thresholds<number_t> &ts) const override {
    crab::CrabStats::count(domain_name() + ".count.widening");
    crab::ScopedCrabStats __st__(domain_name() + ".widening");
    return (m_env | o.m_env);
  }

  sign_domain_t operator&&(const sign_domain_t &o) const override {
    crab::CrabStats::count(domain_name() + ".count.narrowing");
    crab::ScopedCrabStats __st__(domain_name() + ".narrowing");
    return (m_env & o.m_env);
  }

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

  interval_t operator[](const variable_t &v) override {
    return m_env[v].to_interval();
  }

  void operator+=(const linear_constraint_system_t &csts) override {
    crab::CrabStats::count(domain_name() + ".count.add_constraints");
    crab::ScopedCrabStats __st__(domain_name() + ".add_constraints");
    solve_constraints(csts);
  }

  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");
    CRAB_LOG("sign-domain",
	     crab::outs() << x << " := " << e << "\n";);
    if (boost::optional<variable_t> v = e.get_variable()) {
      m_env.set(x, m_env[(*v)]);
    } else {
      m_env.set(x, eval_expr(e));
    }
    CRAB_LOG("sign-domain",
	     crab::outs() << "RES=" << m_env[x] << "\n";);
  }

  void apply(crab::domains::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");

    sign_t yi = m_env[y];
    sign_t zi = m_env[z];
    sign_t xi = sign_t::bottom();

    switch (op) {
    case crab::domains::OP_ADDITION:
      xi = yi + zi;
      break;
    case crab::domains::OP_SUBTRACTION:
      xi = yi - zi;
      break;
    case crab::domains::OP_MULTIPLICATION:
      xi = yi * zi;
      break;
    case crab::domains::OP_SDIV:
      xi = yi / zi;
      break;
    case crab::domains::OP_UDIV:
      xi = yi.UDiv(zi);
      break;
    case crab::domains::OP_SREM:
      xi = yi.SRem(zi);
      break;
    case crab::domains::OP_UREM:
      xi = yi.URem(zi);
      break;
    default:
      CRAB_ERROR("Operation ", op, " not supported");
    }
    m_env.set(x, xi);
  }

  void apply(crab::domains::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");

    sign_t yi = m_env[y];
    sign_t zi(k);
    sign_t xi = sign_t::bottom();

    switch (op) {
    case crab::domains::OP_ADDITION:
      xi = yi + zi;
      break;
    case crab::domains::OP_SUBTRACTION:
      xi = yi - zi;
      break;
    case crab::domains::OP_MULTIPLICATION:
      xi = yi * zi;
      break;
    case crab::domains::OP_SDIV:
      xi = yi / zi;
      break;
    case crab::domains::OP_UDIV:
      xi = yi.UDiv(zi);
      break;
    case crab::domains::OP_SREM:
      xi = yi.SRem(zi);
      break;
    case crab::domains::OP_UREM:
      xi = yi.URem(zi);
      break;
    default:
      CRAB_ERROR("Operation ", op, " not supported");
    }
    m_env.set(x, xi);
  }

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

  // backward arithmetic operations
  void backward_assign(const variable_t &x, const linear_expression_t &e,
                       const sign_domain_t &inv) override {
    crab::CrabStats::count(domain_name() + ".count.backward_assign");
    crab::ScopedCrabStats __st__(domain_name() + ".backward_assign");
    // TODO
  }

  void backward_apply(crab::domains::arith_operation_t op, const variable_t &x,
                      const variable_t &y, number_t z,
                      const sign_domain_t &inv) override {
    crab::CrabStats::count(domain_name() + ".count.backward_apply");
    crab::ScopedCrabStats __st__(domain_name() + ".backward_apply");
    // TODO
  }

  void backward_apply(crab::domains::arith_operation_t op, const variable_t &x,
                      const variable_t &y, const variable_t &z,
                      const sign_domain_t &inv) override {
    crab::CrabStats::count(domain_name() + ".count.backward_apply");
    crab::ScopedCrabStats __st__(domain_name() + ".backward_apply");
    // TODO
  }

  // cast operations
  void apply(crab::domains::int_conv_operation_t /*op*/, const variable_t &dst,
             const variable_t &src) override {
    // ignore the widths
    assign(dst, src);
  }

  // bitwise operations
  void apply(crab::domains::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");

    sign_t yi = m_env[y];
    sign_t zi = m_env[z];
    sign_t xi = sign_t::bottom();

    switch (op) {
    case crab::domains::OP_AND: {
      xi = yi.And(zi);
      break;
    }
    case crab::domains::OP_OR: {
      xi = yi.Or(zi);
      break;
    }
    case crab::domains::OP_XOR: {
      xi = yi.Xor(zi);
      break;
    }
    case crab::domains::OP_SHL: {
      xi = yi.Shl(zi);
      break;
    }
    case crab::domains::OP_LSHR: {
      xi = yi.LShr(zi);
      break;
    }
    case crab::domains::OP_ASHR: {
      xi = yi.AShr(zi);
      break;
    }
    default:
      CRAB_ERROR("unreachable");
    }
    m_env.set(x, xi);
  }

  void apply(crab::domains::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");

    sign_t yi = m_env[y];
    sign_t zi(k);
    sign_t xi = sign_t::bottom();

    switch (op) {
    case crab::domains::OP_AND: {
      xi = yi.And(zi);
      break;
    }
    case crab::domains::OP_OR: {
      xi = yi.Or(zi);
      break;
    }
    case crab::domains::OP_XOR: {
      xi = yi.Xor(zi);
      break;
    }
    case crab::domains::OP_SHL: {
      xi = yi.Shl(zi);
      break;
    }
    case crab::domains::OP_LSHR: {
      xi = yi.LShr(zi);
      break;
    }
    case crab::domains::OP_ASHR: {
      xi = yi.AShr(zi);
      break;
    }
    default:
      CRAB_ERROR("unreachable");
    }
    m_env.set(x, xi);
  }

  virtual void select(const variable_t &lhs, const linear_constraint_t &cond,
                      const linear_expression_t &e1,
                      const linear_expression_t &e2) override {
    crab::CrabStats::count(domain_name() + ".count.select");
    crab::ScopedCrabStats __st__(domain_name() + ".select");

    if (!is_bottom()) {
      sign_domain_t inv1(*this);
      inv1 += cond;
      if (inv1.is_bottom()) {
        assign(lhs, e2);
        return;
      }

      sign_domain_t inv2(*this);
      inv2 += cond.negate();
      if (inv2.is_bottom()) {
        assign(lhs, e1);
        return;
      }

      m_env.set(lhs, eval_expr(e1) | eval_expr(e2));
    }
  }

  /// sign_domain implements only standard abstract operations of
  /// a numerical domain so it is intended to be used as a leaf domain
  /// in the hierarchy of domains.
  BOOL_OPERATIONS_NOT_IMPLEMENTED(sign_domain_t)
  ARRAY_OPERATIONS_NOT_IMPLEMENTED(sign_domain_t)
  REGION_AND_REFERENCE_OPERATIONS_NOT_IMPLEMENTED(sign_domain_t)

  void forget(const variable_vector_t &variables) override {
    if (is_bottom() || is_top()) {
      return;
    }
    for (auto const &var : variables) {
      this->operator-=(var);
    }
  }

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

    m_env.project(variables);
  }

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

    m_env.rename(from, to);
  }

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

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

    m_env.set(new_x, m_env[x]);
  }

  void normalize() override {}

  void minimize() override {}

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

    m_env.write(o);
  }

  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 (this->is_bottom()) {
      csts += linear_constraint_t::get_false();
      return csts;
    }

    for (auto it = m_env.begin(); it != m_env.end(); ++it) {
      const variable_t &v = it->first;
      const sign_t &s = it->second;
      if (s.equal_zero()) {
        csts += linear_constraint_t(v == number_t(0));
      } else if (s.less_than_zero()) {
        csts += linear_constraint_t(v < number_t(0));
      } else if (s.greater_than_zero()) {
        csts += linear_constraint_t(v > number_t(0));
      } else if (s.less_or_equal_than_zero()) {
        csts += linear_constraint_t(v <= number_t(0));
      } else if (s.greater_or_equal_than_zero()) {
        csts += linear_constraint_t(v >= number_t(0));
      } else {
        // we cannot represent not_equal_zero as a linear constraint
        continue;
      }
    }
    return csts;
  }

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

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

}; // class sign_domain
} // namespace domains
} // namespace crab

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

} // namespace domains
} // namespace crab


/*
 * Copyright (c) 2011 Seiya Tokui <beam.web@gmail.com>
 * Copyright (c) 2014 Burkhard Ritter <burkhard@ualberta.ca>
 * This code is distributed under the MIT license.
 *
 * A very simple example program demonstrating how to use Arpaca.
 *
 * To compile this program:
 * g++ \
 *    -I [/path/to/eigen] \
 *    really_simple_example.cpp \
 *    -L [/path/to/libarpack.a] \
 *    -larpack \
 *    -o really_simple_example
 *
 * This assumes that arpaca.hpp is in the same directory.
 */

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include "arpaca.hpp"
using namespace Eigen;
using namespace arpaca;

int main()
{
    // Matrix dimension
    const int n_dim = 4;
    // Number of eigenvalues to compute
    const int n_ev = 3;
    // Which eigenvalues to compute
    const EigenvalueType type = ALGEBRAIC_SMALLEST;

    // A self-adjoint random dense matrix of size n_dim x n_dim and the
    // corresponding sparse matrix
    MatrixXd dm = MatrixXd::Random(n_dim,n_dim).selfadjointView<Upper>();
    SparseMatrix<double> sm = dm.sparseView();

    // Solve for the eigenvalues
    SymmetricEigenSolver<double> s = Solve(sm, n_ev, type);

    // Output
    std::cout << "Matrix: " << std::endl << std::endl
              << dm << std::endl << std::endl << std::endl
              << "Its " << n_ev << " smallest eigenvalues: "
              << std::endl << std::endl
              << s.eigenvalues() << std::endl << std::endl
              << "And the corresponding eigenvectos: "
              << std::endl << std::endl
              << s.eigenvectors() << std::endl;
}


#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix_sparse.hpp>
#include <boost/numeric/bindings/mumps/mumps_driver.hpp>
#include <iostream>
#include <fstream>
#include <complex>

template <typename T>
int test(double eps)
{
  namespace ublas = ::boost::numeric::ublas ;
  namespace mumps = ::boost::numeric::bindings::mumps ;

  int const n = 10 ;

  typedef ublas::coordinate_matrix<T, ublas::column_major, 1, ublas::unbounded_array<int> > coo_type ;

  coo_type coo(n, n, n + 6) ;

  for(int i=0; i<n; ++i) coo(i,i) = i+1.0 ;
  coo(2,3) = T(1.0) ;
  coo(2,4) = T(1.0) ;
  coo(5,6) = T(-1.0) ;
  coo(2,6) = T(1.0) ;
  coo(9,0) = T(1.0) ;
  coo(2,7) = T(-1.0) ;

  coo.sort() ;
  std::cout << "matrix " << coo << std::endl ;

  ublas::vector<T> v(10) ;
  ublas::vector<T> w(10) ;

  std::fill(w.begin(), w.end(), 1.0) ;

  for(int i=1; i<n; ++i)
  {
    w(i) += w(i-1) ;
  }

  for(int i=0; i<n; ++i)
  {
    v[i] = T(coo(i,i)) * w[i] ;
  }
  v[2] += T(coo(2,3)) * w[3] ;
  v[2] += T(coo(2,4)) * w[4] ;
  v[5] += T(coo(5,6)) * w[6] ;
  v[2] += T(coo(2,6)) * w[6] ;
  v[9] += T(coo(9,0)) * w[0] ;
  v[2] += T(coo(2,7)) * w[7] ;
  std::cout << "rhs : " << v << std::endl ;

  mumps::mumps< coo_type > mumps_coo ;

  mumps_coo.icntl[2]=mumps_coo.icntl[3] = 0 ;

  // Analysis
  mumps_coo.job = 1 ;
  matrix_integer_data(mumps_coo, coo) ;
  driver(mumps_coo) ;

  // Factorization
  mumps_coo.job = 2 ;
  matrix_value_data(mumps_coo, coo) ;
  driver(mumps_coo) ;

  // Solve
  mumps_coo.job = 3 ;
  rhs_sol_value_data(mumps_coo, v) ;
  driver(mumps_coo) ;

  std::cout << "w : " << w << std::endl ;
  std::cout << "v : " << v << std::endl ;

  if(norm_2(v - w) > eps * norm_2(v)) return 1 ;

  return 0 ;
}

int main()
{
  if(test<float>(1e-5)) return 1 ;
  if(test<double>(1e-10)) return 2 ;
  if(test< std::complex<float> >(1e-5)) return 3 ;
  if(test< std::complex<double> >(1e-10)) return 4 ;
  return 0 ;
}


// Implement SparseLibrary requirements using Eigen
#include <Eigen/Sparse>
#include <Eigen/SparseQR>
#include <Eigen/SparseLU>

struct EigenShim {
    using value_t = double;
    using triplet_t = Eigen::Triplet<value_t>;

    template<typename Value>
    struct sparse_wrapper_t {
        using wrapped_t = Eigen::SparseMatrix<Value>;
        using index_t = typename wrapped_t::StorageIndex;
        template<typename Iter>     // or use ForwardIterator concept
        sparse_wrapper_t(index_t rows, index_t cols, Iter a, Iter b) : mat_(rows, cols) {
            mat_.setFromTriplets(a, b);
        }
        sparse_wrapper_t(wrapped_t mat) : mat_(std::move(mat)) {}

        // define the product of two sparse wrappers
        friend sparse_wrapper_t operator*(sparse_wrapper_t const& a, sparse_wrapper_t const& b) {
            return Eigen::SparseMatrix<value_t>(a.wrapped() * b.wrapped());
        }

        friend std::ostream & operator<<(std::ostream& os, sparse_wrapper_t const& m) {
            using namespace Eigen;
            IOFormat OctaveFmt(FullPrecision, 0, ", ", ";\n", "", "", "[", "]");

            Matrix<value_t, Dynamic, Dynamic> dense = m.wrapped();   // convert to dense

            os << dense.format(OctaveFmt);
            return os;
        }

        wrapped_t const & wrapped() const { return mat_; }

    private:

        wrapped_t mat_;
    };

    using sparsemat_t = sparse_wrapper_t<value_t>;
    using index_t = sparsemat_t::index_t;

    template<typename Value, typename Index>
    struct lu_wrapper_t {
        using wrapped_t = Eigen::SparseLU<Eigen::SparseMatrix<Value>, Eigen::COLAMDOrdering<Index>>;

        lu_wrapper_t( sparsemat_t const & mat ) : lu_(mat.wrapped()) {
            assert(lu_.info() == Eigen::Success);
        }

        sparse_wrapper_t<Value> solve( sparsemat_t const & rhs ) const {
            return sparse_wrapper_t<Value>(lu_.solve(rhs.wrapped()));
        }

    private:
        wrapped_t lu_;
    };

    template<typename Value, typename Index>
    struct qr_wrapper_t {
        using wrapped_t = Eigen::SparseQR<Eigen::SparseMatrix<Value>, Eigen::COLAMDOrdering<Index>>;

        qr_wrapper_t( sparsemat_t const & mat ) : qr_(mat.wrapped()) {}

        sparsemat_t Q() const {
            using namespace Eigen;
            // Sadly Eigen cannot directly return the Q as a sparse matrix
            // What it *can* do is multiply times a dense matrix
            Matrix<value_t, Dynamic, Dynamic> identity(qr_.rows(), qr_.rank());
            identity.setIdentity();

            Matrix<value_t, Dynamic, Dynamic> result = qr_.matrixQ() * identity;

            // finally, convert to sparse
            return SparseMatrix<value_t>(result.sparseView());
        }

    private:
        wrapped_t qr_;
    };

    using lu_t = lu_wrapper_t<value_t, index_t>;
    using qr_t = qr_wrapper_t<value_t, index_t>;

};


#include <vector>
#include <random>
#include <gtest/gtest.h>
#include <boost/multiprecision/gmp.hpp>
using namespace std;
namespace bm = boost::multiprecision;

template<typename F>
void digit_dc_inner(const vector<int> &num, int base, vector<int> &cur, int d, F &f) { // enumerate all [1, num] with same length
    if (d == (int)num.size()) {
        f(num, base, (int)num.size());
        return;
    }
    for (int i = 0; i < num[d]; ++i) {
        if (d == 0 && i == 0) continue;

        cur.push_back(i);
        f(cur, base, (int)num.size());
        cur.pop_back();
    }

    cur.push_back(num[d]);
    digit_dc_inner(num, base, cur, d + 1, f);
    cur.pop_back();
}

template<typename F>
void digit_dc(const vector<int> &num, int base, F &f) { // enumerate all [1, num]
    // shorter than num
    for (int len = 1; len < (int)num.size(); ++len) {
        for (int i = 1; i < base; ++i) {
            vector<int> cur = {i};
            f(cur, base, len);
        }
    }
    // the same length with num
    vector<int> cur;
    digit_dc_inner<F>(num, base, cur, 0, f);
}

std::mt19937 gen;
std::uniform_int_distribution<> dist_base(2, 20);
std::uniform_int_distribution<> dist_length(1, 100);

vector<int> random_number(int length, int base) {
    std::uniform_int_distribution<> dist0(1, base - 1);
    std::uniform_int_distribution<> dist(0, base - 1);

    vector<int> v;
    for (int i = 0; i < length; ++i) {
        if (i == 0) v.push_back(dist0(gen));
        else v.push_back(dist(gen));
    }
    return v;
}
bm::mpz_int to_mpz(vector<int> num, int base) {
    bm::mpz_int z = 0;
    for (int i = 0; i < (int)num.size(); ++i)
        z = z * base + num[i];
    return z;
}

TEST(Random, Print) {
    struct printer {
        void operator()(const vector<int> &v, int base, int length) {
            for (int i = 0; i < (int)v.size(); ++i) {
                printf("%d", v[i]);
            }
            for (int i = (int)v.size(); i < length; ++i)
                printf("X");
            printf("\n");
        }
    };
    int base = 10;
    static const int TEST_SIZE = 3;

    for (int test_case = 0; test_case < TEST_SIZE; ++test_case) {
        int length = std::uniform_int_distribution<>(1, 10)(gen);
        vector<int> num = random_number(length, base);

        printer p;
        digit_dc(num, base, p);
    }
}

TEST(Random, Count) {
    struct counter {
        bm::mpz_int cnt = 0;
        void operator()(const vector<int> &v, int base, int length) {
            cnt += pow(bm::mpz_int(base), length - (int)v.size());
        }
    };

    static const int TEST_SIZE = 500;
    for (int test_case = 0; test_case < TEST_SIZE; ++test_case) {
        int base = dist_base(gen);
        int length = dist_length(gen);
        vector<int> num = random_number(length, base);

        bm::mpz_int expected = to_mpz(num, base);

        counter c;
        digit_dc(num, base, c);
        bm::mpz_int answer = c.cnt;

        EXPECT_EQ(expected, answer);
    }
}
TEST(Random, Sum) {
    struct summer {
        bm::mpz_int cnt = 0;
        void operator()(const vector<int> &v, int base, int length) {
            int left = length - (int)v.size();
            bm::mpz_int p = pow(bm::mpz_int(base), left);
            cnt += to_mpz(v, base) * p * p;
            cnt += p * (p - 1) / 2;
        }
    };

    static const int TEST_SIZE = 100;
    for (int test_case = 0; test_case < TEST_SIZE; ++test_case) {
        int base = dist_base(gen);
        int length = dist_length(gen);
        vector<int> num = random_number(length, base);
        bm::mpz_int z = to_mpz(num, base);
        bm::mpz_int expected = z * (z + 1) / 2;

        summer s;
        digit_dc(num, base, s);
        bm::mpz_int answer = s.cnt;

        EXPECT_EQ(expected, answer);
    }
}


// Copyright (C) 2011  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <dlib/optimization.h>
#include <dlib/rand.h>

#include "tester.h"

namespace
{
    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.find_max_factor_graph_viterbi");

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

    dlib::rand rnd;

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

    template <
        unsigned long O,
        unsigned long NS,
        unsigned long num_nodes,
        bool all_negative
        >
    class map_problem
    {
    public:
        unsigned long order() const { return O; }
        unsigned long num_states() const { return NS; }

        map_problem()
        {
            data = randm(number_of_nodes(),(long)std::pow(num_states(),(double)order()+1), rnd);
            if (all_negative)
                data = -data;
        }

        unsigned long number_of_nodes (
        ) const
        {
            return num_nodes;
        }

        template <
            typename EXP
            >
        double factor_value (
            unsigned long node_id,
            const matrix_exp<EXP>& node_states
        ) const
        {
            if (node_states.size() == 1)
                return data(node_id, node_states(0));
            else if (node_states.size() == 2)
                return data(node_id, node_states(0) + node_states(1)*NS);
            else if (node_states.size() == 3)
                return data(node_id, (node_states(0) + node_states(1)*NS)*NS + node_states(2));
            else
                return data(node_id, ((node_states(0) + node_states(1)*NS)*NS + node_states(2))*NS + node_states(3));
        }

        matrix<double> data;
    };

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

    template <
        typename map_problem
        >
    void brute_force_find_max_factor_graph_viterbi (
        const map_problem& prob,
        std::vector<unsigned long>& map_assignment
    )
    {
        using namespace dlib::impl;
        const int order = prob.order();
        const int num_states = prob.num_states();

        map_assignment.resize(prob.number_of_nodes());
        double best_score = -std::numeric_limits<double>::infinity();
        matrix<unsigned long,1,0> node_states;
        node_states.set_size(prob.number_of_nodes());
        node_states = 0;
        do
        {
            double score = 0;
            for (unsigned long i = 0; i < prob.number_of_nodes(); ++i)
            {
                score += prob.factor_value(i, (colm(node_states,range(i,i-std::min<int>(order,i)))));
            }

            if (score > best_score)
            {
                for (unsigned long i = 0; i < map_assignment.size(); ++i)
                    map_assignment[i] = node_states(i);
                best_score = score;
            }

        } while(advance_state(node_states,num_states));

    }

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

    template <
        unsigned long order,
        unsigned long num_states,
        unsigned long num_nodes,
        bool all_negative
        >
    void do_test_()
    {
        dlog << LINFO << "order: "<< order
                      << "  num_states:   " << num_states
                      << "  num_nodes:    " << num_nodes
                      << "  all_negative: " << all_negative;

        for (int i = 0; i < 25; ++i)
        {
            print_spinner();
            map_problem<order,num_states,num_nodes,all_negative> prob;
            std::vector<unsigned long> assign, assign2;
            brute_force_find_max_factor_graph_viterbi(prob, assign);
            find_max_factor_graph_viterbi(prob, assign2);

            DLIB_TEST_MSG(mat(assign) == mat(assign2),
                          trans(mat(assign))
                          << trans(mat(assign2))
                          );
        }
    }

    template <
        unsigned long order,
        unsigned long num_states,
        unsigned long num_nodes
        >
    void do_test()
    {
        do_test_<order,num_states,num_nodes,false>();
    }

    template <
        unsigned long order,
        unsigned long num_states,
        unsigned long num_nodes
        >
    void do_test_negative()
    {
        do_test_<order,num_states,num_nodes,true>();
    }

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

    class test_find_max_factor_graph_viterbi : public tester
    {
    public:
        test_find_max_factor_graph_viterbi (
        ) :
            tester ("test_find_max_factor_graph_viterbi",
                    "Runs tests on the find_max_factor_graph_viterbi routine.")
        {}

        void perform_test (
        )
        {
            do_test<1,3,0>();
            do_test<1,3,1>();
            do_test<1,3,2>();
            do_test<0,3,2>();
            do_test_negative<0,3,2>();

            do_test<1,3,8>();
            do_test<2,3,7>();
            do_test_negative<2,3,7>();
            do_test<3,3,8>();
            do_test<4,3,8>();
            do_test_negative<4,3,8>();
            do_test<0,3,8>();
            do_test<4,3,1>();
            do_test<4,3,0>();

            do_test<3,2,1>();
            do_test<3,2,0>();
            do_test<3,2,2>();
            do_test<2,2,1>();
            do_test_negative<3,2,1>();
            do_test_negative<3,2,0>();
            do_test_negative<3,2,2>();
            do_test_negative<2,2,1>();

            do_test<0,3,0>();
            do_test<1,2,8>();
            do_test<2,2,7>();
            do_test<3,2,8>();
            do_test<0,2,8>();

            do_test<1,1,8>();
            do_test<2,1,8>();
            do_test<3,1,8>();
            do_test<0,1,8>();
        }
    } a;

}


/**
 * @file lstm_peephole_test.cpp
 * @author Marcus Edel
 *
 * Tests the LSTM peepholes.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/layer/lstm_layer.hpp>

#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(LSTMPeepholeTest);

/*
 * Test the peephole connections in the forward pass. The test is a modification
 * of the peephole test originally written by Tom Schaul.
 */
BOOST_AUTO_TEST_CASE(LSTMPeepholeForwardTest)
{
  double state1 = 0.2;
  double state2 = 0.345;
  double state3 = -0.135;
  double state4 = 10000;

  arma::colvec input, output;

  LSTMLayer<> hiddenLayer0(1, 6, true);

  hiddenLayer0.InGatePeepholeWeights() = arma::mat("3");
  hiddenLayer0.ForgetGatePeepholeWeights() = arma::mat("4");
  hiddenLayer0.OutGatePeepholeWeights() = arma::mat("5");

  // Set the LSTM state to state1 (state = inGateActivation * cellActivation
  // = 1 / (1 + e^(-1000)) * tanh(atanh(0.2)) = 1 * 0.2 = 0.2).
  // outputActivation = outGateActivation * stateActivation
  // = tanh((0.2)) * (1 / (1 + e^1000)) = 0.
  input << state4 << state4 << std::atanh(state1) << -state4;
  hiddenLayer0.FeedForward(input, output);
  BOOST_REQUIRE_CLOSE(output(0), 0, 1e-3);

  // Verify that the LSTM state is correctly stored.
  input.clear();
  input << -state4 << state4 << state4 << state4;
  hiddenLayer0.FeedForward(input, output);
  BOOST_REQUIRE_CLOSE(output(0), std::tanh(state1), 1e-3);

  // Add state2 to the LSTM state.
  // state = state + forgateGateActivation * state(t - 1) = 0.345 + 1 * 0.2 =
  // 0.545
  input.clear();
  input << state4 << state4 << std::atanh(state2) << state4;
  hiddenLayer0.FeedForward(input, output);
  BOOST_REQUIRE_CLOSE(output(0), std::tanh(state1 + state2), 1e-3);

  // Verify the peephole connection to the forgetgate (weight = 4) by
  // neutralizing its contibution and therefore dividing the LSTM state value
  // by 2.
  input.clear();
  input << -state4 << -(state1 + state2) * 4 << state4 << state4;
  hiddenLayer0.FeedForward(input, output);
  BOOST_REQUIRE_CLOSE(output(0), std::tanh((state1 + state2) / 2), 1e-3);

  // Verify the peephole connection to the inputgate (weight = 3) by
  // neutralizing its contibution and therefore dividing the provided input
  // by 2.
  input.clear();
  input << -(state1 + state2) / 2 * 3 << -state4 << std::atanh(state3)
        << state4;
  hiddenLayer0.FeedForward(input, output);
  BOOST_REQUIRE_CLOSE(output(0), std::tanh(state3 / 2), 1e-3);

  // Verify the peephole connection to the outputgate (weight = 5) by
  // neutralizing its contibution and therefore dividing the provided output
  // by 2.
  input.clear();
  input << -state4 << state4 << state4 << -state3 / 2 * 5;
  hiddenLayer0.FeedForward(input, output);
  BOOST_REQUIRE_CLOSE(output(0), std::tanh(state3 / 2) / 2, 1e-3);
}

BOOST_AUTO_TEST_SUITE_END();


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-ieee
    Function object implementing nextpow2 capabilities

    Returns the greatest integer n such that abs_s(x) is greater or equal to \f$2^n\f$

    @par Semantic:

    @code
    auto n = nextpow2(x);
    @endcode

    is similar to:

    @code
    auto n = floor(log2(saturated_(abs)(x)));
    @endcode

    @see floor, log2, abs, saturated
  **/
  Value nextpow2(Value const & x);
} }
#endif

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

#endif


#pragma once

#include <boost/multiprecision/gmp.hpp>

using CodeNumber = int32_t;

// The following typedefs are used for coefficients of equations
using Coeff16 = int16_t;
using Coeff32 = int32_t;
using Coeff64 = int64_t;

using Integer = boost::multiprecision::mpz_int;

using Rational = boost::multiprecision::mpq_rational;


#include <catch2/catch.hpp>
#include <iostream>
#include <Eigen/Dense>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/readSTL.h>
#include <igl/readOFF.h>
#include <methods_config.h>

using namespace Eigen;
using namespace std;

SCENARIO("icp", "[icp.h]")
{
  const Eigen::MatrixXd V = (Eigen::MatrixXd(8, 3) <<
                                                   0.0, 0.0, 0.0,
      0.0, 0.0, 1.0,
      0.0, 1.0, 0.0,
      0.0, 1.0, 1.0,
      1.0, 0.0, 0.0,
      1.0, 0.0, 1.0,
      1.0, 1.0, 0.0,
      1.0, 1.0, 1.0).finished();

  const Eigen::MatrixXi F = (Eigen::MatrixXi(12, 3) <<
                                                    1, 7, 5,
      1, 3, 7,
      1, 4, 3,
      1, 2, 4,
      3, 8, 7,
      3, 4, 8,
      5, 7, 8,
      5, 8, 6,
      1, 5, 6,
      1, 6, 2,
      2, 6, 8,
      2, 8, 4).finished().array() - 1;

  // Plot the mesh
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(V, F);
  viewer.data().set_face_based(true);
  viewer.launch();
}

SCENARIO("midsole icp", "[icp.h]")
{
  Eigen::MatrixXd VA;
  Eigen::MatrixXi FA;
  const std::string dir = SOURCE_DIR;
  igl::readOFF(dir + "/test/test_data/test_11D.off", VA, FA);

  Eigen::MatrixXd VB;
  Eigen::MatrixXi FB;
  const std::string dir2 = SOURCE_DIR;
  igl::readOFF(dir2 + "/test/test_data/test_11D_rotated.off", VB, FB);

  // merge meshes for viewer
  // Concatenate (VA,FA) and (VB,FB) into (V,F)
  Eigen::MatrixXd V(VA.rows()+VB.rows(),VA.cols());
  V<<VA,VB;
  Eigen::MatrixXi F(FA.rows()+FB.rows(),FA.cols());
  F<<FA,(FB.array()+VA.rows());

  // blue color for faces of first mesh, orange for second
  Eigen::MatrixXd C(F.rows(),3);
  C<<
   Eigen::RowVector3d(0.2,0.3,0.8).replicate(FA.rows(),1),
      Eigen::RowVector3d(1.0,0.7,0.2).replicate(FB.rows(),1);

  // Plot the mesh
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(V, F);
  viewer.data().set_colors(C);
  viewer.data().set_face_based(true);
  viewer.launch();
}


#include <Eigen/Core>
#include "drakeGeometryUtil.h"
#include "testUtil.h"
#include <iostream>
#include <cmath>

using namespace Eigen;
using namespace std;

void testExpmap2quat(const Vector4d &quat);

void testRotationConversionFunctions()
{
  int ntests = 100;
  default_random_engine generator;
  // quat2axis, axis2quat
  for (int i = 0; i < ntests; i++) {
    Vector4d q = uniformlyRandomQuat(generator);
    auto a = quat2axis(q);
    auto q_back = axis2quat(a);
    valuecheck(acos(abs(q.transpose() * q_back)), 0.0, 1e-6);
  }
  // quat2rotmat, rotmat2quat
  for (int i = 0; i < ntests; i++) {
    Vector4d q = uniformlyRandomQuat(generator);
    Matrix3d R = quat2rotmat(q);
    Vector4d q_back = rotmat2quat(R);
    valuecheck(acos(abs(q.transpose() * q_back)), 0.0, 1e-6);
  }
  // quat2rpy, rpy2quat
  for (int i = 0; i < ntests; i++) {
    Vector4d q = uniformlyRandomQuat(generator);
    Vector3d rpy = quat2rpy(q);
    Vector4d q_back = rpy2quat(rpy);
    valuecheck(acos(abs(q.transpose() * q_back)), 0.0, 1e-6);
  }
  // rotmat2axis, axis2rotmat
  for (int i = 0; i < ntests; i++) {
    Matrix3d R = uniformlyRandomRotmat(generator);
    Vector4d a = rotmat2axis(R);
    Matrix3d R_back = axis2rotmat(a);
    valuecheckMatrix(R, R_back, 1e-6);
  }
  // rotmat2rpy, rpy2rotmat
  for (int i = 0; i < ntests; i++) {
    Matrix3d R = uniformlyRandomRotmat(generator);
    Vector3d rpy = rotmat2rpy(R);
    Matrix3d R_back = rpy2rotmat(rpy);
    valuecheckMatrix(R, R_back, 1e-6);
  }
  // rpy2axis, axis2rpy
  for (int i = 0; i < ntests; i++) {
    Vector3d rpy = uniformlyRandomRPY(generator);
    Vector4d axis = rpy2axis(rpy);
    Vector3d rpy_back = axis2rpy(axis);
    valuecheckMatrix(rpy, rpy_back, 1e-6);
  }
  // expmap2quat, quat2expmap
  Vector4d quat_degenerate = Vector4d::Zero();
  quat_degenerate(0) = 1.0;
  testExpmap2quat(quat_degenerate);
  quat_degenerate(0) = -1.0;
  testExpmap2quat(quat_degenerate);
  for (int i = 0; i<ntests; i++)
  {
    Vector4d quat = uniformlyRandomQuat(generator);
    testExpmap2quat(quat);
  }
  // quat2eigenQuaternion
  Vector4d quat = uniformlyRandomQuat(generator);
  Quaterniond eigenQuat = quat2eigenQuaternion(quat);
  Matrix3d R_expected = quat2rotmat(quat);
  Matrix3d R_eigen = eigenQuat.matrix();
  valuecheckMatrix(R_expected, R_eigen, 1e-6);
}

void testDHomogTrans(int ntests) {
  Isometry3d T;
  std::default_random_engine generator;

  for (int testnr = 0; testnr < ntests; testnr++) {
    Vector4d q = uniformlyRandomQuat(generator);
    T = Quaterniond(q(0), q(1), q(2), q(3));
    //  T.setIdentity();
    //  T = AngleAxisd(M_PI_2, Vector3d(1.0, 0.0, 0.0));

    const int nv = 6;
    const int nq = 7;

    auto S = Matrix<double, 6, Dynamic>::Random(6, nv).eval();
//    setLinearIndices(S);
//    S.setIdentity();
//    std::cout << S << "\n\n";

    auto qdot_to_v = MatrixXd::Random(nv, nq).eval();
//    setLinearIndices(qdot_to_v);
//    std::cout << qdot_to_v << "\n\n";

    auto dT = dHomogTrans(T, S, qdot_to_v).eval();
    volatile auto vol = dT;
    //  std::cout << dT << std::endl << std::endl;
  }
}

void testDHomogTransInv(int ntests, bool check) {
  Isometry3d T;
  std::default_random_engine generator;
  for (int testnr = 0; testnr < ntests; testnr++) {
    Vector4d q = uniformlyRandomQuat(generator);
//    T = Quaterniond(q(0), q(1), q(2), q(3)) * Translation3d(Vector3d::Random());
    T = Quaterniond(q(0), q(1), q(2), q(3));

    const int nv = 6;
    const int nq = 7;

    auto S = Matrix<double, 6, Dynamic>::Random(6, nv).eval();
    auto qdot_to_v = MatrixXd::Random(nv, nq).eval();

    auto dT = dHomogTrans(T, S, qdot_to_v).eval();
    auto dTInv = dHomogTransInv(T, dT);
    volatile auto vol = dTInv;

    if (check) {
      auto dTInvInv = dHomogTransInv(T.inverse(), dTInv);

      if (!dT.matrix().isApprox(dTInvInv.matrix(), 1e-10)) {
        std::cout << "dTInv:\n" << dTInv << "\n\n";
        std::cout << "dT:\n" << dT << "\n\n";
        std::cout << "dTInvInv:\n" << dTInvInv << "\n\n";
        std::cout << "dTInvInv - dT:\n" << dTInvInv - dT << "\n\n";

        throw std::runtime_error("wrong");
      }
    }
  }
}

void testDTransformAdjoint(int ntests) {
  const int nv = 6;
  const int nq = 34;
  const int cols_X = 3;

  Isometry3d T;
  std::default_random_engine generator;

  for (int testnr = 0; testnr < ntests; testnr++) {
    Vector4d q = uniformlyRandomQuat(generator);
    T = Quaterniond(q(0), q(1), q(2), q(3)) * Translation3d(Vector3d::Random());
    auto S = Matrix<double, 6, Dynamic>::Random(6, nv).eval();
    auto qdot_to_v = MatrixXd::Random(nv, nq).eval();
    auto dT = dHomogTrans(T, S, qdot_to_v).eval();
    auto X = Matrix<double, 6, Dynamic>::Random(6, cols_X).eval();
    auto dX = MatrixXd::Random(X.size(), nq).eval();
//    auto dX = Matrix<double, X.SizeAtCompileTime, nq>::Random().eval();
    auto dAdT_times_X = dTransformSpatialMotion(T, X, dT, dX).eval();
    volatile auto vol = dAdT_times_X;
  }
}

void testDTransformAdjointTranspose(int ntests) {
  const int nv = 6;
  const int nq = 34;
  const int cols_X = 3;

  Isometry3d T;
  std::default_random_engine generator;

  for (int testnr = 0; testnr < ntests; testnr++) {
    Vector4d q = uniformlyRandomQuat(generator);
    T = Quaterniond(q(0), q(1), q(2), q(3)) * Translation3d(Vector3d::Random());
    auto S = Matrix<double, 6, Dynamic>::Random(6, nv).eval();
    auto qdot_to_v = MatrixXd::Random(nv, nq).eval();
    auto dT = dHomogTrans(T, S, qdot_to_v).eval();
    auto X = Matrix<double, 6, Dynamic>::Random(6, cols_X).eval();
    auto dX = MatrixXd::Random(X.size(), nq).eval();
//    auto dX = Matrix<double, X.SizeAtCompileTime, nq>::Random().eval();
    auto dAdTtranspose_times_X = dTransformSpatialForce(T, X, dT, dX).eval();
    volatile auto vol = dAdTtranspose_times_X;
  }
}

void testNormalizeVec(int ntests) {
  const int x_rows = 4;

  for (int testnr = 0; testnr < ntests; testnr++) {
    auto x = Matrix<double, x_rows, 1>::Random().eval();
    Matrix<double, x_rows, 1> x_norm;
    Matrix<double, x_rows, x_rows> dx_norm;
    Matrix<double, x_rows * x_rows, x_rows> ddx_norm;
    normalizeVec(x, x_norm, &dx_norm, &ddx_norm);
//    std::cout << "gradientNumRows: " << gradientNumRows(x_rows, x_rows, 1) << std::endl;

    volatile auto volx_norm = x_norm;
    volatile auto voldx_norm = dx_norm;
    volatile auto volddx_norm = ddx_norm;

//    std::cout << "x_norm:\n" << x_norm << std::endl << std::endl;
//    std::cout << "dx_norm:\n" << dx_norm << std::endl << std::endl;
//    std::cout << "ddx_norm:\n" << ddx_norm << std::endl << std::endl;
  }
}

void testSpatialCrossProduct()
{
  auto a = (Matrix<double, TWIST_SIZE, 1>::Random()).eval();
  auto b = (Matrix<double, TWIST_SIZE, TWIST_SIZE>::Identity()).eval();
  auto a_crm_b = crossSpatialMotion(a, b);
  auto a_crf_b = crossSpatialForce(a, b);
  valuecheckMatrix(a_crf_b, -a_crm_b.transpose(), 1e-8);
}

void testdrpy2rotmat()
{
  default_random_engine generator;
  Vector3d rpy = uniformlyRandomRPY(generator);
  Matrix3d R = rpy2rotmat(rpy);
  Matrix<double,9,3> dR = drpy2rotmat(rpy);
  Matrix<double,9,3> dR_num = Matrix<double,9,3>::Zero();
  for(int i = 0;i<3;i++)
  {
    Vector3d err = Vector3d::Zero();
    err(i) = 1e-7;
    Vector3d rpyi = rpy+err;
    Matrix3d Ri = rpy2rotmat(rpyi);
    Matrix3d Ri_err = (Ri-R)/err(i);
    for(int j = 0;j<9;j++)
    {
      dR_num(j,i) = Ri_err(j);
      valuecheck(dR(j,i),dR_num(j,i),1e-3);
    }
  }
}

void testExpmap2quat(const Vector4d &quat)
{
  auto expmap = quat2expmap(quat,1);
  auto quat_back  = expmap2quat(expmap.value(),2);
  valuecheck(std::abs(quat.transpose() * quat_back.value()), 1.0, 1e-8);
  Matrix3d expmap_back = expmap.gradient().value()*quat_back.gradient().value();
  Matrix3d identity = Matrix3d::Identity();
  valuecheckMatrix(expmap_back,identity,1E-10);
}

int main(int argc, char **argv)
{
  testRotationConversionFunctions();

  int ntests = 100000;
  std::cout << "testDHomogTrans elapsed time: " << measure<>::execution(testDHomogTrans, ntests) << std::endl;
  std::cout << "testDHomogTransInv elapsed time: " << measure<>::execution(testDHomogTransInv, ntests, false) << std::endl;
  std::cout << "testDTransformAdjoint elapsed time: " << measure<>::execution(testDTransformAdjoint, ntests) << std::endl;
  std::cout << "testDTransformAdjointTranspose elapsed time: " << measure<>::execution(testDTransformAdjointTranspose, ntests) << std::endl;
  std::cout << "testNormalizeVec elapsed time: " << measure<>::execution(testNormalizeVec, ntests) << std::endl;

  testDHomogTransInv(1000, true);
  testSpatialCrossProduct();
	testdrpy2rotmat();

  return 0;
}


#pragma once
// tools
#include <boost/tuple/tuple.hpp>
#pragma once
// mesh
#include "Triangulation.hpp"
// PDE
#include "OseenProblem.hpp"
#include "BoundaryCondition.hpp"
// FEs
#include "AbstractFiniteElement.hpp"
// for global system matrix
#include "CSlCMatrix.hpp"
#include "CSCMatrix.hpp"

namespace FEM {

	namespace Mixed {

		boost::tuple<
			CSlCMatrix<double>, // diffusion + convection + reaction matrix
			CSCMatrix<double>, CSCMatrix<double>, // divirgence matrices
			std::vector<double> // rhs vector
		>
		assembleSystem(
			OseenProblem2D const &, // (1) PDE,
			Triangulation const &, // (2) discretized domain (mesh), and BCs that connects (1) and (2):
			VectorBoundaryCondition2D const &, // natural BC,
			VectorBoundaryCondition2D const &, // essential BC;
			TriangularScalarFiniteElement const &, // for each velocity component
			TriangularScalarFiniteElement const &, // for pressure
			boost::optional<Index&> activeElementIndex = boost::none // index of the active element
		);

	}
}


//////////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::normal::derivative_log_unnormalized_pdf.hpp //
//                                                                                //
//  (C) Copyright 2009 Erwann Rogard                                                //
//  Use, modification and distribution are subject to the                           //
//  Boost Software License, Version 1.0. (See accompanying file                     //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)                //
//////////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_NORMAL_DERIVATIVE_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_NORMAL_DERIVATIVE_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#include <boost/math/distributions/normal.hpp>
#include <boost/numeric/conversion/converter.hpp>
//#include <boost/math/policies/policy.hpp>//TODO

namespace boost{
namespace math{

    template<typename T,typename P>
    T
    derivative_log_unnormalized_pdf(
        const boost::math::normal_distribution<T,P>& d,
        const T& x
    ){
        typedef boost::numeric::converter<T,int> int2real_t;
        T mu = d.location();
        T sigma = d.scale();

        T z = (x-mu)/sigma;
        T dz = int2real_t::convert(1)/sigma;
        return (- z) * dz;
    }

}// math
}// boost

#endif


/**
 * @file
 * @brief Unit tests for local element matrix builders
 * @author Tobias Rohner
 * @date January 2021
 * @copyright MIT License
 */

#include <gtest/gtest.h>
#include <lf/assemble/assemble.h>
#include <lf/fe/fe.h>
#include <lf/fe/test_utils/test_utils.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/mesh/utils/utils.h>
#include <lf/quad/quad.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <cmath>
#include <iostream>

namespace lf::fe::test {

TEST(lf_fe, diffusion_mat_test) {
  // Building the test mesh
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh();
  // Use a relatively high polynomial degree
  const unsigned degree = 15;
  const auto fe_space =
      std::make_shared<lf::fe::HierarchicScalarFESpace<double>>(mesh_p, degree);

  // The analytic solution
  const auto u = [](const Eigen::VectorXd &x) -> double {
    return std::sin(base::kPi * x[0]) * std::sin(base::kPi * x[1]);
  };
  const lf::mesh::utils::MeshFunctionGlobal mf_u(u);

  // Define the load function of the manufactured solution
  const auto load = [](const Eigen::Vector2d &x) -> double {
    return 2 * base::kPi * base::kPi * std::sin(base::kPi * x[0]) *
           std::sin(base::kPi * x[1]);
  };
  const lf::mesh::utils::MeshFunctionGlobal mf_load(load);

  // Assemble the system matrix and right hand side
  const lf::assemble::DofHandler &dofh = fe_space->LocGlobMap();
  lf::assemble::COOMatrix<double> A_COO(dofh.NumDofs(), dofh.NumDofs());
  Eigen::VectorXd rhs = Eigen::VectorXd::Zero(dofh.NumDofs());
  std::cout << "> Assembling System Matrix" << std::endl;
  const lf::mesh::utils::MeshFunctionConstant<double> mf_alpha(1);
  lf::fe::DiffusionElementMatrixProvider element_matrix_provider(fe_space,
                                                                 mf_alpha);
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, element_matrix_provider,
                                      A_COO);
  std::cout << "> Assembling right Hand Side" << std::endl;
  lf::fe::ScalarLoadElementVectorProvider element_vector_provider(fe_space,
                                                                  mf_load);
  lf::assemble::AssembleVectorLocally(0, dofh, element_vector_provider, rhs);

  // Enforce zero dirichlet boundary conditions
  std::cout << "> Enforcing Boundary Conditions" << std::endl;
  const auto boundary = lf::mesh::utils::flagEntitiesOnBoundary(mesh_p);
  const auto selector = [&](unsigned int idx) -> std::pair<bool, double> {
    const auto &entity = dofh.Entity(idx);
    return {entity.Codim() > 0 && boundary(entity), 0};
  };
  lf::assemble::FixFlaggedSolutionComponents(selector, A_COO, rhs);

  // Solve the LSE using the cholesky decomposition
  std::cout << "> Solving LSE" << std::endl;
  Eigen::SparseMatrix<double> A = A_COO.makeSparse();
  Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver(A);
  const Eigen::VectorXd solution = solver.solve(rhs);
  const lf::fe::MeshFunctionFE<double, double> mf_numeric(fe_space, solution);
  const lf::fe::MeshFunctionGradFE<double, double> mf_numeric_grad(fe_space,
                                                                   solution);

  // Compute the L2 error
  std::cout << "> Computing Error Norms" << std::endl;
  const auto qr_segment =
      lf::quad::make_QuadRule(lf::base::RefEl::kSegment(), 2 * degree - 1);
  const auto qr_tria =
      lf::quad::make_QuadRule(lf::base::RefEl::kTria(), 2 * degree - 1);
  const auto qr_quad =
      lf::quad::make_QuadRule(lf::base::RefEl::kQuad(), 2 * degree - 1);
  const auto quadrule_provider = [&](const lf::mesh::Entity &entity) {
    const lf::base::RefEl refel = entity.RefEl();
    switch (refel) {
      case lf::base::RefEl::kTria():
        return qr_tria;
      case lf::base::RefEl::kSegment():
        return qr_segment;
      case lf::base::RefEl::kQuad():
        return qr_quad;
      default:
        return lf::quad::make_QuadRule(refel, 2 * degree - 1);
    }
  };
  const double L2_err = std::sqrt(lf::fe::IntegrateMeshFunction(
      *mesh_p, lf::mesh::utils::squaredNorm(mf_u - mf_numeric),
      quadrule_provider));

  // Assert that the L2 error is small enough
  ASSERT_NEAR(L2_err, 0, 1e-5);
}

TEST(lf_fe, mass_mat_test) {
  // Building the test mesh
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh();
  // Use a relatively high polynomial degree
  const unsigned degree = 15;
  const auto fe_space =
      std::make_shared<lf::fe::HierarchicScalarFESpace<double>>(mesh_p, degree);

  // Define some right hand side
  const auto load = [](const Eigen::Vector2d &x) -> double {
    return 2 * base::kPi * base::kPi * std::sin(base::kPi * x[0]) *
           std::sin(base::kPi * x[1]);
  };
  const lf::mesh::utils::MeshFunctionGlobal mf_load(load);

  // Assemble the system matrix and right hand side
  const lf::assemble::DofHandler &dofh = fe_space->LocGlobMap();
  lf::assemble::COOMatrix<double> A_COO(dofh.NumDofs(), dofh.NumDofs());
  Eigen::VectorXd rhs = Eigen::VectorXd::Zero(dofh.NumDofs());
  std::cout << "> Assembling System Matrix" << std::endl;
  const lf::mesh::utils::MeshFunctionConstant<double> mf_gamma(1);
  lf::fe::MassElementMatrixProvider element_matrix_provider(fe_space, mf_gamma);
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, element_matrix_provider,
                                      A_COO);
  std::cout << "> Assembling right Hand Side" << std::endl;
  lf::fe::ScalarLoadElementVectorProvider element_vector_provider(fe_space,
                                                                  mf_load);
  lf::assemble::AssembleVectorLocally(0, dofh, element_vector_provider, rhs);

  // Enforce zero dirichlet boundary conditions
  std::cout << "> Enforcing Boundary Conditions" << std::endl;
  const auto boundary = lf::mesh::utils::flagEntitiesOnBoundary(mesh_p);
  const auto selector = [&](unsigned int idx) -> std::pair<bool, double> {
    const auto &entity = dofh.Entity(idx);
    return {entity.Codim() > 0 && boundary(entity), 0};
  };
  lf::assemble::FixFlaggedSolutionComponents(selector, A_COO, rhs);

  // Solve the LSE using the cholesky decomposition
  std::cout << "> Solving LSE" << std::endl;
  Eigen::SparseMatrix<double> A = A_COO.makeSparse();
  Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver(A);
  const Eigen::VectorXd solution = solver.solve(rhs);
  const lf::fe::MeshFunctionFE<double, double> mf_numeric(fe_space, solution);
  const lf::fe::MeshFunctionGradFE<double, double> mf_numeric_grad(fe_space,
                                                                   solution);

  // Compute the L2 error
  std::cout << "> Computing Error Norms" << std::endl;
  const auto qr_segment =
      lf::quad::make_QuadRule(lf::base::RefEl::kSegment(), 2 * degree - 1);
  const auto qr_tria =
      lf::quad::make_QuadRule(lf::base::RefEl::kTria(), 2 * degree - 1);
  const auto qr_quad =
      lf::quad::make_QuadRule(lf::base::RefEl::kQuad(), 2 * degree - 1);
  const auto quadrule_provider = [&](const lf::mesh::Entity &entity) {
    const lf::base::RefEl refel = entity.RefEl();
    switch (refel) {
      case lf::base::RefEl::kTria():
        return qr_tria;
      case lf::base::RefEl::kSegment():
        return qr_segment;
      case lf::base::RefEl::kQuad():
        return qr_quad;
      default:
        return lf::quad::make_QuadRule(refel, 2 * degree - 1);
    }
  };
  const double L2_err = std::sqrt(lf::fe::IntegrateMeshFunction(
      *mesh_p, lf::mesh::utils::squaredNorm(mf_load - mf_numeric),
      quadrule_provider));

  // Assert that the L2 error is small enough
  ASSERT_NEAR(L2_err, 0, 1e-5);
}

template <class SCALAR_TEST, class SCALAR_TRIAL, class EMP, class EVALUATOR>
void CheckElementMatrixProvider(const ScalarFESpace<SCALAR_TEST> &fes_test,
                                const ScalarFESpace<SCALAR_TRIAL> &fes_trial,
                                const EMP &emp, EVALUATOR &&evaluator,
                                base::dim_t codim = 0) {
  Eigen::Matrix<SCALAR_TEST, Eigen::Dynamic, 1> coeff_test(
      fes_test.LocGlobMap().NumDofs());
  Eigen::Matrix<SCALAR_TRIAL, Eigen::Dynamic, 1> coeff_trial(
      fes_trial.LocGlobMap().NumDofs());
  coeff_test.setZero();
  coeff_trial.setZero();

  using scalar_matrix_t = decltype(evaluator(coeff_test, coeff_trial));

  // assemble the matrix:
  assemble::COOMatrix<scalar_matrix_t> coo_matrix(coeff_test.rows(),
                                                  coeff_trial.rows());
  assemble::AssembleMatrixLocally(codim, fes_trial.LocGlobMap(),
                                  fes_test.LocGlobMap(), emp, coo_matrix);
  Eigen::Matrix<scalar_matrix_t, Eigen::Dynamic, Eigen::Dynamic> system_matrix =
      coo_matrix.makeDense();

  for (base::size_type i = 0; i < coeff_test.rows(); ++i) {
    coeff_test(i) = 1.;
    for (base::size_type j = 0; j < coeff_trial.rows(); ++j) {
      coeff_trial(j) = 1.;
      scalar_matrix_t result = evaluator(coeff_test, coeff_trial);
      auto entry = system_matrix(i, j);
      ASSERT_LT(std::abs(result - system_matrix(i, j)), 1e-7);
      coeff_trial(j) = 0.;
    }
    coeff_test(i) = 0;
  }
}

template <class SCALAR, class EVP, class EVALUATOR>
void CheckEntityVectorProvider(const ScalarFESpace<SCALAR> &fes_test,
                               const EVP &evp, EVALUATOR &&evaluator,
                               base::dim_t codim) {
  Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> coeff_test(
      fes_test.LocGlobMap().NumDofs());
  coeff_test.setZero();

  using scalar_vector_t = decltype(evaluator(coeff_test));

  // assemble the vector:
  Eigen::Matrix<scalar_vector_t, Eigen::Dynamic, 1> vector(coeff_test.rows());
  vector.setZero();
  assemble::AssembleVectorLocally(codim, fes_test.LocGlobMap(), evp, vector);

  for (base::size_type i = 0; i < coeff_test.rows(); ++i) {
    coeff_test(i) = 1;
    auto result = evaluator(coeff_test);
    auto entry = vector(i);
    ASSERT_LT(std::abs(result - entry), 1e-7);
    coeff_test(i) = 0;
  }
}

TEST(lf_fe, DiffusionElementMatrixProviderComplexCoeff) {
  // get a mesh of affine elements so that the element matrices can be
  // calculated exactly.
  auto mesh = lf::mesh::test_utils::GenerateHybrid2DTestMesh(5);

  //// Generate a variable order hierarchic fe space:
  auto fe_space = std::make_shared<HierarchicScalarFESpace<double>>(
      mesh, [&](const mesh::Entity &e) { return (mesh->Index(e)) % 2 + 1; });

  auto max_degree = 2;

  //// complex diffusion coefficient:
  auto alpha = [&](const mesh::Entity &e, const Eigen::MatrixXd &local) {
    return std::vector<double>(local.cols(), mesh->Index(e) + 10.);
  };

  DiffusionElementMatrixProvider emp(fe_space, alpha);

  // calculate every element of the matrix with MeshFunctions:
  auto evaluator = [&](const Eigen::VectorXd &test_coeff,
                       const Eigen::VectorXd &trial_coeff) {
    auto mf_grad_test = MeshFunctionGradFE(fe_space, test_coeff);
    auto mf_grad_trial = MeshFunctionGradFE(fe_space, trial_coeff);
    return lf::fe::IntegrateMeshFunction(
        *mesh, transpose(mf_grad_test) * (alpha * mf_grad_trial),
        2 * (max_degree))(0);
  };

  CheckElementMatrixProvider(*fe_space, *fe_space, emp, evaluator);
}

TEST(lf_fe, DiffusionElementMatrixProviderTensorCoeff) {
  // get a mesh of affine elements so that the element matrices can be
  // calculated exactly.
  auto mesh = lf::mesh::test_utils::GenerateHybrid2DTestMesh(5);
  //// Generate a variable order hierarchic fe space:
  auto fe_space = std::make_shared<HierarchicScalarFESpace<double>>(
      mesh, [&](const mesh::Entity &e) { return (mesh->Index(e)) % 2 + 1; });

  auto max_degree = 2;

  auto alpha2 = mesh::utils::MeshFunctionGlobal([](const Eigen::Vector2d &x) {
    return (Eigen::Matrix2d() << 1, x.x(), x.y(), 2).finished();
  });
  DiffusionElementMatrixProvider emp2(fe_space, alpha2);
  auto evaluator2 = [&](const Eigen::VectorXd &test_coeff,
                        const Eigen::VectorXd &trial_coeff) {
    auto mf_grad_test = MeshFunctionGradFE(fe_space, test_coeff);
    auto mf_grad_trial = MeshFunctionGradFE(fe_space, trial_coeff);
    return lf::fe::IntegrateMeshFunction(
        *mesh, transpose(mf_grad_test) * (alpha2 * mf_grad_trial),
        2 * (max_degree))(0);
  };
  CheckElementMatrixProvider(*fe_space, *fe_space, emp2, evaluator2);
}

TEST(lf_fe, DiffusionElementMatrixProviderComplexFESpace) {
  // get a mesh of affine elements so that the element matrices can be
  // calculated exactly.
  auto mesh = lf::mesh::test_utils::GenerateHybrid2DTestMesh(5);

  //// complex diffusion coefficient:
  auto alpha = [&](const mesh::Entity &e, const Eigen::MatrixXd &local) {
    return std::vector<double>(local.cols(), mesh->Index(e) + 10.);
  };

  // with a complex fe_space:
  auto fes_complex = test_utils::MakeComplexLagrangeO1FeSpace(mesh);
  DiffusionElementMatrixProvider emp3(fes_complex, alpha);
  auto evaluator3 = [&](const Eigen::VectorXcd &test_coeff,
                        const Eigen::VectorXcd &trial_coeff) {
    auto mf_grad_test = MeshFunctionGradFE(fes_complex, test_coeff);
    auto mf_grad_trial = MeshFunctionGradFE(fes_complex, trial_coeff);
    return lf::fe::IntegrateMeshFunction(
        *mesh, adjoint(mf_grad_test) * (alpha * mf_grad_trial), 2)(0);
  };
  CheckElementMatrixProvider(*fes_complex, *fes_complex, emp3, evaluator3);
}

TEST(lf_fe, MassElementMatrixProvider) {
  auto mesh = mesh::test_utils::GenerateHybrid2DTestMesh(5);

  auto g = [&](const mesh::Entity &e, const Eigen::MatrixXd &local) {
    return std::vector<double>(local.cols(), mesh->Index(e) + 10.);
  };

  auto fes = std::make_shared<HierarchicScalarFESpace<double>>(mesh, 1);
  MassElementMatrixProvider emp(fes, g);
  auto evaluator = [&](const Eigen::VectorXd &test_coeff,
                       Eigen::VectorXd &trial_coeff) {
    auto mf_test = MeshFunctionFE(fes, test_coeff);
    auto mf_trial = MeshFunctionFE(fes, trial_coeff);
    return IntegrateMeshFunction(*mesh, mf_test * mf_trial * g, 2);
  };
  CheckElementMatrixProvider(*fes, *fes, emp, evaluator);
}

TEST(lf_fe, MassElementMatrixProviderComplex) {
  auto mesh = mesh::test_utils::GenerateHybrid2DTestMesh(5);

  auto g = mesh::utils::MeshFunctionConstant(std::complex<double>{1, 2});

  auto fes = fe::test_utils::MakeComplexLagrangeO1FeSpace(mesh);
  MassElementMatrixProvider emp(fes, g);
  auto evaluator = [&](const Eigen::VectorXcd &test_coeff,
                       Eigen::VectorXcd &trial_coeff) {
    auto mf_test = MeshFunctionFE(fes, test_coeff);
    auto mf_trial = MeshFunctionFE(fes, trial_coeff);
    return IntegrateMeshFunction(*mesh, conjugate(mf_test) * mf_trial * g, 2);
  };
  CheckElementMatrixProvider(*fes, *fes, emp, evaluator);
}

TEST(lf_fe, MassEdgeMatrixProvider) {
  auto mesh = mesh::test_utils::GenerateHybrid2DTestMesh(5);

  auto g = [&](const mesh::Entity &e, const Eigen::MatrixXd &local) {
    return std::vector<double>(local.cols(), mesh->Index(e) + 10.);
  };
  auto fes = std::make_shared<HierarchicScalarFESpace<double>>(mesh, 1);
  MassEdgeMatrixProvider emp(fes, g);
  auto evaluator = [&](const Eigen::VectorXd &test_coeff,
                       const Eigen::VectorXd &trial_coeff) {
    auto mf_test = MeshFunctionFE(fes, test_coeff);
    auto mf_trial = MeshFunctionFE(fes, trial_coeff);
    return IntegrateMeshFunction(*mesh, mf_test * g * mf_trial, 2,
                                 base::PredicateTrue{}, 1);
  };
  CheckElementMatrixProvider(*fes, *fes, emp, evaluator, 1);
}

TEST(lf_fe, MassEdgeMatrixProviderComplex) {
  auto mesh = mesh::test_utils::GenerateHybrid2DTestMesh(5);

  auto g = mesh::utils::MeshFunctionConstant(std::complex<double>{1, 2});
  auto fes = test_utils::MakeComplexLagrangeO1FeSpace(mesh);
  auto selector = [&](const mesh::Entity &e) {
    LF_ASSERT_MSG(e.Codim() == 1, "This is not an edge.");
    return mesh->Index(e) < 7;
  };
  MassEdgeMatrixProvider emp(fes, g, selector);
  auto evaluator = [&](const Eigen::VectorXcd &test_coeff,
                       const Eigen::VectorXcd &trial_coeff) {
    auto mf_test = MeshFunctionFE(fes, test_coeff);
    auto mf_trial = MeshFunctionFE(fes, trial_coeff);
    return IntegrateMeshFunction(*mesh, conjugate(mf_test) * g * mf_trial, 2,
                                 selector, 1);
  };
  CheckElementMatrixProvider(*fes, *fes, emp, evaluator, 1);
}

TEST(lf_fe, ScalarLoadElementVectorProvider_complex) {
  auto mesh = mesh::test_utils::GenerateHybrid2DTestMesh(5);

  auto g = mesh::utils::MeshFunctionConstant(std::complex<double>{1, 2});
  auto fes = test_utils::MakeComplexLagrangeO1FeSpace(mesh);
  ScalarLoadElementVectorProvider evp(fes, g);
  auto evaluator = [&](const Eigen::VectorXcd &test_coeff) {
    auto mf_test = MeshFunctionFE(fes, test_coeff);
    return IntegrateMeshFunction(*mesh, conjugate(mf_test) * g, 2);
  };
  CheckEntityVectorProvider(*fes, evp, evaluator, 0);
}

TEST(lf_fe, ScalarLoadEdgeVectorProvider_complex) {
  auto mesh = mesh::test_utils::GenerateHybrid2DTestMesh(5);

  auto g = mesh::utils::MeshFunctionConstant(std::complex<double>{1, 2});
  auto fes = test_utils::MakeComplexLagrangeO1FeSpace(mesh);
  auto selector = [&](const mesh::Entity &e) {
    LF_ASSERT_MSG(e.Codim() == 1, "This is not an edge.");
    return mesh->Index(e) < 7;
  };
  ScalarLoadEdgeVectorProvider evp(fes, g, selector);
  auto evaluator = [&](const Eigen::VectorXcd &test_coeff) {
    auto mf_test = MeshFunctionFE(fes, test_coeff);
    return IntegrateMeshFunction(*mesh, conjugate(mf_test) * g, 2, selector, 1);
  };
  CheckEntityVectorProvider(*fes, evp, evaluator, 1);
}

}  // namespace lf::fe::test


/*
 * Copyright (c) Meta Platforms, Inc. and affiliates.
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */

#define _USE_MATH_DEFINES
#include <cmath>

#include <boost/math/special_functions/polygamma.hpp>

#include "beanmachine/graph/graph.h"
#include "beanmachine/graph/util.h"

namespace beanmachine {
namespace util {

// see https://core.ac.uk/download/pdf/41787448.pdf
const double PHI_APPROX_GAMMA = 1.702;

bool approx_zero(double val) {
  return std::abs(val) < graph::PRECISION;
}

bool sample_logodds(std::mt19937& gen, double logodds) {
  if (logodds < 0) {
    double wt = exp(logodds);
    std::bernoulli_distribution dist(wt / (1 + wt));
    return dist(gen);
  } else {
    double wt = exp(-logodds);
    std::bernoulli_distribution dist(wt / (1 + wt));
    return not dist(gen);
  }
}

bool sample_logprob(std::mt19937& gen, double logprob) {
  if (logprob > 0)
    return true;
  else {
    std::bernoulli_distribution dist(std::exp(logprob));
    return dist(gen);
  }
}

bool flip_coin_with_log_prob(std::mt19937& gen, double logprob) {
  return sample_logprob(gen, logprob);
}

double sample_beta(std::mt19937& gen, double a, double b) {
  std::gamma_distribution<double> distrib_a(a, 1);
  std::gamma_distribution<double> distrib_b(b, 1);
  double x = distrib_a(gen);
  double y = distrib_b(gen);
  if ((x + y) == 0.0) {
    return graph::PRECISION;
  }
  double p = x / (x + y);
  return p;
}

double logistic(double logodds) {
  return 1.0 / (1.0 + std::exp(-logodds));
}

double Phi(double x) {
  return 0.5 * (1 + std::erf(x / M_SQRT2));
}

double Phi_approx(double x) {
  return 1.0 / (1.0 + std::exp(-PHI_APPROX_GAMMA * x));
}

double Phi_approx_inv(double z) {
  return (std::log(z) - std::log(1 - z)) / PHI_APPROX_GAMMA;
}

double log_sum_exp(const std::vector<double>& values) {
  // find the max and subtract it out
  double max = values[0];
  for (std::vector<double>::size_type idx = 1; idx < values.size(); idx++) {
    if (values[idx] > max) {
      max = values[idx];
    }
  }
  double sum = 0;
  for (auto value : values) {
    sum += std::exp(value - max);
  }
  return std::log(sum) + max;
}

double log_sum_exp(double a, double b) {
  double max_val = a > b ? a : b;
  double sum = std::exp(a - max_val) + std::exp(b - max_val);
  return std::log(sum) + max_val;
}

double polygamma(int n, double x) {
  return boost::math::polygamma(n, x);
}

double log1pexp(double x) {
  if (x <= -37) {
    return std::exp(x);
  } else if (x <= 18) {
    return std::log1p(std::exp(x));
  } else if (x <= 33.3) {
    return x + std::exp(-x);
  } else {
    return x;
  }
}

double log1mexp(double x) {
  assert(x <= 0);
  if (x < -0.693) {
    return std::log1p(-std::exp(x));
  } else {
    return std::log(-std::expm1(x));
  }
}

} // namespace util
} // namespace beanmachine