#ifndef HEADER_GUARD_29f4d32dfe1b79d9bff92b3f9359cd94
#define HEADER_GUARD_29f4d32dfe1b79d9bff92b3f9359cd94

#include <openssl/bn.h>
#include "./error.hpp"
#include <boost/endian/conversion.hpp>
#include "jbms/enable_if.hpp"
#include "jbms/is_byte.hpp"
#include "jbms/assign_endian.hpp"
#include "jbms/division.hpp"
#include <vector>
#include <cctype>

namespace jbms {
namespace openssl {

class bn_ctx {
  BN_CTX *x_;
public:
  bn_ctx() {
    x_ = BN_CTX_new();
    throw_last_error_if(x_ == nullptr);
  }
  explicit bn_ctx(BN_CTX *x_)
    : x_(x_)
  {}

  ~bn_ctx() {
    if (x_)
      BN_CTX_free(x_);
  }

  bn_ctx(bn_ctx &&other) {
    x_ = other.x_;
    other.x_ = nullptr;
  }

  bn_ctx(bn_ctx const &) = delete;
  bn_ctx &operator=(bn_ctx const &) = delete;
  bn_ctx &operator=(bn_ctx &&other) = delete;
  operator BN_CTX *() { return x_; }
  BN_CTX *get() { return x_; }
  void swap(bn_ctx &other) {
    std::swap(x_, other.x_);
  }
};

class bignum;
inline bignum &as_bignum(BIGNUM &bn) {
  return reinterpret_cast<bignum &>(bn);
}
inline bignum const &as_bignum(BIGNUM const &bn) {
  return reinterpret_cast<bignum const &>(bn);
}

struct bn_ctx_frame {
  BN_CTX *ctx;
  bn_ctx_frame(BN_CTX *ctx) : ctx(ctx) {
    BN_CTX_start(ctx);
  }
  bignum &get() {
    auto *result = BN_CTX_get(ctx);
    throw_last_error_if(result == nullptr);
    return as_bignum(*result);
  }

  ~bn_ctx_frame() {
    BN_CTX_end(ctx);
  }
};

class bignum {
  BIGNUM bn_;
public:
  operator BIGNUM *() { return &bn_; }
  operator BIGNUM const *() const { return &bn_; }
  BIGNUM *get() { return &bn_; }
  BIGNUM const *get() const { return &bn_; }
  BIGNUM *operator->() { return &bn_; }
  BIGNUM const *operator->() const { return &bn_; }

  bignum() {
    BN_init(get());
  }
  bignum(unsigned long w) : bignum() {
    *this = w;
  }
  ~bignum() {
    BN_free(get());
  }
  bignum(bignum const &other) : bignum() {
    *this = other;
  }

  bignum(bignum &&other) : bignum() {
    swap(other);
  }

  void swap(bignum &other) {
    BN_swap(get(), other.get());
  }

  bignum &operator=(bignum const &other) {
    BN_copy(get(), other.get());
    return *this;
  }

  bignum &operator=(bignum &&other) {
    swap(other);
    return *this;
  }

  void clear() {
    BN_clear(get());
  }

  int num_bytes() const { return BN_num_bytes(get()); }
  int num_bits() const { return BN_num_bits(get()); }

  bool is_negative() const { return BN_is_negative(get()); }
  void set_negative(int n) { BN_set_negative(get(), n); }

  static bignum const &one() { return as_bignum(*BN_value_one()); }
  bignum &operator=(unsigned long w) {
    set_word(w);
    return *this;
  }
  bignum &operator=(long w) {
    if (w >= 0)
      set_word((unsigned long)w);
    else {
      set_word((unsigned long)-w);
      set_negative(true);
    }
    return *this;
  }

  void set_word(unsigned long w) {
    throw_last_error_if(BN_set_word(get(), w) == 0);
  }

  void set_zero() {
    throw_last_error_if(BN_zero(get()) == 0);
  }
  void set_one() {
    throw_last_error_if(BN_one(get()) == 0);
  }
  void set_bit(int n) {
    throw_last_error_if(BN_set_bit(get(), n) == 0);
  }

  // Returns true if number has at least n bits
  bool clear_bit(int n) {
    return (BN_clear_bit(get(), n) == 1);
  }

  bool is_bit_set(int n) const {
    return BN_is_bit_set(get(), n);
  }

  void set_bit(int n, bool value) {
    if (value)
      set_bit(n);
    else
      clear_bit(n);
  }

  // Returns true if the number was longer than n bits
  bool mask_bits(int n) {
    return BN_mask_bits(get(), n) == 1;
  }

  bool is_zero() const {
    return BN_is_zero(get());
  }

  bool is_one() const {
    return BN_is_one(get());
  }

  bool is_word(unsigned long w) const {
    return BN_is_word(get(), w);
  }

  bool is_odd() const {
    return BN_is_odd(get());
  }

  void set_from_hex(const char *x) {
    BIGNUM *ptr = get();
    throw_last_error_if(BN_hex2bn(&ptr, x) == 0);
  }
  void set_from_hex(std::string const &x) {
    set_from_hex(x.c_str());
  }

  std::string to_hex() const {
    char *temp = BN_bn2hex(get());
    throw_last_error_if(temp == nullptr);
    std::string result(temp);
    OPENSSL_free(temp);
    return result;
  }

  // Returns lowercase hex representation without leading zeros
  std::string to_canonical_hex() const {
    // Remove leading zeros
    std::string temp = to_hex();
    size_t i = 0;
    for (; i + 1 < temp.size(); ++i) {
      if (temp[i] != '0')
        break;
    }
    // Make lowercase for consistency
    for (auto &x : temp)
      x = std::tolower(x);
    return temp.substr(i);
  }

  std::string to_dec() const {
    char *temp = BN_bn2dec(get());
    throw_last_error_if(temp == nullptr);
    std::string result(temp);
    OPENSSL_free(temp);
    return result;
  }

  void set_from_dec(const char *x) {
    BIGNUM *ptr = get();
    throw_last_error_if(BN_dec2bn(&ptr, x) == 0);
  }
  void set_from_dec(std::string const &x) {
    set_from_dec(x.c_str());
  }

  bignum &operator+=(unsigned long w) {
    throw_last_error_if(BN_add_word(get(), w) == 0);
    return *this;
  }

  bignum &operator-=(unsigned long w) {
    throw_last_error_if(BN_sub_word(get(), w) == 0);
    return *this;
  }

  bignum &operator*=(unsigned long w) {
    throw_last_error_if(BN_mul_word(get(), w) == 0);
    return *this;
  }
};

/**
 * Invokes wrapper.data on a uint8_t array of length num_bytes
 *
 * x is set to be the contents of that buffer, interpreted as order endianness
 **/
template <class Func, boost::endian::order order,
          JBMS_ENABLE_IF_EXPR(std::declval<Func>()(std::declval<uint8_t *>()))>
inline void fill(bignum &x, size_t num_bytes, endian_wrapper<Func,order> wrapper) {
  x.set_zero();
  size_t num_words = jbms::div_ceil(num_bytes, sizeof(BN_ULONG));

  // ensure there is enough space
  bn_wexpand(x.get(), num_words);
  x.get()->top = num_words;

  // set final word to 0 since it might not be completely overwritten by the function
  if (num_words > 0)
    x.get()->d[num_words-1] = 0;

  // call function
  auto buf = (uint8_t *)x.get()->d;
  wrapper.data(buf);

  // reverse bytes if big endian
  if (order == boost::endian::order::big)
    std::reverse(buf, buf + num_bytes);

  // convert endian (no-op if order == little endian)
  for (size_t i = 0; i < num_words; ++i) {
    boost::endian::little_to_native_inplace(x.get()->d[i]);
  }

  bn_fix_top(x.get());
}

/**
 *  Interoperability with assign_endian functionality.
 **/

// result must have size == x.num_bytes
template <class Dest, JBMS_ENABLE_IF(is_byte_range<Dest>)>
inline void assign(endian_wrapper<Dest,boost::endian::order::big> wrapper,
                   bignum const &x) {
  wrapper.ensure_size_equals(x.num_bytes());

  // always writes exactly x.num_bytes(), doesn't produce errors
  BN_bn2bin(x.get(), wrapper.data.data());
}

// result must have size == x.num_bytes
template <class Dest, JBMS_ENABLE_IF(is_byte_range<Dest>)>
inline void assign(endian_wrapper<Dest,boost::endian::order::little> wrapper,
                   bignum const &x) {
  wrapper.ensure_size_equals(x.num_bytes());

  // always writes exactly x.num_bytes(), doesn't produce errors
  BN_bn2bin(x.get(), wrapper.data.data());

  // OpenSSL doesn't provide access to little endian representation directly, so we just reverse big endian output
  std::reverse(wrapper.data.begin(), wrapper.data.end());
}

template <class Source, boost::endian::order order, JBMS_ENABLE_IF(is_byte_range<Source>)>
void assign(bignum &x, endian_wrapper<Source, order> source) {
  fill(x,
       source.data.size(),
       jbms::make_endian_wrapper<order>([&](uint8_t *buf) { memcpy(buf, source.data.data(), source.data.size()); }));
}

// r = a + b
// r may alias a or b
inline void add(bignum &r, const bignum &a, const bignum &b) {
  throw_last_error_if(BN_add(r.get(), a.get(), b.get()) == 0);
}

// r = a - b
// r may alias a or b
inline void sub(bignum &r, const bignum &a, const bignum &b) {
  throw_last_error_if(BN_sub(r.get(), a.get(), b.get()) == 0);
}

// r = a * b
// r may alias a or b
inline void mul(bignum &r, const bignum &a, const bignum &b, BN_CTX *ctx) {
  throw_last_error_if(BN_mul(r.get(), a.get(), b.get(), ctx) == 0);
}

// r = a * b
// r may alias a
inline void sqr(bignum &r, const bignum &a, BN_CTX *ctx) {
  throw_last_error_if(BN_sqr(r.get(), a.get(), ctx) == 0);
}

// either of dv or rem can be nullptr
inline void div(BIGNUM *dv, BIGNUM *rem, const bignum &a, const bignum &d, BN_CTX *ctx) {
  throw_last_error_if(BN_div(dv, rem, a.get(), d.get(), ctx) == 0);
}

inline void mod(bignum &rem, const BIGNUM *a, const BIGNUM *m, BN_CTX *ctx) {
  throw_last_error_if(BN_mod(rem.get(), a, m, ctx) == 0);
}

inline void nnmod(bignum &r, const BIGNUM *a, const BIGNUM *m, BN_CTX *ctx) {
  throw_last_error_if(BN_nnmod(r.get(), a, m, ctx) == 0);
}

inline void mod_add(bignum &r, const BIGNUM *a, const BIGNUM *b, const BIGNUM *m, BN_CTX *ctx) {
  throw_last_error_if(BN_mod_add(r.get(), a, b, m, ctx) == 0);
}

inline void mod_sub(bignum &r, const BIGNUM *a, const BIGNUM *b, const BIGNUM *m, BN_CTX *ctx) {
  throw_last_error_if(BN_mod_sub(r.get(), a, b, m, ctx) == 0);
}

inline void mod_mul(bignum &r, const BIGNUM *a, const BIGNUM *b, const BIGNUM *m, BN_CTX *ctx) {
  throw_last_error_if(BN_mod_mul(r.get(), a, b, m, ctx) == 0);
}

inline void mod_sqr(bignum &r, const BIGNUM *a, const BIGNUM *m, BN_CTX *ctx) {
  throw_last_error_if(BN_mod_sqr(r.get(), a, m, ctx) == 0);
}

inline void exp(bignum &r, const BIGNUM *a, const BIGNUM *p, BN_CTX *ctx) {
  throw_last_error_if(BN_exp(r.get(), a, p, ctx) == 0);
}

inline void mod_exp(bignum &r, const BIGNUM *a, const BIGNUM *p, const BIGNUM *m, BN_CTX *ctx) {
  throw_last_error_if(BN_mod_exp(r.get(), a, p, m, ctx) == 0);
}

/* r may alias a */
inline void mod_inverse(bignum &r, const BIGNUM *a, const BIGNUM *n, BN_CTX *ctx) {
  throw_last_error_if(BN_mod_inverse(r.get(), a, n, ctx) == 0);
}

inline void gcd(bignum &r, const BIGNUM *a, const BIGNUM *b, BN_CTX *ctx) {
  throw_last_error_if(BN_gcd(r.get(), a, b, ctx) == 0);
}

inline int compare(bignum const &a, bignum const &b) {
  return BN_cmp(a, b);
}

inline int abs_compare(bignum const &a, bignum const &b) {
  return BN_ucmp(a, b);
}

#define OPENSSL_PP_BIGNUM_COMPARE(op)                                               \
  inline bool operator op(bignum const &a, bignum const &b) { return compare(a, b) op 0; } \
/**/

OPENSSL_PP_BIGNUM_COMPARE(==)
OPENSSL_PP_BIGNUM_COMPARE(!=)
OPENSSL_PP_BIGNUM_COMPARE(<)
OPENSSL_PP_BIGNUM_COMPARE(<=)
OPENSSL_PP_BIGNUM_COMPARE(>)
OPENSSL_PP_BIGNUM_COMPARE(>=)

#undef OPENSSL_PP_BIGNUM_COMPARE

inline bool operator==(bignum const &a, unsigned long w) { return a.is_word(w); }
inline bool operator==(unsigned long w, bignum const &a) { return a.is_word(w); }
inline bool operator!=(bignum const &a, unsigned long w) { return !(a == w); }
inline bool operator!=(unsigned long w, bignum const &a) { return !(a == w); }

// We want to be able to use bignum interchangeable with BIGNUM
static_assert(sizeof(bignum) == sizeof(BIGNUM), "");

class bn_recp_ctx {
  BN_RECP_CTX x_;
public:
  bn_recp_ctx() {
    BN_RECP_CTX_init(&x_);
  }
  ~bn_recp_ctx() {
    BN_RECP_CTX_free(&x_);
  }

  bn_recp_ctx(bn_recp_ctx const &other) : bn_recp_ctx() {
    *this = other;
  }
  bn_recp_ctx(bn_recp_ctx &&other) : bn_recp_ctx() {
    swap(other);
  }

  bn_recp_ctx(const BIGNUM *m, BN_CTX *ctx) : bn_recp_ctx() {
    set(m, ctx);
  }

  bn_recp_ctx &operator=(bn_recp_ctx const &other) {
    x_.num_bits = other->num_bits;
    x_.shift = other->shift;
    x_.flags = other->flags;
    divisor() = other.divisor();
    reciprocal() = other.reciprocal();
    return *this;
  }

  void swap(bn_recp_ctx &other) {
    std::swap(x_.num_bits, other->num_bits);
    std::swap(x_.shift, other->shift);
    std::swap(x_.flags, other->flags);
    divisor().swap(other.divisor());
    reciprocal().swap(other.reciprocal());
  }

  bn_recp_ctx &operator=(bn_recp_ctx &&other) {
    swap(other);
    return *this;
  }

  void set(const BIGNUM *m, BN_CTX *ctx) {
    throw_last_error_if(BN_RECP_CTX_set(get(), m, ctx) == 0);
  }

  operator BN_RECP_CTX *() { return &x_; }
  BN_RECP_CTX *get() { return &x_; }

  BN_RECP_CTX *operator->() { return &x_; }
  BN_RECP_CTX const *operator->() const { return &x_; }

  bignum &divisor() { return as_bignum(x_.N); }
  bignum const &divisor() const { return as_bignum(x_.N); }

  bignum &reciprocal() { return as_bignum(x_.Nr); }
  bignum const &reciprocal() const { return as_bignum(x_.Nr); }
};
static_assert(sizeof(bn_recp_ctx) == sizeof(BN_RECP_CTX),"");

inline void div_recp(BIGNUM *dv, BIGNUM *rem, bignum const &a, BN_RECP_CTX *recp, BN_CTX *ctx) {
  throw_last_error_if(BN_div_recp(dv, rem, a.get(), recp, ctx) == 0);
}

inline void mod_mul_reciprocal(bignum &r, BIGNUM const *a, BIGNUM const *b, BN_RECP_CTX *recp, BN_CTX *ctx) {
  throw_last_error_if(BN_mod_mul_reciprocal(r.get(), a, b, recp, ctx) == 0);
}

class bn_mont_ctx {
  BN_MONT_CTX ctx_;
public:
  BN_MONT_CTX *get() { return &ctx_; }
  BN_MONT_CTX const *get() const { return &ctx_; }
  operator BN_MONT_CTX * () { return &ctx_; }
  operator BN_MONT_CTX const * () const { return &ctx_; }
  BN_MONT_CTX *operator->() { return &ctx_; }
  BN_MONT_CTX const *operator->() const { return &ctx_; }

  bn_mont_ctx() {
    BN_MONT_CTX_init(get());
  }
  ~bn_mont_ctx() {
    BN_MONT_CTX_free(get());
  }
  bignum const &RR() const { return as_bignum(ctx_.RR); }
  bignum &RR() { return as_bignum(ctx_.RR); }

  // The modulus
  bignum const &N() const { return as_bignum(ctx_.N); }
  bignum &N() { return as_bignum(ctx_.N); }

  bignum const &Ni() const { return as_bignum(ctx_.Ni); }
  bignum &Ni() { return as_bignum(ctx_.Ni); }

  bn_mont_ctx(bn_mont_ctx const &other) : bn_mont_ctx() {
    *this = other;
  }

  bn_mont_ctx(bn_mont_ctx &&other) : bn_mont_ctx() {
    swap(other);
  }

  bn_mont_ctx(BIGNUM const *m, BN_CTX *ctx) {
    set(m, ctx);
  }

  bn_mont_ctx &operator=(bn_mont_ctx const &other) {
    throw_last_error_if(BN_MONT_CTX_copy(get(), const_cast<BN_MONT_CTX *>(other.get())) == nullptr);
    return *this;
  }

  void swap(bn_mont_ctx &other) {
    std::swap(ctx_.ri, other->ri);
    RR().swap(other.RR());
    N().swap(other.N());
    Ni().swap(other.Ni());
    std::swap(ctx_.n0, other->n0);
    std::swap(ctx_.flags, other->flags);
  }
  bn_mont_ctx &operator=(bn_mont_ctx &&other) {
    swap(other);
    return *this;
  }

  void set(const BIGNUM *m, BN_CTX *ctx) {
    throw_last_error_if(BN_MONT_CTX_set(get(), m, ctx) == 0);
  }

};
static_assert(sizeof(bn_mont_ctx) == sizeof(BN_MONT_CTX), "");

inline void from_montgomery(bignum &r, BIGNUM const *a, BN_MONT_CTX const *mont, BN_CTX *ctx) {
  throw_last_error_if(BN_from_montgomery(r.get(), a, const_cast<BN_MONT_CTX *>(mont), ctx) == 0);
}

inline void to_montgomery(bignum &r, BIGNUM const *a, BN_MONT_CTX const *mont, BN_CTX *ctx) {
  throw_last_error_if(BN_to_montgomery(r.get(), a, const_cast<BN_MONT_CTX *>(mont), ctx) == 0);
}

// r may alias a
// a may alias b
inline void mod_mul_montgomery(bignum &r, BIGNUM const *a, BIGNUM const *b, BN_MONT_CTX const *mont, BN_CTX *ctx) {
  throw_last_error_if(BN_mod_mul_montgomery(r.get(), a, b, const_cast<BN_MONT_CTX *>(mont), ctx) == 0);
}

#ifndef OPENSSL_NO_EC2M

// For all of the variants that accept a const int p[] argument, the modulus p is specified as a (-1)-terminated, decreasing list of 1 coefficients.
// The array variants are more efficient because the non-array variants are mapped by OpenSSL to the array variants.

// r, a, b are GF(2^m) field elements
// r = a + b
// r may alias a or b
inline void GF2m_add(bignum &r, BIGNUM const *a, BIGNUM const *b) {
  throw_last_error_if(BN_GF2m_add(r.get(), a, b) == 0);
}

// r, a, p are GF(2^m) field elements
// r = a mod p
// r may alias a.
inline void GF2m_mod(bignum &r, BIGNUM const *a, BIGNUM const *p) {
  throw_last_error_if(BN_GF2m_mod(r.get(), a, p) == 0);
}
inline void GF2m_mod(bignum &r, BIGNUM const *a, const int p[]) {
  throw_last_error_if(BN_GF2m_mod_arr(r.get(), a, p) == 0);
}

// r, a, b, p are GF(2^m) field elements
// r = (a * b) mod p
// r may alias a or b
// a may alias b
inline void GF2m_mod_mul(bignum &r, BIGNUM const *a, BIGNUM const *b, BIGNUM const *p, BN_CTX *ctx) {
  throw_last_error_if(BN_GF2m_mod_mul(r.get(), a, b, p, ctx) == 0);
}
inline void GF2m_mod_mul(bignum &r, BIGNUM const *a, BIGNUM const *b, const int p[], BN_CTX *ctx) {
  throw_last_error_if(BN_GF2m_mod_mul_arr(r.get(), a, b, p, ctx) == 0);
}

// r, a, p are GF(2^m) field elements
// r = (a * a) mod p
// r may alias a
inline void GF2m_mod_sqr(bignum &r, BIGNUM const *a, BIGNUM const *p, BN_CTX *ctx) {
  throw_last_error_if(BN_GF2m_mod_sqr(r.get(), a, p, ctx) == 0);
}
inline void GF2m_mod_sqr(bignum &r, BIGNUM const *a, const int p[], BN_CTX *ctx) {
  throw_last_error_if(BN_GF2m_mod_sqr_arr(r.get(), a, p, ctx) == 0);
}

// r, a, p are GF(2^m) field elements
// r = a^{-1} mod p
// r may alias a
inline void GF2m_mod_inv(bignum &r, BIGNUM const *a, BIGNUM const *p, BN_CTX *ctx) {
  throw_last_error_if(BN_GF2m_mod_inv(r.get(), a, p, ctx) == 0);
}
inline void GF2m_mod_inv(bignum &r, BIGNUM const *a, const int p[], BN_CTX *ctx) {
  throw_last_error_if(BN_GF2m_mod_inv_arr(r.get(), a, p, ctx) == 0);
}

// r, a, b, p are GF(2^m) field elements
// r = (a / b) mod p
// r may alias a or b
// a may alias b
inline void GF2m_mod_div(bignum &r, BIGNUM const *a, BIGNUM const *b, BIGNUM const *p, BN_CTX *ctx) {
  throw_last_error_if(BN_GF2m_mod_div(r.get(), a, b, p, ctx) == 0);
}
inline void GF2m_mod_div(bignum &r, BIGNUM const *a, BIGNUM const *b, const int p[], BN_CTX *ctx) {
  throw_last_error_if(BN_GF2m_mod_div_arr(r.get(), a, b, p, ctx) == 0);
}

// r, a are GF(2^m) field elements
// Computes r such that r^2 = a
// r may alias a
inline void GF2m_mod_sqrt(bignum &r, BIGNUM const *a, BIGNUM const *p, BN_CTX *ctx) {
  throw_last_error_if(BN_GF2m_mod_sqrt(r.get(), a, p, ctx) == 0);
}
inline void GF2m_mod_sqrt(bignum &r, BIGNUM const *a, const int p[], BN_CTX *ctx) {
  throw_last_error_if(BN_GF2m_mod_sqrt_arr(r.get(), a, p, ctx) == 0);
}

// r, a are GF(2^m) field elements
// b is an integer
// r = a^b mod p
// r may alias a
inline void GF2m_mod_exp(bignum &r, BIGNUM const *a, BIGNUM const *b, BIGNUM const *p, BN_CTX *ctx) {
  throw_last_error_if(BN_GF2m_mod_exp(r.get(), a, b, p, ctx) == 0);
}
inline void GF2m_mod_exp(bignum &r, BIGNUM const *a, BIGNUM const *b, const int p[], BN_CTX *ctx) {
  throw_last_error_if(BN_GF2m_mod_exp_arr(r.get(), a, b, p, ctx) == 0);
}

// r, a, p are GF(2^m) field elements
// r^2 + r = a mod p
// r may alias a
// Returns 0 if no such r exists.
inline void GF2m_mod_solve_quad(bignum &r, BIGNUM const *a, BIGNUM const *p, BN_CTX *ctx) {
  throw_last_error_if(BN_GF2m_mod_solve_quad(r.get(), a, p, ctx) == 0);
}
inline void GF2m_mod_solve_quad(bignum &r, BIGNUM const *a, const int p[], BN_CTX *ctx) {
  throw_last_error_if(BN_GF2m_mod_solve_quad_arr(r.get(), a, p, ctx) == 0);
}

// p is a (-1)-terminated array of coefficients
inline void GF2m_arr2poly(const int p[], bignum &a) {
  throw_last_error_if(BN_GF2m_arr2poly(p, a.get()) == 0);
}

// Sets p to be a (-1)-terminated array of 1 coefficients
// Only up to max values in p are set (the -1 will only be appended if there is sufficient space)
// Returns the size of p that would be required to avoid truncation.
inline int GF2m_poly2arr(bignum const &a, int p[], int max) {
  return BN_GF2m_poly2arr(a.get(), p, max);
}

inline std::vector<int> GF2m_poly2arr(bignum const &a) {
  std::vector<int> result;
  int max = GF2m_poly2arr(a, result.data(), 0);
  result.resize(max);
  GF2m_poly2arr(a, result.data(), max);
  return result;
}

#endif // end if !defined(OPENSSL_NO_EC2M)

/**
 * If top equals:
 *  -1 :  top bit is random (i.e. can be 0)
 *   0 :  top bit will be 1
 *   1 :  top two bits will be 1
 *
 * If bottom equals:
 *   1  : number is odd (bottom bit is 1)
 *   0  : bottom bit is random (i.e. can be 0)
 **/
inline void rand(bignum &r, int bits, int top, int bottom) {
  throw_last_error_if(BN_rand(r.get(), bits, top, bottom) == 0);
}

// Same as rand, but not cryptographically secure
inline void pseudo_rand(bignum &r, int bits, int top, int bottom) {
  throw_last_error_if(BN_pseudo_rand(r.get(), bits, top, bottom) == 0);
}

// Post-condition: 0 <= r < range
inline void rand_range(bignum &r, bignum const &range) {
  throw_last_error_if(BN_rand_range(r.get(), range.get()) == 0);
}
// Same as rand_range, but not cryptographically secure
inline void pseudo_rand_range(bignum &r, bignum const &range) {
  throw_last_error_if(BN_pseudo_rand_range(r.get(), range.get()) == 0);
}

}
}

#endif /* HEADER GUARD */


/// \file   slam.cpp
/// \brief  Implementation of the extended Kalman Filter SLAM
///
/// PARAMETERS:
///     frequency (int): control loop frequency
///     reset_flag (bool): specifies when new pose is read
///     wheel_base (float): The distance between the wheels
///     wheel_radius (float): The radius of the wheels
///     odom_frame_id (std::string): The name of the odometry tf frame
///     body_frame_id (std::string): The name of the body tf frame
///     left_wheel_joint (std::string): The name of the left wheel joint
///     right_wheel_joint (std::string): The name of the right wheel joint
///
///     joint_msg (sensor_msgs::JointState): message to publish wheel angle
///         readings to /joint_states topic
///     static_world_broadcaster (StaticTransformBroadcaster): Broadcast the
///         static transform between world_frame_id and the map_frame_id on /tf
///         using a tf2
///     odom_broadcaster (TransformBroadcaster): Broadcast the transform between
///         map_frame_id and the odom_frame_id on /tf using a tf2
///     map_broadcaster (TransformBroadcaster): Broadcast the transform between
///         odom_frame_id and the body_frame_id on /tf using a tf2
///     static_world_tf(geometry_msgs::TransformStamped): static world transform
///     map_tf(geometry_msgs::TransformStamped): map transform
///     odom_tf(geometry_msgs::TransformStamped): odometry transform
///     odom(nav_msgs::Odometry): odometry message
///     slam (nav_msgs::Odometry): slam message
///     slam_marker_array (visualization_msgs::MarkerArray): an array of
///         obstacles publishing to the slam simulator.
///
///     odom_pose (rigid2d::Config2D): the robot's position (based on the wheel
///         angles)
///     reset_pose (rigid2d::Config2D): the robot's reset position
///     twist(rigid2d::Twist2D): the robot's twist
///     twist_del (rigid2d::Twist2D): the delta between the robot's new twist
///         to the robot's last twist
///     diff_drive (rigid2d::DiffDrive): an instance of the diff_drive robot
///     wheel_vel (rigid2d::WheelVelocity): the velocity of the robot's wheels
///     wheel_angle (rigid2d::WheelAngle): the angle of the robot's wheels
///
///     measurements (std::vector<nuslam::Measurement>): a vector with
///     components of type nuslam::Measurement. m_t (arma::mat): the map matrix,
///     as measured in the slam simulator. q_t(arma::mat): the state matrix, as
///     measured in the slam simulator.
///
/// PUBLISHES:
///     odom (nav_msgs/Odometry): publishes Odometry message on the odom topic.
///     landmarks_pub (visualization_msgs/MarkerArray): publishes real
///     landmarks. slam_landmarks_pub (visualization_msgs/MarkerArray):
///     publishes landmark
///         to be subscribed by the slam simulation.
///
/// SUBSCRIBES:
///     landmarks_sub (visualization_msgs/Marker): Subscribes to the MarkerArray
///         that's publish by the simulation.
///
/// SERVICES:
///     SetPose (set_pose): Restarts the location of the odometry, so that the
///         robot thinksit is at the requested configuration.

#include "nuslam/nuslam.hpp"
#include "rigid2d/diff_drive.hpp"
#include "rigid2d/rigid2d.hpp"

#include "ros/ros.h"

#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/TransformStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/JointState.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <visualization_msgs/MarkerArray.h>

#include <rigid2d/SetPose.h>

#include <armadillo>
#include <cmath>
#include <iostream>
#include <string>
#include <utility>
#include <vector>

/// \brief Class KFSlam
class KFSlam
{
public:
  KFSlam()
  {
    ROS_INFO("Initialize the variables");
    // Init Parameters
    load_parameter();

    // Init publishers, subscribers, and services
    odom_pub = nh.advertise<nav_msgs::Odometry>("/odom", 1);
    slam_landmarks_pub =
        nh.advertise<visualization_msgs::MarkerArray>("/slam_landmarks", 1);

    joint_states_sub =
        nh.subscribe("/joint_states", 1, &KFSlam::joint_state_callback, this);
    landmarks_sub =
        nh.subscribe("/fake_sensor", 1, &KFSlam::landmarks_callback, this);

    set_pose_srv =
        nh.advertiseService("/set_pose", &KFSlam::set_pose_callback, this);
  }

  /// \brief Load the parameters from the parameter server
  /// \returns void
  void load_parameter()
  {
    nh.getParam("wheel_base", wheel_base);     // The distance between the wheels
    nh.getParam("wheel_radius", wheel_radius); // The radius of the wheels
    nh.getParam("world_frame_id",
                world_frame_id);               // The name of the world tf frame
    nh.getParam("map_frame_id", map_frame_id); // The name of the map tf frame
    nh.getParam("odom_frame_id",
                odom_frame_id); // The name of the odometry tf frame
    nh.getParam("body_frame_id",
                body_frame_id); // The name of the body tf frame
    nh.getParam("left_wheel_joint",
                left_wheel_joint); // The name of the left wheel joint
    nh.getParam("right_wheel_joint",
                right_wheel_joint); // The name of the right wheel joint
    nh.getParam("obstacles_radius",
                obstacles_radius); // The radous of the cardboard tubes [m]
  }

  /// \brief Subscribes to the robot's joint_states.
  /// \param joint_state - constant pointer to joint_states
  /// \returns void
  void
  joint_state_callback(const sensor_msgs::JointState::ConstPtr &joint_state)
  {
    // ROS_INFO("Subscribing to joint state");
    right_angle = joint_state->position.at(0);
    left_angle = joint_state->position.at(1);

    joint_state_flag = true;
  }

  /// \brief Subscribes to the map's landmarks.
  /// \param markers - markers visualization msgs.
  /// \returns void
  void landmarks_callback(const visualization_msgs::MarkerArray &markers)
  {
    // ROS_INFO("Subscribing to landmarks");
    for (auto &marker : markers.markers)
    {
      double x = marker.pose.position.x;
      double y = marker.pose.position.y;
      int id = marker.id;
      measurements.push_back(nuslam::Measurement(x, y, id));
    }

    // Raise the landmarks flag
    landmarks_flag = true;
  }

  /// \brief Restarts the location of the odometry, so that the robot thinks
  /// it is at the requested configuration.
  /// \param req - SetPose request.
  /// \param res - SetPose response.
  /// \returns bool
  bool set_pose_callback(rigid2d::SetPose::Request &req,
                         rigid2d::SetPose::Response &res)
  {
    // ROS_INFO("Setting pose");
    reset_pose.x = req.x;
    reset_pose.y = req.y;
    reset_pose.theta = req.theta;
    res.result = true;

    // Raise the reset flag
    reset_flag = true;

    return true;
  }

  /// \brief Gets markers from map
  /// \returns void
  void get_marker_from_map()
  {
    slam_marker_array.markers.resize(m_t.size());
    for (unsigned int i = 0; i < m_t.n_rows / 2; i++)
    {
      slam_marker_array.markers.resize(m_t.n_rows / 2);
      slam_marker_array.markers[i].header.frame_id = map_frame_id;
      slam_marker_array.markers[i].header.stamp = ros::Time();
      slam_marker_array.markers[i].ns = "marker";
      slam_marker_array.markers[i].id = i;
      slam_marker_array.markers[i].type = visualization_msgs::Marker::CYLINDER;
      slam_marker_array.markers[i].action = visualization_msgs::Marker::ADD;
      slam_marker_array.markers[i].pose.position.x = m_t(2 * i, 0);
      slam_marker_array.markers[i].pose.position.y = m_t(2 * i + 1, 0);
      slam_marker_array.markers[i].pose.position.z = 0.0;
      slam_marker_array.markers[i].pose.orientation.x = 0.0;
      slam_marker_array.markers[i].pose.orientation.y = 0.0;
      slam_marker_array.markers[i].pose.orientation.z = 0.0;
      slam_marker_array.markers[i].pose.orientation.w = 1.0;
      slam_marker_array.markers[i].scale.x = 3 * obstacles_radius;
      slam_marker_array.markers[i].scale.y = 3 * obstacles_radius;
      slam_marker_array.markers[i].scale.z = 0.5;
      slam_marker_array.markers[i].color.a = 1.0;
      slam_marker_array.markers[i].color.r = 0.0;
      slam_marker_array.markers[i].color.g = 0.0;
      slam_marker_array.markers[i].color.b = 1.0;

      slam_landmarks_pub.publish(slam_marker_array);
    }
  }

  /// \brief Broadcast the static "world" to "map" transform
  /// \returns void
  void world_map_transform()
  {
    // Transform from "world" to "map" frame
    static_world_tf.header.stamp = ros::Time::now();
    static_world_tf.header.frame_id = world_frame_id;
    static_world_tf.child_frame_id = map_frame_id;
    static_world_tf.transform.translation.x = 0;
    static_world_tf.transform.translation.y = 0;
    static_world_tf.transform.translation.z = 0;
    static_world_tf.transform.rotation.x = 0.0;
    static_world_tf.transform.rotation.y = 0.0;
    static_world_tf.transform.rotation.z = 0.0;
    static_world_tf.transform.rotation.w = 1.0;

    static_world_broadcaster.sendTransform(static_world_tf);
  }

  /// \brief Broadcast the "map" to "odom" transform
  /// \returns void
  void map_odom_transform()
  {
    rigid2d::Transform2D T_mo, T_om;
    rigid2d::Vector2D v_mb, v_ob;
    // double angle_mb, angle_ob;

    // angle_mb = q_t(0, 0);
    v_mb.x = q_t(1, 0);
    v_mb.y = q_t(2, 0);

    rigid2d::Transform2D T_mb(v_mb, 0); // angle_mb

    // angle_ob = odom_pose.theta;
    v_ob.x = odom_pose.x;
    v_ob.y = odom_pose.y;
    rigid2d::Transform2D T_ob(v_ob, 0); // angle_ob

    T_mo = T_mb * T_ob.inv();

    // Transform from "map" to "odom" frame
    map_tf.header.stamp = ros::Time::now(); // TODO
    map_tf.header.frame_id = map_frame_id;
    map_tf.child_frame_id = odom_frame_id;
    map_tf.transform.translation.x = 0; // T_mo.x();
    map_tf.transform.translation.y = 0; // T_mo.y();
    map_tf.transform.translation.z = 0;
    quat.setRPY(0, 0, 0); // T_mo.theta()
    map_quat = tf2::toMsg(quat);
    map_tf.transform.rotation = map_quat;

    map_broadcaster.sendTransform(map_tf);
  }

  /// \brief Broadcast the "odom" to "body" transform
  /// \returns void
  void odom_body_transform()
  {
    // Transform from "odom" to "body" frame
    odom_tf.header.stamp = ros::Time::now();
    odom_tf.header.frame_id = odom_frame_id;
    odom_tf.child_frame_id = body_frame_id;
    odom_tf.transform.translation.x = odom_pose.x;
    odom_tf.transform.translation.y = odom_pose.y;
    odom_tf.transform.translation.z = 0;
    quat.setRPY(0, 0, odom_pose.theta);
    odom_quat = tf2::toMsg(quat);
    odom_tf.transform.rotation = odom_quat;
    odom_broadcaster.sendTransform(odom_tf);
  }

  /// \brief Broadcast the "world" to "slam" transform
  /// \returns void
  void world_slam_transform()
  {
    // Transform from "world" to "slam" frame
    slam_tf.header.stamp = ros::Time::now();
    slam_tf.header.frame_id = world_frame_id;
    slam_tf.child_frame_id = "slam";
    slam_tf.transform.translation.x = q_t(1, 0);
    slam_tf.transform.translation.y = q_t(2, 0);
    slam_tf.transform.translation.z = 0;
    quat.setRPY(0, 0, q_t(0, 0));
    odom_quat = tf2::toMsg(quat);
    slam_tf.transform.rotation = odom_quat;
    slam_broadcaster.sendTransform(slam_tf);
  }

  /// \brief Publish odometry messages
  /// \returns void
  void publish_odometry()
  {
    odom.header.stamp = current_time;
    odom.header.frame_id = odom_frame_id;
    odom.child_frame_id = body_frame_id;
    odom.pose.pose.position.x = odom_pose.x;
    odom.pose.pose.position.y = odom_pose.y;
    odom.pose.pose.position.z = 0.0;
    quat.setRPY(0, 0, odom_pose.theta);
    odom_quat = tf2::toMsg(quat);
    odom.pose.pose.orientation = odom_quat;
    odom.twist.twist.linear.x = twist.xdot;
    odom.twist.twist.linear.y = twist.ydot;
    odom.twist.twist.angular.z = twist.thetadot;

    odom_pub.publish(odom);
  }

  /// \brief Main loop for the turtle's motion
  /// \returns void
  void main_loop()
  {
    // ROS_INFO("Entering the loop");
    ros::Rate loop_rate(frequency);

    // Initializing Extended Kalman Filter
    nuslam::EKF Kalman_Filter;
    diff_drive = rigid2d::DiffDrive();
    wheel_angle_old = {0, 0};

    while (ros::ok())
    {
      current_time = ros::Time::now();

      if (joint_state_flag)
      {
        // ROS_INFO("wheel_angle = %f, %f\n\r", right_angle, left_angle);

        diff_drive.updateOdometryWithAngles(right_angle, left_angle);
        odom_pose = diff_drive.get_config();
        // ROS_INFO("odom_pose = %f, %f\n\r", odom_pose.x, odom_pose.y);

        joint_state_flag = false;
      }

      // If the set_pose_callback was called
      if (reset_flag)
      {
        diff_drive.set_config(reset_pose);

        // Provide the configuration of the robot
        ROS_INFO("Reset robot position:");
        ROS_INFO("x = %f\n\r", diff_drive.get_config().x);
        ROS_INFO("y = %f\n\r", diff_drive.get_config().y);
        ROS_INFO("theta = %f\n\r", diff_drive.get_config().theta);

        // Remove the set_pose flag
        reset_flag = false;
      }

      if (landmarks_flag)
      {
        wheel_angle_new = diff_drive.get_wheel_angle();
        // ROS_INFO("wheel_angle_old = %f, %f \n\r",
        // wheel_angle_old.right_wheel_angle,
        // wheel_angle_old.right_wheel_angle); ROS_INFO("wheel_angle_new = %f,
        // %f \n\r", wheel_angle_new.right_wheel_angle,
        // wheel_angle_new.right_wheel_angle);
        wheel_vel_del.right_wheel_vel =
            rigid2d::normalize_angle(wheel_angle_new.right_wheel_angle -
                                     wheel_angle_old.right_wheel_angle);
        wheel_vel_del.left_wheel_vel =
            rigid2d::normalize_angle(wheel_angle_new.left_wheel_angle -
                                     wheel_angle_old.left_wheel_angle);

        twist_del = diff_drive.wheels2Twist(wheel_vel_del);
        wheel_angle_old = wheel_angle_new;

        // Running Extended Kalman Filter
        Kalman_Filter.run_ekf(twist_del, measurements);
        q_t = Kalman_Filter.output_state();
        m_t = Kalman_Filter.output_map_state();

        measurements.clear();

        // Publishing map markers
        get_marker_from_map();

        landmarks_flag = false;
      }

      // Setting transformations
      world_map_transform();
      world_slam_transform(); // TODO
      map_odom_transform();
      odom_body_transform();

      // Publish odometry messages
      publish_odometry();

      loop_rate.sleep();
      ros::spinOnce();
    }
  }

private:
  int frequency = 10;
  bool joint_state_flag = false;
  bool landmarks_flag = false;
  bool reset_flag = false;
  double wheel_base, wheel_radius, right_angle, left_angle, obstacles_radius;
  std::string world_frame_id, map_frame_id, odom_frame_id, body_frame_id,
      left_wheel_joint, right_wheel_joint;

  arma::mat m_t;
  arma::mat q_t = arma::mat(3, 1).fill(0.0);

  ros::NodeHandle nh;
  ros::Publisher odom_pub, slam_pub, landmarks_pub, slam_landmarks_pub;
  ros::Subscriber joint_states_sub, landmarks_sub;
  ros::ServiceServer set_pose_srv;
  ros::Time current_time;

  visualization_msgs::MarkerArray slam_marker_array;
  tf2::Quaternion quat;
  tf2_ros::TransformBroadcaster map_broadcaster, odom_broadcaster,
      slam_broadcaster;
  tf2_ros::StaticTransformBroadcaster static_world_broadcaster;
  geometry_msgs::TransformStamped map_tf, odom_tf, slam_tf;
  geometry_msgs::TransformStamped static_world_tf;

  geometry_msgs::Quaternion odom_quat, map_quat;
  nav_msgs::Odometry odom, slam;

  rigid2d::Config2D odom_pose, reset_pose;
  rigid2d::Twist2D twist, twist_del;
  rigid2d::DiffDrive diff_drive;
  rigid2d::WheelVelocity wheel_vel, wheel_vel_del;
  rigid2d::WheelAngle wheel_angle, wheel_angle_new, wheel_angle_old;

  std::vector<nuslam::Measurement> measurements;
};

/// \brief Main function
/// \param argc - input int argument
/// \param argv - input array argument
/// \returns int
int main(int argc, char *argv[])
{
  ros::init(argc, argv, "slam");
  KFSlam node;
  node.main_loop();
  ros::spin();
  return 0;
}


#ifndef BOOST_GEOMETRY_PROJECTIONS_MBT_FPS_HPP
#define BOOST_GEOMETRY_PROJECTIONS_MBT_FPS_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/aasincos.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct mbt_fps {};

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace mbt_fps
    {

            static const int MAX_ITER = 10;
            static const double LOOP_TOL = 1e-7;
            static const double C1 = 0.45503;
            static const double C2 = 1.36509;
            static const double C3 = 1.41546;
            static const double C_x = 0.22248;
            static const double C_y = 1.44492;
            //static const double C1_2 = 0.33333333333333333333333333;

            template <typename T>
            inline T C1_2() { return detail::THIRD<T>(); }

            // template class, using CRTP to implement forward/inverse
            template <typename CalculationType, typename Parameters>
            struct base_mbt_fps_spheroid : public base_t_fi<base_mbt_fps_spheroid<CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                inline base_mbt_fps_spheroid(const Parameters& par)
                    : base_t_fi<base_mbt_fps_spheroid<CalculationType, Parameters>,
                     CalculationType, Parameters>(*this, par) {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    static const CalculationType C1_2 = mbt_fps::C1_2<CalculationType>();

                    CalculationType k, V, t;
                    int i;

                    k = C3 * sin(lp_lat);
                    for (i = MAX_ITER; i ; --i) {
                        t = lp_lat / C2;
                        lp_lat -= V = (C1 * sin(t) + sin(lp_lat) - k) /
                            (C1_2 * cos(t) + cos(lp_lat));
                        if (fabs(V) < LOOP_TOL)
                            break;
                    }
                    t = lp_lat / C2;
                    xy_x = C_x * lp_lon * (1. + 3. * cos(lp_lat)/cos(t) );
                    xy_y = C_y * sin(t);
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    CalculationType t;

                    lp_lat = C2 * (t = aasin(xy_y / C_y));
                    lp_lon = xy_x / (C_x * (1. + 3. * cos(lp_lat)/cos(t)));
                    lp_lat = aasin((C1 * sin(t) + sin(lp_lat)) / C3);
                }

                static inline std::string get_name()
                {
                    return "mbt_fps_spheroid";
                }

            };

            // McBryde-Thomas Flat-Pole Sine (No. 2)
            template <typename Parameters>
            inline void setup_mbt_fps(Parameters& par)
            {
                par.es = 0;
            }

    }} // namespace detail::mbt_fps
    #endif // doxygen

    /*!
        \brief McBryde-Thomas Flat-Pole Sine (No. 2) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
        \par Example
        \image html ex_mbt_fps.gif
    */
    template <typename CalculationType, typename Parameters>
    struct mbt_fps_spheroid : public detail::mbt_fps::base_mbt_fps_spheroid<CalculationType, Parameters>
    {
        inline mbt_fps_spheroid(const Parameters& par) : detail::mbt_fps::base_mbt_fps_spheroid<CalculationType, Parameters>(par)
        {
            detail::mbt_fps::setup_mbt_fps(this->m_par);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::mbt_fps, mbt_fps_spheroid, mbt_fps_spheroid)

        // Factory entry(s)
        template <typename CalculationType, typename Parameters>
        class mbt_fps_entry : public detail::factory_entry<CalculationType, Parameters>
        {
            public :
                virtual base_v<CalculationType, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<mbt_fps_spheroid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                }
        };

        template <typename CalculationType, typename Parameters>
        inline void mbt_fps_init(detail::base_factory<CalculationType, Parameters>& factory)
        {
            factory.add_to_factory("mbt_fps", new mbt_fps_entry<CalculationType, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_MBT_FPS_HPP


#ifndef PARSER_FINITEAUTOMATON_HPP
#define PARSER_FINITEAUTOMATON_HPP

#include <iostream>
#include <boost/spirit/home/x3.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/fusion/include/io.hpp>

#include <boost/spirit/include/support_istream_iterator.hpp>
#include <iostream>
#include <sstream>
#include <fstream>

namespace formal_device
{
namespace finite_automaton
{
namespace ast
{

struct Symbol
{
    std::string m_value;
};

struct TransitionSymbols
{
    std::vector<Symbol> m_transition_symbols;
};

struct Transition
{
    std::vector<Symbol> m_transitions;
};

struct State
{
    Symbol m_state;
    std::vector<Transition> m_transitions;
};

struct Document
{
    TransitionSymbols m_transition_symbols;
    std::vector<State> m_states;
};

}
}
}

BOOST_FUSION_ADAPT_STRUCT(formal_device::finite_automaton::ast::Symbol, m_value)
BOOST_FUSION_ADAPT_STRUCT(formal_device::finite_automaton::ast::TransitionSymbols, m_transition_symbols)
BOOST_FUSION_ADAPT_STRUCT(formal_device::finite_automaton::ast::Transition, m_transitions)
BOOST_FUSION_ADAPT_STRUCT(formal_device::finite_automaton::ast::State, m_state, m_transitions)
BOOST_FUSION_ADAPT_STRUCT(formal_device::finite_automaton::ast::Document, m_transition_symbols, m_states)

namespace formal_device
{
namespace finite_automaton
{
namespace parser {
    namespace x3    = boost::spirit::x3;
    namespace ascii = x3::ascii;

    x3::rule<class symbol_, ast::Symbol>         symbol{"symbol"};
    x3::rule<class transition_symbols_, ast::TransitionSymbols> transition_symbols{"transition_symbols"};
    x3::rule<class transition_, ast::Transition> transition{"transition"};
    x3::rule<class state_, ast::State>           state{"state"};
    x3::rule<class document_, ast::Document>     document{"document"};

    const auto identifier             = x3::lexeme[+x3::char_("->a-zA-Z0-9*") - '\n'];
    const auto symbol_def             = identifier;
    const auto transition_symbols_def = x3::lit("+ |") >> symbol % "|";
    const auto transition_def         = -x3::lit("{") >> (symbol % ",") >> -x3::lit("}");
    const auto state_def              = symbol >> "|" >> transition % "|";
    const auto document_def           = transition_symbols >> x3::eol >> (state % x3::eol);

    BOOST_SPIRIT_DEFINE(symbol, transition_symbols, transition, state, document);
}
}
}

#endif


//
//  Copyright (c) 2000-2010
//  Joerg Walter, Mathias Koch, David Bellot
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#ifndef _BOOST_UBLAS_IO_
#define _BOOST_UBLAS_IO_

// Only forward definition required to define stream operations
#include <iosfwd>
#include <sstream>
#include <boost/numeric/ublas/matrix_expression.hpp>

namespace boost { namespace numeric { namespace ublas {

    /** \brief output stream operator for vector expressions
     *
     * Any vector expressions can be written to a standard output stream
     * as defined in the C++ standard library. For example:
     * \code
     * vector<float> v1(3),v2(3);
     * for(size_t i=0; i<3; i++)
     * {
     *       v1(i) = i+0.2;
     *       v2(i) = i+0.3;
     * }
     * cout << v1+v2 << endl;
     * \endcode
     * will display the some of the 2 vectors like this:
     * \code
     * [3](0.5,2.5,4.5)
     * \endcode
     *
     * \param os is a standard basic output stream
     * \param v is a vector expression
     * \return a reference to the resulting output stream
     */
    template<class E, class T, class VE>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    std::basic_ostream<E, T> &operator << (std::basic_ostream<E, T> &os,
                                           const vector_expression<VE> &v) {
        typedef typename VE::size_type size_type;
        size_type size = v ().size ();
        std::basic_ostringstream<E, T, std::allocator<E> > s;
        s.flags (os.flags ());
        s.imbue (os.getloc ());
        s.precision (os.precision ());
        s << '[' << size << "](";
        if (size > 0)
            s << v () (0);
        for (size_type i = 1; i < size; ++ i)
            s << ',' << v () (i);
        s << ')';
        return os << s.str ().c_str ();
    }

    /** \brief input stream operator for vectors
     *
     * This is used to feed in vectors with data stored as an ASCII representation
     * from a standard input stream.
     *
     * From a file or any valid stream, the format is:
     * \c [<vector size>](<data1>,<data2>,...<dataN>) like for example:
     * \code
     * [5](1,2.1,3.2,3.14,0.2)
     * \endcode
     *
     * You can use it like this
     * \code
     * my_input_stream >> my_vector;
     * \endcode
     *
     * You can only put data into a valid \c vector<> not a \c vector_expression
     *
     * \param is is a standard basic input stream
     * \param v is a vector
     * \return a reference to the resulting input stream
     */
    template<class E, class T, class VT, class VA>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    std::basic_istream<E, T> &operator >> (std::basic_istream<E, T> &is,
                                           vector<VT, VA> &v) {
        typedef typename vector<VT, VA>::size_type size_type;
        E ch;
        size_type size;
        if (is >> ch && ch != '[') {
            is.putback (ch);
            is.setstate (std::ios_base::failbit);
        } else if (is >> size >> ch && ch != ']') {
            is.putback (ch);
            is.setstate (std::ios_base::failbit);
        } else if (! is.fail ()) {
            vector<VT, VA> s (size);
            if (is >> ch && ch != '(') {
                is.putback (ch);
                is.setstate (std::ios_base::failbit);
            } else if (! is.fail ()) {
                for (size_type i = 0; i < size; i ++) {
                    if (is >> s (i) >> ch && ch != ',') {
                        is.putback (ch);
                        if (i < size - 1)
                            is.setstate (std::ios_base::failbit);
                        break;
                    }
                }
                if (is >> ch && ch != ')') {
                    is.putback (ch);
                    is.setstate (std::ios_base::failbit);
                }
            }
            if (! is.fail ())
                v.swap (s);
        }
        return is;
    }

    /** \brief output stream operator for matrix expressions
     *
     * it outpus the content of a \f$(M \times N)\f$ matrix to a standard output
     * stream using the following format:
     * \c[<rows>,<columns>]((<m00>,<m01>,...,<m0N>),...,(<mM0>,<mM1>,...,<mMN>))
     *
     * For example:
     * \code
     * matrix<float> m(3,3) = scalar_matrix<float>(3,3,1.0) - diagonal_matrix<float>(3,3,1.0);
     * cout << m << endl;
     * \encode
     * will display
     * \code
     * [3,3]((0,1,1),(1,0,1),(1,1,0))
     * \endcode
     * This output is made for storing and retrieving matrices in a simple way but you can
     * easily recognize the following:
     * \f[ \left( \begin{array}{ccc} 1 & 1 & 1\\ 1 & 1 & 1\\ 1 & 1 & 1 \end{array} \right) - \left( \begin{array}{ccc} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{array} \right) = \left( \begin{array}{ccc} 0 & 1 & 1\\ 1 & 0 & 1\\ 1 & 1 & 0 \end{array} \right) \f]
     *
     * \param os is a standard basic output stream
     * \param m is a matrix expression
     * \return a reference to the resulting output stream
     */
    template<class E, class T, class ME>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    std::basic_ostream<E, T> &operator << (std::basic_ostream<E, T> &os,
                                           const matrix_expression<ME> &m) {
        typedef typename ME::size_type size_type;
        size_type size1 = m ().size1 ();
        size_type size2 = m ().size2 ();
        std::basic_ostringstream<E, T, std::allocator<E> > s;
        s.flags (os.flags ());
        s.imbue (os.getloc ());
        s.precision (os.precision ());
        s << '[' << size1 << ',' << size2 << "](";
        if (size1 > 0) {
            s << '(' ;
            if (size2 > 0)
                s << m () (0, 0);
            for (size_type j = 1; j < size2; ++ j)
                s << ',' << m () (0, j);
            s << ')';
        }
        for (size_type i = 1; i < size1; ++ i) {
            s << ",(" ;
            if (size2 > 0)
                s << m () (i, 0);
            for (size_type j = 1; j < size2; ++ j)
                s << ',' << m () (i, j);
            s << ')';
        }
        s << ')';
        return os << s.str ().c_str ();
    }

    /** \brief input stream operator for matrices
     *
     * This is used to feed in matrices with data stored as an ASCII representation
     * from a standard input stream.
     *
     * From a file or any valid standard stream, the format is:
     * \c[<rows>,<columns>]((<m00>,<m01>,...,<m0N>),...,(<mM0>,<mM1>,...,<mMN>))
     *
     * You can use it like this
     * \code
     * my_input_stream >> my_matrix;
     * \endcode
     *
     * You can only put data into a valid \c matrix<> not a \c matrix_expression
     *
     * \param is is a standard basic input stream
     * \param m is a matrix
     * \return a reference to the resulting input stream
     */
    template<class E, class T, class MT, class MF, class MA>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    std::basic_istream<E, T> &operator >> (std::basic_istream<E, T> &is,
                                           matrix<MT, MF, MA> &m) {
        typedef typename matrix<MT, MF, MA>::size_type size_type;
        E ch;
        size_type size1, size2;
        if (is >> ch && ch != '[') {
            is.putback (ch);
            is.setstate (std::ios_base::failbit);
        } else if (is >> size1 >> ch && ch != ',') {
            is.putback (ch);
            is.setstate (std::ios_base::failbit);
        } else if (is >> size2 >> ch && ch != ']') {
            is.putback (ch);
            is.setstate (std::ios_base::failbit);
        } else if (! is.fail ()) {
            matrix<MT, MF, MA> s (size1, size2);
            if (is >> ch && ch != '(') {
                is.putback (ch);
                is.setstate (std::ios_base::failbit);
            } else if (! is.fail ()) {
                for (size_type i = 0; i < size1; i ++) {
                    if (is >> ch && ch != '(') {
                        is.putback (ch);
                        is.setstate (std::ios_base::failbit);
                        break;
                    }
                    for (size_type j = 0; j < size2; j ++) {
                        if (is >> s (i, j) >> ch && ch != ',') {
                            is.putback (ch);
                            if (j < size2 - 1) {
                                is.setstate (std::ios_base::failbit);
                                break;
                            }
                        }
                    }
                    if (is >> ch && ch != ')') {
                        is.putback (ch);
                        is.setstate (std::ios_base::failbit);
                        break;
                    }
                    if (is >> ch && ch != ',') {
                       is.putback (ch);
                       if (i < size1 - 1) {
                            is.setstate (std::ios_base::failbit);
                            break;
                       }
                    }
                }
                if (is >> ch && ch != ')') {
                    is.putback (ch);
                    is.setstate (std::ios_base::failbit);
                }
            }
            if (! is.fail ())
                m.swap (s);
        }
        return is;
    }

    /** \brief special input stream operator for symmetric matrices
     *
     * This is used to feed in symmetric matrices with data stored as an ASCII
     * representation from a standard input stream.
     *
     * You can simply write your matrices in a file or any valid stream and read them again
     * at a later time with this function. The format is the following:
     * \code [<rows>,<columns>]((<m00>,<m01>,...,<m0N>),...,(<mM0>,<mM1>,...,<mMN>)) \endcode
     *
     * You can use it like this
     * \code
     * my_input_stream >> my_symmetric_matrix;
     * \endcode
     *
     * You can only put data into a valid \c symmetric_matrix<>, not in a \c matrix_expression
     * This function also checks that input data form a valid symmetric matrix
     *
     * \param is is a standard basic input stream
     * \param m is a \c symmetric_matrix
     * \return a reference to the resulting input stream
     */
    template<class E, class T, class MT, class MF1, class MF2, class MA>
    // BOOST_UBLAS_INLINE This function seems to be big. So we do not let the compiler inline it.
    std::basic_istream<E, T> &operator >> (std::basic_istream<E, T> &is,
                                           symmetric_matrix<MT, MF1, MF2, MA> &m) {
        typedef typename symmetric_matrix<MT, MF1, MF2, MA>::size_type size_type;
        E ch;
        size_type size1, size2;
        MT value;
        if (is >> ch && ch != '[') {
            is.putback (ch);
            is.setstate (std::ios_base::failbit);
        } else if (is >> size1 >> ch && ch != ',') {
            is.putback (ch);
            is.setstate (std::ios_base::failbit);
        } else if (is >> size2 >> ch && (size2 != size1 || ch != ']')) { // symmetric matrix must be square
            is.putback (ch);
            is.setstate (std::ios_base::failbit);
        } else if (! is.fail ()) {
            symmetric_matrix<MT, MF1, MF2, MA> s (size1, size2);
            if (is >> ch && ch != '(') {
                is.putback (ch);
                is.setstate (std::ios_base::failbit);
             } else if (! is.fail ()) {
                for (size_type i = 0; i < size1; i ++) {
                    if (is >> ch && ch != '(') {
                        is.putback (ch);
                        is.setstate (std::ios_base::failbit);
                        break;
                    }
                    for (size_type j = 0; j < size2; j ++) {
                        if (is >> value >> ch && ch != ',') {
                            is.putback (ch);
                            if (j < size2 - 1) {
                                is.setstate (std::ios_base::failbit);
                                break;
                            }
                        }
                        if (i <= j) {
                             // this is the first time we read this element - set the value
                            s(i,j) = value;
                        }
                        else if ( s(i,j) != value ) {
                            // matrix is not symmetric
                            is.setstate (std::ios_base::failbit);
                            break;
                        }
                     }
                     if (is >> ch && ch != ')') {
                         is.putback (ch);
                         is.setstate (std::ios_base::failbit);
                         break;
                     }
                     if (is >> ch && ch != ',') {
                        is.putback (ch);
                        if (i < size1 - 1) {
                             is.setstate (std::ios_base::failbit);
                             break;
                        }
                     }
                }
                if (is >> ch && ch != ')') {
                    is.putback (ch);
                    is.setstate (std::ios_base::failbit);
                }
            }
            if (! is.fail ())
                m.swap (s);
        }
        return is;
    }

}}}

#endif


// Copyright (C) 2011-2012 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#ifndef bempp_default_iterative_solver_hpp
#define bempp_default_iterative_solver_hpp

#include "../common/common.hpp"
#include "bempp/common/config_trilinos.hpp"

#ifdef WITH_TRILINOS

#include "solver.hpp"

#include "belos_solver_wrapper_fwd.hpp" // for default parameter lists
#include "preconditioner.hpp"

#include "../common/deprecated.hpp"

#include <boost/scoped_ptr.hpp>

namespace Thyra {
/** \cond FORWARD_DECL */
template <typename ValueType> class PreconditionerBase;
/** \endcond */
} // namespace Thyra

namespace Bempp {

/** \ingroup linalg
  * \brief Default iterative solver for boundary integral equations.
  *
  * This class provides an interface to various iterative solvers available via
  * the Stratimikos interface to the Belos solver family from Trilinos (see <a
  * href="http://trilinos.sandia.gov/packages/docs/r10.10/packages/stratimikos/doc/html/index.html">Stratimikos
  * documentation</a>). Convergence can be tested either in range space or in
  * the dual space to the range space. A standard Galerkin discretisation of
  * the form \f$Ax=b\f$, maps into the dual space of the range of the operator.
  * By choosing to test in the range space the equation \f$M^\dagger
  * Ax=M^\dagger b\f$ is solved, where \f$M\f$ is the mass matrix, mapping from
  * the range space into its dual and \f$M^\dagger\f$ is its pseudoinverse.
  *
  */
template <typename BasisFunctionType, typename ResultType>
class DefaultIterativeSolver : public Solver<BasisFunctionType, ResultType> {
public:
  typedef Solver<BasisFunctionType, ResultType> Base;

  /** \brief Constructor of the <tt>DefaultIterativeSolver</tt> class.
    *
    * \param[in] boundaryOp
    *   Non-blocked boundary operator.
    * \param[in] mode
    *   Convergence test mode. Default:
    *<tt>TEST_CONVERGENCE_IN_DUAL_TO_RANGE</tt>.
    *
    */
  DefaultIterativeSolver(
      const BoundaryOperator<BasisFunctionType, ResultType> &boundaryOp,
      ConvergenceTestMode::Mode mode =
          ConvergenceTestMode::TEST_CONVERGENCE_IN_DUAL_TO_RANGE);

  /** \brief Constructor of the <tt>DefaultIterativeSolver</tt> class.
    *
    * \param[in] boundaryOp
    *   Blocked boundary operator.
    * \param[in] mode
    *   Convergence test mode. Default:
    *<tt>TEST_CONVERGENCE_IN_DUAL_TO_RANGE</tt>.
    *
    */
  DefaultIterativeSolver(
      const BlockedBoundaryOperator<BasisFunctionType, ResultType> &boundaryOp,
      ConvergenceTestMode::Mode mode =
          ConvergenceTestMode::TEST_CONVERGENCE_IN_DUAL_TO_RANGE);

  virtual ~DefaultIterativeSolver();

  /** \brief Define a preconditioner.
    *
    * The preconditioner is passed on to the Belos solver.
    *
    * \param[in] preconditioner
    *
    * \note This function has no effect if it is called after
    *initializeSolver().
    *
    * \deprecated Do not use this function in new code. Instead, pass the
    *   preconditioner to initializeSolver().
    */
  BEMPP_DEPRECATED void
  setPreconditioner(const Preconditioner<ResultType> &preconditioner);

  /** \brief Initialize a Belos iterative solver.
    *
    * \param[in] paramList
    *   Parameter lists can be read in from XML files or defined in code. Use
    *   defaultGmresParameterList() and defaultCgParameterList() to construct
    *   default parameter lists for the GMRES and CG solvers.
    */
  void initializeSolver(const Teuchos::RCP<Teuchos::ParameterList> &paramList);

  /** \brief Initialize a preconditioned Belos iterative solver.
    *
    * \param[in] paramList
    *   Parameter lists can be read in from XML files or defined in code. Use
    *   defaultGmresParameterList() and defaultCgParameterList() to construct
    *   default parameter lists for the GMRES and CG solvers.
    * \param[in] preconditioner
    *   Preconditioner to be used by the solver.
    */
  void initializeSolver(const Teuchos::RCP<Teuchos::ParameterList> &paramList,
                        const Preconditioner<ResultType> &preconditioner);

private:
  virtual Solution<BasisFunctionType, ResultType> solveImplNonblocked(
      const GridFunction<BasisFunctionType, ResultType> &rhs) const;
  virtual BlockedSolution<BasisFunctionType, ResultType> solveImplBlocked(
      const std::vector<GridFunction<BasisFunctionType, ResultType>> &rhs)
      const;

private:
  struct Impl;
  boost::scoped_ptr<Impl> m_impl;
};

} // namespace Bempp

#endif // WITH_TRILINOS

#endif


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

// File generated at Thu 10 May 2018 14:59:12

#ifndef MSSMNoFV_INPUT_PARAMETERS_H
#define MSSMNoFV_INPUT_PARAMETERS_H

#include <complex>
#include <Eigen/Core>

namespace flexiblesusy {

struct MSSMNoFV_input_parameters {
   double TanBeta{};
   int SignMu{1};
   double Qin{};
   double M1{};
   double M2{};
   double M3{};
   double AtIN{};
   double AbIN{};
   double AtauIN{};
   double AcIN{};
   double AsIN{};
   double AmuonIN{};
   double AuIN{};
   double AdIN{};
   double AeIN{};
   double mHd2IN{};
   double mHu2IN{};
   double ml11IN{};
   double ml22IN{};
   double ml33IN{};
   double me11IN{};
   double me22IN{};
   double me33IN{};
   double mq11IN{};
   double mq22IN{};
   double mq33IN{};
   double mu11IN{};
   double mu22IN{};
   double mu33IN{};
   double md11IN{};
   double md22IN{};
   double md33IN{};

   Eigen::ArrayXd get() const;
   void set(const Eigen::ArrayXd&);
};

std::ostream& operator<<(std::ostream&, const MSSMNoFV_input_parameters&);

} // namespace flexiblesusy

#endif


#include <unordered_set>

#include <arrow/compute/api.h>
#include <spdlog/spdlog.h>
#include <Eigen/Dense>

#include "arrow_utils.h"
#include "compute_utils.h"

namespace stream_data_processor {
namespace compute_utils {

namespace {

arrow::Status sort(
    const std::vector<std::string>& column_names, size_t i,
    const std::shared_ptr<arrow::RecordBatch>& source,
    std::vector<std::shared_ptr<arrow::RecordBatch>>* targets) {
  if (i == column_names.size()) {
    targets->push_back(source);
    return arrow::Status::OK();
  }

  if (source->GetColumnByName(column_names[i]) == nullptr) {
    ARROW_RETURN_NOT_OK(sort(column_names, i + 1, source, targets));
    return arrow::Status::OK();
  }

  ARROW_ASSIGN_OR_RAISE(auto sorted_batch,
                        sortByColumn(column_names[i], source));
  while (true) {
    auto sorted_keys = sorted_batch->GetColumnByName(column_names[i]);

    ARROW_ASSIGN_OR_RAISE(auto min_val, sorted_keys->GetScalar(0));

    ARROW_ASSIGN_OR_RAISE(auto max_val,
                          sorted_keys->GetScalar(sorted_keys->length() - 1));

    arrow::Datum equals_datum;
    ARROW_ASSIGN_OR_RAISE(
        equals_datum,
        arrow::compute::Compare(sorted_keys, min_val,
                                arrow::compute::CompareOptions(
                                    arrow::compute::CompareOperator::EQUAL)));

    ARROW_ASSIGN_OR_RAISE(auto filter_datum,
                          arrow::compute::Filter(sorted_batch, equals_datum));

    ARROW_RETURN_NOT_OK(
        sort(column_names, i + 1, filter_datum.record_batch(), targets));

    if (min_val->Equals(max_val)) {
      break;
    }

    arrow::Datum not_equals_datum;
    ARROW_ASSIGN_OR_RAISE(
        not_equals_datum,
        arrow::compute::Compare(
            sorted_keys, min_val,
            arrow::compute::CompareOptions(
                arrow::compute::CompareOperator::NOT_EQUAL)));

    arrow::Datum filter_not_equals_datum;
    ARROW_ASSIGN_OR_RAISE(
        filter_not_equals_datum,
        arrow::compute::Filter(sorted_batch, not_equals_datum));

    sorted_batch = filter_not_equals_datum.record_batch();
  }

  return arrow::Status::OK();
}

}  // namespace

arrow::Result<arrow::RecordBatchVector> groupSortingByColumns(
    const std::vector<std::string>& column_names,
    const std::shared_ptr<arrow::RecordBatch>& record_batch) {
  arrow::RecordBatchVector grouped;
  ARROW_RETURN_NOT_OK(sort(column_names, 0, record_batch, &grouped));
  return grouped;
}

arrow::Result<std::shared_ptr<arrow::RecordBatch>> sortByColumn(
    const std::string& column_name,
    const std::shared_ptr<arrow::RecordBatch>& source) {
  auto sorting_column = source->GetColumnByName(column_name);
  if (sorting_column == nullptr) {
    return arrow::Status::KeyError(
        fmt::format("No such column with name {}", column_name));
  }

  if (sorting_column->type_id() == arrow::Type::TIMESTAMP) {
    ARROW_ASSIGN_OR_RAISE(sorting_column,
                          sorting_column->View(arrow::int64()));
  }

  ARROW_ASSIGN_OR_RAISE(auto sorted_idx,
                        arrow::compute::SortIndices(*sorting_column));

  ARROW_ASSIGN_OR_RAISE(auto sorted_datum,
                        arrow::compute::Take(source, sorted_idx));

  return sorted_datum.record_batch();
}

arrow::Result<std::pair<size_t, size_t>> argMinMax(
    std::shared_ptr<arrow::Array> array) {
  if (array->type_id() == arrow::Type::TIMESTAMP) {
    ARROW_ASSIGN_OR_RAISE(array, array->View(arrow::int64()));
  }

  ARROW_ASSIGN_OR_RAISE(auto min_max_ts, arrow::compute::MinMax(array));

  int64_t arg_min = -1;
  int64_t arg_max = -1;
  size_t i = 0;
  while (i < array->length() && (arg_min == -1 || arg_max == -1)) {
    ARROW_ASSIGN_OR_RAISE(auto value_scalar, array->GetScalar(i));
    if (value_scalar->Equals(
            min_max_ts.scalar_as<arrow::StructScalar>().value[0])) {
      arg_min = i;
    }

    if (value_scalar->Equals(
            min_max_ts.scalar_as<arrow::StructScalar>().value[1])) {
      arg_max = i;
    }

    ++i;
  }

  return std::pair{arg_min, arg_max};
}

arrow::Result<size_t> tsLowerBound(const arrow::Array& sorted_ts_array,
                                   const std::function<bool(int64_t)>& pred,
                                   arrow::TimeUnit::type time_unit) {
  size_t left_bound = 0;
  size_t right_bound = sorted_ts_array.length();
  while (left_bound != right_bound - 1) {
    auto middle = (left_bound + right_bound) / 2;

    ARROW_ASSIGN_OR_RAISE(auto ts_scalar,
                          arrow_utils::castTimestampScalar(
                              sorted_ts_array.GetScalar(middle), time_unit));

    int64_t ts =
        std::static_pointer_cast<arrow::TimestampScalar>(ts_scalar)->value;

    if (pred(ts)) {
      right_bound = middle;
    } else {
      left_bound = middle;
    }
  }

  ARROW_ASSIGN_OR_RAISE(
      auto ts_scalar, arrow_utils::castTimestampScalar(
                          sorted_ts_array.GetScalar(left_bound), time_unit));

  int64_t ts =
      std::static_pointer_cast<arrow::TimestampScalar>(ts_scalar)->value;

  if (pred(ts)) {
    return left_bound;
  } else {
    return right_bound;
  }
}

double FDDerivativeCalculator::calculateDerivative(
    const std::deque<double>& xs, const std::deque<double>& ys, double x_der,
    size_t order) const {
  if (xs.size() != ys.size()) {
    throw ComputeException(
        fmt::format("Argument and value arrays have different sizes: {} and "
                    "{}",
                    xs.size(), ys.size()));
  }

  if (xs.size() < order + 1) {
    throw ComputeException(
        fmt::format("For calculating {}-order derivative at least {} values "
                    "are needed",
                    order, order + 1));
  }

  std::unordered_set<double> xs_set;
  Eigen::MatrixXd taylor_coeffs_matrix(xs.size(), xs.size());
  for (size_t k = 0; k < xs.size(); ++k) {
    if (xs_set.find(xs[k]) != xs_set.end()) {
      throw ComputeException(fmt::format(
          "Found repeating argument {} which is not allowed", xs[k]));
    } else {
      xs_set.insert(xs[k]);
    }

    double delta = xs[k] - x_der;
    double coeff = 1;
    taylor_coeffs_matrix(0, k) = coeff;
    for (int64_t n = 1; n < xs.size(); ++n) {
      coeff *= delta / n;
      taylor_coeffs_matrix(n, k) = coeff;
    }
  }

  Eigen::VectorXd result_vector = Eigen::VectorXd::Zero(xs.size());
  result_vector(order) = 1;

  Eigen::VectorXd der_coeffs;

  switch (linear_solver_) {
    case AUTO:
    case PARTIAL_PIV_LU:
      der_coeffs = taylor_coeffs_matrix.partialPivLu().solve(result_vector);
      break;
    case HOUSEHOLDER_QR:
      der_coeffs = taylor_coeffs_matrix.householderQr().solve(result_vector);
      break;
    case COL_PIV_HOUSEHOLDER_QR:
      der_coeffs =
          taylor_coeffs_matrix.colPivHouseholderQr().solve(result_vector);
      break;
    default:
      throw ComputeException(
          fmt::format("Unexpected linear solver type: {}", linear_solver_));
  }

  double der_result = 0;
  for (size_t i = 0; i < ys.size(); ++i) {
    der_result += ys[i] * der_coeffs(i);
  }

  return der_result;
}

}  // namespace compute_utils
}  // namespace stream_data_processor


/**
 * @file   SuccessiveLinearizationOptimizer.cpp
 * @brief
 * @date   Jul 24, 2012
 * @author Yong-Dian Jian
 */

#include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/shared_ptr.hpp>
#include <stdexcept>

namespace gtsam {

void SuccessiveLinearizationParams::setIterativeParams(const SubgraphSolverParameters &params) {
  iterativeParams = boost::make_shared<SubgraphSolverParameters>(params);
}

void SuccessiveLinearizationParams::print(const std::string& str) const {
  NonlinearOptimizerParams::print(str);
  switch ( linearSolverType ) {
  case MULTIFRONTAL_CHOLESKY:
    std::cout << "         linear solver type: MULTIFRONTAL CHOLESKY\n";
    break;
  case MULTIFRONTAL_QR:
    std::cout << "         linear solver type: MULTIFRONTAL QR\n";
    break;
  case SEQUENTIAL_CHOLESKY:
    std::cout << "         linear solver type: SEQUENTIAL CHOLESKY\n";
    break;
  case SEQUENTIAL_QR:
    std::cout << "         linear solver type: SEQUENTIAL QR\n";
    break;
  case CHOLMOD:
    std::cout << "         linear solver type: CHOLMOD\n";
    break;
  case CONJUGATE_GRADIENT:
    std::cout << "         linear solver type: CONJUGATE GRADIENT\n";
    break;
  default:
    std::cout << "         linear solver type: (invalid)\n";
    break;
  }

  if(ordering)
    std::cout << "                   ordering: custom\n";
  else
    std::cout << "                   ordering: COLAMD\n";

  std::cout.flush();
}

VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const SuccessiveLinearizationParams &params) {
  gttic(solveGaussianFactorGraph);
  VectorValues delta;
  if (params.isMultifrontal()) {
    delta = GaussianJunctionTree(gfg).optimize(params.getEliminationFunction());
  } else if(params.isSequential()) {
    const boost::shared_ptr<GaussianBayesNet> gbn =
      EliminationTree<GaussianFactor>::Create(gfg)->eliminate(params.getEliminationFunction());
    delta = gtsam::optimize(*gbn);
  }
  else if ( params.isCG() ) {
    if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ...");
    if ( boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams) ) {
      SubgraphSolver solver (gfg, *boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams));
      delta = solver.optimize();
    }
    else {
      throw std::runtime_error("solveGaussianFactorGraph: special cg parameter type is not handled in LM solver ...");
    }
  }
  else {
    throw std::runtime_error("solveGaussianFactorGraph: Optimization parameter is invalid");
  }
  return delta;
}

}