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

#pragma once

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

#include <boost/optional.hpp>

namespace ikos {

template <typename Number> class congruence {
  using interval_t = interval<Number>;

public:
  using congruence_t = congruence<Number>;

private:
  bool _is_bottom;

  /// A congruence is denoted by aZ + b, where b \in Z and a \in N.
  /// The abstract state aZ + b represents all numbers that are
  /// congruent to b modulo a.
  Number _a; // modulo
  Number _b; // remainder

  // Notes about the % operator
  //
  // The semantics of r = n % d is to set r to "n mod d". The sign of
  // the d is ignored and r is always non-negative.
  //
  // We assume that n % d (also n /d) raises a runtime error if d==0.

  void normalize(void) {
    // Set to standard form: 0 <= b < a for a != 0
    if (_a != 0) {
      _b = _b % _a;
    }
  }

  // if true then top (1Z + 0) else bottom
  congruence(bool b) : _is_bottom(!b), _a(1), _b(0) {}

  congruence(int n) : _is_bottom(false), _a(0), _b(n) {}

  congruence(Number a, Number b) : _is_bottom(false), _a(a), _b(b) {
    normalize();
  }

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

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

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

  Number gcd(Number x, Number y, Number z) const { return gcd(x, gcd(y, z)); }
  // Not to be called explicitly outside of gcd
  Number gcd_helper(Number x, Number y) const {
    return (y == 0) ? x : gcd_helper(y, x % y);
  }
  Number gcd(Number x, Number y) const { return gcd_helper(abs(x), abs(y)); }

  Number lcm(Number x, Number y) const {
    Number tmp = gcd(x, y);
    return abs(x * y) / tmp;
  }

  bool is_zero() const { return !is_bottom() && _a == 0 && _b == 0; }

  bool all_ones() const { return !is_bottom() && _a == 0 && _b == -1; }

  interval_t to_interval() const {
    assert(singleton());
    return interval_t(*(singleton()));
  }

public:
  static congruence_t top() { return congruence(true); }

  static congruence_t bottom() { return congruence(false); }

  congruence() : _is_bottom(false), _a(1), _b(0) {}

  congruence(Number n) : _is_bottom(false), _a(0), _b(n) {}

  congruence(const congruence_t &o)
      : _is_bottom(o._is_bottom), _a(o._a), _b(o._b) {}

  congruence_t operator=(congruence_t o) {
    _is_bottom = o._is_bottom;
    _a = o._a;
    _b = o._b;
    return *this;
  }

  bool is_bottom() const { return _is_bottom; }

  bool is_top() const { return _a == 1; }

  boost::optional<Number> singleton() const {
    if (!this->is_bottom() && _a == 0) {
      return boost::optional<Number>(_b);
    } else {
      return boost::optional<Number>();
    }
  }

  Number get_modulo() const { return _a; }

  Number get_remainder() const { return _b; }

  bool operator==(congruence_t o) const {
    return (is_bottom() == o.is_bottom() && _a == o._a && _b == o._b);
  }

  bool operator!=(congruence_t x) const { return !this->operator==(x); }

  /** Lattice Operations **/

  bool operator<=(const congruence_t &o) const {
    if (is_bottom()) {
      return true;
    } else if (o.is_bottom()) {
      return false;
    } else if (_a == 0 && o._a == 0) {
      return (_b == o._b);
    } else if (_a == 0) {
      if ((_b % o._a) == (o._b % o._a)) {
        return true;
      }
    } else if (o._a == 0) {
      if (_b % _a == (o._b % _a)) {
        return false;
      }
    }
    return (_a % o._a == 0) && (_b % o._a == o._b % o._a);
  }

  congruence_t operator|(congruence_t o) {
    if (is_bottom()) {
      return o;
    } else if (o.is_bottom()) {
      return *this;
    } else if (is_top() || o.is_top()) {
      return top();
    } else {
      return congruence_t(gcd(_a, o._a, abs(_b - o._b)), min(_b, o._b));
    }
  }

  congruence_t operator&(congruence_t o) {
    if (is_bottom() || o.is_bottom()) {
      return bottom();
    }

    // lcm has meaning only if both a and o.a are not 0
    if (_a == 0 && o._a == 0) {
      if (_b == o._b) {
        return *this;
      } else {
        return bottom();
      }
    } else if (_a == 0) {
      // b & a'Z + b' iff \exists k such that a'*k + b' = b iff ((b - b') %a' ==
      // 0)
      if ((_b - o._b) % o._a == 0) {
        return *this;
      } else {
        return bottom();
      }
    } else if (o._a == 0) {
      // aZ+b & b' iff \exists k such that a*k+b  = b' iff ((b'-b %a) == 0)
      if ((o._b - _b) % _a == 0) {
        return o;
      } else {
        return bottom();
      }
    } else {
      // pre: a and o.a != 0
      Number x = gcd(_a, o._a);
      if (_b % x == (o._b % x)) {
        // the part max(b,o.b) needs to be verified. What we really
        // want is to find b'' such that
        // 1) b'' % lcm(a,a') == b  % lcm(a,a'), and
        // 2) b'' % lcm(a,a') == b' % lcm(a,a').
        // An algorithm for that is provided in Granger'89.
        return congruence_t(lcm(_a, o._a), max(_b, o._b));
      } else {
        return congruence_t::bottom();
      }
    }
  }

  congruence_t operator||(congruence_t o) {
    // Equivalent to join, domain is flat
    return *this | o;
  }

  congruence_t operator&&(congruence_t o) {
    // Simply refines top element
    return (is_top()) ? o : *this;
  }

  /** Arithmetic Operators **/

  congruence_t operator+(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else
      return congruence_t(gcd(_a, o._a), _b + o._b);
  }

  congruence_t operator-(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else
      return congruence_t(gcd(_a, o._a), _b - o._b);
  }

  congruence_t operator-() {
    if (this->is_bottom() || this->is_top())
      return *this;
    else
      return congruence_t(_a, -_b + _a);
  }

  congruence_t operator*(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if ((this->is_top() || o.is_top()) && _a != 0 && o._a != 0)
      return congruence_t::top();
    else
      return congruence_t(gcd(_a * o._a, _a * o._b, o._a * _b), _b * o._b);
  }

  // signed division
  congruence_t operator/(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (o == congruence(0))
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {

      /*
         aZ+b / 0Z+b':
            if b'|a then  (a/b')Z + b/b'
            else          top
      */
      if (o._a == 0) {
        if (_a % o._b == 0)
          return congruence_t(_a / o._b, _b / o._b);
        else
          return congruence_t::top();
      }

      /*
         0Z+b / a'Z+b':
            if N>0   (b div N)Z + 0
            else     0Z + 0

           where N = a'((b-b') div a') + b'
      */
      if (_a == 0) {
        Number n(o._a * (((_b - o._b) / o._a) + o._b));
        if (n > 0) {
          return congruence_t(_b / n, 0);
        } else {
          return congruence_t(0, 0);
        }
      }

      /*
        General case: no singleton
      */
      return congruence_t::top();
    }
  }

  // signed remainder operator
  congruence_t operator%(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (o == congruence(0))
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {
      /*
         aZ+b mod 0Z+b':
             if b'|a then  (a/b')Z + b/b'
             else          top
      */
      if (o._a == 0) {
        if (_a % o._b == 0) {
          return congruence_t(0, _b % o._b);
        } else {
          return congruence_t(gcd(_a, o._b), _b);
        }
      }

      /*
          0Z+b mod a'Z+b':
           if N<=0           then 0Z+b
           if (b div N) == 1 then gcd(b',a')Z + b
           if (b div N) >= 2 then N(b div N)Z  + b

         where N = a'((b-b') div a') + b'
      */
      if (_a == 0) {
        Number n(o._a * (((_b - o._b) / o._a) + o._b));
        if (n <= 0) {
          return congruence_t(_a, _b);
        } else if (_b == n) {
          return congruence_t(gcd(o._b, o._a), _b);
        } else if ((_b / n) >= 2) {
          return congruence_t(_b, _b);
        } else {
          CRAB_ERROR("unreachable");
        }
      }

      /*
          general case: no singleton
      */
      return congruence_t(gcd(_a, o._a, o._b), _b);
    }
  }

  /**
      Bitwise operators.
      They are very imprecise because we ignore bitwidth.

      Bitwise operation can be implemented more precisely based on
      Stefan Bygde's paper: Static WCET analysis based on abstract
      interpretation and counting of elements, Vasteras : School of
      Innovation, Design and Engineering, Malardalen University (2010).
   **/

  congruence_t And(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {
      if (is_zero() || o.is_zero()) {
        return congruence_t(0);
      } else if (all_ones()) {
        return o;
      } else if (o.all_ones()) {
        return *this;
      } else if (_a == 0 && o._a == 0) {
        return congruence_t(_b & o._b);
      } else {
        return top();
      }
    }
  }

  congruence_t Or(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {
      if (all_ones() || o.all_ones()) {
        return congruence_t(-1);
      } else if (is_zero()) {
        return o;
      } else if (o.is_zero()) {
        return *this;
      } else if (_a == 0 && o._a == 0) {
        return congruence_t(_b | o._b);
      } else {
        return top();
      }
    }
  }

  congruence_t Xor(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {
      if (is_zero()) {
        return o;
      } else if (o.is_zero()) {
        return *this;
      } else if (_a == 0 && o._a == 0) {
        return congruence_t(_b ^ o._b);
      } else {
        return top();
      }
    }
  }

  congruence_t Shl(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {

      if (o._a == 0) { // singleton

        if (o._b < 0) {
          return bottom();
        }

        // aZ + b << 0Z + b'  = (a*2^b')Z + b*2^b'
        Number x = Number(1) << o._b;
        return congruence_t(_a * x, _b * x);
      } else {

        Number x = Number(1) << o._b;
        Number y = Number(1) << o._a;
        // aZ + b << a'Z + b' = (gcd(a, b * (2^a' - 1)))*(2^b')Z + b*(2^b')
        return congruence_t(gcd(_a, _b * (y - 1)) * x, _b * x);
      }
    }
  }

  congruence_t AShr(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {

      if (o._a == 0) { // singleton
        // aZ + b >> 0Z + b'
        if (o._b < 0) {
          return congruence_t::bottom();
        }
      }

      if (singleton() && o.singleton()) {
        interval_t res = to_interval().AShr(o.to_interval());
        if (boost::optional<Number> n = res.singleton()) {
          return congruence(*n);
        }
      }

      return congruence_t::top();
    }
  }

  congruence_t LShr(congruence_t o) {
    if (this->is_bottom() || o.is_bottom())
      return congruence_t::bottom();
    else if (this->is_top() || o.is_top())
      return congruence_t::top();
    else {

      if (o._a == 0) {
        // aZ + b >> 0Z + b'
        if (o._b < 0) {
          return congruence_t::bottom();
        }
      }

      if (singleton() && o.singleton()) {
        interval_t res = to_interval().LShr(o.to_interval());
        if (boost::optional<Number> n = res.singleton()) {
          return congruence(*n);
        }
      }

      return congruence_t::top();
    }
  }

  // division and remainder operations

  congruence_t SDiv(congruence_t x) { return this->operator/(x); }

  congruence_t UDiv(congruence_t x) { return congruence_t::top(); }

  congruence_t SRem(congruence_t x) { return this->operator%(x); }

  congruence_t URem(congruence_t x) { return congruence_t::top(); }

  void write(crab::crab_os &o) const {
    if (is_bottom()) {
      o << "_|_";
      return;
    }

    if (_a == 0) {
      o << _b;
      return;
    }

    o << _a << "Z+" << _b;
  }
}; // end class congruence

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

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

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

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

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

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

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

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

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

template <typename Number, typename VariableName, typename CongruenceCollection>
class equality_congruence_solver {
  // TODO: check correctness of the solver. Granger provides a sound
  // and more precise solver for equality linear congruences (see
  // Theorem 4.4).
private:
  using congruence_t = congruence<Number>;
  using variable_t = crab::variable<Number, VariableName>;
  using linear_expression_t = linear_expression<Number, VariableName>;
  using linear_constraint_t = linear_constraint<Number, VariableName>;
  using linear_constraint_system_t =
      linear_constraint_system<Number, VariableName>;

  using cst_table_t = std::vector<linear_constraint_t>;
  using variable_set_t = std::set<variable_t>;

  std::size_t m_max_cycles;
  bool m_is_contradiction;
  cst_table_t m_cst_table;
  variable_set_t m_refined_variables;
  std::size_t m_op_count;

private:
  bool refine(const variable_t &v, congruence_t i, CongruenceCollection &env) {
    congruence_t old_i = env[v];
    congruence_t new_i = old_i & i;
    if (new_i.is_bottom()) {
      return true;
    }
    if (old_i != new_i) {
      env.set(v, new_i);
      m_refined_variables.insert(v);
      ++(m_op_count);
    }
    return false;
  }

  congruence_t compute_residual(const linear_constraint_t &cst,
                                const variable_t &pivot,
                                CongruenceCollection &env) {
    congruence_t residual(cst.constant());
    for (auto kv : cst) {
      const variable_t &v = kv.second;
      if (!(v == pivot)) {
        residual = residual - (kv.first * env[v]);
        ++(m_op_count);
      }
    }
    return residual;
  }

  bool propagate(const linear_constraint_t &cst, CongruenceCollection &env) {
    for (auto kv : cst) {
      Number c = kv.first;
      const variable_t &pivot = kv.second;
      congruence_t rhs = compute_residual(cst, pivot, env) / congruence_t(c);

      if (cst.is_equality()) {
        if (refine(pivot, rhs, env)) {
          return true;
        }
      } else if (cst.is_inequality() || cst.is_strict_inequality()) {
        // Inequations (>=, <=, >, and <) do not work well with
        // congruences because for any number n there is always x and y
        // \in gamma(aZ+b) such that n < x and n > y.
        //
        // The only cases we can catch is when all the expressions
        // are constants. We do not bother because any product
        // with intervals or constants should get those cases.
        continue;
      } else {
        // TODO: cst is a disequation
      }
    }
    return false;
  }

  bool solve_system(CongruenceCollection &env) {
    std::size_t cycle = 0;
    do {
      ++cycle;
      m_refined_variables.clear();
      for (const linear_constraint_t &cst : m_cst_table) {
        if (propagate(cst, env)) {
          return true;
        }
      }
    } while (m_refined_variables.size() > 0 && cycle <= m_max_cycles);
    return false;
  }

public:
  equality_congruence_solver(const linear_constraint_system_t &csts,
                             std::size_t max_cycles)
      : m_max_cycles(max_cycles), m_is_contradiction(false) {
    for (auto const &cst : csts) {
      if (cst.is_contradiction()) {
        m_is_contradiction = true;
        return;
      } else if (cst.is_tautology()) {
        continue;
      } else {
        m_cst_table.push_back(cst);
      }
    }
  }

  void run(CongruenceCollection &env) {
    if (m_is_contradiction) {
      env.set_to_bottom();
    } else {
      if (solve_system(env)) {
        env.set_to_bottom();
      }
    }
  }

}; // class equality_congruence_solver

template <typename Number, typename VariableName>
class congruence_domain final : public crab::domains::abstract_domain_api<
                                    congruence_domain<Number, VariableName>> {
public:
  using congruence_t = congruence<Number>;

private:
  // note that this is assuming that all variables have the same bit
  // width which is unrealistic.
  using congruence_domain_t = congruence_domain<Number, VariableName>;
  using abstract_domain_t =
      crab::domains::abstract_domain_api<congruence_domain_t>;

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

private:
  using separate_domain_t = separate_domain<variable_t, congruence_t>;
  using solver_t =
      equality_congruence_solver<number_t, varname_t, separate_domain_t>;

public:
  using iterator = typename separate_domain_t::iterator;

private:
  separate_domain_t _env;

private:
  congruence_domain(separate_domain_t env) : _env(env) {}

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

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

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

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

  congruence_domain() : _env(separate_domain_t::top()) {}

  congruence_domain(const congruence_domain_t &e) : _env(e._env) {
    crab::CrabStats::count(domain_name() + ".count.copy");
    crab::ScopedCrabStats __st__(domain_name() + ".copy");
  }

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

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

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

  bool is_bottom() const override { return this->_env.is_bottom(); }

  bool is_top() const override { return this->_env.is_top(); }

  bool operator<=(const congruence_domain_t &e) const override {
    crab::CrabStats::count(domain_name() + ".count.leq");
    crab::ScopedCrabStats __st__(domain_name() + ".leq");
    return this->_env <= e._env;
  }

  void operator|=(const congruence_domain_t &e) override {
    crab::CrabStats::count(domain_name() + ".count.join");
    crab::ScopedCrabStats __st__(domain_name() + ".join");
    this->_env = this->_env | e._env;
  }

  congruence_domain_t operator|(const congruence_domain_t &e) const override {
    crab::CrabStats::count(domain_name() + ".count.join");
    crab::ScopedCrabStats __st__(domain_name() + ".join");
    return this->_env | e._env;
  }

  congruence_domain_t operator&(const congruence_domain_t &e) const override {
    crab::CrabStats::count(domain_name() + ".count.meet");
    crab::ScopedCrabStats __st__(domain_name() + ".meet");
    return this->_env & e._env;
  }

  congruence_domain_t operator||(const congruence_domain_t &e) const override {
    crab::CrabStats::count(domain_name() + ".count.widening");
    crab::ScopedCrabStats __st__(domain_name() + ".widening");
    return this->_env || e._env;
  }

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

  congruence_domain_t operator&&(const congruence_domain_t &e) const override {
    crab::CrabStats::count(domain_name() + ".count.narrowing");
    crab::ScopedCrabStats __st__(domain_name() + ".narrowing");
    return this->_env && e._env;
  }

  void set(const variable_t &v, congruence_t i) {
    crab::CrabStats::count(domain_name() + ".count.assign");
    crab::ScopedCrabStats __st__(domain_name() + ".assign");
    this->_env.set(v, i);
  }

  void set(const variable_t &v, number_t n) {
    crab::CrabStats::count(domain_name() + ".count.assign");
    crab::ScopedCrabStats __st__(domain_name() + ".assign");
    this->_env.set(v, congruence_t(n));
  }

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

  virtual interval_t operator[](const variable_t &v) override {
    CRAB_WARN(domain_name(), "::operator[] not implemented");
    return interval_t::top();
  }

  congruence_t to_congruence(const variable_t &v) { return this->_env[v]; }

  congruence_t to_congruence(const linear_expression_t &expr) {
    congruence_t r(expr.constant());
    for (auto kv : expr) {
      congruence_t c(kv.first);
      r = r + (c * to_congruence(kv.second));
    }
    return r;
  }

  void operator+=(const linear_constraint_system_t &csts) override {
    crab::CrabStats::count(domain_name() + ".count.add_constraints");
    crab::ScopedCrabStats __st__(domain_name() + ".add_constraints");
    const std::size_t threshold = 10;
    if (!this->is_bottom()) {
      solver_t solver(csts, threshold);
      solver.run(this->_env);
    }
  }

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

    congruence_t r = e.constant();
    for (auto kv : e) {
      r = r + (kv.first * this->_env[kv.second]);
    }
    this->_env.set(x, r);
  }

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

    congruence_t yi = this->_env[y];
    congruence_t zi = this->_env[z];
    congruence_t xi = congruence_t::bottom();

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

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

    congruence_t yi = this->_env[y];
    congruence_t zi(k);
    congruence_t xi = congruence_t::bottom();

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

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

    crab::domains::BackwardAssignOps<congruence_domain_t>::assign(*this, x, e,
                                                                  inv);
  }

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

    crab::domains::BackwardAssignOps<congruence_domain_t>::apply(*this, op, x,
                                                                 y, z, inv);
  }

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

    crab::domains::BackwardAssignOps<congruence_domain_t>::apply(*this, op, x,
                                                                 y, z, inv);
  }

  // cast operations

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

  // bitwise operations

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

    congruence_t yi = this->_env[y];
    congruence_t zi = this->_env[z];
    congruence_t xi = congruence_t::bottom();

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

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

    congruence_t yi = this->_env[y];
    congruence_t zi(k);
    congruence_t xi = congruence_t::bottom();

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

  DEFAULT_SELECT(congruence_domain_t)

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

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

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

    _env.project(variables);
  }

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

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

    set(new_x, this->_env[x]);
  }

  void normalize() override {}

  void minimize() override {}

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

    _env.rename(from, to);
  }

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

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

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

    this->_env.write(o);
  }

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

    linear_constraint_system_t csts;
    if (is_bottom()) {
      csts += linear_constraint_t::get_false();
      return csts;
    }

    for (iterator it = this->_env.begin(); it != this->_env.end(); ++it) {
      const variable_t &v = it->first;
      congruence_t c = it->second;
      boost::optional<number_t> n = c.singleton();
      if (n) {
        csts += (v == *n);
      }
    }
    return csts;
  }

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

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

}; // class congruence_domain

} // namespace ikos

namespace crab {
namespace domains {

template <typename Number, typename VariableName>
struct abstract_domain_traits<ikos::congruence_domain<Number, VariableName>> {
  using number_t = Number;
  using varname_t = VariableName;
};

} // namespace domains
} // namespace crab


#define DEBUG 1
/**
 * File    : E.cpp
 * Author  : Kazune Takahashi
 * Created : 6/2/2020, 12:34:10 PM
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::multiprecision::cpp_int;
using ll = long long;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{30000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
template <typename T>
T gcd(T x, T y) { return y ? gcd(y, x % y) : x; }
template <typename T>
T lcm(T x, T y) { return x / gcd(x, y) * y; }
// ----- for C++17 -----
template <typename T>
int popcount(T x) // C++20
{
  int ans{0};
  while (x != 0)
  {
    ans += x & 1;
    x >>= 1;
  }
  return ans;
}
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
constexpr ll infty{1000000000000000LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}
// ----- main() -----

struct Edge
{
  bool valid;
  int src, dst;
  ll cost;

  Edge() {}
  Edge(int src, int dst, ll cost) : valid{true}, src{src}, dst{dst}, cost{cost} {}

  void added_edge(vector<vector<int>> &V)
  {
    V[src].push_back(dst);
  }

  void added_rev(vector<vector<int>> &V)
  {
    V[dst].push_back(src);
  }
};

class Solve
{
  int N, M;
  ll P;
  vector<Edge> E;

public:
  Solve(int N, int M) : N{N}, M{M}, E(M)
  {
    cin >> P;
    for (auto i = 0; i < M; ++i)
    {
      int A, B;
      ll C;
      cin >> A >> B >> C;
      --A;
      --B;
      E[i] = Edge(A, B, P - C);
    }
  }

  void flush()
  {
    determine_validness();
    cout << bf() << endl;
  }

private:
  ll bf()
  {
    vector<ll> D(N, infty);
    D[0] = 0;
    bool updated{false};
    for (auto t = 0; t < N + 2; ++t)
    {
      updated = false;
      for (auto const &e : E)
      {
        if (!e.valid)
        {
          continue;
        }
        auto tmp{D[e.src] + e.cost};
        if (D[e.dst] > tmp)
        {
          D[e.dst] = tmp;
          updated = true;
        }
      }
      if (!updated)
      {
        return max(0LL, -D[N - 1]);
      }
      if (t == N + 1)
      {
        return -1;
      }
    }
    assert(false);
    return -2;
  }

  void determine_validness()
  {
    auto table{valid_vertexes()};
    for (auto &e : E)
    {
      if (!(table[e.src] && table[e.dst]))
      {
        e.valid = false;
      }
#if DEBUG == 1
      else
      {
        cerr << "src: " << e.src << ", dst: " << e.dst << ", cost: " << e.cost << endl;
      }
#endif
    }
  }

  vector<bool> valid_vertexes()
  {
    vector<vector<int>> V(N);
    for (auto i = 0; i < M; ++i)
    {
      E[i].added_edge(V);
    }
    vector<bool> X(N, false);
    dfs(V, X, 0);
    vector<vector<int>> W(N);
    for (auto i = 0; i < M; ++i)
    {
      E[i].added_rev(W);
    }
    vector<bool> Y(N, false);
    dfs(W, Y, N - 1);
    vector<bool> res(N, false);
    for (auto i = 0; i < N; ++i)
    {
      res[i] = X[i] && Y[i];
    }
    return res;
  }

  void dfs(vector<vector<int>> const &V, vector<bool> &visited, int src, int parent = -1)
  {
    visited[src] = true;
    for (auto dst : V[src])
    {
      if (dst != parent && !visited[dst])
      {
        dfs(V, visited, dst, src);
      }
    }
  }
};

int main()
{
  int N, M;
  cin >> N >> M;
  Solve solve(N, M);
  solve.flush();
}


//
// robust_pose_pnp.cpp
//
// Manolis Lourakis (lourakis **at** ics forth gr), February 2022

#include <cmath>
#include <cstdio>
#include <iostream>
#include <cstring>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <RansacLib/ransac.h>
#include "robust_pose_pnp.h"

#undef ROBUST_POSE_PNP_DEBUG  // enables some sample size checks

namespace robust_pose_pnp {

PoseEstimator::PoseEstimator(const std::vector<sqpnp::_Point> *_3dpoints, const std::vector<sqpnp::_Projection> *_projections,
                             int samplesz, int nm_samplesz) {
  _3dpoints_ = _3dpoints;
  _projections_ = _projections;

  set_sample_sizes(samplesz, nm_samplesz);
}

// Robustly solves for camera pose lambda*x = R*X+t  with lambda > 0.
// Inlier and outlier indices are optionally returned (in increasing order) in idxInliers & idxOutliers.
// Returns 0 if successful, zero otherwise
int PoseEstimator::ransacfit(int miniter, int maxiter, double inlPcent, double outlThresh, robust_pose_pnp::Matrix34d& best_pose, std::vector<int> *idxInliers, std::vector<int> *idxOutliers) const {
  ransac_lib::LORansacOptions options;

  options.min_num_iterations_ = (unsigned int)miniter;
  options.max_num_iterations_ = (unsigned int)maxiter;
  // make sure that min_num_iterations_ are at least as many as predicted for the given outlier ratio and sample size
  options.min_num_iterations_ = ransac_lib::utils::NumRequiredIterations(inlPcent, 1.0-0.99, samplesz_, options.min_num_iterations_, options.max_num_iterations_);
  options.squared_inlier_threshold_ = outlThresh * outlThresh;
  options.final_least_squares_ = true;

  // additional params with their defaults
  //options.min_sample_multiplicator_ = 7;
  //options.num_lsq_iterations_ = 4;
  //options.num_lo_steps_ = 10;

#if 0
  std::random_device rand_dev;
  options.random_seed_ = rand_dev();
#endif

  ransac_lib::LocallyOptimizedMSAC<robust_pose_pnp::Matrix34d,
                                  std::vector<robust_pose_pnp::Matrix34d, Eigen::aligned_allocator<robust_pose_pnp::Matrix34d> >,
                                  robust_pose_pnp::PoseEstimator>
  lomsac;
  ransac_lib::RansacStatistics ransac_stats;

//std::cout << "... running LOMSAC" << std::endl;
  int num_inliers = lomsac.EstimateModel(options, *this, &best_pose, &ransac_stats);

  int npts = _3dpoints_->size();
  int num_outliers = npts - num_inliers;

  if (idxInliers != nullptr) {
    idxInliers->clear();
    idxInliers->resize(num_inliers);

    for (int i=0; i<num_inliers; ++i)
      (*idxInliers)[i] = ransac_stats.inlier_indices[i];
  }

  if (idxOutliers != nullptr) {
    idxOutliers->clear();
    idxOutliers->resize(num_outliers);

    // num_inliers == ransac_stats.inlier_indices.size()
    std::vector<int> isoutl(npts, 1);
    for (int i=0; i<num_inliers; ++i)
      isoutl[ransac_stats.inlier_indices[i]] = 0;
    for (int i=0, j=0; i<npts; ++i)
      if (isoutl[i]) (*idxOutliers)[j++] = i;
  }

/****/
  std::cout << "... LOMSAC found " << num_inliers << " inliers in "
            << ransac_stats.num_iterations << " iterations with an inlier "
            << "ratio of " << ransac_stats.inlier_ratio << std::endl;
/****/

  return num_inliers == 0;
}

#if 1
// this version creates local copies for the points corresponding to sample
int PoseEstimator::sqpnp_solve(const std::vector<int>& sample,
                                 std::vector<robust_pose_pnp::Matrix34d, Eigen::aligned_allocator<robust_pose_pnp::Matrix34d> > *poses) const {
  const int nsample = sample.size();
//std::cout<< "in solve "<<nsample<<"\n"<<std::flush;

  // pose from sample
  std::vector<sqpnp::_Point> points(nsample);
  std::vector<sqpnp::_Projection> projections(nsample);

  for(int i=0; i<nsample; ++i){
    int j = sample[i];

    points[i] = (*_3dpoints_)[j];
    projections[i] = (*_projections_)[j];
  }

  sqpnp::SolverParameters params;
  // more relaxed convergence criteria below
  //params.sqp_max_iteration = 10;
  //params.rank_tolerance = 1E-06;
  //params.sqp_squared_tolerance = 1E-09;

  std::vector<double>wghts(nsample, 1.0);
  sqpnp::PnPSolver solver(points, projections, wghts, params);

  if(solver.IsValid()){
    solver.Solve();

    poses->resize(solver.NumberOfSolutions());
    for (int i = 0; i < solver.NumberOfSolutions(); i++)
    {
      const sqpnp::SQPSolution *sol = solver.SolutionPtr(i);

//      std::cout << "\nSolution " << i << ":\n";
//      std::cout << *sol << std::endl;
//      std::cout << " Average squared projection error : " << solver.AverageSquaredProjectionErrors().at(i) << std::endl;

      (*poses)[i].block<3,3>(0,0) = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> >( sol->r_hat.data() );
      (*poses)[i].block<3,1>(0,3) = Eigen::Map<const Eigen::Matrix<double, 3, 1, Eigen::ColMajor> >( sol->t.data() );
    }
  }
  else return 0;

  return solver.NumberOfSolutions();
}

#else
// this uses directly the correspondences using the weights: 1 for those in sample, 0 for the rest
int PoseEstimator::sqpnp_solve(const std::vector<int>& sample,
                                 std::vector<robust_pose_pnp::Matrix34d, Eigen::aligned_allocator<robust_pose_pnp::Matrix34d> > *poses) const {
  const int npts = _3dpoints_->size();
  const int nsample = sample.size();
//std::cout<< "in solve "<<nsample<<"\n"<<std::flush;

  // pose from sample
  std::vector<double>wghts(npts, 0.0);
  for(int i=0; i<nsample; ++i){
    wghts[sample[i]] = 1.0;
  }

  sqpnp::SolverParameters params;
  // more relaxed convergence parameters next
  //params.sqp_max_iteration = 10;
  //params.rank_tolerance = 1E-06;
  //params.sqp_squared_tolerance = 1E-09;
  sqpnp::PnPSolver solver(*_3dpoints_, *_projections_, wghts, params);

  if(solver.IsValid()){
    solver.Solve();

    poses->resize(solver.NumberOfSolutions());
    for (int i = 0; i < solver.NumberOfSolutions(); i++)
    {
      const sqpnp::SQPSolution *sol = solver.SolutionPtr(i);

//      std::cout << "\nSolution " << i << ":\n";
//      std::cout << *sol << std::endl;
//      std::cout << " Average squared projection error : " << solver.AverageSquaredProjectionErrors().at(i) << std::endl;

      (*poses)[i].block<3,3>(0,0) = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> >( sol->r_hat.data() );
      (*poses)[i].block<3,1>(0,3) = Eigen::Map<const Eigen::Matrix<double, 3, 1, Eigen::ColMajor> >( sol->t.data() );
    }
  }
  else return 0;

  return solver.NumberOfSolutions();
}
#endif

// minimal solver with SQPnP
int PoseEstimator::MinimalSolver(const std::vector<int>& sample,
                                 std::vector<robust_pose_pnp::Matrix34d, Eigen::aligned_allocator<robust_pose_pnp::Matrix34d> > *poses) const {

  //const int npts = sample.size();
  //std::cout<< "in minimal solver (sqpnp) "<<npts<<"\n"<<std::flush;

  poses->clear();

#ifdef ROBUST_POSE_PNP_DEBUG
  if (npts < samplesz_) return 0;
#endif

  return PoseEstimator::sqpnp_solve(sample, poses);
}

// non minimal solver with SQPnP
int PoseEstimator::NonMinimalSolver(const std::vector<int>& sample,
                                    robust_pose_pnp::Matrix34d *pose) const {
  const int npts = sample.size();
//std::cout<< "in non minimal solver "<<npts<<"\n"<<std::flush;

  // note that RansacLib might call the non minimal solver with fewer than nm_samplesz_ points, hence the following check
  if (npts < nm_samplesz_) return 0;

  std::vector<Matrix34d, Eigen::aligned_allocator<Matrix34d> > poses;

  int n = PoseEstimator::sqpnp_solve(sample, &poses);

  if (n > 1) std::cerr << "More than one solution in PoseEstimator::NonMinimalSolver()!\n" << std::flush;

  if (n > 0) *pose = poses[0];

  return n > 0;
}

// Evaluates the pose on the i-th point pair.
double PoseEstimator::EvaluateModelOnPoint(const robust_pose_pnp::Matrix34d& pose,
                                           int i) const {
  const sqpnp::_Point& xyz = (*_3dpoints_)[i];

#if 0
  double Xc     =       pose(0, 0)*xyz.vector[0] + pose(0, 1)*xyz.vector[1] + pose(0, 2)*xyz.vector[2] + pose(0, 3),
         Yc     =       pose(1, 0)*xyz.vector[0] + pose(1, 1)*xyz.vector[1] + pose(1, 2)*xyz.vector[2] + pose(1, 3),
         inv_Zc = 1./ ( pose(2, 0)*xyz.vector[0] + pose(2, 1)*xyz.vector[1] + pose(2, 2)*xyz.vector[2] + pose(2, 3) );

#else
  Eigen::Vector3d prod = pose.block<3,3>(0,0) * xyz.vector + pose.block<3,1>(0,3);
  double Xc     =     prod(0),
         Yc     =     prod(1),
         inv_Zc = 1./ prod(2);

  double dx = Xc*inv_Zc - (*_projections_)[i].vector[0];
  double dy = Yc*inv_Zc - (*_projections_)[i].vector[1];
#endif

  return dx*dx + dy*dy;
}

}  // namespace robust_pose_pnp


/*
 * Mod_p.cpp
 *
 *  Created on: 15.09.2010
 *      Author: stephaniebayer
 */

#include "Mod_p.h"

#include "G_q.h"
extern G_q G;

#include <NTL/ZZ.h>
NTL_CLIENT

Mod_p::Mod_p() {
	// TODO Auto-generated constructor stub

}

//Creates an instance which belongs to Z_p
Mod_p::Mod_p(long p){

	mod = to_ZZ(p);
}

//Creates an instance which belongs to Z_p
Mod_p::Mod_p(ZZ p){

	mod = p;
}

//Creates an instance which belongs to Z_p with value v
Mod_p::Mod_p(long v, long p){

	mod = to_ZZ(p);
	val = to_ZZ(v) % mod;
}

//Creates an instance which belongs to Z_p with value v
Mod_p::Mod_p(ZZ v, long p){

	mod = to_ZZ(p);
	val = v % mod;
}

//Creates an instance which belongs to Z_p with value v
Mod_p::Mod_p(long v, ZZ p){

	mod = p;
	val = to_ZZ(v) % mod;
}

//Creates an instance which belongs to Z_p with value v
Mod_p::Mod_p(ZZ v, ZZ p){

	mod = p;
	val = v % mod;
}

Mod_p::~Mod_p() {
	// TODO Auto-generated destructor stub
}

//Changes the modular value p
void Mod_p::set_mod(long p){

	mod = to_ZZ(p);
}

//Changes the modular value p
void Mod_p::set_mod(ZZ p){

	mod = p;
}

//Sets or changes the value val
void Mod_p::set_val(long v){
	if (mod ==0)
		cout << "Please set a value for the modulus p" << endl;
	else
		val = to_ZZ(v)% mod;
}

//Sets or changes the value val
void Mod_p::set_val(ZZ p){

	if (mod ==0)
			cout << "Please set a value for the modulus p" << endl;
	else
		val = p % mod;
}

//Returns the modular value
ZZ Mod_p::get_mod() const{

	return mod;
}

//returns the value of the instance
ZZ Mod_p::get_val() const{

	return val;
}

//Assigment operator
void Mod_p::operator =(const Mod_p& el){

	mod = el.get_mod();
	val = el.get_val();
}

//Addition
Mod_p Mod_p::operator +(const Mod_p& el) const{
	ZZ temp;
	if (mod != el.get_mod())
	{		cout <<"It is not possible to add these elements" << endl;
			return Mod_p(0,1);}
	else
	{
		temp = AddMod(val, el.get_val(),mod);
		return Mod_p(temp, mod);}
}

//Subtraction
Mod_p Mod_p::operator -(const Mod_p& el) const{
	ZZ temp;
	if (mod != el.get_mod())
	{		cout <<"It is not possible to subtract these elements" << endl;
			return Mod_p(0,1);}
	else
	{
		temp = SubMod(val , el.get_val(),mod);
		return Mod_p(temp, mod);}
}

//Unary plus
Mod_p Mod_p::operator +() const{

	return Mod_p(val, mod);

}

//Unary Minus
Mod_p Mod_p::operator -() const{

	ZZ temp;
	temp = -val % mod;
	return Mod_p(temp, mod);

}

//Multiplication
Mod_p Mod_p::operator *(const Mod_p& el) const{

	ZZ temp;
	if (mod != el.get_mod())
	{		cout <<"It is not possible to multiply these elements" << endl;
			return Mod_p(0,1);}
	else
	{
		temp = MulMod(val,el.get_val(),mod);
			return Mod_p(temp, mod);}
}

//Division
Mod_p Mod_p::operator /(const Mod_p& el) const{

	ZZ temp;
	if (mod != el.get_mod())
	{		cout <<"It is not possible to divide these elements" << endl;
			return Mod_p(0,1);}
	else
	{
		temp = val*InvMod(el.get_val(), mod) ;
			return Mod_p(temp, mod);}
}

//Increment prefix
Mod_p& Mod_p::operator ++(){

	++val;
	return *this;
}

//Increment suffix
Mod_p Mod_p::operator ++(int){

    Mod_p temp = *this;
    ++val;
    return temp;

}

//Decrement prefix
Mod_p& Mod_p::operator --(){

	--val;
	return *this;
}

//Decrement suffix
Mod_p Mod_p::operator --(int){

    Mod_p temp = *this;
    --val;
    return temp;

}

//Equal to
bool Mod_p::operator ==(const Mod_p& el) const{

	if (mod == el.get_mod())
	{
		if (val == el.get_val())
		{
			return true;
		}
		else
		{
			return false;
		}
	}
	else
	{
		return false;
	}
}

//Not equal to
bool Mod_p::operator !=(const Mod_p& el) const{

	if (mod != el.get_mod())
	{

		return true;

	}
	else
	{
		if (val != el.get_val())
		{
			return true;
		}
		else
		{
			return false;
		}
	}
}

//Smaller
bool Mod_p::operator <(const Mod_p& el) const{

	if (mod != el.get_mod())
	{
		cout << "It is not possible to compare to elements with different modulus" << endl;
		return false;

	}
	else
	{
		if (val < el.get_val())
		{
			return true;
		}
		else
		{
			return false;
		}
	}
}

//Bigger
bool Mod_p::operator >(const Mod_p& el) const{

	if (mod != el.get_mod())
	{
		cout << "It is not possible to compare to elements with different modulus" << endl;
		return false;

	}
	else
	{
		if (val > el.get_val())
		{
			return true;
		}
		else
		{
			return false;
		}
	}
}

// Smaller equal
bool Mod_p::operator <=(const Mod_p& el) const{

	if (mod != el.get_mod())
	{
		cout << "It is not possible to compare to elements with different modulus" << endl;
		return false;

	}
	else
	{
		if (val <=el.get_val())
		{
			return true;
		}
		else
		{
			return false;
		}
	}
}

//Bigger equal
bool Mod_p::operator >=(const Mod_p& el) const{

	if (mod != el.get_mod())
	{
		cout << "It is not possible to compare to elements with different modulus" << endl;
		return false;

	}
	else
	{
		if (val >=el.get_val())
		{
			return true;
		}
		else
		{
			return false;
		}
	}
}

//Addition assignment
Mod_p& Mod_p::operator +=(const Mod_p& el){

	if (mod != el.get_mod())
	{
		cout << "It is not possible to add these elements" << endl;
		return *this;
	}
	else
	{
		val = AddMod(val, el.get_val(), mod);
		return *this;
	}
}

//Subtraction assignment
Mod_p& Mod_p::operator -=(const Mod_p& el){

	if (mod != el.get_mod())
	{
		cout << "It is not possible to add these elements" << endl;
		return *this;
	}
	else
	{
		val = SubMod(val, el.get_val(), mod);
		return *this;
	}
}

//Multiplication assignment
Mod_p& Mod_p::operator *=(const Mod_p& el){

	if (mod != el.get_mod())
	{
		cout << "It is not possible to add these elements" << endl;
		return *this;
	}
	else
	{
		val = MulMod(val, el.get_val(),mod);
		return *this;
	}
}

//Division assignment
Mod_p& Mod_p::operator /=(const Mod_p& el){

	if (mod != el.get_mod())
	{
		cout << "It is not possible to add these elements" << endl;
		return *this;
	}
	else
	{
		val = MulMod(val,InvMod(el.get_val(),mod), mod);
		return *this;
	}
}

//Output operator, output format is val (modular mod)
ostream& operator <<(ostream& os , const Mod_p &b){

	return os << b.get_val();
}

//Input operator,
istream& operator>>(istream& is, Mod_p &b){
	ZZ val, mod;

	is>>val;

	mod = G.get_mod();
	b = Mod_p(val, mod);
	return is;
}

//Returns the inverse modular p of the element
Mod_p Mod_p::inv(){

	ZZ temp;
	temp = InvMod(val,mod);
	return Mod_p(temp,mod);
}

//Returns the inverse modular p of the element el
Mod_p Mod_p::inv(const Mod_p& el){
	ZZ temp;
	ZZ mod=el.get_mod();
	temp = InvMod(el.get_val(),mod);
	return Mod_p(temp, mod);

}

void Mod_p::inv(Mod_p&a, const Mod_p& el){
	ZZ temp;
	ZZ mod=el.get_mod();
	InvMod(temp, el.get_val(),mod);
	a= Mod_p(temp, mod);

}

//Multiplication functions
void Mod_p::mult(Mod_p& a , const Mod_p& b, const Mod_p& c){
	ZZ temp;
	ZZ mod=b.get_mod();
	MulMod(temp,b.get_val(), c.get_val(),mod);
	a= Mod_p(temp,mod);
}

//exponentiation functions
Mod_p Mod_p::expo(const long e){

	ZZ temp;
	temp = PowerMod(val,e,mod);
	return Mod_p(temp,mod);
}

Mod_p Mod_p::expo(Mod_p& a, long e){

	ZZ temp;
	ZZ mod=a.get_mod();
	temp = PowerMod(a.get_val(),e,mod);
	return Mod_p(temp,mod);
}

void Mod_p::expo(Mod_p& a ,const Mod_p& b,const long e){
	ZZ temp;
	ZZ mod=b.get_mod();
	PowerMod(temp, b.get_val(),e,mod);
	a= Mod_p(temp,mod);
}

void Mod_p::expo(Mod_p& a , const Mod_p& b, const ZZ e){
	ZZ temp;
	ZZ mod=b.get_mod();
	PowerMod(temp, b.get_val(), e, mod);
	a= Mod_p(temp,mod);
}

Mod_p Mod_p::expo(const ZZ e){

	ZZ temp;
	PowerMod(temp, val,e, mod);
	return Mod_p(temp,mod);
}

Mod_p Mod_p::expo( Mod_p& a, ZZ e){

	ZZ temp;
	ZZ mod=a.get_mod();
	temp = PowerMod(a.get_val(),e,mod);
	return Mod_p(temp,a.mod);
}


#include <algorithm>
#include <cmath>
#include <iostream>

#include <Eigen/Cholesky>

#include "convex_nmf.hpp"
#include "progress_reporter.hpp"
#include "projections.hpp"

namespace convexnmf {

void ConvexNMF::SetSparsifierType(const std::string &type_name, bool verbose) {

    if (type_name == "L2") {
        if (verbose)
            std::cout << "Set Sparsifier to L2." << std::endl;
        sparsifier_type = SparsifierType::L2;
    } else if (type_name == "LInfty") {
        if (verbose)
            std::cout << "Set Sparsifier to LInfty." << std::endl;
        sparsifier_type = SparsifierType::LInfty;
    } else {
        std::string current_setting = sparsifier_type == SparsifierType::L2 ? "L2" : "LInfty";
        std::cout << "Sparsifier type not recognized so not set, choose either 'L2' or 'LInfty'.";
        std::cout << " Currently set to " + current_setting + " \n";
    }
}

ConvexNMFResults ConvexNMF::Fit(const MatrixRef &X, Scalar lambda) {
    // TODO: Can move this to the Python front end.
    int    N      = X.cols();
    Matrix W_init = Matrix::Identity(N, N);
    return Fit(X, lambda, W_init);
}

ConvexNMFResults ConvexNMF::Fit(const MatrixRef &X, Scalar lambda, const MatrixRef &W_init) {

    ConvexNMFProgressReporter reporter;
    auto sparsifier = RowSparsifier::Create(sparsifier_type);
    // W Update set up
    int N = X.cols();
    int T = X.rows();

    Matrix XTXplusDeltaId = X.transpose() * X + rho * Matrix::Identity(N, N);
    Matrix XTX            = X.transpose() * X;

    std::cout << "Factoring .... ";
    Eigen::LLT<Matrix> cholesky_solver;
    cholesky_solver.compute(XTXplusDeltaId);
    std::cout << "Done. \n";

    // Set up W, Z, U
    const Scalar sqrt_size = static_cast<Scalar>(std::sqrt(N * N));
    Matrix       W         = W_init;
    Matrix       Z_1 = W_init, Z_2 = W_init, Z_3 = W_init;
    Matrix Y_1 = Matrix::Identity(N, N), Y_2 = Matrix::Identity(N, N), Y_3 = Matrix::Identity(N, N);

    if (report_progress) {
        reporter.PrintHeader();
    }

    for (int iteration = 1; iteration <= max_iterations; ++iteration) {
        // Updates

        Matrix previous_average = (Z_1 + Z_2 + Z_3) / 3.0; // Used in monitoring convergence.

        Z_1 = cholesky_solver.solve(XTX + rho * W - Y_1);
        Z_2 = sparsifier->Evaluate(W - Y_2 / rho, lambda, rho);
        Z_3 = (W - Y_3 / rho).cwiseMax(0.0);

        W = (Z_1 + Z_2 + Z_3 + (Y_1 + Y_2 + Y_3) / rho) / 3.0;

        Y_1 = Y_1 + rho * (Z_1 - W);
        Y_2 = Y_2 + rho * (Z_2 - W);
        Y_3 = Y_3 + rho * (Z_3 - W);

        // check primal and dual residual
        Scalar primal_squared =
            (W - Z_1).squaredNorm() + (W - Z_2).squaredNorm() + (W - Z_3).squaredNorm();
        Scalar primal = std::sqrt(primal_squared);

        Matrix current_average = (Z_1 + Z_2 + Z_3) / 3.0;
        Scalar dual            = 3.0 * rho * (current_average - previous_average).norm();

        Scalar z_norm     = std::sqrt(Z_1.squaredNorm() + Z_2.squaredNorm() + Z_3.squaredNorm());
        Scalar primal_tol = std::sqrt(3.0) * abs_tolerance * sqrt_size +
                            relative_tolerance * std::max(3 * W.norm(), z_norm);
        Scalar dual_tol = abs_tolerance * sqrt_size + relative_tolerance * (Y_1 + Y_2 + Y_3).norm();

        if (report_progress && (iteration % iteration_to_print == 0)) {
            reporter.PrintStatistics(iteration, primal, primal_tol, dual, dual_tol);
        }
        if ((primal < primal_tol) && (dual < dual_tol)) {

            ConvexNMFResults results{W, primal, dual, true};
            if (report_progress) {
                reporter.PrintConverged(iteration, primal, primal_tol, dual, dual_tol);
            }
            return results;
        } else if (iteration == max_iterations) {
            ConvexNMFResults results{W, primal, dual, false};
            if (report_progress) {
                reporter.PrintHitMaxIterations(iteration, primal, primal_tol, dual, dual_tol);
            }
            return results;
        }
    }
}

} // namespace convexnmf


/* Boost libs/numeric/odeint/performance/openmp/osc_chain_1d_system.hpp

 Copyright 2013 Karsten Ahnert
 Copyright 2013 Mario Mulansky
 Copyright 2013 Pascal Germroth

 stronlgy nonlinear hamiltonian lattice

 Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef SYSTEM_HPP
#define SYSTEM_HPP

#include <vector>
#include <cmath>
#include <iostream>

#include <omp.h>

#include <boost/math/special_functions/sign.hpp>
#include <boost/numeric/odeint/external/openmp/openmp.hpp>

namespace checked_math {
    inline double pow( double x , double y )
    {
        if( x==0.0 )
            // 0**y = 0, don't care for y = 0 or NaN
            return 0.0;
        using std::pow;
        using std::abs;
        return pow( abs(x) , y );
    }
}

double signed_pow( double x , double k )
{
    using boost::math::sign;
    return checked_math::pow( x , k ) * sign(x);
}

struct osc_chain {

    const double m_kap, m_lam;

    osc_chain( const double kap , const double lam )
        : m_kap( kap ) , m_lam( lam )
    { }

    // Simple case with openmp_range_algebra
    void operator()( const std::vector<double> &q ,
                           std::vector<double> &dpdt ) const
    {
        const size_t N = q.size();
        double coupling_lr = 0;
        size_t last_i = N;
        #pragma omp parallel for firstprivate(coupling_lr, last_i) lastprivate(coupling_lr) schedule(runtime)
        for(size_t i = 0 ; i < N - 1 ; ++i)
        {
            if(i > 0 && i != last_i + 1)
                coupling_lr = signed_pow( q[i-1]-q[i] , m_lam-1 );
            dpdt[i] = -signed_pow( q[i] , m_kap-1 ) + coupling_lr;
            coupling_lr = signed_pow( q[i] - q[i+1] , m_lam-1 );
            dpdt[i] -= coupling_lr;
            last_i = i;
        }
        dpdt[N-1] = -signed_pow( q[N-1] , m_kap-1 ) + coupling_lr;
    }

    // Split case with openmp_algebra
    void operator()( const boost::numeric::odeint::openmp_state<double> &q ,
                           boost::numeric::odeint::openmp_state<double> &dpdt ) const
    {
        const size_t M = q.size();
        #pragma omp parallel for schedule(runtime)
        for(size_t i = 0 ; i < M ; ++i)
        {
            const std::vector<double> &_q = q[i];
            std::vector<double> &_dpdt = dpdt[i];
            const size_t N = q[i].size();
            double coupling_lr = 0;
            if(i > 0) coupling_lr = signed_pow( q[i-1].back() - _q[0] , m_lam-1 );
            for(size_t j = 0 ; j < N-1 ; ++j)
            {
                _dpdt[j] = -signed_pow( _q[j] , m_kap-1 ) + coupling_lr;
                coupling_lr = signed_pow( _q[j] - _q[j+1] , m_lam-1 );
                _dpdt[j] -= coupling_lr;
            }
            _dpdt[N-1] = -signed_pow( _q[N-1] , m_kap-1 ) + coupling_lr;
            if(i + 1 < M) _dpdt[N-1] -= signed_pow( _q[N-1] - q[i+1].front() , m_lam-1 );
        }
    }

};

#endif


#include <Eigen/Core>
#include <fstream>
#include <iostream>
#include <string>

#include <sophus/se3.h>
#include <sophus/so3.h>

#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>

using namespace std;
using Sophus::SE3;
using Sophus::SO3;

/************************************************
 * 本程序演示如何用 gtsam 进行位姿图优化
 * sphere.g2o 是人工生成的一个 Pose graph，我们来优化它。
 * 与 g2o 相似，在 gtsam 中添加的是因子，相当于误差
 * **********************************************/

int main(int argc, char** argv) {
  if (argc != 2) {
    cout << "Usage: pose_graph_gtsam sphere.g2o" << endl;
    return 1;
  }
  ifstream fin(argv[1]);
  if (!fin) {
    cout << "file " << argv[1] << " does not exist." << endl;
    return 1;
  }

  gtsam::NonlinearFactorGraph::shared_ptr graph(
      new gtsam::NonlinearFactorGraph);                  // gtsam的因子图
  gtsam::Values::shared_ptr initial(new gtsam::Values);  // 初始值
  // 从g2o文件中读取节点和边的信息
  int cntVertex = 0, cntEdge = 0;
  cout << "reading from g2o file" << endl;

  while (!fin.eof()) {
    string tag;
    fin >> tag;
    if (tag == "VERTEX_SE3:QUAT") {
      // 顶点
      gtsam::Key id;
      fin >> id;
      double data[7];
      for (int i = 0; i < 7; i++) fin >> data[i];
      // 转换至gtsam的Pose3
      gtsam::Rot3 R =
          gtsam::Rot3::Quaternion(data[6], data[3], data[4], data[5]);
      gtsam::Point3 t(data[0], data[1], data[2]);
      initial->insert(id, gtsam::Pose3(R, t));  // 添加初始值
      cntVertex++;
    } else if (tag == "EDGE_SE3:QUAT") {
      // 边，对应到因子图中的因子
      gtsam::Matrix m = gtsam::I_6x6;  // 信息矩阵
      gtsam::Key id1, id2;
      fin >> id1 >> id2;
      double data[7];
      for (int i = 0; i < 7; i++) fin >> data[i];
      gtsam::Rot3 R =
          gtsam::Rot3::Quaternion(data[6], data[3], data[4], data[5]);
      gtsam::Point3 t(data[0], data[1], data[2]);
      for (int i = 0; i < 6; i++)
        for (int j = i; j < 6; j++) {
          double mij;
          fin >> mij;
          m(i, j) = mij;
          m(j, i) = mij;
        }

      // g2o的信息矩阵定义方式与gtsam不同，这里对它进行修改
      gtsam::Matrix mgtsam = gtsam::I_6x6;
      mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3);  // cov rotation
      mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0);  // cov translation
      mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3);  // off diagonal
      mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0);  // off diagonal

      gtsam::SharedNoiseModel model =
          gtsam::noiseModel::Gaussian::Information(mgtsam);  // 高斯噪声模型
      gtsam::NonlinearFactor::shared_ptr factor(
          new gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(R, t),
                                                 model)  // 添加一个因子
          );
      graph->push_back(factor);
      cntEdge++;
    }
    if (!fin.good()) break;
  }

  cout << "read total " << cntVertex << " vertices, " << cntEdge << " edges."
       << endl;
  // 固定第一个顶点，在gtsam中相当于添加一个先验因子
  gtsam::NonlinearFactorGraph graphWithPrior = *graph;
  gtsam::noiseModel::Diagonal::shared_ptr priorModel =
      gtsam::noiseModel::Diagonal::Variances(
          (gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6).finished());
  gtsam::Key firstKey = 0;
  for (const gtsam::Values::ConstKeyValuePair& key_value : *initial) {
    cout << "Adding prior to g2o file " << endl;
    graphWithPrior.add(gtsam::PriorFactor<gtsam::Pose3>(
        key_value.key, key_value.value.cast<gtsam::Pose3>(), priorModel));
    break;
  }

  // 开始因子图优化，配置优化选项
  cout << "optimizing the factor graph" << endl;
  // 我们使用 LM 优化
  gtsam::LevenbergMarquardtParams params_lm;
  params_lm.setVerbosity("ERROR");
  params_lm.setMaxIterations(20);
  params_lm.setLinearSolverType("MULTIFRONTAL_QR");
  gtsam::LevenbergMarquardtOptimizer optimizer_LM(graphWithPrior, *initial,
                                                  params_lm);

  // 你可以尝试下 GN
  // gtsam::GaussNewtonParams params_gn;
  // params_gn.setVerbosity("ERROR");
  // params_gn.setMaxIterations(20);
  // params_gn.setLinearSolverType("MULTIFRONTAL_QR");
  // gtsam::GaussNewtonOptimizer optimizer ( graphWithPrior, *initial, params_gn
  // );

  gtsam::Values result = optimizer_LM.optimize();
  cout << "Optimization complete" << endl;
  cout << "initial error: " << graph->error(*initial) << endl;
  cout << "final error: " << graph->error(result) << endl;

  cout << "done. write to g2o ... " << endl;
  // 写入 g2o 文件，同样伪装成 g2o 中的顶点和边，以便用 g2o_viewer 查看。
  // 顶点咯
  ofstream fout("result_gtsam.g2o");
  for (const gtsam::Values::ConstKeyValuePair& key_value : result) {
    gtsam::Pose3 pose = key_value.value.cast<gtsam::Pose3>();
    gtsam::Point3 p = pose.translation();
    gtsam::Quaternion q = pose.rotation().toQuaternion();
    fout << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " " << p.y()
         << " " << p.z() << " " << q.x() << " " << q.y() << " " << q.z() << " "
         << q.w() << " " << endl;
  }
  // 边咯
  for (gtsam::NonlinearFactor::shared_ptr factor : *graph) {
    gtsam::BetweenFactor<gtsam::Pose3>::shared_ptr f =
        dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose3>>(factor);
    if (f) {
      gtsam::SharedNoiseModel model = f->noiseModel();
      gtsam::noiseModel::Gaussian::shared_ptr gaussianModel =
          dynamic_pointer_cast<gtsam::noiseModel::Gaussian>(model);
      if (gaussianModel) {
        // write the edge information
        gtsam::Matrix info =
            gaussianModel->R().transpose() * gaussianModel->R();
        gtsam::Pose3 pose = f->measured();
        gtsam::Point3 p = pose.translation();
        gtsam::Quaternion q = pose.rotation().toQuaternion();
        fout << "EDGE_SE3:QUAT " << f->key1() << " " << f->key2() << " "
             << p.x() << " " << p.y() << " " << p.z() << " " << q.x() << " "
             << q.y() << " " << q.z() << " " << q.w() << " ";
        gtsam::Matrix infoG2o = gtsam::I_6x6;
        infoG2o.block(0, 0, 3, 3) = info.block(3, 3, 3, 3);  // cov translation
        infoG2o.block(3, 3, 3, 3) = info.block(0, 0, 3, 3);  // cov rotation
        infoG2o.block(0, 3, 3, 3) = info.block(0, 3, 3, 3);  // off diagonal
        infoG2o.block(3, 0, 3, 3) = info.block(3, 0, 3, 3);  // off diagonal
        for (int i = 0; i < 6; i++)
          for (int j = i; j < 6; j++) {
            fout << infoG2o(i, j) << " ";
          }
        fout << endl;
      }
    }
  }
  fout.close();
  cout << "done." << endl;
}


#include <iostream>
#include <vector>

#include <boost/range/adaptors.hpp>
namespace ba = boost::adaptors;

#include <dionysus/simplex.h>
#include <dionysus/filtration.h>
#include <dionysus/fields/zp.h>
#include <dionysus/fields/z2.h>
#include <dionysus/zigzag-persistence.h>

namespace d = dionysus;

#include <format.h>

//typedef     d::Z2Field                      K;
typedef     d::ZpField<>                    K;
typedef     d::Simplex<>                    Simplex;
typedef     d::Filtration<Simplex>          Filtration;
//typedef     d::ZigzagFiltration<Simplex>    Filtration;
typedef     d::ZigzagPersistence<K>         Persistence;

typedef     typename Persistence::Index                     Index;
typedef     typename Filtration::Cell                       Cell;
typedef     d::ChainEntry<K, Cell>                          CellChainEntry;
typedef     d::ChainEntry<K, Index>                         ChainEntry;

int main()
{
    K k(11);
    Filtration      filtration { Simplex{0}, Simplex{1}, Simplex{2}, Simplex{0,1}, Simplex{0,2}, Simplex{1,2}, Simplex{0,1,2} };
    Persistence     persistence(k);

    unsigned op = 0;
    for(auto& c : filtration)
    {
        fmt::print("[{}] Adding: {} : {}\n", op++, c, boost::distance(c.boundary(persistence.field())));
        Index pair = persistence.add(c.boundary(persistence.field()) |
                                                ba::transformed([&filtration](const CellChainEntry& e)
                                                { return ChainEntry(e.element(), filtration.index(e.index())); }));
        //if (pair != persistence.unpaired())
        //    std::cout << "[" << pair << " - " << i << "]" << std::endl;
        //++i;
    }

    for (int i = 6; i >= 0; --i)
    {
        fmt::print("[{}] Removing: {}\n", op++, i);
        Index pair = persistence.remove(i);
        if (pair == Persistence::unpaired())
            fmt::print("Birth\n");
        else
            fmt::print("Death: {}\n", pair);
    }
}


#include <iostream>
// #include <boost/mpi.hpp>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

template< typename matrix_type, typename vector_type, typename left_precon_type,
	  typename right_precon_type, typename iter_type >
void test_solver(const matrix_type& A, vector_type& x, const vector_type& b,
	   const left_precon_type& L, const right_precon_type& R, iter_type& iter)
{
    itl::bicg(A, x, b, L, iter); // GEHT NICHT!
    itl::bicgstab(A, x, b, L, iter);
    itl::bicgstab_2(A, x, b, L, iter);
    itl::bicgstab_ell(A, x, b, L, R, iter, 3);
    itl::cg(A, x, b, L, R, iter);
    itl::cgs(A, x, b, L, iter);
    itl::gmres(A, x, b, L, R, iter, 30);
    itl::qmr(A, x, b, L, R, iter);
    itl::idr_s(A, x, b, L, R, iter,3);
    itl::tfqmr(A, x, b, L, R, iter);
}

template< typename matrix_type, typename vector_type >
int trans_test(const matrix_type& A, vector_type& x, const vector_type& b)
{
    using namespace mtl;
    using namespace itl;

    typedef mat::transposed_view<const matrix_type> trans_matrix_type;
    trans_matrix_type B(A);

    // Create an ILU(0) preconditioner
    pc::ilu_0<matrix_type>        	P0(A);
    pc::ilu_0<trans_matrix_type>	P1(B);

    pc::identity<matrix_type>		Id0(A);
    pc::identity<trans_matrix_type> 	Id1(B);

    // Termination criterion: r < 1e-6 * b or N iterations
    noisy_iteration<double>       iter(b, 500, 1.e-6);

    test_solver(A, x, b, P0, Id0, iter);
    test_solver(A, x, b, P1, Id1, iter); // GEHT NICHT!
    test_solver(B, x, b, P0, Id0, iter);
    test_solver(B, x, b, P1, Id1, iter); // GEHT NICHT!

    return 0;
}

int main(// int argc, char* argv[]
	 )
{
    using namespace mtl;

    // mtl::par::environment env(argc, argv);

    const int size = 10, N = size * size;
    // typedef mat::distributed<compressed2D<double> >  matrix_type;
    typedef compressed2D<double>   matrix_type;
    matrix_type                                         A;
    laplacian_setup(A, size, size);

    mtl::dense_vector<double>                           x(N, 1.0), b;

    b= A * x;
    x= 0;

    int error_code = trans_test(A, x, b);

    return error_code;
}


////////////////////////////////////////////////////////////////////////////////////////////////////
//                               This file is part of CosmoScout VR                               //
//      and may be used under the terms of the MIT license. See the LICENSE file for details.     //
//                        Copyright: (c) 2019 German Aerospace Center (DLR)                       //
////////////////////////////////////////////////////////////////////////////////////////////////////

#ifndef CS_UTILS_CONVERSIONS_HPP
#define CS_UTILS_CONVERSIONS_HPP

#include "cs_utils_export.hpp"

#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>

/// This namespace contains utility functions for converting numbers between different units of
/// measuring.
namespace cs::utils::convert {

template <typename T>
T lightyearsToMeters(T lightyears) {
  return lightyears * 9460730472580800.0;
}

template <typename T>
T metersToLightyears(T meters) {
  return meters / 9460730472580800.0;
}

/// Converts AU to meters. One AU is equivalent to the average distance between the Earth and the
///// Sun.
template <typename T>
T astronomicalUnitsToMeters(T astronomicalUnits) {
  return astronomicalUnits * 149597870700.0;
}

/// Converts meters to AU. One AU is equivalent to the average distance between the Earth and the
/// Sun.
template <typename T>
T metersToAstronomicalUnits(T meters) {
  return meters / 149597870700.0;
}

template <typename T>
T toRadians(T degrees) {
  return degrees * glm::pi<double>() / 180.0;
}

template <typename T>
T toDegrees(T radians) {
  return radians * 180.0 / glm::pi<double>();
}

/// Transform cartesian (x,y,z) coordinates to geodetic (lng, lat, height above surface)
/// coordinates.
CS_UTILS_EXPORT glm::dvec3 toLngLatHeight(
    glm::dvec3 const& cartesian, double radiusE, double radiusP);

/// Transform geodetic coordinates (lng, lat) LngLat and elevation height to cartesian (x,y,z)
/// coordinates for an ellipsoid of equatorial radius radiusE and polar radius radiusP. Height is an
/// offset along the normal of the ellipsoid at (lng, lat).
CS_UTILS_EXPORT glm::dvec3 toCartesian(
    glm::dvec2 const& lngLat, double radiusE, double radiusP, double height = 0.0);

/// Convert latitudes.
CS_UTILS_EXPORT double geocentricToGeodetic(double lat, double radiusE, double radiusP);
CS_UTILS_EXPORT double geocentricToParametric(double lat, double radiusE, double radiusP);
CS_UTILS_EXPORT double geodeticToGeocentric(double lat, double radiusE, double radiusP);
CS_UTILS_EXPORT double geodeticToParametric(double lat, double radiusE, double radiusP);
CS_UTILS_EXPORT double parametricToGeocentric(double lat, double radiusE, double radiusP);
CS_UTILS_EXPORT double parametricToGeodetic(double lat, double radiusE, double radiusP);

/// Returns the normal vector (unit length) to the ellipsoid with equatorial radius radiusE and
/// polar radius radiusP at geodetic coordinates (lng, lat) LngLat.
CS_UTILS_EXPORT glm::dvec3 lngLatToNormal(glm::dvec2 const& lngLat, double radiusE, double radiusP);

/// Returns the geodetic coordinates (lng, lat) for a given normal vector.
CS_UTILS_EXPORT glm::dvec2 normalToLngLat(glm::dvec3 const& normal, double radiusE, double radiusP);

/// Convert boost::posix_time::ptime to spice time, which is defined by the
/// Barycentric Dynamical Time.
CS_UTILS_EXPORT double toSpiceTime(boost::posix_time::ptime const& tIn);

/// Convert a time string to spice time, which is defined by the Barycentric Dynamical Time.
CS_UTILS_EXPORT double toSpiceTime(std::string const& tIn);

/// Convert time in seconds since 2000-01-01 12:00:00.000 to boost::posix_time::ptime. Be
/// aware, that fractional seconds will be truncated. //DocTODO
CS_UTILS_EXPORT boost::posix_time::ptime toBoostTime(double tIn);

} // namespace cs::utils::convert

#endif // CS_UTILS_CONVERSIONS_HPP


// C++ code to wrap some Boost functions and be able to use them in C.

#include <boost/integer/common_factor.hpp>
#include "common_factor.h"

int boost_gcd (int a, int b) {
        return boost::integer::gcd(a, b);
}

int boost_lcm (int a, int b) {
        return boost::integer::lcm(a, b);
}


/*
 *
 * Copyright (c) Toon Knapen, Karl Meerbergen & Kresimir Fresl 2003
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_GEES_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_GEES_HPP

#include <boost/numeric/bindings/traits/type.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>
#include <boost/numeric/bindings/traits/detail/utils.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // Schur factorization of general matrix.
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * gees() computes a Schur factorization of an N-by-N matrix A.
     *
     * The Schur decomposition is A = U S * herm(U)  where  U  is a
     * unitary matrix and S is upper triangular. The eigenvalues of A
     * are on the main diagonal of S. If A is real, S is in pseudo
     * upper triangular form.
     *
     * Workspace is organized following the arguments in the calling sequence.
     *  optimal_workspace() : for optimizing use of blas 3 kernels
     *  minimal_workspace() : minimum size of workarrays, but does not allow for optimization
     *                        of blas 3 kernels
     *  workspace( work ) for real matrices where work is a real array with
     *                    vector_size( work ) >= 3*matrix_size1( a )
     *  workspace( work, rwork ) for complex matrices where work is a complex
     *                           array with vector_size( work ) >= 2*matrix_size1( a )
     *                           and rwork is a real array with
     *                           vector_size( rwork ) >= matrix_size1( a ).
     */

    namespace detail {
      inline
      void gees (char const jobvs, char const sort, logical_t* select, int const n,
                 float* a, int const lda, int& sdim, traits::complex_f* w,
                 float* vs, int const ldvs, float* work, int const lwork,
                 bool* bwork, int& info)
      {
        traits::detail::array<float> wr(n);
        traits::detail::array<float> wi(n);
        LAPACK_SGEES (&jobvs, &sort, select, &n, a, &lda, &sdim,
                      traits::vector_storage(wr), traits::vector_storage(wi),
                      vs, &ldvs, work, &lwork, bwork, &info);
        traits::detail::interlace(traits::vector_storage(wr),
                                  traits::vector_storage(wr)+n,
                                  traits::vector_storage(wi),
                                  w);
      }

      inline
      void gees (char const jobvs, char const sort, logical_t* select, int const n,
                 double* a, int const lda, int& sdim, traits::complex_d* w,
                 double* vs, int const ldvs, double* work, int const lwork,
                 bool* bwork, int& info)
      {
        traits::detail::array<double> wr(n);
        traits::detail::array<double> wi(n);
        LAPACK_DGEES (&jobvs, &sort, select, &n, a, &lda, &sdim,
                      traits::vector_storage(wr), traits::vector_storage(wi),
                      vs, &ldvs, work, &lwork, bwork, &info);
        traits::detail::interlace(traits::vector_storage(wr),
                                  traits::vector_storage(wr)+n,
                                  traits::vector_storage(wi),
                                  w);
      }

      inline
      void gees (char const jobvs, char const sort, logical_t* select, int const n,
                 traits::complex_f* a, int const lda, int& sdim, traits::complex_f* w,
                 traits::complex_f* vs, int const ldvs,
                 traits::complex_f* work, int lwork, float* rwork, bool* bwork,
                 int& info)
      {
        LAPACK_CGEES (&jobvs, &sort, select, &n, traits::complex_ptr(a), &lda, &sdim,
                      traits::complex_ptr(w), traits::complex_ptr (vs), &ldvs,
                      traits::complex_ptr(work), &lwork, rwork, bwork, &info);
      }

      inline
      void gees (char const jobvs, char const sort, logical_t* select, int const n,
                 traits::complex_d* a, int const lda, int& sdim, traits::complex_d* w,
                 traits::complex_d* vs, int const ldvs,
                 traits::complex_d* work, int lwork, double* rwork, bool* bwork,
                 int& info)
      {
        LAPACK_ZGEES (&jobvs, &sort, select, &n, traits::complex_ptr(a), &lda, &sdim,
                      traits::complex_ptr(w), traits::complex_ptr(vs), &ldvs,
                      traits::complex_ptr(work), &lwork, rwork, bwork, &info);
      }

    }

    namespace detail {
       /// Compute Schur factorization, passing one work array.
       template <typename MatrA, typename SchVec, typename EigVal, typename Work>
       inline
       int gees (char jobvs, MatrA& a, EigVal& w, SchVec& vs, Work& work) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
         BOOST_STATIC_ASSERT((boost::is_same<
           typename traits::matrix_traits<MatrA>::matrix_structure,
           traits::general_t
         >::value));
         BOOST_STATIC_ASSERT((boost::is_same<
           typename traits::matrix_traits<SchVec>::matrix_structure,
           traits::general_t
         >::value));
#endif

         typedef typename MatrA::value_type                            value_type ;

         int const n = traits::matrix_size1 (a);
         assert (n == traits::matrix_size2 (a));
         assert (n == traits::matrix_size1 (vs));
         assert (n == traits::matrix_size2 (vs));
         assert (n == traits::vector_size (w));
         assert (3*n <= traits::vector_size (work));

         logical_t* select=0;
         bool* bwork=0;

         int info, sdim;
         detail::gees (jobvs, 'N', select, n,
                       traits::matrix_storage (a),
                       traits::leading_dimension (a),
                       sdim,
                       traits::vector_storage (w),
                       traits::matrix_storage (vs),
                       traits::leading_dimension (vs),
		       traits::vector_storage( work ),
		       traits::vector_size( work ),
                       bwork, info);
	 return info ;
       } // gees()

       /// Compute Schur factorization, passing two work arrays.
       template <typename MatrA, typename SchVec, typename EigVal,
                 typename Work, typename RWork>
       inline
       int gees (char jobvs, MatrA& a, EigVal& w, SchVec& vs,
		 Work& work, RWork& rwork) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
         BOOST_STATIC_ASSERT((boost::is_same<
           typename traits::matrix_traits<MatrA>::matrix_structure,
           traits::general_t
         >::value));
         BOOST_STATIC_ASSERT((boost::is_same<
           typename traits::matrix_traits<SchVec>::matrix_structure,
           traits::general_t
         >::value));
#endif

         typedef typename MatrA::value_type                            value_type ;

         int const n = traits::matrix_size1 (a);
         assert (n == traits::matrix_size2 (a));
         assert (n == traits::matrix_size1 (vs));
         assert (n == traits::matrix_size2 (vs));
         assert (n == traits::vector_size (w));
         assert (2*n <= traits::vector_size (work));
         assert (n <= traits::vector_size (rwork));

         logical_t* select=0;
         bool* bwork=0;

         int info, sdim;
         detail::gees (jobvs, 'N', select, n,
                       traits::matrix_storage (a),
                       traits::leading_dimension (a),
                       sdim,
                       traits::vector_storage (w),
                       traits::matrix_storage (vs),
                       traits::leading_dimension (vs),
		       traits::vector_storage( work ),
		       traits::vector_size( work ),
		       traits::vector_storage( rwork ),
                       bwork, info);
	 return info ;
       } // gees()

       /// Compute Schur factorization, depending on whether we have one or
       /// two workspace arrays. N= the number of workspace arrays.
       template <int N>
       struct Gees {};

       template <>
       struct Gees< 2 > {
          template <typename MatrA, typename SchVec, typename EigVal>
          inline
          int operator() (char jobvs, MatrA& a, EigVal& w, SchVec& vs, optimal_workspace ) const {
             typedef typename MatrA::value_type                            value_type ;
             typedef typename traits::type_traits< value_type >::real_type real_type ;

             int n = traits::matrix_size1( a );

             traits::detail::array<value_type> work( 2*n );
             traits::detail::array<real_type>  rwork( n );

             return gees( jobvs, a, w, vs, work, rwork );
          } // gees()

          template <typename MatrA, typename SchVec, typename EigVal>
          inline
          int operator() (char jobvs, MatrA& a, EigVal& w, SchVec& vs, minimal_workspace ) const {
             typedef typename MatrA::value_type                            value_type ;
             typedef typename traits::type_traits< value_type >::real_type real_type ;

             int n = traits::matrix_size1( a );

             traits::detail::array<value_type> work( 2*n );
             traits::detail::array<real_type>  rwork( n );

             return gees( jobvs, a, w, vs, work, rwork );
          } // gees()

          /// Compute Schur factorization, passing workspace2 as workspace
          template <typename MatrA, typename SchVec, typename EigVal, typename RWork, typename Work>
          inline
          int operator() (char jobvs, MatrA& a, EigVal& w, SchVec& vs, workspace2<Work,RWork>& workspace ) const {
             return gees( jobvs, a, w, vs, workspace.w_, workspace.wr_ );
          } // gees()
       }; // Gees<2>

       template <>
       struct Gees< 1 > {
          template <typename MatrA, typename SchVec, typename EigVal>
          inline
          int operator() (char jobvs, MatrA& a, EigVal& w, SchVec& vs, optimal_workspace ) const {
             typedef typename MatrA::value_type                            value_type ;
             typedef typename traits::type_traits< value_type >::real_type real_type ;

             int n = traits::matrix_size1( a );

             traits::detail::array<value_type> work( 3*n );

             return gees( jobvs, a, w, vs, work );
          } // gees()

          template <typename MatrA, typename SchVec, typename EigVal>
          inline
          int operator() (char jobvs, MatrA& a, EigVal& w, SchVec& vs, minimal_workspace ) const {
             typedef typename MatrA::value_type                            value_type ;
             typedef typename traits::type_traits< value_type >::real_type real_type ;

             int n = traits::matrix_size1( a );

             traits::detail::array<value_type> work( 3*n );

             return gees( jobvs, a, w, vs, work );
          } // gees()

          /// Compute Schur factorization, passing workspace1 as workspace
          template <typename MatrA, typename SchVec, typename EigVal, typename Work>
          inline
          int operator() (char jobvs, MatrA& a, EigVal& w, SchVec& vs, detail::workspace1<Work> workspace ) const {
             return gees( jobvs, a, w, vs, workspace.w_ );
          } // gees()
       }; // Gees<1>

    } // namespace detail

    /// Compute Schur factorization with Schur vectors.
    ///
    /// Workspace can be the following :
    /// optimal_workspace() : for optimizing use of blas 3 kernels
    /// minimal_workspace() : minimum size of workarrays, but does not allow for optimization
    ///                       of blas 3 kernels
    /// workspace( real_work ) for real matrices where
    ///                       vector_size( real_work ) >= 3*matrix_size1( a )
    /// workspace( complex_work, real_work ) for complex matrices where
    ///                       vector_size( complex_work ) >= 2*matrix_size1( a )
    ///                       and vector_size( real_work ) >= matrix_size1( a ).
    template <typename MatrA, typename SchVec, typename EigVal, typename Workspace>
    inline
    int gees (MatrA& a, EigVal& e, SchVec& vs, Workspace workspace ) {
       return detail::Gees< n_workspace_args<typename MatrA::value_type>::value>()
               ( 'V', a, e, vs, workspace );
    } // gees()

    // Compute Schur factorization without Schur vectors.
    ///
    /// Workspace can be the following :
    /// optimal_workspace() : for optimizing use of blas 3 kernels
    /// minimal_workspace() : minimum size of workarrays, but does not allow for optimization
    ///                       of blas 3 kernels
    /// workspace( real_work ) for real matrices where
    ///                       vector_size( real_work ) >= 3*matrix_size1( a )
    /// workspace( complex_work, real_work ) for complex matrices where
    ///                       vector_size( complex_work ) >= 2*matrix_size1( a )
    ///                       and vector_size( real_work ) >= matrix_size1( a ).
    template <typename MatrA, typename EigVal, typename Workspace>
    inline
    int gees (MatrA& a, EigVal& e, Workspace workspace) {
      return detail::Gees< n_workspace_args<typename MatrA::value_type>::value>()
              ('N', a, e, a, workspace );
    }

  }

}}}

#endif