/*******************************************************************************
 *
 * A simple class for representing intervals and performing interval
 * arithmetic.
 *
 ******************************************************************************/

#pragma once

#include <boost/optional.hpp>
#include <crab/common/stats.hpp>
#include <crab/common/types.hpp>
#include <crab/domains/linear_interval_solver.hpp>
#include <crab/numbers/bignums.hpp>

namespace ikos {

template <typename Number> class bound {
public:
  typedef bound<Number> bound_t;

private:
  bool _is_infinite;
  Number _n;

private:
  bound();

  bound(bool is_infinite, Number n) : _is_infinite(is_infinite), _n(n) {
    if (is_infinite) {
      if (n > 0)
        _n = 1;
      else
        _n = -1;
    }
  }

public:
  static bound_t min(bound_t x, bound_t y) { return (x.operator<=(y) ? x : y); }

  static bound_t min(bound_t x, bound_t y, bound_t z) {
    return min(x, min(y, z));
  }

  static bound_t min(bound_t x, bound_t y, bound_t z, bound_t t) {
    return min(x, min(y, z, t));
  }

  static bound_t max(bound_t x, bound_t y) { return (x.operator<=(y) ? y : x); }

  static bound_t max(bound_t x, bound_t y, bound_t z) {
    return max(x, max(y, z));
  }

  static bound_t max(bound_t x, bound_t y, bound_t z, bound_t t) {
    return max(x, max(y, z, t));
  }

  static bound_t plus_infinity() { return bound_t(true, 1); }

  static bound_t minus_infinity() { return bound_t(true, -1); }

public:
  bound(int n) : _is_infinite(false), _n(n) {}

  bound(std::string s) : _n(1) {
    if (s == "+oo") {
      _is_infinite = true;
    } else if (s == "-oo") {
      _is_infinite = true;
      _n = -1;
    } else {
      _is_infinite = false;
      _n = Number(s);
    }
  }

  bound(Number n) : _is_infinite(false), _n(n) {}

  bound(const bound_t &o) : _is_infinite(o._is_infinite), _n(o._n) {}

  bound_t &operator=(const bound_t &o) {
    if (this != &o) {
      _is_infinite = o._is_infinite;
      _n = o._n;
    }
    return *this;
  }

  bool is_infinite() const { return _is_infinite; }

  bool is_finite() const { return !_is_infinite; }

  bool is_plus_infinity() const { return (is_infinite() && _n > 0); }

  bool is_minus_infinity() const { return (is_infinite() && _n < 0); }

  bound_t operator-() const { return bound_t(_is_infinite, -_n); }

  bound_t operator+(bound_t x) const {
    if (is_finite() && x.is_finite()) {
      return bound_t(_n + x._n);
    } else if (is_finite() && x.is_infinite()) {
      return x;
    } else if (is_infinite() && x.is_finite()) {
      return *this;
    } else if (_n == x._n) {
      return *this;
    } else {
      CRAB_ERROR("Bound: undefined operation -oo + +oo");
    }
  }

  bound_t &operator+=(bound_t x) { return operator=(operator+(x)); }

  bound_t operator-(bound_t x) const { return operator+(x.operator-()); }

  bound_t &operator-=(bound_t x) { return operator=(operator-(x)); }

  bound_t operator*(bound_t x) const {
    if (x._n == 0)
      return x;
    else if (_n == 0)
      return *this;
    else
      return bound_t(_is_infinite || x._is_infinite, _n * x._n);
  }

  bound_t &operator*=(bound_t x) { return operator=(operator*(x)); }

  bound_t operator/(bound_t x) const {
    if (x._n == 0) {
      CRAB_ERROR("Bound: division by zero");
    } else if (is_finite() && x.is_finite()) {
      return bound_t(false, _n / x._n);
    } else if (is_finite() && x.is_infinite()) {
      if (_n > 0) {
        return x;
      } else if (_n == 0) {
        return *this;
      } else {
        return x.operator-();
      }
    } else if (is_infinite() && x.is_finite()) {
      if (x._n > 0) {
        return *this;
      } else {
        return operator-();
      }
    } else {
      return bound_t(true, _n * x._n);
    }
  }

  bound_t &operator/=(bound_t x) { return operator=(operator/(x)); }

  bool operator<(bound_t x) const { return !operator>=(x); }

  bool operator>(bound_t x) const { return !operator<=(x); }

  bool operator==(bound_t x) const {
    return (_is_infinite == x._is_infinite && _n == x._n);
  }

  bool operator!=(bound_t x) const { return !operator==(x); }

  /*	operator<= and operator>= use a somewhat optimized implementation.
   *	results include up to 20% improvements in performance in the octagon
   *domain over a more naive implementation.
   */
  bool operator<=(bound_t x) const {
    if (_is_infinite xor x._is_infinite) {
      if (_is_infinite) {
        return _n < 0;
      }
      return x._n > 0;
    }
    return _n <= x._n;
  }

  bool operator>=(bound_t x) const {
    if (_is_infinite xor x._is_infinite) {
      if (_is_infinite) {
        return _n > 0;
      }
      return x._n < 0;
    }
    return _n >= x._n;
  }

  bound_t abs() const {
    if (operator>=(0)) {
      return *this;
    } else {
      return operator-();
    }
  }

  boost::optional<Number> number() const {
    if (is_infinite()) {
      return boost::optional<Number>();
    } else {
      return boost::optional<Number>(_n);
    }
  }

  void write(crab::crab_os &o) const {
    if (is_plus_infinity()) {
      o << "+oo";
    } else if (is_minus_infinity()) {
      o << "-oo";
    } else {
      o << _n;
    }
  }

}; // class bound

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

typedef bound<z_number> z_bound;
typedef bound<q_number> q_bound;

namespace bounds_impl {
// Conversion between z_bound and q_bound
// template<class B1, class B2>
// inline void convert_bounds(B1 b1, B2& b2);

inline void convert_bounds(z_bound b1, z_bound &b2) { std::swap(b1, b2); }
inline void convert_bounds(q_bound b1, q_bound &b2) { std::swap(b1, b2); }
inline void convert_bounds(z_bound b1, q_bound &b2) {
  if (b1.is_plus_infinity())
    b2 = q_bound::plus_infinity();
  else if (b1.is_minus_infinity())
    b2 = q_bound::minus_infinity();
  else
    b2 = q_bound(q_number(*b1.number()));
}
inline void convert_bounds(q_bound b1, z_bound &b2) {
  if (b1.is_plus_infinity())
    b2 = z_bound::plus_infinity();
  else if (b1.is_minus_infinity())
    b2 = z_bound::minus_infinity();
  else
    b2 = z_bound((*(b1.number())).round_to_lower());
}
} // namespace bounds_impl

template <typename Number> class interval {

public:
  typedef bound<Number> bound_t;
  typedef interval<Number> interval_t;

private:
  bound_t _lb;
  bound_t _ub;

public:
  static interval_t top() {
    return interval_t(bound_t::minus_infinity(), bound_t::plus_infinity());
  }

  static interval_t bottom() { return interval_t(); }

private:
  interval() : _lb(0), _ub(-1) {}

  static Number abs(Number x) { return x < 0 ? -x : x; }

  static Number max(Number x, Number y) { return x.operator<=(y) ? y : x; }

  static Number min(Number x, Number y) { return x.operator<(y) ? x : y; }

public:
  interval(bound_t lb, bound_t ub) : _lb(lb), _ub(ub) {
    if (lb > ub) {
      _lb = 0;
      _ub = -1;
    }
  }

  interval(bound_t b) : _lb(b), _ub(b) {
    if (b.is_infinite()) {
      _lb = 0;
      _ub = -1;
    }
  }

  interval(Number n) : _lb(n), _ub(n) {}

  interval(std::string b) : _lb(b), _ub(b) {
    if (_lb.is_infinite()) {
      _lb = 0;
      _ub = -1;
    }
  }

  interval(const interval_t &i) : _lb(i._lb), _ub(i._ub) {}

  interval_t &operator=(interval_t i) {
    _lb = i._lb;
    _ub = i._ub;
    return *this;
  }

  bound_t lb() const { return _lb; }

  bound_t ub() const { return _ub; }

  bool is_bottom() const { return (_lb > _ub); }

  bool is_top() const { return (_lb.is_infinite() && _ub.is_infinite()); }

  interval_t lower_half_line() const {
    return interval_t(bound_t::minus_infinity(), _ub);
  }

  interval_t upper_half_line() const {
    return interval_t(_lb, bound_t::plus_infinity());
  }

  bool operator==(interval_t x) const {
    if (is_bottom()) {
      return x.is_bottom();
    } else {
      return (_lb == x._lb) && (_ub == x._ub);
    }
  }

  bool operator!=(interval_t x) const { return !operator==(x); }

  bool operator<=(interval_t x) const {
    if (is_bottom()) {
      return true;
    } else if (x.is_bottom()) {
      return false;
    } else {
      return (x._lb <= _lb) && (_ub <= x._ub);
    }
  }

  interval_t operator|(interval_t x) const {
    if (is_bottom()) {
      return x;
    } else if (x.is_bottom()) {
      return *this;
    } else {
      return interval_t(bound_t::min(_lb, x._lb), bound_t::max(_ub, x._ub));
    }
  }

  interval_t operator&(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return interval_t(bound_t::max(_lb, x._lb), bound_t::min(_ub, x._ub));
    }
  }

  interval_t operator||(interval_t x) const {
    if (is_bottom()) {
      return x;
    } else if (x.is_bottom()) {
      return *this;
    } else {
      return interval_t(x._lb < _lb ? bound_t::minus_infinity() : _lb,
                        _ub < x._ub ? bound_t::plus_infinity() : _ub);
    }
  }

  template <typename Thresholds>
  interval_t widening_thresholds(interval_t x, const Thresholds &ts) {
    if (is_bottom()) {
      return x;
    } else if (x.is_bottom()) {
      return *this;
    } else {
      bound_t lb = (x._lb < _lb ? ts.get_prev(x._lb) : _lb);
      bound_t ub = (_ub < x._ub ? ts.get_next(x._ub) : _ub);
      return interval_t(lb, ub);
    }
  }

  interval_t operator&&(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return interval_t(_lb.is_infinite() && x._lb.is_finite() ? x._lb : _lb,
                        _ub.is_infinite() && x._ub.is_finite() ? x._ub : _ub);
    }
  }

  interval_t operator+(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return interval_t(_lb + x._lb, _ub + x._ub);
    }
  }

  interval_t &operator+=(interval_t x) { return operator=(operator+(x)); }

  interval_t operator-() const {
    if (is_bottom()) {
      return bottom();
    } else {
      return interval_t(-_ub, -_lb);
    }
  }

  interval_t operator-(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return interval_t(_lb - x._ub, _ub - x._lb);
    }
  }

  interval_t &operator-=(interval_t x) { return operator=(operator-(x)); }

  interval_t operator*(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      bound_t ll = _lb * x._lb;
      bound_t lu = _lb * x._ub;
      bound_t ul = _ub * x._lb;
      bound_t uu = _ub * x._ub;
      return interval_t(bound_t::min(ll, lu, ul, uu),
                        bound_t::max(ll, lu, ul, uu));
    }
  }

  interval_t &operator*=(interval_t x) { return operator=(operator*(x)); }

  interval_t operator/(interval_t x) const;

  interval_t &operator/=(interval_t x) { return operator=(operator/(x)); }

  boost::optional<Number> singleton() const {
    if (!is_bottom() && _lb == _ub) {
      return _lb.number();
    } else {
      return boost::optional<Number>();
    }
  }

  bool operator[](Number n) const {
    if (is_bottom()) {
      return false;
    } else {
      bound_t b(n);
      return (_lb <= b) && (b <= _ub);
    }
  }

  void write(crab::crab_os &o) const {
    if (is_bottom()) {
      o << "_|_";
    } else {
      o << "[" << _lb << ", " << _ub << "]";
    }
  }

  // division and remainder operations

  interval_t UDiv(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

  interval_t SRem(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

  interval_t URem(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

  // bitwise operations
  interval_t And(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

  interval_t Or(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

  interval_t Xor(interval_t x) const { return Or(x); }

  interval_t Shl(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

  interval_t LShr(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

  interval_t AShr(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

}; //  class interval

template <>
inline interval<q_number> interval<q_number>::
operator/(interval<q_number> x) const {
  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else {
    boost::optional<q_number> d = x.singleton();
    if (d && *d == 0) {
      // [_, _] / 0 = _|_
      return bottom();
    } else if (x[0]) {
      boost::optional<q_number> n = singleton();
      if (n && *n == 0) {
        // 0 / [_, _] = 0
        return interval_t(q_number(0));
      } else {
        return top();
      }
    } else {
      bound_t ll = _lb / x._lb;
      bound_t lu = _lb / x._ub;
      bound_t ul = _ub / x._lb;
      bound_t uu = _ub / x._ub;
      return interval_t(bound_t::min(ll, lu, ul, uu),
                        bound_t::max(ll, lu, ul, uu));
    }
  }
}

template <>
inline interval<z_number> interval<z_number>::
operator/(interval<z_number> x) const {
  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else {
    // Divisor is a singleton:
    //   the linear interval solver can perform many divisions where
    //   the divisor is a singleton interval. We optimize for this case.
    if (boost::optional<z_number> n = x.singleton()) {
      z_number c = *n;
      if (c == 1) {
        return *this;
      } else if (c > 0) {
        return interval_t(_lb / c, _ub / c);
      } else if (c < 0) {
        return interval_t(_ub / c, _lb / c);
      } else {
      }
    }
    // Divisor is not a singleton
    typedef interval<z_number> z_interval;
    if (x[0]) {
      z_interval l(x._lb, z_bound(-1));
      z_interval u(z_bound(1), x._ub);
      return (operator/(l) | operator/(u));
    } else if (operator[](0)) {
      z_interval l(_lb, z_bound(-1));
      z_interval u(z_bound(1), _ub);
      return ((l / x) | (u / x) | z_interval(z_number(0)));
    } else {
      // Neither the dividend nor the divisor contains 0
      z_interval a =
          (_ub < 0) ? (*this + ((x._ub < 0) ? (x + z_interval(z_number(1)))
                                            : (z_interval(z_number(1)) - x)))
                    : *this;
      bound_t ll = a._lb / x._lb;
      bound_t lu = a._lb / x._ub;
      bound_t ul = a._ub / x._lb;
      bound_t uu = a._ub / x._ub;
      return interval_t(bound_t::min(ll, lu, ul, uu),
                        bound_t::max(ll, lu, ul, uu));
    }
  }
}

template <>
inline interval<z_number> interval<z_number>::SRem(interval<z_number> x) const {
  // note that the sign of the divisor does not matter

  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else if (singleton() && x.singleton()) {
    z_number dividend = *singleton();
    z_number divisor = *x.singleton();

    if (divisor == 0) {
      return bottom();
    }

    return interval_t(dividend % divisor);
  } else if (x.ub().is_finite() && x.lb().is_finite()) {
    z_number max_divisor = max(abs(*x.lb().number()), abs(*x.ub().number()));

    if (max_divisor == 0) {
      return bottom();
    }

    if (lb() < 0) {
      if (ub() > 0) {
        return interval_t(-(max_divisor - 1), max_divisor - 1);
      } else {
        return interval_t(-(max_divisor - 1), 0);
      }
    } else {
      return interval_t(0, max_divisor - 1);
    }
  } else {
    return top();
  }
}

template <>
inline interval<z_number> interval<z_number>::URem(interval<z_number> x) const {

  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else if (singleton() && x.singleton()) {
    z_number dividend = *singleton();
    z_number divisor = *x.singleton();

    if (divisor < 0) {
      return top();
    } else if (divisor == 0) {
      return bottom();
    } else if (dividend < 0) {
      // dividend is treated as an unsigned integer.
      // we would need the size to be more precise
      return interval_t(0, divisor - 1);
    } else {
      return interval_t(dividend % divisor);
    }
  } else if (x.ub().is_finite() && x.lb().is_finite()) {
    z_number max_divisor = *x.ub().number();

    if (x.lb() < 0 || x.ub() < 0) {
      return top();
    } else if (max_divisor == 0) {
      return bottom();
    }

    return interval_t(0, max_divisor - 1);
  } else {
    return top();
  }
}

template <>
inline interval<z_number> interval<z_number>::And(interval<z_number> x) const {
  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else {
    boost::optional<z_number> left_op = singleton();
    boost::optional<z_number> right_op = x.singleton();

    if (left_op && right_op) {
      return interval_t((*left_op) & (*right_op));
    } else if (lb() >= 0 && x.lb() >= 0) {
      return interval_t(0, bound_t::min(ub(), x.ub()));
    } else {
      return top();
    }
  }
}

template <>
inline interval<z_number> interval<z_number>::Or(interval<z_number> x) const {
  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else {
    boost::optional<z_number> left_op = singleton();
    boost::optional<z_number> right_op = x.singleton();

    if (left_op && right_op) {
      return interval_t((*left_op) | (*right_op));
    } else if (lb() >= 0 && x.lb() >= 0) {
      boost::optional<z_number> left_ub = ub().number();
      boost::optional<z_number> right_ub = x.ub().number();

      if (left_ub && right_ub) {
        z_number m = (*left_ub > *right_ub ? *left_ub : *right_ub);
        return interval_t(0, m.fill_ones());
      } else {
        return interval_t(0, bound_t::plus_infinity());
      }
    } else {
      return top();
    }
  }
}

template <>
inline interval<z_number> interval<z_number>::Xor(interval<z_number> x) const {
  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else {
    boost::optional<z_number> left_op = singleton();
    boost::optional<z_number> right_op = x.singleton();

    if (left_op && right_op) {
      return interval_t((*left_op) ^ (*right_op));
    } else {
      return Or(x);
    }
  }
}

template <>
inline interval<z_number> interval<z_number>::Shl(interval<z_number> x) const {
  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else {
    if (boost::optional<z_number> shift = x.singleton()) {
      z_number k = *shift;
      if (k < 0) {
        // CRAB_ERROR("lshr shift operand cannot be negative");
        return top();
      }
      // Some crazy linux drivers generate shl instructions with
      // huge shifts.  We limit the number of times the loop is run
      // to avoid wasting too much time on it.
      if (k <= 128) {
        z_number factor = 1;
        for (int i = 0; k > i; i++) {
          factor *= 2;
        }
        return (*this) * factor;
      }
    }
    return top();
  }
}

template <>
inline interval<z_number> interval<z_number>::AShr(interval<z_number> x) const {
  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else {
    if (boost::optional<z_number> shift = x.singleton()) {
      z_number k = *shift;
      if (k < 0) {
        return top();
      }
      // Some crazy linux drivers generate ashr instructions with
      // huge shifts.  We limit the number of times the loop is run
      // to avoid wasting too much time on it.
      if (k <= 128) {
        z_number factor = 1;
        for (int i = 0; k > i; i++) {
          factor *= 2;
        }
        return (*this) / factor;
      }
    }
    return top();
  }
}

template <>
inline interval<z_number> interval<z_number>::LShr(interval<z_number> x) const {
  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else {
    if (boost::optional<z_number> shift = x.singleton()) {
      z_number k = *shift;
      if (k < 0) {
        return top();
      }
      if (lb() >= 0 && ub().is_finite()) {
        z_number lb = *this->lb().number();
        z_number ub = *this->ub().number();
        return interval<z_number>(lb >> k, ub >> k);
      }
    }
    return this->top();
  }
}

template <typename Number>
inline interval<Number> operator+(Number c, interval<Number> x) {
  return interval<Number>(c) + x;
}

template <typename Number>
inline interval<Number> operator+(interval<Number> x, Number c) {
  return x + interval<Number>(c);
}

template <typename Number>
inline interval<Number> operator*(Number c, interval<Number> x) {
  return interval<Number>(c) * x;
}

template <typename Number>
inline interval<Number> operator*(interval<Number> x, Number c) {
  return x * interval<Number>(c);
}

template <typename Number>
inline interval<Number> operator/(Number c, interval<Number> x) {
  return interval<Number>(c) / x;
}

template <typename Number>
inline interval<Number> operator/(interval<Number> x, Number c) {
  return x / interval<Number>(c);
}

template <typename Number>
inline interval<Number> operator-(Number c, interval<Number> x) {
  return interval<Number>(c) - x;
}

template <typename Number>
inline interval<Number> operator-(interval<Number> x, Number c) {
  return x - interval<Number>(c);
}

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

namespace linear_interval_solver_impl {

typedef interval<z_number> z_interval;
typedef interval<q_number> q_interval;

template <> inline z_interval trim_interval(z_interval i, z_interval j) {
  if (boost::optional<z_number> c = j.singleton()) {
    if (i.lb() == *c) {
      return z_interval(*c + 1, i.ub());
    } else if (i.ub() == *c) {
      return z_interval(i.lb(), *c - 1);
    } else {
    }
  }
  return i;
}

template <> inline q_interval trim_interval(q_interval i, q_interval /* j */) {
  // No refinement possible for disequations over rational numbers
  return i;
}

template <>
inline z_interval lower_half_line(z_interval i, bool /*is_signed*/) {
  return i.lower_half_line();
}

template <>
inline q_interval lower_half_line(q_interval i, bool /*is_signed*/) {
  return i.lower_half_line();
}

template <>
inline z_interval upper_half_line(z_interval i, bool /*is_signed*/) {
  return i.upper_half_line();
}

template <>
inline q_interval upper_half_line(q_interval i, bool /*is_signed*/) {
  return i.upper_half_line();
}
} // namespace linear_interval_solver_impl

} // namespace ikos


/**
 * @file Optimizer.cpp
 * @brief Class implementing batch and incremental nonlinear equation solvers.
 * @author David Rosen
 * @author Michael Kaess
 * @version $Id: Optimizer.cpp 6371 2012-03-29 22:22:23Z kaess $
 *
 * Copyright (C) 2009-2013 Massachusetts Institute of Technology.
 * Michael Kaess, Hordur Johannsson, David Rosen,
 * Nicholas Carlevaris-Bianco and John. J. Leonard
 *
 * This file is part of iSAM.
 *
 * iSAM is free software; you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the
 * Free Software Foundation; either version 2.1 of the License, or (at
 * your option) any later version.
 *
 * iSAM is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
 * License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with iSAM.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <Eigen/Dense>

#include "isam/Optimizer.h"
#include "isam/OptimizationInterface.h"

using namespace std;
using namespace Eigen;
#include "iostream"
namespace isam {

/* Use Powell's Dog-Leg stopping criteria for all of the batch algorithms? */
// #define USE_PDL_STOPPING_CRITERIA

void Optimizer::permute_vector(const VectorXd& v, VectorXd& p,
    const int* permutation) {
  for (int i = 0; i < v.size(); i++) {
    p(permutation[i]) = v(i);
  }
}

VectorXd Optimizer::compute_gauss_newton_step(const SparseSystem& jacobian,
    SparseSystem* R, double lambda) {
  VectorXd delta_ordered;

//   std::cout<<"come here !!"<<jacobian<<std::endl;

  _cholesky->factorize(jacobian, &delta_ordered, lambda);
  if (R != NULL) {
    _cholesky->get_R(*R);
  }

  // delta has new ordering, need to return result with default ordering
  int nrows = delta_ordered.size();
  VectorXd delta(nrows);
  permute_vector(delta_ordered, delta, _cholesky->get_order());

  return delta;
}

VectorXd Optimizer::compute_dog_leg(double alpha, const VectorXd& h_sd,
    const VectorXd& h_gn, double delta, double& gain_ratio_denominator) {
  if (h_gn.norm() <= delta) {
    gain_ratio_denominator = current_SSE_at_linpoint;
    return h_gn;
  }

  double h_sd_norm = h_sd.norm();

  if ((alpha * h_sd_norm) >= delta) {
    gain_ratio_denominator = delta * (2 * alpha * h_sd_norm - delta)
        / (2 * alpha);
    return (delta / h_sd_norm) * h_sd;
  } else {
    // complicated case: calculate intersection of trust region with
    // line between Gauss-Newton and steepest descent solutions
    VectorXd a = alpha * h_sd;
    VectorXd b = h_gn;
    double c = a.dot(b - a);
    double b_a_norm2 = (b - a).squaredNorm();
    double a_norm2 = a.squaredNorm();
    double delta2 = delta * delta;
    double sqrt_term = sqrt(c * c + b_a_norm2 * (delta2 - a_norm2));
    double beta;
    if (c <= 0) {
      beta = (-c + sqrt_term) / b_a_norm2;
    } else {
      beta = (delta2 - a_norm2) / (c + sqrt_term);
    }

    gain_ratio_denominator = .5 * alpha * (1 - beta) * (1 - beta) * h_sd_norm
        * h_sd_norm + beta * (2 - beta) * current_SSE_at_linpoint;
    return (alpha * h_sd + beta * (h_gn - alpha * h_sd));
  }
}

void Optimizer::update_trust_radius(double rho, double hdl_norm) {
  if (rho < .25) {
    Delta /= 2.0;
  }
  if (rho > .75) {
    Delta = max(Delta, 3 * hdl_norm);
  }
}

void Optimizer::relinearize(const Properties& prop) {
  // We're going to relinearize about the current estimate.
  function_system.estimate_to_linpoint();

  // prepare factorization
  SparseSystem jac = function_system.jacobian();

  // factorization and new rhs based on new linearization point will be in _R
  VectorXd h_gn = compute_gauss_newton_step(jac, &function_system._R); // modifies _R

  if (prop.method == DOG_LEG) {
    // Compute the gradient and cache it.
    gradient = mul_SparseMatrixTrans_Vector(jac, jac.rhs());

    //Get the value of the sum-of-squared errors at the current linearization point.
    current_SSE_at_linpoint = jac.rhs().squaredNorm();

    // NB: alpha's denominator will be zero iff the gradient vector is zero
    // (since J is full-rank by hypothesis).  But the gradient is zero iff
    // we're already at the minimum, so we don't actually need to to any
    // updates; just set the estimate to be the current linearization point,
    // since we're already at the minimum.

    double alpha_denominator = (jac * gradient).squaredNorm();

    if (alpha_denominator > 0) {
      double alpha_numerator = gradient.squaredNorm();
      double alpha = alpha_numerator / alpha_denominator;

      // These values will be used to update the estimate of Delta
      double F_0, F_h;

      F_0 = current_SSE_at_linpoint;

      double rho_denominator, rho;

      VectorXd h_dl;

      do {
        // We repeat the computation of the dog-leg step, shrinking the
        // trust-region radius if necessary, until we generate a sufficiently
        // small region of trust that we accept the proposed step.

        // Compute dog-leg step.
        // NOTE: Here we use -h_gn because of the weird sign change in the exmap functions.
        h_dl = compute_dog_leg(alpha, -gradient, -h_gn, Delta, rho_denominator);

        // Update the estimate.
        // NOTE:  Here we use -h_dl because of the weird sign change in the exmap functions.
        function_system.apply_exmap(-h_dl);

        // Get the value of the sum-of-squared errors at the new estimate.
        F_h = function_system.weighted_errors(ESTIMATE).squaredNorm();

        // Compute gain ratio.
        rho = (F_0 - F_h) / (rho_denominator);

        update_trust_radius(rho, h_dl.norm());
      } while (rho < 0);

      // Cache last accepted dog-leg step
      last_accepted_hdl = h_dl;
    } else {
      function_system.linpoint_to_estimate();
      last_accepted_hdl = VectorXd::Zero(gradient.size());
    }

  } else {
    // For Gauss-Newton just apply the update directly.
    function_system.apply_exmap(h_gn);
  }
}

bool Optimizer::powells_dog_leg_update(double epsilon1, double epsilon3,
    SparseSystem& jacobian, VectorXd& f_x, VectorXd& grad) {
  jacobian = function_system.jacobian();
  f_x = function_system.weighted_errors(LINPOINT);
  grad = mul_SparseMatrixTrans_Vector(jacobian, f_x);
  return (f_x.lpNorm<Eigen::Infinity>() <= epsilon3)
      || (grad.lpNorm<Eigen::Infinity>() <= epsilon1);
}

void Optimizer::augment_sparse_linear_system(SparseSystem& W,
    const Properties& prop) {
  if (prop.method == DOG_LEG) {
    // We're using the incremental version of Powell's Dog-Leg, so we need
    // to form the updated gradient.
    const VectorXd& f_new = W.rhs();

    // Augment the running count for the sum-of-squared errors at the current
    // linearization point.
    current_SSE_at_linpoint += f_new.squaredNorm();

    // Allocate the new gradient vector
    VectorXd g_new(W.num_cols());

    // Compute W^T \cdot f_new
    VectorXd increment = mul_SparseMatrixTrans_Vector(W, f_new);

    // Set g_new = (g_old 0)^T + W^T f_new.
    g_new.head(gradient.size()) = gradient + increment.head(gradient.size());
    g_new.tail(W.num_cols() - gradient.size()) = increment.tail(
        W.num_cols() - gradient.size());

    // Cache the new gradient vector
    gradient = g_new;
  }

  // Apply Givens to QR factorize the newly augmented sparse system.
  for (int i = 0; i < W.num_rows(); i++) {
    SparseVector new_row = W.get_row(i);
    function_system._R.add_row_givens(new_row, W.rhs()(i));
  }
}

void Optimizer::update_estimate(const Properties& prop) {
  // Solve for the Gauss-Newton step.
  VectorXd h_gn_reordered = function_system._R.solve();

  // permute from R-ordering to J-ordering
  VectorXd h_gn(h_gn_reordered.size());
  permute_vector(h_gn_reordered, h_gn, function_system._R.r_to_a());

  if (prop.method == GAUSS_NEWTON) {
    function_system.apply_exmap(h_gn);
  } else { //method == DOG_LEG
    // Compute alpha.  Note that since the variable ordering of the factor
    // R differs from that of the original Jacobian J,
    // we must first rearrange the ordering of the elements in the gradient.
    VectorXd reordered_gradient(gradient.size());

    // Permute from J-ordering to R-ordering
    permute_vector(gradient, reordered_gradient, function_system._R.a_to_r());

    double alpha_denominator =
        (function_system._R * reordered_gradient).squaredNorm();

    if (alpha_denominator > 0) {
      double alpha = (gradient.squaredNorm()) / alpha_denominator;

      double rho_denominator;

      VectorXd h_dl = compute_dog_leg(alpha, -gradient, -h_gn, Delta,
          rho_denominator);
      function_system.apply_exmap(-h_dl);

      // Compute the gain ratio
      double rho = (current_SSE_at_linpoint
          - function_system.weighted_errors(ESTIMATE).squaredNorm())
          / rho_denominator;

      update_trust_radius(rho, h_dl.norm());

      if (rho < 0) {
        // The proposed update actually /increased/ the value of the
        // objective function; restore the last good estimate that we had
        VectorXd restore_step(gradient.size());
        restore_step.head(last_accepted_hdl.size()) = last_accepted_hdl;
        restore_step.tail(gradient.size() - last_accepted_hdl.size()).setZero();

        function_system.apply_exmap(-restore_step);
      } else {
        // The proposed update was accepted; cache the dog-leg step used
        // to produce it.
        last_accepted_hdl = h_dl;
      }
    }

    // NOTE:  The negatives prepended to "compute_dog_leg()" and "h_gn"
    // are due to the weird sign change in the exmap functions.
  }
}

void Optimizer::gauss_newton(const Properties& prop, int* num_iterations) {
  // Batch optimization
  int num_iter = 0;

  // Set the new linearization point to be the current estimate.
  function_system.estimate_to_linpoint();

  // Compute Jacobian about current estimate.
  SparseSystem jacobian = function_system.jacobian();

  // Get the current error residual vector
  VectorXd r = function_system.weighted_errors(LINPOINT);

#ifdef USE_PDL_STOPPING_CRITERIA
  // Compute the current gradient direction vector
  VectorXd g = mul_SparseMatrixTrans_Vector(jacobian, r);
#else
  double error = r.squaredNorm();
  double error_new;
  // We haven't computed a step yet, so this initialization is to ensure
  // that we never skip over the while loop as a result of failing
  // change-in-error check.
  double error_diff = prop.epsilon_rel * error + 1;
#endif

  // Compute Gauss-Newton step h_{gn} to get to the next estimated optimizing point.
  VectorXd delta = compute_gauss_newton_step(jacobian);
  while (
  // We ALWAYS use these criteria
  ((prop.max_iterations <= 0) || (num_iter < prop.max_iterations))
      && (delta.norm() > prop.epsilon2)

#ifdef USE_PDL_STOPPING_CRITERIA
      && (r.lpNorm<Eigen::Infinity>() > prop.epsilon3)
      && (g.lpNorm<Eigen::Infinity>() > prop.epsilon1)

#else // Custom stopping criteria for GN
      && (error > prop.epsilon_abs)
      && (fabs(error_diff) > prop.epsilon_rel * error)
#endif

  ) // end while conditional
  {
    num_iter++;
    // Apply the Gauss-Newton step h_{gn} to...
    function_system.apply_exmap(delta);
    // ...set the new linearization point to be the current estimate.
    function_system.estimate_to_linpoint();
    // Relinearize about the new current estimate.
    jacobian = function_system.jacobian();
    // Compute the error residual vector at the new estimate.
    r = function_system.weighted_errors(LINPOINT);

#ifdef USE_PDL_STOPPING_CRITERIA
    g = mul_SparseMatrixTrans_Vector(jacobian, r);
#else
    // Update the error difference in errors between the previous and
    // current estimates.
    error_new = r.squaredNorm();
    error_diff = error - error_new;
    error = error_new;  // Record the absolute error at the current estimate
#endif

    // Compute Gauss-Newton step h_{gn} to get to the next estimated
    // optimizing point.
    delta = compute_gauss_newton_step(jacobian);
    if (!prop.quiet) {
      cout << "Iteration " << num_iter << ": residual ";

#ifdef USE_PDL_STOPPING_CRITERIA
      cout << r.squaredNorm();
#else
      cout << error;
#endif
      cout << endl;
    }
  }  //end while

  if (num_iterations != NULL) {
    *num_iterations = num_iter;
  }
  _cholesky->get_R(function_system._R);
}

void Optimizer::levenberg_marquardt(const Properties& prop,
    int* num_iterations) {
  int num_iter = 0;
  double lambda = prop.lm_lambda0;
  // Using linpoint as current estimate below.
  function_system.estimate_to_linpoint();

  // Get the current Jacobian at the linearization point.
  SparseSystem jacobian = function_system.jacobian();

  // Get the error residual vector at the current linearization point.
  VectorXd r = function_system.weighted_errors(LINPOINT);

  // Record the absolute sum-of-squares error (i.e., objective function value) here.
  double error = r.squaredNorm();

#ifdef USE_PDL_STOPPING_CRITERIA
  // Compute the gradient direction vector at the current linearization point
  VectorXd g = mul_SparseMatrixTrans_Vector(jacobian, r);
#endif

  double error_diff, error_new;

  // solve at J'J + lambda*diag(J'J)
  VectorXd delta = compute_gauss_newton_step(jacobian, &function_system._R,
      lambda);

  while (
  // We ALWAYS use these stopping criteria
  ((prop.max_iterations <= 0) || (num_iter < prop.max_iterations))
      && (delta.norm() > prop.epsilon2)

#ifdef USE_PDL_STOPPING_CRITERIA
      && (r.lpNorm<Eigen::Infinity>() > prop.epsilon3)
      && (g.lpNorm<Eigen::Infinity>() > prop.epsilon1)
#else
      && (error > prop.epsilon_abs)
#endif
  )  // end while conditional
  {
    num_iter++;

    // remember the last accepted linearization point
    function_system.linpoint_to_estimate();
    // Apply the delta vector DIRECTLY TO THE LINEARIZATION POINT!
    function_system.self_exmap(delta);
    error_new = function_system.weighted_errors(LINPOINT).squaredNorm();
    error_diff = error - error_new;
    // feedback
    if (!prop.quiet) {
      cout << "LM Iteration " << num_iter << ": (lambda=" << lambda << ") ";
      if (error_diff > 0.) {
        cout << "residual: " << error_new << endl;
      } else {
        cout << "rejected" << endl;
      }
    }
    // decide if acceptable
    if (error_diff > 0.) {

#ifndef USE_PDL_STOPPING_CRITERIA
      if (error_diff < prop.epsilon_rel * error) {
        break;
      }
#endif

      // Update lambda
      lambda /= prop.lm_lambda_factor;

      // Record the error at the newly-accepted estimate.
      error = error_new;

      // Relinearize around the newly-accepted estimate.
      jacobian = function_system.jacobian();

#ifdef USE_PDL_STOPPING_CRITERIA
      r = function_system.weighted_errors(LINPOINT);
      g = mul_SparseMatrixTrans_Vector(jacobian, r);
#endif
    } else {
      // reject new estimate
      lambda *= prop.lm_lambda_factor;
      // restore previous estimate
      function_system.estimate_to_linpoint();
    }

    // Compute the step for the next iteration.
    delta = compute_gauss_newton_step(jacobian, &function_system._R, lambda);

  } // end while

  if (num_iterations != NULL) {
    *num_iterations = num_iter;
  }
  // Copy current estimate contained in linpoint.
  function_system.linpoint_to_estimate();
}

void Optimizer::powells_dog_leg(int* num_iterations, double delta0,
    int max_iterations, double epsilon1, double epsilon2, double epsilon3) {
  // Batch optimization
  int num_iter = 0;
  // current estimate is used as new linearization point
  function_system.estimate_to_linpoint();

  double delta = delta0;
  SparseSystem jacobian(1, 1);
  VectorXd f_x;
  VectorXd grad;

  bool found = powells_dog_leg_update(epsilon1, epsilon3, jacobian, f_x, grad);

  double rho_denominator;

  while ((not found) && (max_iterations == 0 || num_iter < max_iterations)) {
    num_iter++;
    cout << "PDL Iteration " << num_iter << " residual: " << f_x.squaredNorm()
        << endl;
    // compute alpha
    double alpha = grad.squaredNorm() / (jacobian * grad).squaredNorm();
    // steepest descent
    VectorXd h_sd = -grad;
    // solve Gauss Newton
    VectorXd h_gn = compute_gauss_newton_step(jacobian, &function_system._R);
    // compute dog leg h_dl
    // x0 = x: remember (and return) linearization point of R
    function_system.linpoint_to_estimate();
    VectorXd h_dl = compute_dog_leg(alpha, h_sd, h_gn, delta, rho_denominator);
    // Evaluate new solution, update estimate and trust region.
    if (h_dl.norm() <= epsilon2) {
      found = true;
    } else {
      // new estimate
      // change linearization point directly (original LP saved in estimate)
      function_system.self_exmap(h_dl);
      // calculate gain ratio rho
      VectorXd f_x_new = function_system.weighted_errors(LINPOINT);
      double rho = (f_x.squaredNorm() - f_x_new.squaredNorm())
          / (rho_denominator);
      if (rho > 0) {
        // accept new estimate
        cout << "accepted" << endl;
        f_x = f_x_new;
        found = powells_dog_leg_update(epsilon1, epsilon3, jacobian, f_x, grad);
      } else {
        // reject new estimate, overwrite with last saved one
        cout << "rejected" << endl;
        function_system.estimate_to_linpoint();
      }
      if (rho > 0.75) {
        delta = max(delta, 3.0 * h_dl.norm());
      } else if (rho < 0.25) {
        delta *= 0.5;
        found = (delta <= epsilon2);
      }
    }
  }
  if (num_iterations) {
    *num_iterations = num_iter;
  }
  // Overwrite potentially rejected linearization point with last saved one
  // (could be identical if it was accepted in the last iteration).

  function_system.swap_estimates();

}

void Optimizer::batch_optimize(const Properties& prop, int* num_iterations) {

  const double delta0 = 1.0;

  switch (prop.method) {
  case GAUSS_NEWTON:
    gauss_newton(prop, num_iterations);
    break;
  case DOG_LEG:
    powells_dog_leg(num_iterations, delta0, prop.max_iterations, prop.epsilon1,
        prop.epsilon2, prop.epsilon3); // modifies x0,R
    break;
  case LEVENBERG_MARQUARDT:
    levenberg_marquardt(prop, num_iterations);
    break;
  }

}

}


#include "knowledge_of_valid_opening.h"
#include <NTL/GF2X.h>
#include <utils/utils.h>
#include <hamming/jain_commitment/jain_commitment.h>

void hamming_metric::knowledge_of_valid_opening::generate_public_key(
        public_key_t * public_key,
        const private_key_t *private_key) {

    NTL::vec_GF2 r;
    utils::generate_random_binary_matrix(
            public_key->A,
            JAIN_K,
            JAIN_L + JAIN_V);

    utils::generate_random_binary_vector(
            r,
            JAIN_L);

    hamming_metric::commitment::generate_commitment(
            public_key->commitment,
            public_key->A,
            r,
            private_key->m,
            private_key->e);
}

void hamming_metric::knowledge_of_valid_opening::generate_private_key(
        private_key_t * private_key) {

    private_key->e.kill();
    private_key->m.kill();

    utils::generate_random_binary_vector(
            private_key->m,
            JAIN_V);

    utils::generate_vector_of_weight_w(
            private_key->e,
            JAIN_K,
            W);
}

void hamming_metric::knowledge_of_valid_opening::generate_random_values(
        random_values_t *random_values) {

    ::utils::generate_random_binary_vector(
            random_values->v,
            JAIN_L + JAIN_V);

    ::utils::generate_random_binary_vector(
            random_values->f,
            JAIN_K);
}

void hamming_metric::knowledge_of_valid_opening::generate_revealed_values(
        revealed_values_t *revealed_values) {

    utils::create_permutation_matrix(
            revealed_values->P,
            JAIN_K);
}

void hamming_metric::knowledge_of_valid_opening::generate_commitments_and_responses(
        responses_t *responses,
        commitments_t *commitments,
        const random_values_t *random_values,
        const revealed_values_t *revealed_values,
        const public_key_t *public_key,
        const private_key_t *private_key) {

    ::hamming_metric::knowledge_of_valid_opening::generate_commitment_and_response_0(
            commitments->c0,
            commitments->r0,
            responses->t0,
            random_values->v,
            public_key->A,
            random_values->f);

    ::hamming_metric::knowledge_of_valid_opening::generate_commitment_and_response_1(
            commitments->c1,
            commitments->r1,
            responses->t1,
            public_key->A,
            revealed_values->P,
            random_values->f);

    ::hamming_metric::knowledge_of_valid_opening::generate_commitment_and_response_2(
            commitments->c2,
            commitments->r2,
            responses->t2,
            public_key->A,
            revealed_values->P,
            random_values->f,
            private_key->e);
}

void hamming_metric::knowledge_of_valid_opening::encode(
        NTL::vec_GF2 &out,
        const NTL::vec_GF2 &v) {

    out = utils::encode_binary_vector(v, JAIN_V);
}

void hamming_metric::knowledge_of_valid_opening::commit(
        NTL::vec_GF2 &c,
        NTL::vec_GF2 &r,
        const NTL::vec_GF2 &v,
        const NTL::mat_GF2 &A) {

    utils::generate_random_binary_vector(
            r,
            JAIN_L);

    hamming_metric::commitment::generate_commitment(
            c,
            A,
            r,
            v);
}

void hamming_metric::knowledge_of_valid_opening::generate_response_0(
        NTL::vec_GF2 &t0,
        const NTL::vec_GF2 &n,
        const NTL::mat_GF2 &A,
        const NTL::vec_GF2 &f) {

    t0 = (A * n) + f;
}

void hamming_metric::knowledge_of_valid_opening::generate_commitment_and_response_0(
        NTL::vec_GF2 &c0,
        NTL::vec_GF2 &r0,
        NTL::vec_GF2 &t0,
        const NTL::vec_GF2 &n,
        const NTL::mat_GF2 &A,
        const NTL::vec_GF2 &f) {

    generate_response_0(
            t0,
            n,
            A,
            f);

    NTL::vec_GF2 encoded;
    encode(
            encoded,
            t0);

    commit(
            c0,
            r0,
            encoded,
            A);

}

void hamming_metric::knowledge_of_valid_opening::generate_response_1(
        NTL::vec_GF2 &t1,
        const NTL::mat_GF2 &P,
        const NTL::vec_GF2 &f) {

    t1 = P * f;
}

void hamming_metric::knowledge_of_valid_opening::generate_commitment_and_response_1(
        NTL::vec_GF2 &c1,
        NTL::vec_GF2 &r1,
        NTL::vec_GF2 &t1,
        const NTL::mat_GF2 &A,
        const NTL::mat_GF2 &P,
        const NTL::vec_GF2 &f) {

    generate_response_1(
            t1,
            P,
            f);

    NTL::vec_GF2 encoded;
    encode(
            encoded,
           t1);

    commit(
            c1,
           r1,
           encoded,
           A);
}

void hamming_metric::knowledge_of_valid_opening::generate_response_2(
        NTL::vec_GF2 &t2,
        const NTL::mat_GF2 &P,
        const NTL::vec_GF2 &f,
        const NTL::vec_GF2 &e) {

    t2 = P * (f + e);
}

void hamming_metric::knowledge_of_valid_opening::generate_commitment_and_response_2(
        NTL::vec_GF2 &c2,
        NTL::vec_GF2 &r2,
        NTL::vec_GF2 &t2,
        const NTL::mat_GF2 &A,
        const NTL::mat_GF2 &P,
        const NTL::vec_GF2 &f,
        const NTL::vec_GF2 &e) {

    generate_response_2(
            t2,
            P,
            f,
            e);

    NTL::vec_GF2 encoded;
    encode(
            encoded,
           t2);

    commit(
            c2,
           r2,
           encoded,
           A);
}

int hamming_metric::knowledge_of_valid_opening::verify_0(
        const NTL::vec_GF2 &c0,
        const NTL::vec_GF2 &r0,
        const NTL::vec_GF2 &t0,
        const NTL::vec_GF2 &c1,
        const NTL::vec_GF2 &r1,
        const NTL::vec_GF2 &t1,
        const NTL::mat_GF2 &P,
        const public_key_t *public_key) {

    auto _t0 = utils::encode_binary_vector(t0, JAIN_V);
    auto _t1 = utils::encode_binary_vector(t1, JAIN_V);

    if (hamming_metric::commitment::verify(
            c0,
            public_key->A,
            r0,
            _t0) != 0) {
        std::cout << "Knowledge of valid opening. Verification failed on ch = 0 and c0" << std::endl;
        return 1;
    }

    if (hamming_metric::commitment::verify(
            c1,
            public_key->A,
            r1,
            _t1) != 0) {
        std::cout << "Knowledge of valid opening. Verification failed on ch = 0 and c1" << std::endl;
        return 1;
    }

    auto sum = t0 + (NTL::inv(P) * t1);

    NTL::vec_GF2 result;
    if(utils::solve_equation(
            result,
            public_key->A,
            sum) != 0) {
        std::cout << "No solutions" << std::endl;
        return 1;
    }

    return 0;
}

int hamming_metric::knowledge_of_valid_opening::verify_1(
        const NTL::vec_GF2 &c0,
        const NTL::vec_GF2 &r0,
        const NTL::vec_GF2 &t0,
        const NTL::vec_GF2 &c2,
        const NTL::vec_GF2 &r2,
        const NTL::vec_GF2 &t2,
        const NTL::mat_GF2 &P,
        const public_key_t *public_key) {

    auto _t0 = utils::encode_binary_vector(t0, JAIN_V);
    auto _t2 = utils::encode_binary_vector(t2, JAIN_V);

    if (hamming_metric::commitment::verify(
            c0,
            public_key->A,
            r0,
            _t0) != 0) {
        std::cout << "Knowledge of valid opening. Verification failed on ch = 1 and c0" << std::endl;
        return 1;
    }

    if (hamming_metric::commitment::verify(
            c2,
            public_key->A,
            r2,
            _t2) != 0) {
        std::cout << "Knowledge of valid opening. Verification failed on ch = 1 and c2" << std::endl;
        return 1;
    }

    auto sum = t0 + public_key->commitment + (NTL::inv(P) * t2);

    NTL::vec_GF2 result;
    if(utils::solve_equation(
            result,
            public_key->A,
            sum) != 0) {
        std::cout << "No solutions" << std::endl;
        return 1;
    }

    return 0;
}

int hamming_metric::knowledge_of_valid_opening::verify_2(
        const NTL::vec_GF2 &c1,
        const NTL::vec_GF2 &r1,
        const NTL::vec_GF2 &t1,
        const NTL::vec_GF2 &c2,
        const NTL::vec_GF2 &r2,
        const NTL::vec_GF2 &t2,
        const public_key_t *public_key) {

    auto _t1 = utils::encode_binary_vector(t1, JAIN_V);
    auto _t2 = utils::encode_binary_vector(t2, JAIN_V);

    if (hamming_metric::commitment::verify(
            c1,
            public_key->A,
            r1,
            _t1) != 0) {
        std::cout << "Knowledge of valid opening. Verification failed on ch = 2 and c1" << std::endl;
        return 1;
    }

    if (hamming_metric::commitment::verify(
            c2,
            public_key->A,
            r2,
            _t2) != 0) {
        std::cout << "Knowledge of valid opening. Verification failed on ch = 2 and c2" << std::endl;
        return 1;
    }

    if (NTL::weight(t1 + t2) != W) {
        return 1;
    }

    return 0;
}


#ifndef SPARSE_LINEAR_SOLVERS_HPP
#define SPARSE_LINEAR_SOLVERS_HPP

#include "SparseMatrix.hpp"
#include "Utils.hpp"

#ifdef USEMKL
#include "MklLayer.hpp"
#include "mkl.h"
#endif

#include <Eigen/Sparse>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <iostream>
#include <iterator>
#include <map>
#include <memory>
#include <unordered_map>
#include <vector>

namespace cask {
  namespace sparse_linear_solvers {

    class Solver {
      public:
        virtual void analyze(
            const Eigen::SparseMatrix<double> &A) {
        }

        virtual void preprocess(
            const Eigen::SparseMatrix<double> &A) {
  }

        virtual Eigen::VectorXd solve(
            const Eigen::SparseMatrix<double>& A,
            const Eigen::VectorXd& b) = 0;
    };

    class EigenSolver: public Solver {
      public:
        virtual Eigen::VectorXd solve(
            const Eigen::SparseMatrix<double>& A,
            const Eigen::VectorXd& b);
    };

    class DfeCgSolver: public Solver {
      public:
        virtual Eigen::VectorXd solve(
            const Eigen::SparseMatrix<double>& A,
            const Eigen::VectorXd& b);
    };

    class DfeBiCgSolver: public Solver {
      public:
        virtual Eigen::VectorXd solve(
            const Eigen::SparseMatrix<double>& A,
            const Eigen::VectorXd& b);
    };

// Equivalent to un-precontitioned CG
class IdentityPreconditioner {
 public:
  IdentityPreconditioner(const CsrMatrix& a) {
      // nothing to do, but maintain a consistent interface
  }

  virtual std::vector<double> apply(const std::vector<double>& x) {
      return x;
  }
};

#ifdef USEMKL

class ILUPreconditioner {
 public:
  DokMatrix pc;
  CsrMatrix l, u; // lower and upper factors stored independently

  // cached values, row and col ptrs; the latter two are 1 based indexed, as required by unitrrsolve
  std::vector<double> Lvalues, Uvalues;
  std::vector<int> Lrow_ptr, Lcol_ind, Urow_ptr, Ucol_ind;
  std::vector<double> res;

  // pre - a is a symmetric matrix
  ILUPreconditioner(const CsrMatrix &a) {
      if (!a.isSymmetric())
          throw std::invalid_argument("ILUPreconditioner only supports symmetric CSR matrices");
      pc = a.toDok();
      for (int i = 1; i < a.n; i++) {
          if (pc.dok.count(i) == 0)
              continue;

          for (auto&p : pc.dok[i]) {
            int k = p.first;
            if (k >= i)
                break;
            if (!pc.isNnz(k, k))
              continue;
            pc.dok[i][k] = pc.dok[i][k] / pc.dok[k][k];
            double beta = pc.dok[i][k];

            for (auto&p : pc.dok[i]) {
              int j = p.first;
              if (j < k + 1)
                continue;

              if (pc.isNnz(k, j)) {
                pc.dok[i][j] = pc.dok[i][j] - pc.dok[k][j] * beta;
              }
            }
          }

          // simplified original version of the inner loops
          // for (int k = 0; k < i; k++) {
          //     // update pivot - a[i,k] = a[i, k] / a[k, k]
          //     if (pc.isNnz(i, k) && pc.isNnz(k, k)) {
          //         pc.get(i, k) = pc.get(i, k) / pc.get(k, k);
          //         double beta = pc.get(i, k);
          //         for (int j = k + 1; j < pc.n; j++) {
          //             // update row - a[i, j] -= a[k, j] * a[i, k]
          //             if (pc.isNnz(i, j) && pc.isNnz(k, j)) {
          //                 pc.get(i, j) = pc.get(i, j) - pc.get(k, j) * beta;
          //             }
          //         }
          //     }
          // }
      }

      l = CsrMatrix{pc.getLowerTriangular()};
      Lvalues = l.values;
      Lrow_ptr = l.getRowPtrWithOneBasedIndex();
      Lcol_ind = l.getColIndWithOneBasedIndex();
      u = CsrMatrix{pc.getUpperTriangular()};
      Uvalues = u.values;
      Urow_ptr = u.getRowPtrWithOneBasedIndex();
      Ucol_ind = u.getColIndWithOneBasedIndex();
      res = std::vector<double>(l.n);
  }

  virtual std::vector<double> apply(const std::vector<double>& x) {
      // solve z = M^-1 r <==> Mz = r <==> LUz = r
      // solve: Ly = r
      cask::mkl::unittrsolve(Lvalues.data(), Lrow_ptr.data(), Lcol_ind.data(), x, res.data(), true);
      // then solve Uz = y
      auto y = res;
      cask::mkl::unittrsolve(Uvalues.data(), Urow_ptr.data(), Ucol_ind.data(), y, res.data(), false);
      return res;
  }

  void pretty_print() {
      pc.pretty_print();
  }
};

/**
 *  Standard preconditioned CG, (Saad et al)
 *  https://en.wikipedia.org/wiki/Conjugate_gradient_method
 */
template<typename T=double, typename Precon=IdentityPreconditioner>
bool pcg(const CsrMatrix& a, double *rhs, double *x, int &iterations, bool verbose = false, cask::utils::Timer* t = nullptr) {
    // configuration (TODO Should be exposed through params)
    char tr = 'l';
    int maxiters = 2000;
    double tol = 1E-5;
    if (t)
      t->tic("cg:setup");
    Precon precon{a};

    int n = a.n;
    auto values  = a.values;
    auto row_ptr = a.getRowPtrWithOneBasedIndex();
    auto col_ind = a.getColIndWithOneBasedIndex();
    assert(row_ptr[0] == 1 && "Expecting one based indexing for use with mkl_?csrsymv");

    std::vector<double> r(n);             // residual
    std::vector<double> b(rhs, rhs + n);  // rhs
    std::vector<double> p(n);             //
    std::vector<double> z(n);             //
    if (t)
      t->toc("cg:setup");

    if (t)
      t->tic("cg:solve");

    //  r = b - A * x
    mkl_dcsrsymv(&tr, &n, values.data(), row_ptr.data(), col_ind.data(), &x[0], &r[0]);
    cblas_daxpby(n, 1.0, &b[0], 1, -1.0, &r[0], 1);

    // z = M^-1 * r
    z = precon.apply(r);

    p = z;

    // rsold = r * z
    double rsold = cblas_ddot(n, &r[0], 1, &z[0], 1);

    for (int i = 0; i < maxiters; i++) {
        if (verbose) {
            std::cout << " rsold " << rsold << "iteration " << iterations << "\n";
        }
        std::vector<double> Ap(n);
        // Ap = A * p
        mkl_dcsrsymv(&tr, &n, values.data(), row_ptr.data(), col_ind.data(), &p[0], &Ap[0]);
        // alpha = rsold / (p * Ap)
        double alpha = rsold / cblas_ddot(n, &p[0], 1, &Ap[0], 1);
        // x = x + alpha * p
        cblas_daxpy(n, alpha, &p[0], 1, &x[0], 1);
        // r = r - alpha * Ap
        cblas_daxpby(n, -alpha, &Ap[0], 1, 1.0, &r[0], 1);

        // z = M^-1 * r
        z = precon.apply(r);

        // rsnew = r * z
        double rsnew = cblas_ddot(n, &r[0], 1, &z[0], 1);

        if (rsnew <= tol * tol) {
            // std::cout << "Found solution" << std::endl;
            // print_array("x", &x[0], n);
          if (t)
            t->toc("cg:solve");
          return true;
        }

        // p = r + (rsnew/rsold) * p
        cblas_daxpby(n, 1, &z[0], 1, rsnew / rsold, &p[0], 1);
        rsold = rsnew;
        iterations = i;
    }

    mkl_free_buffers ();

    if (t)
      t->toc("cg:solve");
    return false;
}
#endif

  }
}

#endif /* end of include guard: SPARSE_LINEAR_SOLVERS_HPP */


#include <NTL/mat_ZZ_pE.h>
#include <NTL/BasicThreadPool.h>

NTL_START_IMPL

//===================================================

#define PAR_THRESH (40000.0)

static double
ZZ_pE_SizeInWords()
{
   return double(deg(ZZ_pE::modulus()))*double(ZZ_p::ModulusSize());
}

static
void mul_aux(Mat<ZZ_pE>& X, const Mat<ZZ_pE>& A, const Mat<ZZ_pE>& B)
{
   long n = A.NumRows();
   long l = A.NumCols();
   long m = B.NumCols();

   if (l != B.NumRows())
      LogicError("matrix mul: dimension mismatch");

   X.SetDims(n, m);

   ZZ_pContext ZZ_p_context;
   ZZ_p_context.save();
   ZZ_pEContext ZZ_pE_context;
   ZZ_pE_context.save();
   double sz = ZZ_pE_SizeInWords();

   bool seq = (double(n)*double(l)*double(m)*sz*sz < PAR_THRESH);

   NTL_GEXEC_RANGE(seq, m, first, last)
   NTL_IMPORT(n)
   NTL_IMPORT(l)
   NTL_IMPORT(m)

   ZZ_p_context.restore();
   ZZ_pE_context.restore();

   long i, j, k;
   ZZ_pX acc, tmp;

   Vec<ZZ_pE> B_col;
   B_col.SetLength(l);

   for (j = first; j < last; j++) {
      for (k = 0; k < l; k++) B_col[k] = B[k][j];

      for (i = 0; i < n; i++) {
         clear(acc);
         for (k = 0; k < l; k++) {
            mul(tmp, rep(A[i][k]), rep(B_col[k]));
            add(acc, acc, tmp);
         }
         conv(X[i][j], acc);
      }
   }

   NTL_GEXEC_RANGE_END
}

void mul(mat_ZZ_pE& X, const mat_ZZ_pE& A, const mat_ZZ_pE& B)
{
   if (&X == &A || &X == &B) {
      mat_ZZ_pE tmp;
      mul_aux(tmp, A, B);
      X = tmp;
   }
   else
      mul_aux(X, A, B);
}

void inv(ZZ_pE& d, Mat<ZZ_pE>& X, const Mat<ZZ_pE>& A)
{
   long n = A.NumRows();

   if (A.NumCols() != n)
      LogicError("inv: nonsquare matrix");

   if (n == 0) {
      set(d);
      X.SetDims(0, 0);
      return;
   }

   const ZZ_pXModulus& G = ZZ_pE::modulus();

   ZZ_pX t1, t2;
   ZZ_pX pivot;
   ZZ_pX pivot_inv;

   Vec< Vec<ZZ_pX> > M;
   // scratch space

   M.SetLength(n);
   for (long i = 0; i < n; i++) {
      M[i].SetLength(n);
      for (long j = 0; j < n; j++) {
         M[i][j].SetMaxLength(2*deg(G)-1);
         M[i][j] = rep(A[i][j]);
      }
   }

   ZZ_pX det;
   det = 1;

   Vec<long> P;
   P.SetLength(n);
   for (long k = 0; k < n; k++) P[k] = k;
   // records swap operations

   ZZ_pContext ZZ_p_context;
   ZZ_p_context.save();
   double sz = ZZ_pE_SizeInWords();

   bool seq = double(n)*double(n)*sz*sz < PAR_THRESH;

   bool pivoting = false;

   for (long k = 0; k < n; k++) {

      long pos = -1;

      for (long i = k; i < n; i++) {
         rem(pivot, M[i][k], G);
         if (pivot != 0) {
            InvMod(pivot_inv, pivot, G);
            pos = i;
            break;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
            P[k] = pos;
            pivoting = true;
         }

         MulMod(det, det, pivot, G);

         {
            // multiply row k by pivot_inv
            ZZ_pX *y = &M[k][0];
            for (long j = 0; j < n; j++) {
               rem(t2, y[j], G);
               MulMod(y[j], t2, pivot_inv, G);
            }
            y[k] = pivot_inv;
         }

         NTL_GEXEC_RANGE(seq, n, first, last)
         NTL_IMPORT(n)
         NTL_IMPORT(k)

         ZZ_p_context.restore();

         ZZ_pX *y = &M[k][0];
         ZZ_pX t1, t2;

         for (long i = first; i < last; i++) {
            if (i == k) continue; // skip row k

            ZZ_pX *x = &M[i][0];
            rem(t1, x[k], G);
            negate(t1, t1);
            x[k] = 0;
            if (t1 == 0) continue;

            // add t1 * row k to row i
            for (long j = 0; j < n; j++) {
               mul(t2, y[j], t1);
               add(x[j], x[j], t2);
            }
         }
         NTL_GEXEC_RANGE_END
      }
      else {
         clear(d);
         return;
      }
   }

   if (pivoting) {
      // pivot colums, using reverse swap sequence

      for (long i = 0; i < n; i++) {
         ZZ_pX *x = &M[i][0];

         for (long k = n-1; k >= 0; k--) {
            long pos = P[k];
            if (pos != k) swap(x[pos], x[k]);
         }
      }
   }

   X.SetDims(n, n);
   for (long i = 0; i < n; i++)
      for (long j = 0; j < n; j++)
         conv(X[i][j], M[i][j]);

   conv(d, det);
}

static
void solve_impl(ZZ_pE& d, Vec<ZZ_pE>& X,
                const Mat<ZZ_pE>& A, const Vec<ZZ_pE>& b, bool trans)

{
   long n = A.NumRows();
   if (A.NumCols() != n)
      LogicError("solve: nonsquare matrix");

   if (b.length() != n)
      LogicError("solve: dimension mismatch");

   if (n == 0) {
      set(d);
      X.SetLength(0);
      return;
   }

   ZZ_pX t1, t2;

   const ZZ_pXModulus& G = ZZ_pE::modulus();

   Vec< Vec<ZZ_pX> > M;

   M.SetLength(n);

   for (long i = 0; i < n; i++) {
      M[i].SetLength(n+1);
      for (long j = 0; j < n; j++) M[i][j].SetMaxLength(2*deg(G)-1);

      if (trans)
         for (long j = 0; j < n; j++) M[i][j] = rep(A[j][i]);
      else
         for (long j = 0; j < n; j++) M[i][j] = rep(A[i][j]);

      M[i][n] = rep(b[i]);
   }

   ZZ_pX det;
   set(det);

   ZZ_pContext ZZ_p_context;
   ZZ_p_context.save();
   double sz = ZZ_pE_SizeInWords();

   for (long k = 0; k < n; k++) {
      long pos = -1;
      for (long i = k; i < n; i++) {
         rem(t1, M[i][k], G);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         MulMod(det, det, M[k][k], G);

         // make M[k, k] == -1 mod G, and make row k reduced

         InvMod(t1, M[k][k], G);
         negate(t1, t1);
         for (long j = k+1; j <= n; j++) {
            rem(t2, M[k][j], G);
            MulMod(M[k][j], t2, t1, G);
         }

         bool seq =
            double(n-(k+1))*(n-(k+1))*sz*sz < PAR_THRESH;

         NTL_GEXEC_RANGE(seq, n-(k+1), first, last)
         NTL_IMPORT(n)
         NTL_IMPORT(k)

         ZZ_p_context.restore();

         ZZ_pX t1, t2;

         for (long ii = first; ii < last; ii++) {
            long i = ii + k+1;

            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];   // this is already reduced

            ZZ_pX *x = M[i].elts() + (k+1);
            ZZ_pX *y = M[k].elts() + (k+1);

            for (long j = k+1; j <= n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }

         NTL_GEXEC_RANGE_END

      }
      else {
         clear(d);
         return;
      }
   }

   X.SetLength(n);
   for (long i = n-1; i >= 0; i--) {
      clear(t1);
      for (long j = i+1; j < n; j++) {
         mul(t2, rep(X[j]), M[i][j]);
         add(t1, t1, t2);
      }
      sub(t1, t1, M[i][n]);
      conv(X[i], t1);
   }

   conv(d, det);
}

void solve(ZZ_pE& d, Vec<ZZ_pE>& x,
               const Mat<ZZ_pE>& A, const Vec<ZZ_pE>& b)
{
   solve_impl(d, x, A, b, true);
}

void solve(ZZ_pE& d, const Mat<ZZ_pE>& A,
               Vec<ZZ_pE>& x, const Vec<ZZ_pE>& b)
{
   solve_impl(d, x, A, b, false);
}

long gauss(Mat<ZZ_pE>& M_in, long w)
{
   ZZ_pX t1, t2;
   ZZ_pX piv;

   long n = M_in.NumRows();
   long m = M_in.NumCols();

   if (w < 0 || w > m)
      LogicError("gauss: bad args");

   const ZZ_pXModulus& G = ZZ_pE::modulus();

   Vec< Vec<ZZ_pX> > M;

   M.SetLength(n);
   for (long i = 0; i < n; i++) {
      M[i].SetLength(m);
      for (long j = 0; j < m; j++) {
         M[i][j].SetLength(2*deg(G)-1);
         M[i][j] = rep(M_in[i][j]);
      }
   }

   ZZ_pContext ZZ_p_context;
   ZZ_p_context.save();
   double sz = ZZ_pE_SizeInWords();

   long l = 0;
   for (long k = 0; k < w && l < n; k++) {

      long pos = -1;
      for (long i = l; i < n; i++) {
         rem(t1, M[i][k], G);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

      if (pos != -1) {
         swap(M[pos], M[l]);

         InvMod(piv, M[l][k], G);
         negate(piv, piv);

         for (long j = k+1; j < m; j++) {
            rem(M[l][j], M[l][j], G);
         }

         bool seq =
            double(n-(l+1))*double(m-(k+1))*sz*sz < PAR_THRESH;

         NTL_GEXEC_RANGE(seq, n-(l+1), first, last)
         NTL_IMPORT(m)
         NTL_IMPORT(k)
         NTL_IMPORT(l)

         ZZ_p_context.restore();

         ZZ_pX t1, t2;

         for (long ii = first; ii < last; ii++) {
            long i = ii + l+1;

            // M[i] = M[i] + M[l]*M[i,k]*piv

            MulMod(t1, M[i][k], piv, G);

            clear(M[i][k]);

            ZZ_pX *x = M[i].elts() + (k+1);
            ZZ_pX *y = M[l].elts() + (k+1);

            for (long j = k+1; j < m; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(t2, t2, *x);
               *x = t2;
            }
         }

         NTL_GEXEC_RANGE_END

         l++;
      }
   }

   for (long i = 0; i < n; i++)
      for (long j = 0; j < m; j++)
         conv(M_in[i][j], M[i][j]);

   return l;
}

long gauss(Mat<ZZ_pE>& M)
{
   return gauss(M, M.NumCols());
}

void image(Mat<ZZ_pE>& X, const Mat<ZZ_pE>& A)
{
   Mat<ZZ_pE> M;
   M = A;
   long r = gauss(M);
   M.SetDims(r, M.NumCols());
   X = M;
}

void kernel(Mat<ZZ_pE>& X, const Mat<ZZ_pE>& A)
{
   long m = A.NumRows();
   long n = A.NumCols();

   const ZZ_pXModulus& G = ZZ_pE::modulus();

   Mat<ZZ_pE> M;

   transpose(M, A);
   long r = gauss(M);

   if (r == 0) {
      ident(X, m);
      return;
   }

   X.SetDims(m-r, m);

   if (m-r == 0 || m == 0) return;

   Vec<long> D;
   D.SetLength(m);
   for (long j = 0; j < m; j++) D[j] = -1;

   Vec<ZZ_pE> inverses;
   inverses.SetLength(m);

   for (long i = 0, j = -1; i < r; i++) {
      do {
         j++;
      } while (IsZero(M[i][j]));

      D[j] = i;
      inv(inverses[j], M[i][j]);
   }

   ZZ_pEContext ZZ_pE_context;
   ZZ_pE_context.save();
   ZZ_pContext ZZ_p_context;
   ZZ_p_context.save();
   double sz = ZZ_pE_SizeInWords();

   bool seq =
      double(m-r)*double(r)*double(r)*sz*sz < PAR_THRESH;

   NTL_GEXEC_RANGE(seq, m-r, first, last)
   NTL_IMPORT(m)
   NTL_IMPORT(r)

   ZZ_p_context.restore();
   ZZ_pE_context.restore();

   ZZ_pX t1, t2;
   ZZ_pE T3;

   for (long k = first; k < last; k++) {
      Vec<ZZ_pE>& v = X[k];
      long pos = 0;
      for (long j = m-1; j >= 0; j--) {
         if (D[j] == -1) {
            if (pos == k)
               set(v[j]);
            else
               clear(v[j]);
            pos++;
         }
         else {
            long i = D[j];

            clear(t1);

            for (long s = j+1; s < m; s++) {
               mul(t2, rep(v[s]), rep(M[i][s]));
               add(t1, t1, t2);
            }

            conv(T3, t1);
            mul(T3, T3, inverses[j]);
            negate(v[j], T3);
         }
      }
   }

   NTL_GEXEC_RANGE_END
}

void determinant(ZZ_pE& d, const Mat<ZZ_pE>& M_in)
{
   ZZ_pX t1, t2;

   const ZZ_pXModulus& G = ZZ_pE::modulus();

   long n = M_in.NumRows();

   if (M_in.NumCols() != n)
      LogicError("determinant: nonsquare matrix");

   if (n == 0) {
      set(d);
      return;
   }

   Vec< Vec<ZZ_pX> > M;

   M.SetLength(n);
   for (long i = 0; i < n; i++) {
      M[i].SetLength(n);
      for (long j = 0; j < n; j++) {
         M[i][j].SetMaxLength(2*deg(G)-1);
         M[i][j] = rep(M_in[i][j]);
      }
   }

   ZZ_pX det;
   set(det);

   ZZ_pContext ZZ_p_context;
   ZZ_p_context.save();
   double sz = ZZ_pE_SizeInWords();

   for (long k = 0; k < n; k++) {
      long pos = -1;
      for (long i = k; i < n; i++) {
         rem(t1, M[i][k], G);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1))
            pos = i;
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         MulMod(det, det, M[k][k], G);

         // make M[k, k] == -1 mod G, and make row k reduced

         InvMod(t1, M[k][k], G);
         negate(t1, t1);
         for (long j = k+1; j < n; j++) {
            rem(t2, M[k][j], G);
            MulMod(M[k][j], t2, t1, G);
         }

         bool seq =
            double(n-(k+1))*(n-(k+1))*sz*sz < PAR_THRESH;

         NTL_GEXEC_RANGE(seq, n-(k+1), first, last)
         NTL_IMPORT(n)
         NTL_IMPORT(k)

         ZZ_p_context.restore();

         ZZ_pX t1, t2;

         for (long ii = first; ii < last; ii++) {
            long i = ii + k+1;

            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];   // this is already reduced

            ZZ_pX *x = M[i].elts() + (k+1);
            ZZ_pX *y = M[k].elts() + (k+1);

            for (long j = k+1; j < n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }

         NTL_GEXEC_RANGE_END

      }
      else {
         clear(d);
         return;
      }
   }

   conv(d, det);
}

//===================================================

void add(mat_ZZ_pE& X, const mat_ZZ_pE& A, const mat_ZZ_pE& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix add: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         add(X(i,j), A(i,j), B(i,j));
}

void sub(mat_ZZ_pE& X, const mat_ZZ_pE& A, const mat_ZZ_pE& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix sub: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         sub(X(i,j), A(i,j), B(i,j));
}

void negate(mat_ZZ_pE& X, const mat_ZZ_pE& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         negate(X(i,j), A(i,j));
}

static
void mul_aux(vec_ZZ_pE& x, const mat_ZZ_pE& A, const vec_ZZ_pE& b)
{
   long n = A.NumRows();
   long l = A.NumCols();

   if (l != b.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(n);

   long i, k;
   ZZ_pX acc, tmp;

   for (i = 1; i <= n; i++) {
      clear(acc);
      for (k = 1; k <= l; k++) {
         mul(tmp, rep(A(i,k)), rep(b(k)));
         add(acc, acc, tmp);
      }
      conv(x(i), acc);
   }
}

void mul(vec_ZZ_pE& x, const mat_ZZ_pE& A, const vec_ZZ_pE& b)
{
   if (&b == &x || A.alias(x)) {
      vec_ZZ_pE tmp;
      mul_aux(tmp, A, b);
      x = tmp;
   }
   else
      mul_aux(x, A, b);
}

static
void mul_aux(vec_ZZ_pE& x, const vec_ZZ_pE& a, const mat_ZZ_pE& B)
{
   long n = B.NumRows();
   long l = B.NumCols();

   if (n != a.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(l);

   long i, k;
   ZZ_pX acc, tmp;

   for (i = 1; i <= l; i++) {
      clear(acc);
      for (k = 1; k <= n; k++) {
         mul(tmp, rep(a(k)), rep(B(k,i)));
         add(acc, acc, tmp);
      }
      conv(x(i), acc);
   }
}

void mul(vec_ZZ_pE& x, const vec_ZZ_pE& a, const mat_ZZ_pE& B)
{
   if (&a == &x) {
      vec_ZZ_pE tmp;
      mul_aux(tmp, a, B);
      x = tmp;
   }
   else
      mul_aux(x, a, B);

}

void ident(mat_ZZ_pE& X, long n)
{
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            set(X(i, j));
         else
            clear(X(i, j));
}

long IsIdent(const mat_ZZ_pE& A, long n)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (!IsOne(A(i, j))) return 0;
         }

   return 1;
}

void transpose(mat_ZZ_pE& X, const mat_ZZ_pE& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   long i, j;

   if (&X == & A) {
      if (n == m)
         for (i = 1; i <= n; i++)
            for (j = i+1; j <= n; j++)
               swap(X(i, j), X(j, i));
      else {
         mat_ZZ_pE tmp;
         tmp.SetDims(m, n);
         for (i = 1; i <= n; i++)
            for (j = 1; j <= m; j++)
               tmp(j, i) = A(i, j);
         X.kill();
         X = tmp;
      }
   }
   else {
      X.SetDims(m, n);
      for (i = 1; i <= n; i++)
         for (j = 1; j <= m; j++)
            X(j, i) = A(i, j);
   }
}

void mul(mat_ZZ_pE& X, const mat_ZZ_pE& A, const ZZ_pE& b_in)
{
   ZZ_pE b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void mul(mat_ZZ_pE& X, const mat_ZZ_pE& A, const ZZ_p& b_in)
{
   NTL_ZZ_pRegister(b);
   b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void mul(mat_ZZ_pE& X, const mat_ZZ_pE& A, long b_in)
{
   NTL_ZZ_pRegister(b);
   b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void diag(mat_ZZ_pE& X, long n, const ZZ_pE& d_in)
{
   ZZ_pE d = d_in;
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            X(i, j) = d;
         else
            clear(X(i, j));
}

long IsDiag(const mat_ZZ_pE& A, long n, const ZZ_pE& d)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (A(i, j) != d) return 0;
         }

   return 1;
}

long IsZero(const mat_ZZ_pE& a)
{
   long n = a.NumRows();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

void clear(mat_ZZ_pE& x)
{
   long n = x.NumRows();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

mat_ZZ_pE operator+(const mat_ZZ_pE& a, const mat_ZZ_pE& b)
{
   mat_ZZ_pE res;
   add(res, a, b);
   NTL_OPT_RETURN(mat_ZZ_pE, res);
}

mat_ZZ_pE operator*(const mat_ZZ_pE& a, const mat_ZZ_pE& b)
{
   mat_ZZ_pE res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(mat_ZZ_pE, res);
}

mat_ZZ_pE operator-(const mat_ZZ_pE& a, const mat_ZZ_pE& b)
{
   mat_ZZ_pE res;
   sub(res, a, b);
   NTL_OPT_RETURN(mat_ZZ_pE, res);
}

mat_ZZ_pE operator-(const mat_ZZ_pE& a)
{
   mat_ZZ_pE res;
   negate(res, a);
   NTL_OPT_RETURN(mat_ZZ_pE, res);
}

vec_ZZ_pE operator*(const mat_ZZ_pE& a, const vec_ZZ_pE& b)
{
   vec_ZZ_pE res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_ZZ_pE, res);
}

vec_ZZ_pE operator*(const vec_ZZ_pE& a, const mat_ZZ_pE& b)
{
   vec_ZZ_pE res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_ZZ_pE, res);
}

void inv(mat_ZZ_pE& X, const mat_ZZ_pE& A)
{
   ZZ_pE d;
   inv(d, X, A);
   if (d == 0) ArithmeticError("inv: non-invertible matrix");
}

void power(mat_ZZ_pE& X, const mat_ZZ_pE& A, const ZZ& e)
{
   if (A.NumRows() != A.NumCols()) LogicError("power: non-square matrix");

   if (e == 0) {
      ident(X, A.NumRows());
      return;
   }

   mat_ZZ_pE T1, T2;
   long i, k;

   k = NumBits(e);
   T1 = A;

   for (i = k-2; i >= 0; i--) {
      sqr(T2, T1);
      if (bit(e, i))
         mul(T1, T2, A);
      else
         T1 = T2;
   }

   if (e < 0)
      inv(X, T1);
   else
      X = T1;
}

void random(mat_ZZ_pE& x, long n, long m)
{
   x.SetDims(n, m);
   for (long i = 0; i < n; i++) random(x[i], m);
}

NTL_END_IMPL


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

#include "theia/sfm/camera/radial_distortion.h"

#include <Eigen/Core>
#include <glog/logging.h>

#include <algorithm>
#include <cmath>
#include <limits>

#include "theia/math/polynomial.h"

namespace theia {

// Solves for the undistorted image points. We assume that the radial distortion
// model is:
//   r = x * x + y * y;
//   d = 1 + r * (k1 + k2 * r);
//   xp = x * d;
//   yp = y * d;
//
// Given this model, we know that:
//   xp / x  =  yp / y
// So we can rewrite r such that
//   r = (1 + yp * yp / (yp * xp)) * x * x
// and now we have r in terms of a single unknown x. Plugging this into
// xp = x * d we have a 5th order polynomial in x that we can solve with an
// iterative solver.
void RadialUndistortPoint(const Eigen::Vector2d& distorted_point,
                          const double radial_distortion1,
                          const double radial_distortion2,
                          Eigen::Vector2d* undistorted_point) {
  const double kMinRadius = 1e-5;
  const double kEpsilon = 1e-8;
  const int kMaxIter = 10;

  if (std::max(std::abs(distorted_point.x()), std::abs(distorted_point.y())) <
      kMinRadius) {
    *undistorted_point = distorted_point;
    return;
  }

  // Choose which variable we solve around to improve stability.
  if (std::abs(distorted_point.x()) > std::abs(distorted_point.y())) {
    const double point_ratio = distorted_point.y() / distorted_point.x();
    const double r_sq = 1 + point_ratio * point_ratio;
    Eigen::VectorXd quintic_polynomial(6);
    quintic_polynomial(0) = radial_distortion2 * r_sq * r_sq;
    quintic_polynomial(1) = 0;
    quintic_polynomial(2) = radial_distortion1 * r_sq;
    quintic_polynomial(3) = 0;
    quintic_polynomial(4) = 1;
    quintic_polynomial(5) = -distorted_point.x();

    undistorted_point->x() = FindRootIterativeLaguerre(
        quintic_polynomial, distorted_point.x(), kEpsilon, kMaxIter);
    undistorted_point->y() = point_ratio * undistorted_point->x();
  } else {
    const double point_ratio = distorted_point.x() / distorted_point.y();
    const double r_sq = 1 + point_ratio * point_ratio;
    Eigen::VectorXd quintic_polynomial(6);
    quintic_polynomial(0) = radial_distortion2 * r_sq * r_sq;
    quintic_polynomial(1) = 0;
    quintic_polynomial(2) = radial_distortion1 * r_sq;
    quintic_polynomial(3) = 0;
    quintic_polynomial(4) = 1;
    quintic_polynomial(5) = -distorted_point.y();

    undistorted_point->y() = FindRootIterativeLaguerre(
        quintic_polynomial, distorted_point.y(), kEpsilon, kMaxIter);
    undistorted_point->x() = point_ratio * undistorted_point->y();
  }
}

}  // namespace theia


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/boost/graph/split_graph_into_polylines.h>

#include <boost/graph/adjacency_list.hpp>

#include <map>
#include <vector>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;

typedef boost::adjacency_list<boost::vecS, boost::setS, boost::undirectedS, Point_2 > Graph;

typedef boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;

typedef std::map<Point_2, vertex_descriptor> Point_vertex_map;

typedef std::vector<Point_2> Polyline_2;

// inserts a polyline into a graph
void insert(const std::vector<Point_2>& poly, Graph& graph, Point_vertex_map& pvmap)
{
    vertex_descriptor u = boost::graph_traits<Graph>::null_vertex();
    vertex_descriptor v;
    for (std::size_t i = 0; i < poly.size(); i++) {
        // check if the point is not yet in the graph
        if (pvmap.find(poly[i]) == pvmap.end()) {
            v = add_vertex(graph);
            pvmap[poly[i]] = v;
        }
        else {
            v = pvmap[poly[i]];
        }
        graph[v] = poly[i];  // associate the point to the vertex
        if (i != 0) {
            add_edge(u, v, graph);
        }
        u = v;
    }
}

template <typename Graph>
struct Polyline_visitor
{
  std::list<Polyline_2>& polylines;
  const Graph& points_pmap;

  Polyline_visitor(std::list<Polyline_2>& lines,
                   const Graph& points_property_map)
    : polylines(lines),
      points_pmap(points_property_map)
  {}

  void start_new_polyline()
  {
    Polyline_2 V;
    polylines.push_back(V);
  }

  void add_node(typename boost::graph_traits<Graph>::vertex_descriptor vd)
  {
    Polyline_2& polyline = polylines.back();
    polyline.push_back(points_pmap[vd]);
  }

  void end_polyline()
  {}

};

int main()
{
  Polyline_2 polyA = { Point_2(0,0), Point_2(1,0), Point_2(2,0), Point_2(3,0), Point_2(4,0)};
  Polyline_2 polyB = { Point_2(1,-1), Point_2(1,0), Point_2(2,0), Point_2(2,1), Point_2(2,2) };

  Graph graph;
  Point_vertex_map pvmap;

  insert(polyA, graph, pvmap);
  insert(polyB, graph, pvmap);

  std::list<Polyline_2> polylines;
  Polyline_visitor<Graph> polyline_visitor(polylines, graph);

  CGAL::split_graph_into_polylines( graph,
                                    polyline_visitor);

  for(std::list<Polyline_2>::iterator it = polylines.begin(); it!= polylines.end(); ++it){
     Polyline_2& poly = *it;
     std::size_t n;
     if(poly.front() == poly.back()){
       std::cout << "POLYGON" << std::endl;
       n = poly.size() -1;
     }else{
       std::cout << "POLYLINE" << std::endl;
       n = poly.size();
     }
     for(std::size_t j=0; j < n; j++){
       std::cout << poly[j] << std::endl;
     }
     std::cout << std::endl;
   }

  return 0;

}


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */
#include "Utils/Geometry/StructuralCompletion.h"
#include <Eigen/Geometry>

namespace Scine {
namespace Utils {

void StructuralCompletion::generate3TetrahedronCornersFrom1Other(const Eigen::Vector3d& v1, Eigen::Ref<Eigen::Vector3d> v2,
                                                                 Eigen::Ref<Eigen::Vector3d> v3,
                                                                 Eigen::Ref<Eigen::Vector3d> v4) {
  /*
   * Generate a vector perpendicular to the given one and then perform rotation around this vector to generate v2, then
   * rotate v2 around v1 to generate v3 and v4.
   */

  // Get a perpendicular vector;
  Eigen::Vector3d perpendicularVector = v1.cross(Eigen::Vector3d(1, 0, 0));
  // Check that it was different enough from {1,0,0}; if not, create it from {0,1,0}
  if (perpendicularVector.squaredNorm() < 0.00001) {
    perpendicularVector = v1.cross(Eigen::Vector3d(0, 1, 0));
  }
  perpendicularVector.normalize();

  Eigen::AngleAxisd t(tetrahedronAngle, perpendicularVector);
  auto R = t.toRotationMatrix();
  v2 = R * v1;
  double ang2 = 120.0 * Constants::rad_per_degree;
  Eigen::AngleAxisd t2(ang2, v1);
  R = t2.toRotationMatrix();
  v3 = R * v2;
  v4 = R * v3;
}

void StructuralCompletion::generate2TetrahedronCornersFrom2Others(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2,
                                                                  Eigen::Ref<Eigen::Vector3d> v3,
                                                                  Eigen::Ref<Eigen::Vector3d> v4) {
  /*
   * Generate the vector a, which is in the middle between v1 and v2, and the vector b, which is perpendicular to a but
   * in the same plane as v1 and v2, and then rotate a around b to generate v3 and v4.
   */

  Eigen::Vector3d a = (v1 + v2).normalized();
  Eigen::Vector3d b = (v2 - v1).normalized();

  double angle = (360 * Constants::rad_per_degree - tetrahedronAngle) / 2;
  Eigen::AngleAxisd t(angle, b);
  auto R = t.toRotationMatrix();
  v3 = R * a;
  v4 = R * v3;
}

void StructuralCompletion::generate1TetrahedronCornerFrom3Others(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2,
                                                                 const Eigen::Vector3d& v3, Eigen::Ref<Eigen::Vector3d> v4) {
  Eigen::Vector3d res = -(v1 + v2 + v3);
  // check that the three vectors were non-planar enough
  if (res.squaredNorm() < 0.4 * 0.4) {
    res = v1.cross(v2);
  }

  v4 = res.normalized();
}

void StructuralCompletion::generate2TriangleCornersFrom1Other(const Eigen::Vector3d& v1, Eigen::Ref<Eigen::Vector3d> v2,
                                                              Eigen::Ref<Eigen::Vector3d> v3) {
  /*
   * Generate a vector perpendicular to the given one and then perform rotation around this vector to generate v2 and
   * v3.
   */

  // Get a perpendicular vector;
  auto perpendicularVector = v1.cross(Eigen::Vector3d(1, 0, 0));
  // Check that it was different enough from {1,0,0}; if not, create it from {0,1,0}
  if (perpendicularVector.squaredNorm() < 0.00001) {
    perpendicularVector = v1.cross(Eigen::Vector3d(0, 1, 0));
  }
  perpendicularVector.normalize();

  Eigen::AngleAxisd t(120 * Constants::rad_per_degree, perpendicularVector);
  auto R = t.toRotationMatrix();
  v2 = R * v1;
  v3 = R * v2;
}

void StructuralCompletion::generate1TriangleCornerFrom2Others(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2,
                                                              Eigen::Ref<Eigen::Vector3d> v3) {
  v3 = -(v1 + v2).normalized();
}

constexpr double StructuralCompletion::tetrahedronAngle;

} /* namespace Utils */
} /* namespace Scine */


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_SOLVERS_FDDP_HPP_
#define CROCODDYL_CORE_SOLVERS_FDDP_HPP_

#include <Eigen/Cholesky>
#include <vector>

#include "crocoddyl/core/solvers/ddp.hpp"

namespace crocoddyl {

/**
 * @brief Feasibility-driven Differential Dynamic Programming (FDDP) solver
 *
 * The FDDP solver computes an optimal trajectory and control commands by iterates running `backwardPass()` and
 * `forwardPass()`. The backward pass accepts infeasible guess as described in the `SolverDDP::backwardPass()`.
 * Additionally, the forward pass handles infeasibility simulations that resembles the numerical behaviour of
 * a multiple-shooting formulation, i.e.:
 * \f{eqnarray}
 *   \mathbf{\hat{x}}_0 &=& \mathbf{\tilde{x}}_0 - (1 - \alpha)\mathbf{\bar{f}}_0,\\
 *   \mathbf{\hat{u}}_k &=& \mathbf{u}_k + \alpha\mathbf{k}_k + \mathbf{K}_k(\mathbf{\hat{x}}_k-\mathbf{x}_k),\\
 *   \mathbf{\hat{x}}_{k+1} &=& \mathbf{f}_k(\mathbf{\hat{x}}_k,\mathbf{\hat{u}}_k) - (1 -
 * \alpha)\mathbf{\bar{f}}_{k+1}.
 * \f}
 * Note that the forward pass keeps the gaps \f$\mathbf{\bar{f}}_s\f$ open according to the step length \f$\alpha\f$
 * that has been accepted. This solver has shown empirically greater globalization strategy. Additionally, the
 * expected improvement computation considers the gaps in the dynamics:
 * \f{equation}
 *   \Delta J(\alpha) = \Delta_1\alpha + \frac{1}{2}\Delta_2\alpha^2,
 * \f}
 * with
 * \f{eqnarray}
 *   \Delta_1 = \sum_{k=0}^{N-1} \mathbf{k}_k^\top\mathbf{Q}_{\mathbf{u}_k} +\mathbf{\bar{f}}_k^\top(V_{\mathbf{x}_k} -
 *   V_{\mathbf{xx}_k}\mathbf{x}_k),\nonumber\\ \Delta_2 = \sum_{k=0}^{N-1}
 *   \mathbf{k}_k^\top\mathbf{Q}_{\mathbf{uu}_k}\mathbf{k}_k + \mathbf{\bar{f}}_k^\top(2 V_{\mathbf{xx}_k}\mathbf{x}_k
 * - V_{\mathbf{xx}_k}\mathbf{\bar{f}}_k). \f}
 *
 * For more details about the feasibility-driven differential dynamic programming algorithm see:
 * \include mastalli-icra20.bib
 *
 * \sa `SolverDDP()`, `backwardPass()`, `forwardPass()`, `expectedImprovement()` and `updateExpectedImprovement()`
 */
class SolverFDDP : public SolverDDP {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /**
   * @brief Initialize the FDDP solver
   *
   * @param[in] problem  shooting problem
   */
  explicit SolverFDDP(boost::shared_ptr<ShootingProblem> problem);
  virtual ~SolverFDDP();

  virtual bool solve(const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
                     const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR, const std::size_t maxiter = 100,
                     const bool is_feasible = false, const double regInit = 1e-9);

  /**
   * @copybrief SolverAbstract::expectedImprovement
   *
   * This function requires to first run `updateExpectedImprovement()`. The expected improvement computation considers
   * the gaps in the dynamics: \f{equation} \Delta J(\alpha) = \Delta_1\alpha + \frac{1}{2}\Delta_2\alpha^2, \f} with
   * \f{eqnarray}
   *   \Delta_1 = \sum_{k=0}^{N-1} \mathbf{k}_k^\top\mathbf{Q}_{\mathbf{u}_k} +\mathbf{\bar{f}}_k^\top(V_{\mathbf{x}_k}
   * - V_{\mathbf{xx}_k}\mathbf{x}_k),\nonumber\\ \Delta_2 = \sum_{k=0}^{N-1}
   *   \mathbf{k}_k^\top\mathbf{Q}_{\mathbf{uu}_k}\mathbf{k}_k + \mathbf{\bar{f}}_k^\top(2
   * V_{\mathbf{xx}_k}\mathbf{x}_k
   * - V_{\mathbf{xx}_k}\mathbf{\bar{f}}_k). \f}
   */
  virtual const Eigen::Vector2d& expectedImprovement();

  /**
   * @brief Update internal values for computing the expected improvement
   */
  void updateExpectedImprovement();
  virtual void forwardPass(const double stepLength);

  /**
   * @brief Return the threshold used for accepting step along ascent direction
   */
  double get_th_acceptnegstep() const;

  /**
   * @brief Modify the threshold used for accepting step along ascent direction
   */
  void set_th_acceptnegstep(const double th_acceptnegstep);

 protected:
  double dg_;  //!< Internal data for computing the expected improvement
  double dq_;  //!< Internal data for computing the expected improvement
  double dv_;  //!< Internal data for computing the expected improvement

 private:
  double th_acceptnegstep_;  //!< Threshold used for accepting step along ascent direction
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_SOLVERS_FDDP_HPP_


#include "problemes.h"
#include "arithmetique.h"

#include <fstream>

#include <boost/algorithm/string.hpp>
#include <numeric>

typedef unsigned long long nombre;

ENREGISTRER_PROBLEME(59, "XOR decryption") {
    // Each character on a computer is assigned a unique code and the preferred standard is ASCII
    // (American Standard Code for Information Interchange). For example, uppercase A = 65, asterisk (*) = 42,
    // and lowercase k = 107.
    //
    // A modern encryption method is to take a text file, convert the bytes to ASCII, then XOR each byte
    // with a given value, taken from a secret key. The advantage with the XOR function is that using
    // the same encryption key on the cipher text, restores the plain text; for example, 65 XOR 42 = 107,
    // then 107 XOR 42 = 65.
    //
    // For unbreakable encryption, the key is the same length as the plain text message, and the key is made
    // up of random bytes. The user would keep the encrypted message and the encryption key in different
    // locations, and without both "halves", it is impossible to decrypt the message.
    //
    // Unfortunately, this method is impractical for most users, so the modified method is to use a password
    // as a key. If the password is shorter than the message, which is likely, the key is repeated cyclically
    // throughout the message. The balance for this method is using a sufficiently long password key for security,
    // but short enough to be memorable.
    //
    // Your task has been made easy, as the encryption key consists of three lower case characters.
    // Using cipher.txt (right click and 'Save Link/Target As...'), a file containing the encrypted ASCII codes,
    // and the knowledge that the plain text must contain common English words, decrypt the message and find
    // the sum of the ASCII values in the original text.
    std::ifstream ifs("data/p059_cipher.txt");
    std::string entree;
    ifs >> entree;
    std::vector<std::string> names;
    boost::split(names, entree, boost::is_any_of(","));

    std::set<char> lettres = {' ', ',', '(', ')', '[', ']', '.', '!', '\'', ';', '"', '+', '-', '/', '*', ':'};
    for (char c = 'a'; c <= 'z'; ++c) lettres.insert(c);
    for (char c = 'A'; c <= 'Z'; ++c) lettres.insert(c);
    for (char c = '0'; c <= '9'; ++c) lettres.insert(c);

    std::vector<char> data;
    std::transform(names.begin(), names.end(), std::back_inserter(data),
                   [](const std::string &str) { return std::stoi(str); });

    std::string message;
    for (char key1 = 'a'; key1 <= 'z'; ++key1) {
        if (lettres.find(data[0] ^ key1) == lettres.end())
            continue;
        for (char key2 = 'a'; key2 <= 'z'; ++key2) {
            if (lettres.find(data[1] ^ key2) == lettres.end())
                continue;
            for (char key3 = 'a'; key3 <= 'z'; ++key3) {
                if (lettres.find(data[2] ^ key3) == lettres.end())
                    continue;

                std::string key = {key1, key2, key3};
                std::string decode;
                for (size_t n = 0; n < data.size(); ++n) {
                    const char c = data[n] ^key[n % 3];
                    if (lettres.find(c) == lettres.end())
                        break;
                    decode.push_back(c);
                }
                if (decode.size() == data.size()) {
                    message = decode;
                    std::cout << key << ": " << decode << std::endl;
                }
            }
        }
    }

    nombre resultat = std::accumulate(message.begin(), message.end(), 0ULL,
                                      [](const nombre r, const char c) { return r + static_cast<nombre>(c); }
    );
    return std::to_string(resultat);
}