//---------------------------------------------------------------------------//
// Copyright (c) 2020 Mikhail Komarov <nemo@nil.foundation>
//
// 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 BOOST_MULTIPRECISION_MODULAR_BACKENDS_INVERSE_HPP
#define BOOST_MULTIPRECISION_MODULAR_BACKENDS_INVERSE_HPP

#include <boost/container/vector.hpp>
#include <boost/type_traits/is_integral.hpp>

#include <nil/crypto3/multiprecision/detail/default_ops.hpp>

#include <nil/crypto3/multiprecision/cpp_int.hpp>
#include <nil/crypto3/multiprecision/cpp_int/cpp_int_config.hpp>

namespace nil {
    namespace crypto3 {
        namespace multiprecision {
            namespace backends {

                template<typename Backend>
                constexpr Backend eval_extended_euclidean_algorithm(Backend& a, Backend& b, Backend& x, Backend& y) {
                    if (eval_is_zero(a)) {
                        using ui_type = typename std::tuple_element<0, typename Backend::unsigned_types>::type;
                        x = ui_type(0u);
                        y = ui_type(1u);
                        return b;
                    }
                    Backend x1, y1, tmp = b;
                    eval_modulus(tmp, a);
                    Backend d = eval_extended_euclidean_algorithm(tmp, a, x1, y1);
                    tmp = b;
                    eval_divide(tmp, a);
                    eval_multiply(tmp, x1);
                    x = y1;
                    eval_subtract(x, tmp);
                    y = x1;
                    return d;
                }

                template<typename Backend>
                constexpr Backend eval_inverse_extended_euclidean_algorithm(const Backend& a, const Backend& m) {
                    using Backend_doubled = typename default_ops::double_precision_type<Backend>::type;

                    Backend aa = a, mm = m, x, y, g;
                    using ui_type = typename std::tuple_element<0, typename Backend::unsigned_types>::type;
                    g = eval_extended_euclidean_algorithm(aa, mm, x, y);
                    if (!eval_eq(g, ui_type(1u))) {
                        // BOOST_THROW_EXCEPTION(std::invalid_argument("eval_inverse_with_gcd: no inverse element"));
                        return ui_type(0u);
                    } else {
                        eval_modulus(x, m);
                        Backend_doubled tmp(x);
                        eval_add(tmp, m);
                        eval_modulus(tmp, m);
                        return static_cast<Backend>(tmp);
                    }
                }

                template<typename Backend>
                constexpr typename std::tuple_element<0, typename Backend::signed_types>::type
                    eval_monty_inverse(typename std::tuple_element<0, typename Backend::signed_types>::type a) {
                    using si_type = typename std::tuple_element<0, typename Backend::signed_types>::type;

                    if (a % 2 == 0) {
                        throw std::invalid_argument("monty_inverse only valid for odd integers");
                    }

                    /*
                     * From "A New Algorithm for Inversion mod p^k" by Çetin Kaya Koç
                     * https://eprint.iacr.org/2017/411.pdf sections 5 and 7.
                     */

                    si_type b = 1;
                    si_type r = 0;

                    for (size_t i = 0; i != sizeof(si_type) * CHAR_BIT; ++i) {
                        const si_type bi = b % 2;
                        r >>= 1;
                        r += bi << (sizeof(si_type) * CHAR_BIT - 1);

                        b -= a * bi;
                        b >>= 1;
                    }

                    // Now invert in addition space
                    r = (~static_cast<si_type>(0) - r) + 1;

                    return r;
                }

                template<typename Backend>
                constexpr void eval_monty_inverse(Backend& res, const Backend& a, const Backend& p, const Backend& k) {

                    using default_ops::eval_abs;
                    using default_ops::eval_gt;
                    using default_ops::eval_modulus;
                    using default_ops::eval_subtract;

                    using ui_type = typename std::tuple_element<0, typename Backend::unsigned_types>::type;
                    Backend zero = ui_type(0u);
                    Backend one = ui_type(1u);
                    Backend two = ui_type(2u);

                    /*
                     * From "A New Algorithm for Inversion mod p^k" by Çetin Kaya Koç
                     * https://eprint.iacr.org/2017/411.pdf sections 5 and 7.
                     */
                    Backend c, tmp;

                    // a^(-1) mod p:
                    c = eval_inverse_extended_euclidean_algorithm(a, p);

                    Backend bi = one, bt, i = zero, k_negone = k, xi, nextp = one;
                    eval_subtract(k_negone, one);
                    res = zero;

                    // ui_type kn = cpp_int(k_negone);

                    while (!eval_eq(i, k)) {
                        // xi:
                        xi = bi;
                        eval_multiply(xi, c);
                        eval_modulus(xi, p);

                        if (eval_get_sign(xi) < 0) {
                            tmp = xi;
                            eval_abs(tmp, tmp);
                            eval_modulus(tmp, p);
                            xi = p;
                            eval_subtract(xi, tmp);
                        }

                        // bi:
                        tmp = a;
                        eval_multiply(tmp, xi);
                        eval_subtract(bi, tmp);
                        eval_divide(bi, p);

                        // res:
                        tmp = xi;
                        eval_multiply(tmp, nextp);
                        eval_multiply(nextp, p);
                        eval_add(res, tmp);
                        eval_add(i, one);
                    }
                }

                /*
                                template <typename Backend>
                                inline void bigint_shr1(typename boost::mpl::front<typename
                   Backend::unsigned_types>::type x[], size_t x_size, size_t word_shift, size_t bit_shift)
                                {
                                   typedef typename boost::mpl::front<typename Backend::unsigned_types>::type ui_type;

                                   const size_t top = x_size >= word_shift ? (x_size - word_shift) : 0;

                                   if (top > 0)
                                      copy_mem(x, x + word_shift, top);
                                   clear_mem(x + top, std::min(word_shift, x_size));

                                   const auto   carry_mask  = CT::Mask<ui_type>::expand(bit_shift);
                                   const size_t carry_shift = carry_mask.if_set_return(BOTAN_MP_WORD_BITS - bit_shift);

                                   ui_type carry = 0;

                                   for (size_t i = 0; i != top; ++i)
                                   {
                                      const ui_type w = x[top - i - 1];
                                      x[top - i - 1]  = (w >> bit_shift) | carry;
                                      carry           = carry_mask.if_set_return(w << carry_shift);
                                   }
                                }

                                template <typename Backend>
                                inline typename boost::mpl::front<typename Backend::unsigned_types>::type
                   bigint_add2_nc( typename boost::mpl::front<typename Backend::unsigned_types>::type x[], size_t
                   x_size, const typename boost::mpl::front<typename Backend::unsigned_types>::type y[], size_t y_size)
                                {
                                   typedef typename boost::mpl::front<typename Backend::unsigned_types>::type ui_type;

                                   ui_type carry = 0;

                                   BOOST_ASSERT_MSG(x_size >= y_size, "Expected sizes");

                                   const size_t blocks = y_size - (y_size % 8);

                                   for (size_t i = 0; i != blocks; i += 8)
                                      carry = word8_add2(x + i, y + i, carry);

                                   for (size_t i = blocks; i != y_size; ++i)
                                      x[i] = word_add(x[i], y[i], &carry);

                                   for (size_t i = y_size; i != x_size; ++i)
                                      x[i] = word_add(x[i], 0, &carry);

                                   return carry;
                                }

                                template <typename Backend>
                                inline typename boost::mpl::front<typename Backend::unsigned_types>::type
                   bigint_cnd_sub( typename boost::mpl::front<typename Backend::unsigned_types>::type cnd, typename
                   boost::mpl::front<typename Backend::unsigned_types>::type x[], size_t x_size, const typename
                   boost::mpl::front<typename Backend::unsigned_types>::type y[], size_t y_size)
                                {
                                   BOOST_ASSERT_MSG(x_size >= y_size, "Expected sizes");

                                   typedef typename boost::mpl::front<typename Backend::unsigned_types>::type ui_type;

                                   const auto mask = CT::Mask<ui_type>::expand(cnd);

                                   ui_type carry = 0;

                                   const size_t blocks = y_size - (y_size % 8);
                                   ui_type      z[8]   = {0};

                                   for (size_t i = 0; i != blocks; i += 8)
                                   {
                                      carry = word8_sub3(z, x + i, y + i, carry);
                                      mask.select_n(x + i, z, x + i, 8);
                                   }

                                   for (size_t i = blocks; i != y_size; ++i)
                                   {
                                      z[0] = word_sub(x[i], y[i], &carry);
                                      x[i] = mask.select(z[0], x[i]);
                                   }

                                   for (size_t i = y_size; i != x_size; ++i)
                                   {
                                      z[0] = word_sub(x[i], 0, &carry);
                                      x[i] = mask.select(z[0], x[i]);
                                   }

                                   return mask.if_set_return(carry);
                                }

                                template <typename Backend>
                                inline typename boost::mpl::front<typename Backend::unsigned_types>::type
                   bigint_cnd_add( typename boost::mpl::front<typename Backend::unsigned_types>::type       cnd,
                                    typename boost::mpl::front<typename Backend::unsigned_types>::type       x[],
                                    typename boost::mpl::front<typename Backend::unsigned_types>::type       x_size,
                                    const typename boost::mpl::front<typename Backend::unsigned_types>::type y[], size_t
                   y_size)
                                {
                                   BOTAN_ASSERT(x_size >= y_size, "Expected sizes");

                                   typedef typename boost::mpl::front<typename Backend::unsigned_types>::type ui_type;

                                   const auto mask = CT::Mask<ui_type>::expand(cnd);

                                   ui_type carry = 0;

                                   const size_t blocks = y_size - (y_size % 8);
                                   ui_type      z[8]   = {0};

                                   for (size_t i = 0; i != blocks; i += 8)
                                   {
                                      carry = word8_add3(z, x + i, y + i, carry);
                                      mask.select_n(x + i, z, x + i, 8);
                                   }

                                   for (size_t i = blocks; i != y_size; ++i)
                                   {
                                      z[0] = word_add(x[i], y[i], &carry);
                                      x[i] = mask.select(z[0], x[i]);
                                   }

                                   for (size_t i = y_size; i != x_size; ++i)
                                   {
                                      z[0] = word_add(x[i], 0, &carry);
                                      x[i] = mask.select(z[0], x[i]);
                                   }

                                   return mask.if_set_return(carry);
                                }

                                template <typename Backend>
                                inline void bigint_cnd_abs(typename boost::mpl::front<typename
                   Backend::unsigned_types>::type cnd, typename boost::mpl::front<typename
                   Backend::unsigned_types>::type x[], size_t size)
                                {
                                   typedef typename boost::mpl::front<typename Backend::unsigned_types>::type ui_type;
                                   const auto                                                          mask =
                                CT::Mask<ui_type>::expand(cnd);

                                   ui_type carry = mask.if_set_return(1);
                                   for (size_t i = 0; i != size; ++i)
                                   {
                                      const ui_type z = word_add(~x[i], 0, &carry);
                                      x[i]            = mask.select(z, x[i]);
                                   }
                                }

                                template <typename Backend>
                                inline void bigint_cnd_swap(typename boost::mpl::front<typename
                   Backend::unsigned_types>::type cnd, typename boost::mpl::front<typename
                   Backend::unsigned_types>::type x[], typename boost::mpl::front<typename
                   Backend::unsigned_types>::type y[], size_t size)
                                {
                                   typedef typename boost::mpl::front<typename Backend::unsigned_types>::type ui_type;
                                   const auto                                                          mask =
                                CT::Mask<ui_type>::expand(cnd);

                                   for (size_t i = 0; i != size; ++i)
                                   {
                                      const ui_type a = x[i];
                                      const ui_type b = y[i];
                                      x[i]            = mask.select(b, a);
                                      y[i]            = mask.select(a, b);
                                   }
                                }

                                template <typename Backend>
                                void eval_inverse_mod_odd_modulus(Backend& res, const Backend& n, const Backend& mod)
                                {
                                   typedef typename boost::mpl::front<typename Backend::signed_types>::type   si_type;
                                   typedef typename boost::mpl::front<typename Backend::unsigned_types>::type ui_type;

                                   // Caller should assure these preconditions:
                                   BOOST_ASSERT(eval_gt(n, 0));
                                   BOOST_ASSERT(eval_gt(mod, 0));
                                   BOOST_ASSERT(eval_lt(n, mod));
                                   BOOST_ASSERT(eval_ge(mod, 3) && eval_modulus(mod, 2) == 1);*/

                /*
                                This uses a modular inversion algorithm designed by Niels Möller
                                and implemented in Nettle. The same algorithm was later also
                                adapted to GMP in mpn_sec_invert.
                                It can be easily implemented in a way that does not depend on
                                secret branches or memory lookups, providing resistance against
                                some forms of side channel attack.
                                There is also a description of the algorithm in Appendix 5 of "Fast
                                Software Polynomial Multiplication on ARM Processors using the NEON Engine"
                                by Danilo Câmara, Conrado P. L. Gouvêa, Julio López, and Ricardo
                                Dahab in LNCS 8182
                                   https://conradoplg.cryptoland.net/files/2010/12/mocrysen13.pdf
                                Thanks to Niels for creating the algorithm, explaining some things
                                about it, and the reference to the paper.
                                */
                /*
                                   const size_t mod_words = mod.size();
                                   BOOST_ASSERT_MSG(mod_words > 0, "Not empty");

                                   std::vector<ui_type> tmp_mem(5 * mod_words);

                                   ui_type* v_w   = &tmp_mem[0];
                                   ui_type* u_w   = &tmp_mem[1 * mod_words];
                                   ui_type* b_w   = &tmp_mem[2 * mod_words];
                                   ui_type* a_w   = &tmp_mem[3 * mod_words];
                                   ui_type* mp1o2 = &tmp_mem[4 * mod_words];

                                   //   ct::poison(tmp_mem.data(), tmp_mem.size());

                                   copy_mem(a_w, n.data(), std::min(n.size(), mod_words));
                                   copy_mem(b_w, mod.data(), std::min(mod.size(), mod_words));
                                   u_w[0] = 1;
                                   // v_w = 0

                                   // compute (mod + 1) / 2 which [because mod is odd] is equal to
                                   // (mod / 2) + 1
                                   copy_mem(mp1o2, mod.data(), std::min(mod.size(), mod_words));
                                   bigint_shr1(mp1o2, mod_words, 0, 1);
                                   ui_type carry = bigint_add2_nc(mp1o2, mod_words, u_w, 1);
                                   BOOST_ASSERT(carry == 0);

                                   // Only n.bits() + mod.bits() iterations are required, but avoid leaking the size of
                   n const size_t execs = 2 * eval_msb(mod);

                                   for (size_t i = 0; i != execs; ++i)
                                   {
                                      const ui_type odd_a = a_w[0] & 1;

                                      //if(odd_a) a -= b
                                      ui_type underflow = bigint_cnd_sub(odd_a, a_w, b_w, mod_words);

                                      //if(underflow) { b -= a; a = abs(a); swap(u, v); }
                                      bigint_cnd_add(underflow, b_w, a_w, mod_words);
                                      bigint_cnd_abs(underflow, a_w, mod_words);
                                      bigint_cnd_swap(underflow, u_w, v_w, mod_words);

                                      // a >>= 1
                                      bigint_shr1(a_w, mod_words, 0, 1);

                                      //if(odd_a) u -= v;
                                      ui_type borrow = bigint_cnd_sub(odd_a, u_w, v_w, mod_words);

                                      // if(borrow) u += p
                                      bigint_cnd_add(borrow, u_w, mod.data(), mod_words);

                                      const ui_type odd_u = u_w[0] & 1;

                                      // u >>= 1
                                      bigint_shr1(u_w, mod_words, 0, 1);

                                      //if(odd_u) u += mp1o2;
                                      bigint_cnd_add(odd_u, u_w, mp1o2, mod_words);
                                   }

                                   auto a_is_0 = CT::Mask<ui_type>::set();
                                   for (size_t i = 0; i != mod_words; ++i)
                                      a_is_0 &= CT::Mask<ui_type>::is_zero(a_w[i]);

                                   auto b_is_1 = CT::Mask<ui_type>::is_equal(b_w[0], 1);
                                   for (size_t i = 1; i != mod_words; ++i)
                                      b_is_1 &= CT::Mask<ui_type>::is_zero(b_w[i]);

                                   BOOST_ASSERT_MSG(a_is_0.is_set(), "A is zero");

                                   // if b != 1 then gcd(n,mod) > 1 and inverse does not exist
                                   // in which case zero out the result to indicate this
                                   (~b_is_1).if_set_zero_out(v_w, mod_words);*/

                /*
                 * We've placed the result in the lowest words of the temp buffer.
                 * So just clear out the other values and then give that buffer to a
                 * BigInt.
                 */
                /*
                                   clear_mem(&tmp_mem[mod_words], 4 * mod_words);

                                   CT::unpoison(tmp_mem.data(), tmp_mem.size());

                                   Backend r;
                                   r.swap_reg(tmp_mem);
                                   return r;
                                }*/

                /*
                                template <typename Backend, expression_template_option ExpressionTemplates>
                                void inverse_mod_odd_modulus(number<Backend, ExpressionTemplates>&       res,
                                                             const number<Backend, ExpressionTemplates>& n,
                                                             const number<Backend, ExpressionTemplates>& mod)
                                {
                                   eval_inverse_mod_odd_modulus(res.backend(), n.backend(), mod.backend());
                                }
                                 */

                /*
                                template <typename Backend>
                                std::size_t eval_almost_montgomery_inverse(Backend& result, const Backend& a,
                                                                           const Backend& p)
                                {
                                   size_t k = 0;

                                   Backend u = p, v = a, r = 0, s = 1;

                                   while (eval_gt(v, 0))
                                   {
                                      if (eval_integer_modulus(u, 2) == 0)
                                      {
                                         eval_right_shift(u, 1);
                                         eval_left_shift(s, 1);
                                      }
                                      else if (eval_integer_modulus(v, 2) == 0)
                                      {
                                         eval_right_shift(v, 1);
                                         eval_left_shift(r, 1);
                                      }
                                      else if (eval_gt(u, v))
                                      {
                                         eval_subtract(u, v);
                                         eval_right_shift(u, 1);
                                         eval_add(r, s);
                                         eval_left_shift(s, 1);
                                      }
                                      else
                                      {
                                         eval_subtract(v, u);
                                         eval_right_shift(v, 1);
                                         eval_add(s, r);
                                         eval_left_shift(r, 1);
                                      }

                                      k++;
                                   }

                                   if (!eval_gt(p, r))
                                   {
                                      eval_subtract(r, p);
                                   }

                                   result = p;

                                   eval_subtract(result, r);

                                   return k;
                                }
                                */

                /*
                                template <typename Backend, expression_template_option ExpressionTemplates>
                                std::size_t almost_montgomery_inverse(number<Backend, ExpressionTemplates>& result,
                                                                      const number<Backend, ExpressionTemplates>& a,
                                                                      const number<Backend, ExpressionTemplates>& p)
                                {
                                   return eval_almost_montgomery_inverse(result.backend(), a.backend(), p.backend());
                                }
                                */

                /*
                                template <typename Backend>
                                Backend eval_normalized_montgomery_inverse(const Backend& a, const Backend& p)
                                {
                                   Backend     r;
                                   std::size_t k = eval_almost_montgomery_inverse(r, a, p);

                                   for (std::size_t i = 0; i != k; ++i)
                                   {
                                      if (eval_integer_modulus(p, 2) == 1)
                                      {
                                         eval_add(r, p);
                                      }
                                      eval_right_shift(r, 1);
                                   }

                                   return r;
                                }
                                */

                /*
                                template <typename Backend, expression_template_option ExpressionTemplates>
                                number<Backend, ExpressionTemplates> normalized_montgomery_inverse(
                                    const number<Backend, ExpressionTemplates>& a,
                                    const number<Backend, ExpressionTemplates>& p)
                                {
                                   return number<Backend, ExpressionTemplates>(
                                       evaL_normalized_montgomery_inverse(a.backned(), p.backend()));
                                }
                                 */

                /*
                                template <typename Backend>
                                Backend eval_inverse_mod_pow2(Backend& a1, size_t k)
                                {
                                   typedef typename boost::mpl::front<typename Backend::unsigned_types>::type ui_type;*/
                /*
                 * From "A New Algorithm for Inversion mod p^k" by Çetin Kaya Koç
                 * https://eprint.iacr.org/2017/411.pdf sections 5 and 7.
                 */
                /*
                                   if (eval_integer_modulus(a1, 2) == 0)
                                      return 0;

                                   Backend a = a1;
                                   eval_bit_set(a, k);

                                   Backend b = 1, X = 0, newb;

                                   const std::size_t a_words = a.sig_words();

                                   X.grow_to(round_up(k, sizeof(ui_type) * CHAR_BIT) / sizeof(ui_type) * CHAR_BIT);
                                   b.grow_to(a_words);
                                   */
                /*
                                Hide the exact value of k. k is anyway known to word length
                                granularity because of the length of a, so no point in doing more
                                than this.
                                */
                /*

                                   const std::size_t iter = round_up(k, sizeof(ui_type) * CHAR_BIT);

                                   for (std::size_t i = 0; i != iter; ++i)
                                   {
                                      const bool b0 = eval_bit_test(b, 0);
                                      X.conditionally_set_bit(i, b0);
                                      newb = b;
                                      eval_subtract(newb, a);
                                      b.ct_cond_assign(b0, newb);
                                      eval_right_shift(b, 1);
                                   }
                                   eval_bit_set(X, k);
                                   X.const_time_unpoison();
                                   return X;
                                }
                                */

                /*
                                template <typename Backend, expression_template_option ExpressionTemplates>
                                number<Backend, ExpressionTemplates> inverse_mod_pow2(
                                    const number<Backend, ExpressionTemplates>& a1, size_t k)
                                {
                                   return number<Backend, ExpressionTemplates>(
                                       eval_inverse_mod_pow2(a1.backend(), k.backend()));
                                }
                                */

                /*
                                template <typename Backend>
                                Backend eval_inverse_mod(Backend& res, const Backend& n, const Backend& mod)
                                {
                                   if (eval_is_zero(mod))
                                   {
                                      BOOST_THROW_EXCEPTION(
                                          std::invalid_argument("eval_inverse_mod: mod must be non zero"));
                                   }
                                   if ((eval_get_sign(mod) < 0) || (eval_get_sign(n) < 0))
                                   {
                                      BOOST_THROW_EXCEPTION(
                                          std::invalid_argument("eval_inverse_mod: arguments must be non-negative"));
                                   }
                                   if (eval_is_zero(n) || (eval_integer_modulus(n, 2) == 0 && eval_integer_modulus(mod,
                   2) == 0))
                                   {
                                      return 0;
                                   }
                                   if (eval_integer_modulus(n, 2) == 1)
                                   {*/
                /*
                                Fastpath for common case. This leaks information if n > mod
                                but we don't guarantee const time behavior in that case.
                                */
                /*
                                      if (eval_gt(mod, n))
                                         return eval_inverse_mod_odd_modulus(n, mod);
                                      else
                                         return eval_inverse_mod_odd_modulus(ct_modulo(n, mod), mod);
                                   }

                                   const std::size_t mod_lz = eval_lsb(mod);
                                   BOOST_ASSERT(mod_lz > 0);
                                   const std::size_t mod_bits = eval_msb(mod);
                                   BOOST_ASSERT(mod_bits > mod_lz);

                                   if (mod_lz == mod_bits - 1)
                                   {
                                      // In this case we are performing an inversion modulo 2^k
                                      return eval_inverse_mod_pow2(n, mod_lz);
                                   }*/

                /*
                 * In this case we are performing an inversion modulo 2^k*o for
                 * some k > 1 and some odd (not necessarily prime) integer.
                 * Compute the inversions modulo 2^k and modulo o, then combine them
                 * using CRT, which is possible because 2^k and o are relatively prime.
                 */
                /*
                                   Backend o = mod;

                                   eval_right_shift(mod, mod_lz);

                                   Backend n_redc = ct_modulo(n, o);
                                   Backend inv_o  = eval_inverse_mod_odd_modulus(n_redc, o);
                                   Backend inv_2k = eval_inverse_mod_pow2(n, mod_lz);

                                   // No modular inverse in this case:
                                   if (eval_is_zero(o) || eval_is_zero(inv_2k))
                                      return 0;

                                   Backend m2k = mod_lz;
                                   eval_multiply(m2k, m2k);
                                   // Compute the CRT parameter
                                   Backend c = inverse_mod_pow2(o, mod_lz);

                                   // Compute h = c*(inv_2k-inv_o) mod 2^k
                                   Backend h = inv_2k;

                                   eval_subtract(h, inv_o);
                                   eval_multiply(h, c);

                                   const bool h_neg = (eval_get_sign(h) < 0);

                                   eval_abs(h); // h.set_sign(BigInt::Positive);
                                   eval_bit_set(h, mod_lz);

                                   const bool h_nonzero = !eval_is_zero(h);

                                   eval_subtracr(m2k, h);
                                   h.ct_cond_assign(h_nonzero && h_neg, m2k);

                                   // Return result inv_o + h * o
                                   eval_multiply(h, o);
                                   eval_add(h, inv_o);

                                   return h;
                                }
                                */
            }    // namespace backends
        }        // namespace multiprecision
    }            // namespace crypto3
}    // namespace nil

#endif


/* Greg Anderson
 *
 * Wrapper classes for Apron abstractions.
 */

#ifndef _ABSTRACT_H_
#define _ABSTRACT_H_

#include <ap_abstract0.h>
#include <ap_disjunction.h>
#include <t1p.h>
#include <box.h>
#include <pk.h>

#include <cstdlib>
#include <vector>
#include <map>
#include <memory>
#include <iostream>
#include <optional>
#include <Eigen/Dense>

class ArithExpr {
  private:
    ap_texpr0_t* expr;

  public:
    ArithExpr();
    ArithExpr(double constant);
    ArithExpr(int ind);
    ArithExpr(double lower, double upper);
    ArithExpr(ap_texpr0_t* expr);
    ArithExpr(const ArithExpr& other);
    ArithExpr(ArithExpr&& other);
    ArithExpr(const std::string& str);
    ~ArithExpr();
    ArithExpr& operator=(const ArithExpr& other);
    ArithExpr& operator=(ArithExpr&& other);
    ArithExpr negate() const;
    ArithExpr operator+(const ArithExpr& other) const;
    ArithExpr operator-(const ArithExpr& other) const;
    ArithExpr operator*(const ArithExpr& other) const;
    ArithExpr operator/(const ArithExpr& other) const;
    // Be careful about the precedence of ^. C++ uses this operator for XOR so
    // it has much lower precedence than you would expect for a power operator.
    ArithExpr operator^(int power) const;

    inline ap_texpr0_t* get_texpr() const {
      return expr;
    }
};

/**
 * A set of linear constraints. A point x satisfies these constraints if
 * `weights * x <= biases`.
 */
class LinCons {
  public:
    Eigen::MatrixXd weights;
    Eigen::VectorXd biases;
    LinCons();
    LinCons(const Eigen::MatrixXd& ws, const Eigen::VectorXd& bs);
    double distance_from(const Eigen::VectorXd& x) const;
};

enum class AbstractDomain { ZONOTOPE, INTERVAL, POLYHEDRA };

/**
 * An abstract value over some underlying Apron value. This is just a
 * convenient wrapper class around Apron values.
 */
class AbstractVal {
  protected:
    ap_manager_t* man;
    ap_abstract0_t* value;
    virtual std::unique_ptr<AbstractVal> make_new(ap_abstract0_t* a) const;
    AbstractDomain domain;

  public:
    AbstractVal();
    AbstractVal(ap_manager_t* man, ap_abstract0_t* v);
    // Construct a new abstract value in the given domain subject to the
    // set of linear constraints a x <= b
    AbstractVal(AbstractDomain dom, const std::vector<Eigen::VectorXd>& a,
        const std::vector<double>& b);
    AbstractVal(AbstractDomain dom, const LinCons& lc);

    // Construct a new abstract value in the given domain from the given
    // interval
    AbstractVal(AbstractDomain dom, const Eigen::VectorXd& lowers,
        const Eigen::VectorXd& uppers);
    AbstractVal(const AbstractVal& other);
    AbstractVal(AbstractVal&& other);
    virtual ~AbstractVal();
    AbstractVal& operator=(const AbstractVal& other) = delete;
    AbstractVal& operator=(AbstractVal&& other) = delete;

    inline ap_manager_t* get_manager() const {
      return man;
    }

    inline ap_abstract0_t* get_value() const {
      return value;
    }

    inline AbstractDomain get_domain() const {
      return domain;
    }

    std::unique_ptr<AbstractVal> add_trailing_dimensions(int n) const;
    std::unique_ptr<AbstractVal> add_leading_dimensions(int n) const;
    std::unique_ptr<AbstractVal> remove_trailing_dimensions(int n) const;

    /**
     * Meet this value with the linear constraints a x <= b.
     */
    virtual std::unique_ptr<AbstractVal> meet_linear_constraint(
        const Eigen::MatrixXd& a,
        const Eigen::VectorXd& b) const;

    /**
     * Perform a specific affine transformation.
     */
    virtual std::unique_ptr<AbstractVal> scalar_affine(
        const Eigen::MatrixXd& w,
        const Eigen::VectorXd& b) const;

    /**
     * Perform an abstract transformation where each coefficient is an interval.
     */
    virtual std::unique_ptr<AbstractVal> interval_affine(
        const Eigen::MatrixXd& wl,
        const Eigen::MatrixXd& wu,
        const Eigen::VectorXd& bl,
        const Eigen::VectorXd& bu) const;

    /**
     * A relu is computed as follows: for each dimension i, compute
     * x_l = meet(x, x_i < 0) and x_u = meet(x, x_i >= 0). Compute
     * x'_l = x_l[x_i <- 0]. Let x = join(x'_l, x_u).
     */
    virtual std::unique_ptr<AbstractVal> relu() const;

    virtual std::unique_ptr<AbstractVal> join(const AbstractVal& other) const;

    virtual std::unique_ptr<AbstractVal> meet(const AbstractVal& other) const;

    virtual std::unique_ptr<AbstractVal> widen(const AbstractVal& other) const;

    virtual bool operator==(const AbstractVal& other) const;

    /**
     * Create an abstract value by adding each dimension of b to this and
     * maintain the relations among variables in b.
     */
    virtual std::unique_ptr<AbstractVal> append(const AbstractVal& b) const;

    virtual std::unique_ptr<AbstractVal> arith_computation(
        const std::vector<ArithExpr>& exprs) const;

    inline bool is_bottom() const {
      return ap_abstract0_is_bottom(man, value);
    }

    inline bool is_top() const {
      return ap_abstract0_is_top(man, value);
    }

    bool contains_point(const Eigen::VectorXd& x) const;
    bool contains(const AbstractVal& x) const;
    Eigen::VectorXd get_center() const;
    virtual Eigen::VectorXd get_contained_point() const;

    /**
     * Get the number of dimensions of this abstract value.
     */
    inline size_t dims() const {
      return ap_abstract0_dimension(man, value).realdim;
    }

    virtual std::unique_ptr<AbstractVal> clone() const;

    virtual std::unique_ptr<AbstractVal> bottom() const;

    LinCons get_lincons() const;

    //virtual double distance_to_point(const Eigen::VectorXd& x) const;
    void print(FILE* out) const;
};

/**
 * Powerset is used for a bounded powerset domain. It is based on Aprons
 * disjunctive domain, but applies a consolidation step after each join or
 * merge.
 */
class Powerset: public AbstractVal {
  private:
    size_t size;

  protected:
    std::unique_ptr<AbstractVal> make_new(ap_abstract0_t* a) const override;

  public:
    Powerset(ap_manager_t* m, ap_abstract0_t* v, size_t s);
    Powerset(const Powerset& p);
    Powerset(AbstractDomain dom, size_t size,
        const std::vector<Eigen::VectorXd>& a,
        const std::vector<double>& b);
    Powerset(AbstractDomain dom, size_t size, const LinCons& lc);
    Powerset(AbstractDomain dom, size_t size,
        const Eigen::VectorXd& lowers,
        const Eigen::VectorXd& uppers);
    Powerset& operator=(const Powerset& other);
    std::unique_ptr<AbstractVal> join(const AbstractVal& other) const override;
    std::unique_ptr<AbstractVal> meet(const AbstractVal& other) const override;
    std::unique_ptr<AbstractVal> arith_computation(
        const std::vector<ArithExpr>& exprs) const override;
    Eigen::VectorXd get_contained_point() const;
    std::unique_ptr<AbstractVal> clone() const override;
    std::unique_ptr<AbstractVal> bottom() const override;
    //double distance_to_point(const Eigen::VectorXd& x) const;
};

//class Managers {
//  private:
//    ap_manager_t* t1p_man;
//    ap_manager_t* box_man;
//    std::map<ap_manager_t*, ap_manager_t*> disj_mans;
//
//  public:
//    Managers(ap_manager_t*, ap_manager_t*);
//    ~Managers();
//    inline ap_manager_t* get_t1p_manager() const {
//      return t1p_man;
//    }
//    inline ap_manager_t* get_box_manager() const {
//      return box_man;
//    }
//    inline ap_manager_t* get_disj_manager(ap_manager_t* base) const {
//      return disj_mans.at(base);
//    }
//};

#endif


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <scitbx/math/parabolic_cylinder_d.h>
#include <scitbx/math/bessel.h>
#include <scitbx/math/chebyshev.h>
#include <scitbx/math/dihedral.h>
#include <scitbx/math/erf.h>
#include <scitbx/math/euler_angles.h>
#include <scitbx/math/floating_point_epsilon.h>
#include <scitbx/math/gamma.h>
#include <scitbx/math/gcd.h>
#include <scitbx/math/halton.h>
#include <scitbx/math/lambertw.h>
#include <scitbx/math/phase_error.h>
#include <scitbx/math/resample.h>
#include <scitbx/math/superpose.h>
#include <scitbx/math/utils.h>
#include <boost/rational.hpp> // for boost::gcd
#include <scitbx/math/approx_equal.h>
#include <scitbx/math/orthonormal_basis.h>
#include <scitbx/math/gaussian_fit_1d_analytical.h>
#include <scitbx/math/cubic_equation.h>
#include <scitbx/math/distance_difference.h>
#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <scitbx/math/fast_approx_math.h>
#include <scitbx/math/g_function.h>

namespace scitbx { namespace math {
namespace boost_python {

  void wrap_basic_statistics();
  void wrap_gaussian();
  void wrap_golay();
  void wrap_minimum_covering_sphere();
  void wrap_principal_axes_of_inertia();
  void wrap_row_echelon();
  void wrap_tensor_rank_2();
  void wrap_icosahedron();
  void wrap_chebyshev_base();
  void wrap_chebyshev_polynome();
  void wrap_chebyshev_fitter();
  void wrap_chebyshev_lsq();
  void wrap_slatec();
  void wrap_line_search();
  void wrap_r3_rotation();
  void wrap_resample();
  void wrap_quadrature();
  void wrap_unimodular_generator();
  void wrap_halton();
  void wrap_least_squares_plane();
  void wrap_continued_fraction();
  void wrap_numeric_limits();
  void wrap_distributions();
  void wrap_exp_functions();
  void wrap_zernike();
  void wrap_zernike_mom();
  void wrap_2d_zernike_mom();
  void wrap_weighted_covariance();
  void wrap_dmatrix();
  void wrap_correlation();
  void wrap_interpolation();
  void wrap_tetrahedron();
  void wrap_angle_derivative();

namespace {

  int
  time_gcd_int_boost(
    int n)
  {
    int result = 0;
    for(int a=0;a<n;a++) {
      for(int b=0;b<n;b++) {
        int c = boost::gcd(a, b);
        if (result < c) result = c;
      }
    }
    return result;
  }

  long
  time_gcd_long_boost(
    long n)
  {
    long result = 0;
    for(long a=0;a<n;a++) {
      for(long b=0;b<n;b++) {
        long c = boost::gcd(a, b);
        if (result < c) result = c;
      }
    }
    return result;
  }

  int
  time_gcd_int_simple(
    int n)
  {
    int result = 0;
    for(int a=0;a<n;a++) {
      for(int b=0;b<n;b++) {
        int c = gcd_int_simple(a, b);
        if (result < c) result = c;
      }
    }
    return result;
  }

  long
  time_gcd_long_simple(
    long n)
  {
    long result = 0;
    for(long a=0;a<n;a++) {
      for(long b=0;b<n;b++) {
        long c = gcd_long_simple(a, b);
        if (result < c) result = c;
      }
    }
    return result;
  }

  long
  time_gcd_unsigned_long_binary(
    unsigned long n)
  {
    unsigned long result = 0;
    for(unsigned long a=0;a<n;a++) {
      for(unsigned long b=0;b<n;b++) {
        unsigned long c = gcd_unsigned_long_binary(a, b);
        if (result < c) result = c;
      }
    }
    return result;
  }

  long
  time_gcd_long_binary(
    long n)
  {
    long result = 0;
    for(long a=0;a<n;a++) {
      for(long b=0;b<n;b++) {
        long c = gcd_long_binary(a, b);
        if (result < c) result = c;
      }
    }
    return result;
  }

#if defined(SCITBX_MATH_GCD_USING_ASM)
  int
  time_gcd_int32_asm(
    int n)
  {
    int result = 0;
    for(int a=0;a<n;a++) {
      for(int b=0;b<n;b++) {
        int c = gcd_int32_asm(a, b);
        if (result < c) result = c;
      }
    }
    return result;
  }

# if defined(__x86_64__)
  long
  time_gcd_int64_asm(
    long n)
  {
    long result = 0;
    for(long a=0;a<n;a++) {
      for(long b=0;b<n;b++) {
        long c = gcd_int64_asm(a, b);
        if (result < c) result = c;
      }
    }
    return result;
  }
# endif
#endif

  template <typename SitesType>
  boost::optional<double>
  dihedral_angle(
    SitesType const& sites,
    bool deg)
  {
    return dihedral(sites).angle(deg);
  }

  mat3<double>
  superpose_kearsley_rotation(
    af::const_ref<vec3<double> > const& reference_sites,
    af::const_ref<vec3<double> > const& other_sites)
  {
    return superpose::superposition<>::kearsley_rotation(
      reference_sites, other_sites);
  }

  mat3< double >
  euler_angles_xyz_matrix(
      const double& ax,
      const double& ay,
      const double& az )
  {
    return euler_angles::xyz_matrix( ax, ay, az );
  }

  vec3< double >
  euler_angles_xyz_angles(
      const mat3< double >& m,
      const double& eps = 1e-12 )
  {
    return euler_angles::xyz_angles( m, eps );
  }

  mat3< double >
  euler_angles_yzx_matrix(
      const double& ay,
      const double& az,
      const double& ax )
  {
    return euler_angles::yzx_matrix( ay, az, ax );
  }

  vec3< double >
  euler_angles_yzx_angles(
      const mat3< double >& m,
      const double& eps = 1e-12 )
  {
    return euler_angles::yzx_angles( m, eps );
  }

  mat3< double >
  euler_angles_zyz_matrix(
      const double& az1,
      const double& ay,
      const double& az3 )
  {
    return euler_angles::zyz_matrix( az1, ay, az3 );
  }

  vec3< double >
  euler_angles_zyz_angles(
      const mat3< double >& m,
      const double& eps = 1e-12 )
  {
    return euler_angles::zyz_angles( m, eps );
  }

  template <typename T>
  struct approx_equal_relatively_wrapper
  {
    typedef math::approx_equal_relatively<T> wt;
    typedef T arg_t;
    typedef typename wt::amplitude_type amplitude_t;
    static bool form_1(arg_t x, arg_t y, amplitude_t relative_error) {
      wt p(relative_error);
      return p(x, y);
    }
    static bool form_2(arg_t x, arg_t y, amplitude_t relative_error,
                       amplitude_t near_zero_threshold)
    {
      wt p(relative_error, near_zero_threshold);
      return p(x, y);
    }

    static void wrap() {
      using namespace boost::python;
      def("approx_equal_relatively", form_1,
          (arg("x"), arg("y"), arg("relative_error")));
      def("approx_equal_relatively", form_2,
          (arg("x"), arg("y"), arg("relative_error"),
           arg("near_zero_threshold")));
    }
  };

  void init_module()
  {
    using namespace boost::python;

    def("time_gcd_int_boost", time_gcd_int_boost);
    def("time_gcd_long_boost", time_gcd_long_boost);
    def("gcd_int_simple", gcd_int_simple, (arg("a"), arg("b")));
    def("time_gcd_int_simple", time_gcd_int_simple);
    def("gcd_long_simple", gcd_long_simple, (arg("a"), arg("b")));
    def("time_gcd_long_simple", time_gcd_long_simple);
    def("time_gcd_unsigned_long_binary", time_gcd_unsigned_long_binary);
    def("gcd_long_binary", gcd_long_binary, (arg("a"), arg("b")));
    def("time_gcd_long_binary", time_gcd_long_binary);
#if defined(SCITBX_MATH_GCD_USING_ASM)
    def("gcd_int32_asm", gcd_int32_asm, (arg("a"), arg("b")));
    def("time_gcd_int32_asm", time_gcd_int32_asm);
# if defined(__x86_64__)
    def("gcd_int64_asm", gcd_int64_asm, (arg("a"), arg("b")));
    def("time_gcd_int64_asm", time_gcd_int64_asm);
# endif
#endif

    def("floating_point_epsilon_float_get",
      &floating_point_epsilon<float>::get);
    def("floating_point_epsilon_double_get",
      &floating_point_epsilon<double>::get);

    def("erf", (double(*)(double const&)) erf);
    def("erf",
      (scitbx::af::shared<double>(*)(
        scitbx::af::const_ref<double> const&)) erf);
    def("erfc", (double(*)(double const&)) erfc);
    def("erfcx", (double(*)(double const&)) erfcx);

    def("parabolic_cylinder_d", (double(*)(double, double))
      parabolic_cylinder_d::dv);

    // G-function
    def("GfuncOfRSsqr_approx", (double(*)(double))
      g_function::GfuncOfRSsqr_approx);

    def("bessel_i1_over_i0", (double(*)(double const&)) bessel::i1_over_i0);
    def("bessel_i1_over_i0",
      (scitbx::af::shared<double>(*)(scitbx::af::const_ref<double> const&))
        bessel::i1_over_i0);
    def("bessel_inverse_i1_over_i0",
      (double(*)(double const&)) bessel::inverse_i1_over_i0);
    def("inverse_bessel_i1_over_i0", (scitbx::af::shared<double>(*)(
         scitbx::af::const_ref<double> const&)) bessel::inverse_i1_over_i0);
    def("bessel_i0", (double(*)(double const&)) bessel::i0);
    def("bessel_i1", (double(*)(double const&)) bessel::i1);
    def("bessel_ln_of_i0", (double(*)(double const&)) bessel::ln_of_i0);
    def("ei1", (double(*)(double const&)) bessel::ei1);
    def("ei0", (double(*)(double const&)) bessel::ei0);

    typedef return_value_policy<return_by_value> rbv;
    namespace smg=scitbx::math::gaussian_fit_1d_analytical;
    class_<smg::compute<> >("gaussian_fit_1d_analytical")
      .def(init<
           af::const_ref<double> const&,
           af::const_ref<double> const&,
           af::const_ref<double> const& >(
             (arg("x"),
              arg("y"),
              arg("z"))))
      .def(init<
           af::const_ref<double> const&,
           af::const_ref<double> const& >(
             (arg("x"),
              arg("y"))))
      .add_property("a", make_getter(&smg::compute<>::a, rbv()))
      .add_property("b", make_getter(&smg::compute<>::b, rbv()))
    ;

    //typedef return_value_policy<return_by_value> rbv;
    namespace cueq=scitbx::math::cubic_equation;
    class_<cueq::real<> >("cubic_equation_real")
      .def(init<
           double const&,
           double const&,
           double const&,
           double const& >(
             (arg("a"),
              arg("b"),
              arg("c"),
              arg("d"))))
      .def("residual", &cueq::real<>::residual)
      .add_property("x", make_getter(&cueq::real<>::x, rbv()))
      .add_property("A", make_getter(&cueq::real<>::A,  rbv()))
      .add_property("B", make_getter(&cueq::real<>::B,  rbv()))
      .add_property("D", make_getter(&cueq::real<>::D,  rbv()))
    ;

#if defined(SCITBX_MATH_BESSEL_HAS_SPHERICAL)
    def("spherical_bessel",
      (double(*)(int const&, double const&))
        bessel::spherical_bessel);
    def("spherical_bessel_array",
      (scitbx::af::shared< double> (*)(
        int const&, scitbx::af::shared<double> const&))
          bessel::spherical_bessel_array);
    def("bessel_J",
      (double(*)(int const&, double const&))
        bessel::bessel_J);
    def("bessel_J_array",
      (scitbx::af::shared< double> (*)(
        int const&, scitbx::af::shared<double> const&))
          bessel::bessel_J_array);
    def ("bessel_J_zeroes",
           (scitbx::af::shared< double> (*)(
        double const&, int const&))
        bessel::bessel_J_zeroes);
    def ("sph_bessel_j_zeroes",
           (scitbx::af::shared< double> (*)(
        double const&, int const&))
        bessel::sph_bessel_j_zeroes);

#endif

    def("gamma_complete", (double(*)(double const&, bool))
      gamma::complete, (
        arg("x"),
        arg("minimax")=true));
    def("gamma_incomplete", (double(*)(double const&,
                                       double const&,
                                       unsigned))
      gamma::incomplete, (arg("a"),
        arg("x"),
        arg("max_iterations")=500));
    def("gamma_incomplete_complement",(double(*)(double const&,
                                                 double const&,
                                                 unsigned))
      gamma::incomplete_complement, (
        arg("a"),
        arg("x"),
        arg("max_iterations")=500));
    def("exponential_integral_e1z", (double(*)(double const&))
         gamma::exponential_integral_e1z );

    def("lambertw", (double(*)(double const&, unsigned))
      lambertw, (
        arg("x"),
        arg("max_iterations")=100));

    wrap_basic_statistics();
    wrap_gaussian();
    wrap_golay();
    wrap_minimum_covering_sphere();
    wrap_principal_axes_of_inertia();
    wrap_row_echelon();
    wrap_tensor_rank_2();
    wrap_icosahedron();
    wrap_chebyshev_base();
    wrap_chebyshev_polynome();
    wrap_chebyshev_fitter();
    wrap_chebyshev_lsq();
    wrap_slatec();
    wrap_line_search();
    wrap_r3_rotation();
    wrap_resample();
    wrap_quadrature();
    wrap_unimodular_generator();
    wrap_halton();
    wrap_least_squares_plane();
    wrap_continued_fraction();
    wrap_numeric_limits();
    wrap_distributions();
    wrap_exp_functions();
    wrap_zernike();
    wrap_zernike_mom();
    wrap_2d_zernike_mom();
    wrap_weighted_covariance();
    wrap_dmatrix();
    wrap_correlation();
    wrap_interpolation();
    wrap_tetrahedron();
    wrap_angle_derivative();

    def("superpose_kearsley_rotation", superpose_kearsley_rotation, (
      arg("reference_sites"), arg("other_sites")));

    def("dihedral_angle",
      dihedral_angle<af::tiny<vec3<double>, 4> >, (
        arg("sites"), arg("deg")=false));
    def("dihedral_angle",
      dihedral_angle<af::const_ref<vec3<double> > >, (
        arg("sites"), arg("deg")=false));

    def( "euler_angles_xyz_matrix", euler_angles_xyz_matrix, (
      arg("ax"), arg("ay"), arg("az")));
    def( "euler_angles_xyz_angles", euler_angles_xyz_angles, (
      arg("m"), arg("eps")=1e-12));

    def( "euler_angles_yzx_matrix", euler_angles_yzx_matrix, (
      arg("ay"), arg("az"), arg("ax")));
    def( "euler_angles_yzx_angles", euler_angles_yzx_angles, (
      arg("m"), arg("eps")=1e-12));

    def( "euler_angles_zyz_matrix", euler_angles_zyz_matrix, (
      arg("az1"), arg("ay"), arg("az3")));
    def( "euler_angles_zyz_angles", euler_angles_zyz_angles, (
      arg("m"), arg("eps")=1e-12));

    def("approx_sqrt", math::approx_sqrt);

    def("cos_table",
      (double(*)(
         af::const_ref<double> const&,
         double,
         double const& ,
         int const& ,
         bool))
           math::cos_table, (
             arg("table"), arg("arg"), arg("step"), arg("n"),
             arg("interpolate")));

    def("sin_table",
      (double(*)(
         af::const_ref<double> const&,
         double,
         double const& ,
         int const& ,
         bool))
           math::sin_table, (
             arg("table"), arg("arg"), arg("step"), arg("n"),
             arg("interpolate")));

    def("signed_phase_error",
      (double(*)(
        double const&, double const&, bool))
          math::signed_phase_error, (
            arg("phi1"), arg("phi2"), arg("deg")=false));
    def("signed_phase_error",
      (af::shared<double>(*)(
        af::const_ref<double> const&, af::const_ref<double> const&, bool))
          math::signed_phase_error, (
            arg("phi1"), arg("phi2"), arg("deg")=false));
    def("phase_error",
      (double(*)(
        double const&, double const&, bool))
          math::phase_error, (
            arg("phi1"), arg("phi2"), arg("deg")=false));
    def("phase_error",
      (af::shared<double>(*)(
        af::const_ref<double> const&, af::const_ref<double> const&, bool))
          math::phase_error, (
            arg("phi1"), arg("phi2"), arg("deg")=false));
    def("nearest_phase",
      (double(*)(
        double const&, double const&, bool))
          math::nearest_phase, (
            arg("reference"), arg("other"), arg("deg")=false));
    def("nearest_phase",
      (af::shared<double>(*)(
        af::const_ref<double> const&, af::const_ref<double> const&, bool))
          math::nearest_phase, (
            arg("reference"), arg("other"), arg("deg")=false));
    def("divmod", math::divmod);
    approx_equal_relatively_wrapper<double>::wrap();
    approx_equal_relatively_wrapper<std::complex<double> >::wrap();
    {
      af::tiny<vec3<double>, 3> (*f1)(vec3<double> const &, vec3<double> const &,
                                      bool) = &orthonormal_basis;
      af::tiny<vec3<double>, 3> (*f2)(vec3<double> const &, int,
                                      vec3<double> const &, int,
                                      bool) = &orthonormal_basis;
      def("orthonormal_basis", f1, (arg("v0"), arg("v1"),
                                    arg("right_handed")=true));
      def("orthonormal_basis", f2, (arg("v0"), arg("axis_index_1"),
                                    arg("v1"), arg("axis_index_2"),
                                    arg("right_handed")=true));
    }
    def("distance_difference_matrix", distance_difference_matrix<double>, (
      arg("sites1"), arg("sites2")));
  }

}}}} // namespace scitbx::math::boost_python::<anonymous>

BOOST_PYTHON_MODULE(scitbx_math_ext)
{
  scitbx::math::boost_python::init_module();
}


#include "PCE.h"
#include "GeomCommonFunctions.h"
#include <Gui/Application.h>
#include <BRepBuilderAPI_GTransform.hxx>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <tchar.h>
#include <iostream>

using namespace PartDesignGui;

LocalCS::LocalCS()
{
	origin = Vector2d(0.0,0.0);
	axis_x = Vector2d(1.0,0.0);
	axis_y = Math::Functs::RotationAxis2d(axis_x, -Math::Math_PI / 2.0, Vector2d(0.0, 0.0));
	Math::Functs::SetVectorLength(axis_x, 1.0);
	Math::Functs::SetVectorLength(axis_y, 1.0);
	angle = Math::Functs::GetAngleBetween(axis_x, Vector2d(1.0, 0.0));
	if (axis_x[1] < 0.0) angle = 2 * M_PI - angle;
	origin_max_y = 0.0;
	origin_min_y = 0.0;
	origin_min_x = 0.0;
	origin_max_x = 0.0;
	offset_max_y = 0.0;
	offset_min_y = 0.0;
	offset_min_x = 0.0;
	offset_max_x = 0.0;
}
LocalCS::LocalCS(const Vector2d& o, const Vector2d& x)
{
	origin = o;
	axis_x = x;
	axis_y = Math::Functs::RotationAxis2d(axis_x, -Math::Math_PI/2.0, Vector2d(0.0, 0.0));
	Math::Functs::SetVectorLength(axis_x, 1.0);
	Math::Functs::SetVectorLength(axis_y, 1.0);
	angle = Math::Functs::GetAngleBetween(axis_x, Vector2d(1.0, 0.0));
	if (axis_x[1] < 0.0) angle = 2 * M_PI - angle;
	origin_max_y = 0.0;
	origin_min_y = 0.0;
	origin_min_x = 0.0;
	origin_max_x = 0.0;
	offset_max_y = 0.0;
	offset_min_y = 0.0;
	offset_min_x = 0.0;
	offset_max_x = 0.0;
}

void LocalCS::SetCS(const Vector2d& o, const Vector2d& x)
{
	origin = o;
	axis_x = x;
	axis_y = Math::Functs::RotationAxis2d(axis_x, -Math::Math_PI / 2.0, Vector2d(0.0, 0.0));
	Math::Functs::SetVectorLength(axis_x, 1.0);
	Math::Functs::SetVectorLength(axis_y, 1.0);
	angle = Math::Functs::GetAngleBetween(axis_x, Vector2d(1.0, 0.0));
	if (axis_x[1] < 0.0) angle = 2 * M_PI - angle;
	origin_max_y = 0.0;
	origin_min_y = 0.0;
	origin_min_x = 0.0;
	origin_max_x = 0.0;
	offset_max_y = 0.0;
	offset_min_y = 0.0;
	offset_min_x = 0.0;
	offset_max_x = 0.0;
}

Vector2d LocalCS::GetLocal(const Vector2d& v) const
{
	Vector2d result = v - origin;
	result = Math::Functs::RotationAxis2d(result, angle, Vector2d(0.0, 0.0));
	return result;
}

Vector2d1 LocalCS::GetLocal(const Vector2d1& points) const
{
	Vector2d1 results;
	for (auto p : points)
		results.emplace_back(GetLocal(p));
	return results;
}

//need to check
Vector2d LocalCS::GetGlobalPos(const Vector2d& v) const
{
	Vector2d result = Math::Functs::RotationAxis2d(v, -angle, Vector2d(0.0, 0.0));
	result = result + origin;
	return result;
}

Vector2d LocalCS::GetGlobalVec(const Vector2d& v) const
{
	Vector2d result = Math::Functs::RotationAxis2d(v, -angle, Vector2d(0.0, 0.0));
	return result;
}

bool LocalCS::GetSegOrigin(const HMODULE& hModule, const double part_match_error, const Vector2d& s, const Vector2d& e, double& min_x, double& max_x) const
{
	auto cgal_2d_inter_line_line = (CGAL_2D_Intersection_Line_Line)GetProcAddress(hModule, "CGAL_2D_Intersection_Line_Line");
	Vector2d low_v = e;
	Vector2d upper_v = s;
	if (s[1] <= e[1])
	{
		low_v = s;
		upper_v = e;
	}

	//outside
	if (low_v[1] > origin_max_y || upper_v[1] < origin_min_y ||
		Math::Functs::IsAlmostZero_Double(low_v[1] - origin_max_y, part_match_error) || Math::Functs::IsAlmostZero_Double(upper_v[1] - origin_min_y, part_match_error))
		return false;

	//all in
	if ((low_v[1] > origin_min_y||Math::Functs::IsAlmostZero_Double(low_v[1]- origin_min_y, part_match_error)) &&
		(upper_v[1] < origin_max_y||Math::Functs::IsAlmostZero_Double(upper_v[1]- origin_max_y, part_match_error)))
	{
		min_x = low_v[0];
		max_x = upper_v[0];
		if (low_v[0] > upper_v[0])
		{
			min_x = upper_v[0];
			max_x = low_v[0];
		}
		return true;
	}

	//cutting lower
	if (low_v[1]< origin_min_y && upper_v[1]>origin_min_y && (upper_v[1] < origin_max_y||
		Math::Functs::IsAlmostZero_Double(upper_v[1]- origin_max_y, part_match_error)))
	{
		Vector2d inter;
		if (cgal_2d_inter_line_line(low_v, upper_v, Vector2d(0.0, origin_min_y), Vector2d(1.0, origin_min_y), inter))
		{
			min_x = inter[0];
			max_x = upper_v[0];
			if (inter[0] > upper_v[0])
			{
				min_x = upper_v[0];
				max_x = inter[0];
			}
			return true;
		}
	}

	//cutting upper
	if (upper_v[1] > origin_max_y && (low_v[1] > origin_min_y||Math::Functs::IsAlmostZero_Double(low_v[1]- origin_min_y, part_match_error)) && low_v[1] < origin_max_y)
	{
		Vector2d inter;
		if (cgal_2d_inter_line_line(low_v, upper_v, Vector2d(0.0, origin_max_y), Vector2d(1.0, origin_max_y), inter))
		{
			min_x = low_v[0];
			max_x = inter[0];
			if (low_v[0] > inter[0])
			{
				min_x = inter[0];
				max_x = low_v[0];
			}
			return true;
		}
	}

	if (upper_v[1] > origin_max_y && low_v[1] < origin_min_y)
	{
		Vector2d inter_0;
		bool b0 = cgal_2d_inter_line_line(low_v, upper_v, Vector2d(0.0, origin_min_y), Vector2d(1.0, origin_min_y), inter_0);
		Vector2d inter_1;
		bool b1 = cgal_2d_inter_line_line(low_v, upper_v, Vector2d(0.0, origin_max_y), Vector2d(1.0, origin_max_y), inter_1);

		if (b0 && b1)
		{
			min_x = inter_0[0];
			max_x = inter_1[0];
			if (inter_0[0] > inter_1[0])
			{
				min_x = inter_1[0];
				max_x = inter_0[0];
			}
			return true;
		}
	}
	return false;
}

bool LocalCS::GetSegOffset(const HMODULE& hModule, const double part_match_error, const Vector2d& s, const Vector2d& e, double& min_x, double& max_x) const
{
	auto cgal_2d_inter_line_line = (CGAL_2D_Intersection_Line_Line)GetProcAddress(hModule, "CGAL_2D_Intersection_Line_Line");

	Vector2d low_v = e;
	Vector2d upper_v = s;
	if (s[1] <= e[1])
	{
		low_v = s;
		upper_v = e;
	}

	//outside
	if (low_v[1] > offset_max_y || upper_v[1] < offset_min_y ||
		Math::Functs::IsAlmostZero_Double(low_v[1] - offset_max_y, part_match_error) ||
		Math::Functs::IsAlmostZero_Double(upper_v[1] - offset_min_y, part_match_error))
		return false;

	//all in
	if (low_v[1] >= offset_min_y && upper_v[1] <= offset_max_y)
	{
		min_x = low_v[0];
		max_x = upper_v[0];
		if (low_v[0] > upper_v[0])
		{
			min_x = upper_v[0];
			max_x = low_v[0];
		}
		return true;
	}

	//cutting lower
	if (low_v[1]< offset_min_y && upper_v[1]>offset_min_y && upper_v[1] <= offset_max_y)
	{
		Vector2d inter;
		if (cgal_2d_inter_line_line(low_v, upper_v, Vector2d(0.0, offset_min_y), Vector2d(1.0, offset_min_y), inter))
		{
			min_x = inter[0];
			max_x = upper_v[0];
			if (inter[0] > upper_v[0])
			{
				min_x = upper_v[0];
				max_x = inter[0];
			}
			return true;
		}
	}

	//cutting upper
	if (upper_v[1] > offset_max_y && low_v[1] >= offset_min_y && low_v[1] < offset_max_y)
	{
		Vector2d inter;
		if (cgal_2d_inter_line_line(low_v, upper_v, Vector2d(0.0, offset_max_y), Vector2d(1.0, offset_max_y), inter))
		{
			min_x = low_v[0];
			max_x = inter[0];
			if (low_v[0] > inter[0])
			{
				min_x = inter[0];
				max_x = low_v[0];
			}
			return true;
		}
	}

	if (upper_v[1] > offset_max_y && low_v[1] < offset_min_y)
	{
		Vector2d inter_0;
		bool b0 = cgal_2d_inter_line_line(low_v, upper_v, Vector2d(0.0, offset_min_y), Vector2d(1.0, offset_min_y), inter_0);
		Vector2d inter_1;
		bool b1 = cgal_2d_inter_line_line(low_v, upper_v, Vector2d(0.0, offset_max_y), Vector2d(1.0, offset_max_y), inter_1);

		if (b0 && b1)
		{
			min_x = inter_0[0];
			max_x = inter_1[0];
			if (inter_0[0] > inter_1[0])
			{
				min_x = inter_1[0];
				max_x = inter_0[0];
			}
			return true;
		}
	}
	return false;
}

Vector2d1 LocalCS::SortX(const Vector2d1& segs) const
{
	Vector2d1 sort_segs;
	if (segs.size() == 0) return sort_segs;
	double minimal_x = segs[0][0];
	double maximal_x = segs[0][1];

	for (int iter = 0; iter < segs.size(); iter++)
	{
		auto& seg = segs[iter];
		if (seg[0] >= minimal_x && seg[0] <= maximal_x)
		{
			if (seg[1] > maximal_x)
				maximal_x = seg[1];
		}
		else
		{
			sort_segs.emplace_back(minimal_x, maximal_x);
			minimal_x = segs[iter][0];
			maximal_x = segs[iter][1];
		}

		if (iter == segs.size() - 1)
		{
			sort_segs.emplace_back(minimal_x, maximal_x);
			break;
		}
	}
	return sort_segs;
}

Vector2d1 LocalCS::EmptyX(const Vector2d1& sort_segs) const
{
	Vector2d1 empty_segs;
	for (int i = 1; i < sort_segs.size(); i++)
		empty_segs.emplace_back(sort_segs[i - 1][1], sort_segs[i][0]);
	return empty_segs;
}


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2015 Scientific Computing and Imaging Institute,
   University of Utah.

   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.
*/
/// @todo Documentation Core/Datatypes/Legacy/Matrix/DenseMatrix.cc

#include <sci_defs/lapack_defs.h>
#include <sci_defs/blas_defs.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <cstdio>

#include <iostream>
#include <limits>
#include <sstream>
#include <stdexcept>
#include <string.h>
#include <vector>

#include <Core/Datatypes/ColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>

#include <Core/Exceptions/DimensionMismatch.h>

#include <boost/shared_array.hpp>

#include <Core/Math/MiscMath.h>

namespace SCIRun {

// NOTE: returns 1 if successful, or 0 if unsuccessful (i.e. ignore the solution vector)
int
LinearAlgebra::solve(const DenseMatrix& matrix, ColumnMatrix& sol)
{
  ColumnMatrix b(sol);
  return solve(matrix, b, sol);
}

// NOTE: returns 1 if successful, or 0 if unsuccessful (i.e. ignore the solution vector)
int
LinearAlgebra::solve(const DenseMatrix& matrix, const ColumnMatrix& rhs, ColumnMatrix& lhs)
{
  ASSERT(matrix.nrows() == matrix.ncols());
  ASSERT(rhs.nrows() == matrix.ncols());
  lhs=rhs;

  double **A;
  double **cpy = 0;
  double *lhsp = lhs.get_data_pointer();

  cpy = new double*[matrix.nrows()];

  for (index_type j=0; j < matrix.nrows(); j++)
    cpy[j] = matrix.get_raw_2D_pointer()[j];

  A = cpy;

  // Gauss-Jordan with partial pivoting
  index_type i;
  for (i=0; i < matrix.nrows(); i++)
  {
    double max=Abs(A[i][i]);
    index_type row=i;
    index_type j;
    for (j=i+1; j < matrix.nrows(); j++)
    {
      if(Abs(A[j][i]) > max)
      {
        max=Abs(A[j][i]);
        row=j;
      }
    }
    //  ASSERT(Abs(max) > 1.e-12);
    if (Abs(max) < 1.e-12)
    {
      lhs=rhs;
      delete cpy;
      return 0;
    }
    if(row != i)
    {
      // Switch rows (actually their pointers)
      std::swap(A[i], A[row]);
      std::swap(lhsp[i], lhsp[row]);
    }
    double denom=1./A[i][i];
    double* r1=A[i];
    double s1=lhsp[i];
    for (j=i+1; j<matrix.nrows(); j++)
    {
      double factor=A[j][i]*denom;
      double* r2=A[j];
      for (int k=i; k<matrix.nrows(); k++)
        r2[k]-=factor*r1[k];
      lhsp[j]-=factor*s1;
    }
  }

  // Back-substitution
  for (i=1; i<matrix.nrows(); i++)
  {
    //  cout << "Solve: " << i << " of " << nr << endl;
    //  ASSERT(Abs(A[i][i]) > 1.e-12);
    if (Abs(A[i][i]) < 1.e-12)
    {
      lhs=rhs;
      delete cpy;
      return 0;
    }
    double denom=1./A[i][i];
    double* r1=A[i];
    double s1=lhsp[i];
    for (index_type j=0;j<i;j++)
    {
      double factor=A[j][i]*denom;
      double* r2=A[j];
      for (index_type k=i; k<matrix.nrows(); k++)
        r2[k] -= factor*r1[k];
      lhsp[j] -= factor*s1;
    }
  }

  // Normalize
  for (i=0; i<matrix.nrows(); i++)
  {
    //  cout << "Solve: " << i << " of " << nr << endl;
    //  ASSERT(Abs(A[i][i]) > 1.e-12);
    if (Abs(A[i][i]) < 1.e-12)
    {
      lhs=rhs;
      delete cpy;
      return 0;
    }
    double factor=1./A[i][i];
    for (index_type j=0; j<matrix.nrows(); j++)
      A[i][j] *= factor;
    lhsp[i] *= factor;
  }
  delete cpy;
  return 1;
}

#if defined(HAVE_LAPACK)

void
LinearAlgebra::solve_lapack(const DenseMatrix& matrix, const ColumnMatrix& rhs, ColumnMatrix& lhs)
{
  if (matrix.nrows() != matrix.ncols())
  {
    SCI_THROW(DimensionMismatch(matrix.nrows(), matrix.ncols(), __FILE__, __LINE__));
  }

  if (rhs.nrows() != matrix.ncols())
  {
    SCI_THROW(DimensionMismatch(rhs.nrows(), matrix.ncols(), __FILE__, __LINE__));
  }

  ColumnMatrix rhsCopy(rhs); //need to make a copy because

  try
  {
    lapacksolvelinearsystem(matrix.get_raw_2D_pointer(), matrix.nrows(), matrix.ncols(), rhsCopy.get_data_pointer(), rhs.nrows(), rhs.ncols());
  }
  catch (const SCIRun::LapackError& exception)
  {
    std::ostringstream oss;
    oss << "Caught LapackError exception: " << exception.message();
    // in the absence of a logging service
    std::cerr << oss.str() << std::endl;
    throw;
  }

  lhs=rhsCopy;

}

void
LinearAlgebra::svd(const DenseMatrix& matrix, DenseMatrix& U, SparseRowMatrixHandle& S, DenseMatrix& VT)
{
  ASSERTEQ(U.ncols(), U.nrows());
  ASSERTEQ(VT.ncols(), VT.nrows());
  ASSERTEQ(U.nrows(), matrix.nrows());
  ASSERTEQ(VT.ncols(), matrix.ncols());

  /*
   * LAPACK function dgesvd argument S is a DOUBLE PRECISION array
   * with dimension (min(M,N)).
   */
  const size_type SIGMA_LEN = std::min(matrix.nrows(), matrix.ncols());
  boost::shared_array<double> sigma(new double[SIGMA_LEN]);

  try
  {
    lapacksvd(matrix.get_raw_2D_pointer(), matrix.nrows(), matrix.ncols(), sigma.get(), U.get_raw_2D_pointer(), VT.get_raw_2D_pointer());
  }
  catch (const SCIRun::LapackError& exception)
  {
    std::ostringstream oss;
    oss << "Caught LapackError exception: " << exception.message();
    // in the absence of a logging service
    std::cerr << oss.str() << std::endl;
    throw;
  }

  size_type nnz = 0;
  for (size_type i = 0; i < SIGMA_LEN; ++i)
  {
    if ( fabs(sigma[i]) >= std::numeric_limits<double>::epsilon() )
    {
      ++nnz;
    }
  }

  SparseRowMatrix::Data sparseData(matrix.nrows() + 1, nnz);
  const SparseRowMatrix::Rows& rows = sparseData.rows();
  const SparseRowMatrix::Columns& columns = sparseData.columns();
  const SparseRowMatrix::Storage& a = sparseData.data();
  if (!sparseData.allocated())
  {
    std::cerr << "Could not allocate memory for sparse matrix buffers "
    << __FILE__ << ": " << __LINE__ << std::endl;

    return;
  }

  // singular values on diagonal
  index_type count = 0;
  index_type i = 0;
  const index_type NROWS = matrix.nrows();
  for (index_type r = 0; r < NROWS; r++)
  {
    rows[r] = count;
    if ( r < matrix.ncols() && i < SIGMA_LEN && fabs(sigma[i]) >= std::numeric_limits<double>::epsilon() )
    {
      columns[count] = r;
      a[count] = sigma[i];
      ++count;
      ++i;
    }
  }
  rows[matrix.nrows()] = count;

  S = new SparseRowMatrix(matrix.nrows(), matrix.ncols(), sparseData, nnz);
}

void
LinearAlgebra::svd(const DenseMatrix& matrix, DenseMatrix& U, DenseMatrix& S, DenseMatrix& VT)
{
  ASSERTEQ(U.ncols(), U.nrows());
  ASSERTEQ(VT.ncols(), VT.nrows());
  ASSERTEQ(U.nrows(), matrix.nrows());
  ASSERTEQ(VT.ncols(), matrix.ncols());
  ASSERTEQ(S.nrows(), matrix.nrows());
  ASSERTEQ(S.ncols(), matrix.ncols());

  /*
   * LAPACK function dgesvd argument S is a DOUBLE PRECISION array
   * with dimension (min(M,N)).
   */
  const size_type SIGMA_LEN = std::min(matrix.nrows(), matrix.ncols());
  boost::shared_array<double> sigma(new double[SIGMA_LEN]);

  try
  {
    lapacksvd(matrix.get_raw_2D_pointer(), matrix.nrows(), matrix.ncols(), sigma.get(), U.get_raw_2D_pointer(), VT.get_raw_2D_pointer());
  }
  catch (const SCIRun::LapackError& exception)
  {
    std::ostringstream oss;
    oss << "Caught LapackError exception: " << exception.message();
    // in the absence of a logging service
    std::cerr << oss.str() << std::endl;
    throw;
  }

  // Put singular values on diagonal.
  for (size_type i = 0; i < SIGMA_LEN; ++i)
    S.put(i, i, sigma[i]);
}

void
LinearAlgebra::svd(const DenseMatrix& matrix, DenseMatrix& U, ColumnMatrix& S, DenseMatrix& VT)
{
  // Check whether matrices are square
  if (U.ncols() != U.nrows())
  {
    SCI_THROW(DimensionMismatch(U.ncols(), U.nrows(), __FILE__, __LINE__));
  }

  if (VT.ncols() != VT.nrows())
  {
    SCI_THROW(DimensionMismatch(VT.ncols(), VT.nrows(), __FILE__, __LINE__));
  }

  if (U.nrows())
  {
    if (U.nrows() != matrix.nrows())
    {
      SCI_THROW(DimensionMismatch(U.nrows(), matrix.nrows(), __FILE__, __LINE__));
    }
  }
  if (VT.ncols())
  {
    if (VT.ncols() != matrix.ncols())
    {
      SCI_THROW(DimensionMismatch(VT.ncols(), matrix.ncols(), __FILE__, __LINE__));
    }
  }

  if (matrix.nrows() < matrix.ncols())
  {
    if (S.nrows() != matrix.nrows())
    {
      SCI_THROW(DimensionMismatch(S.nrows(), matrix.nrows(), __FILE__, __LINE__));
    }
  }
  else
  {
    if (S.nrows() != matrix.ncols())
    {
      SCI_THROW(DimensionMismatch(S.nrows(), matrix.ncols(), __FILE__, __LINE__));
    }
  }

  try
  {
    if (U.nrows() == 0 && VT.nrows() == 0)
      lapacksvd(matrix.get_raw_2D_pointer(), matrix.nrows(), matrix.ncols(), S.get_data_pointer(), 0, 0);
    else if (U.nrows() != 0 && VT.nrows() == 0)
      lapacksvd(matrix.get_raw_2D_pointer(), matrix.nrows(), matrix.ncols(), S.get_data_pointer(), U.get_raw_2D_pointer(), 0);
    else if (U.nrows() == 0 && VT.nrows() != 0)
      lapacksvd(matrix.get_raw_2D_pointer(), matrix.nrows(), matrix.ncols(), S.get_data_pointer(), 0, VT.get_raw_2D_pointer());
    else
      lapacksvd(matrix.get_raw_2D_pointer(), matrix.nrows(), matrix.ncols(), S.get_data_pointer(), U.get_raw_2D_pointer(), VT.get_raw_2D_pointer());
  }
  catch (const SCIRun::LapackError& exception)
  {
    std::ostringstream oss;
    oss << "Caught LapackError exception: " << exception.message();
    // in the absence of a logging service
    std::cerr << oss.str() << std::endl;
    throw;
  }
}

void
LinearAlgebra::eigenvalues(const DenseMatrix& matrix, ColumnMatrix& R, ColumnMatrix& I)
{
  ASSERTEQ(matrix.ncols(), matrix.nrows());
  ASSERTEQ(R.nrows(), I.nrows());
  ASSERTEQ(matrix.ncols(), R.nrows());

  boost::shared_array<double> Er(new double[matrix.nrows()]);
  boost::shared_array<double> Ei(new double[matrix.nrows()]);

  try
  {
    lapackeigen(matrix.get_raw_2D_pointer(), matrix.nrows(), Er.get(), Ei.get());
  }
  catch (const SCIRun::LapackError& exception)
  {
    std::ostringstream oss;
    oss << "Caught LapackError exception: " << exception.message();
    // in the absence of a logging service
    std::cerr << oss.str() << std::endl;
    throw;
  }

  for (index_type i = 0; i < matrix.nrows(); i++)
  {
    R[i] = Er[i];
    I[i] = Ei[i];
  }
}

void
LinearAlgebra::eigenvectors(const DenseMatrix& matrix, ColumnMatrix& R, ColumnMatrix& I, DenseMatrix& Vecs)
{
  ASSERTEQ(matrix.ncols(), matrix.nrows());
  ASSERTEQ(R.nrows(), I.nrows());
  ASSERTEQ(matrix.ncols(), R.nrows());

  boost::shared_array<double> Er(new double[matrix.nrows()]);
  boost::shared_array<double> Ei(new double[matrix.nrows()]);

  try
  {
    lapackeigen(matrix.get_raw_2D_pointer(), matrix.nrows(), Er.get(), Ei.get(), Vecs.get_raw_2D_pointer());
  }
  catch (const SCIRun::LapackError& exception)
  {
    std::ostringstream oss;
    oss << "Caught LapackError exception: " << exception.message();
    // in the absence of a logging service
    std::cerr << oss.str() << std::endl;
    throw;
  }

  for (index_type i = 0; i<matrix.nrows(); i++)
  {
    R[i] = Er[i];
    I[i] = Ei[i];
  }
}

#else

void
LinearAlgebra::solve_lapack(const DenseMatrix& matrix, const ColumnMatrix& rhs, ColumnMatrix& lhs)
{
  ASSERTFAIL("Build was not configured with LAPACK. LinearAlgebra::solve_lapack is not available.");
}

void
LinearAlgebra::svd(const DenseMatrix& matrix, DenseMatrix& U, SparseRowMatrixHandle& S, DenseMatrix& VT)
{
  ASSERTFAIL("Build was not configured with LAPACK LinearAlgebra::svd is not available.");
}

void
LinearAlgebra::svd(const DenseMatrix& matrix, DenseMatrix& U, ColumnMatrix& S, DenseMatrix& VT)
{
  ASSERTFAIL("Build was not configured with LAPACK LinearAlgebra::svd is not available.");
}

void
LinearAlgebra::svd(const DenseMatrix& matrix, DenseMatrix& U, DenseMatrix& S, DenseMatrix& VT)
{
  ASSERTFAIL("Build was not configured with LAPACK LinearAlgebra::svd is not available.");
}

void
LinearAlgebra::eigenvalues(const DenseMatrix& matrix, ColumnMatrix& R, ColumnMatrix& I)
{
  ASSERTFAIL("Build was not configured with LAPACK LinearAlgebra::svd is not available.");
}

void
LinearAlgebra::eigenvectors(const DenseMatrix& matrix, ColumnMatrix& R, ColumnMatrix& I, DenseMatrix& Vecs)
{
  ASSERTFAIL("Build was not configured with LAPACK LinearAlgebra::svd is not available.");
}

#endif

} // End namespace SCIRun


#include <Rodin/Mesh.h>
#include <Rodin/Solver.h>
#include <Rodin/Variational.h>
#include <RodinExternal/MMG.h>
#include <vector>
#include <typeinfo>

// Dependencies required for eigenvalue computation
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Spectra/GenEigsSolver.h>
#include <Spectra/MatOp/SparseGenMatProd.h>
#include <Spectra/SymGEigsShiftSolver.h>

using namespace Rodin;
using namespace Rodin::Variational;
using namespace Rodin::External;

Eigen::SparseMatrix<double> mfemToEigenSparse(mfem::SparseMatrix m)
{
  typedef Eigen::Triplet<double> Triplet;

  int nonZero = m.NumNonZeroElems();
  std::vector<Triplet> tripletList;

  // Get the values (i,j, data)
  int* m_I = m.GetI();
  int* m_J = m.GetJ();
  double* m_data = m.GetData();

  for(int r = 0; r < m.NumRows(); ++r){
    for(int j=m_I[r]; j<m_I[r+1]; ++j){
      tripletList.push_back(Triplet(r, m_J[j], m_data[j])); // ???? Inverse row and column since Eigen is CSC
    }
  }
  Eigen::SparseMatrix<double> m_sparse(m.NumRows(), m.NumCols());
  m_sparse.setFromTriplets(tripletList.begin(), tripletList.end()); //SEGFAULT

  return m_sparse;
}

class EigenSolver{

  public:
    // Constructors
    EigenSolver();

    // Destructor
    ~EigenSolver() = default;

    // Accessors
    GridFunction<H1>& getEigenFunction(int index);

    double getEigenValue(int index);
    std::vector<double> getEigenValues();

    // Options
    EigenSolver& setNumEV(unsigned int nev);
    EigenSolver& setShift(double shift);

    // Solves the problem
    void solve(mfem::SparseMatrix A, mfem::SparseMatrix B, FiniteElementSpace<H1>& fes);

  private:
    std::vector<double> m_eigenvalues;
    std::vector<GridFunction<H1>*> m_eigenfunctions;
    unsigned int m_nev;
    double m_shift;
};

int main(int argc, char** argv)
{
  const char* meshFile = "../resources/mfem/levelset-cantilever2d-example.mesh";

  // Define interior and exterior for level set discretization
  int Interior = 1, Exterior = 2;

  // Define boundary attributes
  int Gamma0 = 1, GammaD = 2, GammaN = 3, Gamma = 4;

  // Load mesh
  Mesh Omega;
  Omega.load(meshFile);

  // Solver for hilbertian regularization
  auto solver = Solver::UMFPack();

  // Optimization parameters
  size_t maxIt = 100;
  double eps = 1e-6;
  double hmax = 0.05;
  int k = 1; // The eigenvalue to optimize
  auto alpha = ScalarFunction(4 * hmax * hmax); // Parameter for hilbertian regularization

  std::vector<double> obj;

  /*
  // Scalar field finite element space over the whole domain
  FiniteElementSpace<H1> Vh(Omega);

  // Trim the exterior part of the mesh to solve the elasticity system
  SubMesh trimmed = Omega.trim(Exterior, Gamma);

  // Build a finite element space over the trimmed mesh
  FiniteElementSpace<H1> VhInt(trimmed);

  // Elasticity equation
  TrialFunction uInt(VhInt);
  TestFunction  vInt(VhInt);

  // A
  Problem stiffness(uInt, vInt);
  stiffness = Integral(Grad(uInt), Grad(vInt));
  stiffness.update().assemble();

  // B
  Problem mass(uInt, vInt);
  mass = Integral(uInt, vInt);
  mass.update().assemble();

  auto& m1 = stiffness.getStiffnessMatrix();
  auto& m2 = mass.getStiffnessMatrix();

  // Solve eigenvalue problem
  EigenSolver ES;
  ES.setShift(1.0).setNumEV(k+4).solve(m1, m2, VhInt);

  // Get solution and transfer to original domain
  GridFunction u(Vh);
  ES.getEigenFunction(k).transfer(u);
  double mu = ES.getEigenValue(k);

  // Save solution
  u.save("u.gf");
  Omega.save("Omega.mesh");
  */

  // Optimization loop
  for (size_t i = 0; i < maxIt; i++)
  {
    // Scalar field finite element space over the whole domain
    FiniteElementSpace<H1> Vh(Omega);

    // Trim the exterior part of the mesh to solve the elasticity system
    SubMesh trimmed = Omega.trim(Exterior, Gamma);

    // Build a finite element space over the trimmed mesh
    FiniteElementSpace<H1> VhInt(trimmed);

    // Elasticity equation
    TrialFunction uInt(VhInt);
    TestFunction  vInt(VhInt);

    // A
    Problem stiffness(uInt, vInt);
    stiffness = Integral(Grad(uInt), Grad(vInt));
    stiffness.update().assemble();

    // B
    Problem mass(uInt, vInt);
    mass = Integral(uInt, vInt);
    mass.update().assemble();

    auto& m1 = stiffness.getStiffnessMatrix();
    auto& m2 = mass.getStiffnessMatrix();

    // Solve eigenvalue problem
    EigenSolver ES;
    ES.setShift(1.0).setNumEV(k+4).solve(m1, m2, VhInt);

    // Get solution and transfer to original domain
    GridFunction u(Vh);
    ES.getEigenFunction(k).transfer(u);
    double mu = ES.getEigenValue(k);

    // Compute the boundary shape gradientv
    // Vector field finite element space over the whole domain
    FiniteElementSpace<H1> Uh(Omega, 2);
    auto n = Normal(2);

    // GridFunction dJ(Uh);
    //dJ = Omega.getVolume(Interior)*(Dot(Grad(u), Grad(u)) + mu*Dot(u,u))*n + mu*n; //  /Integral(Dot(u,u)) sur le sous-mesh
    //std::cout << dJ.getFiniteElementSpace().getVectorDimension() << std::endl;

    // Note from Carlos: 26/Avril/2022
    // Salut !
    // 1. I changed some things around, in particular I do not project the
    // expression for dJ on a GridFunction. Instead I wrote the thing directly
    // and hope that Rodin understands what I want hehe
    // 2. I fixed the bug in the transfer function, thank you for testing this
    // and narrowing down the problem! It helps a lot :)
    // 3. I tried to run your code and it seems that the objective goes down
    // but at one point the computation fails. I think due to parasitic
    // components.
    // 4. In the hilbertian procedure I removed the use of DirichletBC and used
    // the usual expression.
    //
    // See you soon!

    // Hilbert extension-regularization procedure
    // This is bullshit
    auto dJ = Dot(Grad(u).traceOf(Interior), Grad(u).traceOf(Interior)) * n;
    TrialFunction g(Uh);
    TestFunction  v(Uh);
    Problem hilbert(g, v);
    hilbert = Integral(alpha * Jacobian(g), Jacobian(v))
            + Integral(g, v)
            + BoundaryIntegral(dJ, v).over(Gamma);
    solver.solve(hilbert);

    // Save data to inspect
    // Omega.save("Omegai.mesh");
    // g.getGridFunction().save("g.gf");

    // Update objective
    obj.push_back(
        mu * Omega.getVolume(Interior));
    std::cout << "[" << i << "] Objective: " << obj.back() << std::endl;

    // Convert data types to mmg types
    auto mmgMesh = Cast(Omega).to<MMG::Mesh2D>();
    auto mmgVel = Cast(g.getGridFunction()).to<MMG::VectorSolution2D>(mmgMesh);

    // Generate signed distance function
    auto mmgLs = MMG::Distancer2D().setInteriorDomain(Interior).distance(mmgMesh);

    // Advect the level set function
    double gInf = std::max(g.getGridFunction().max(), -g.getGridFunction().min());
    double dt = hmax / gInf;
    MMG::Advect2D(mmgLs, mmgVel).step(dt);

    // Recover the implicit domain
    auto mmgImplicit =
      MMG::ImplicitDomainMesher2D().split(Interior, {Interior, Exterior})
                                   .split(Exterior, {Interior, Exterior})
                                   .setRMC(1e-3)
                                   .setHMax(hmax)
                                   .setBoundaryReference(Gamma)
                                   .discretize(mmgLs);

    // Convert back to Rodin data type
    Omega = Cast(mmgImplicit).to<Rodin::Mesh<>>();

    // Save mesh
    Omega.save("Omega.mesh");

    // Test for convergence
    if (obj.size() >= 2 && abs(obj[i] - obj[i - 1]) < eps)
    {
      std::cout << "Convergence!" << std::endl;
      break;
    }

    std::ofstream plt("obj.txt", std::ios::trunc);
    for (size_t i = 0; i < obj.size(); i++)
      plt << i << "," << obj[i] << "\n";
  }

  return 0;
}

// Constructor
EigenSolver::EigenSolver(){
  m_nev = 0;
  m_shift = 0.0;
}

// Accessors
GridFunction<H1>& EigenSolver::getEigenFunction(int index){
  return *m_eigenfunctions[index];
}

double EigenSolver::getEigenValue(int index){
    return m_eigenvalues[index];
}

std::vector<double> EigenSolver::getEigenValues(){
  return m_eigenvalues;
}

// Options
EigenSolver& EigenSolver::setNumEV(unsigned int nev){
  m_nev = nev;
  return *this;
}

EigenSolver& EigenSolver::setShift(double shift){
  m_shift = shift;

  return *this;
}

void EigenSolver::solve(mfem::SparseMatrix A,mfem::SparseMatrix B, FiniteElementSpace<H1>& fes){

  // 1. CA and B to Eigen::SparseMatrix
  //WARNING: MAY BE USELESS SINCE SPECTRA HANDLE OTHER MATRICES TYPES
  //BUT MAY BE FASTER. IN FACT I DON'T HAVE A CLUE.
  Eigen::SparseMatrix<double> A_sparse = mfemToEigenSparse(A);
  Eigen::SparseMatrix<double> B_sparse = mfemToEigenSparse(B);

  // 2. Use Spectra with the shift-inverse method to compute eigenvalue
  // Define the shift-inverse and vector-multiplication operations
  using OpType = Spectra::SymShiftInvert<double, Eigen::Sparse, Eigen::Sparse>;
  using BOpType = Spectra::SparseSymMatProd<double>;
  OpType op(A_sparse, B_sparse);
  BOpType Bop(B_sparse);

  // Construct the generalized eigensolver object
  int ncv = 2*m_nev+1;
  Spectra::SymGEigsShiftSolver<OpType, BOpType, Spectra::GEigsMode::ShiftInvert>
    geigs(op, Bop, m_nev, ncv, m_shift);

  // Initialize and compute
  geigs.init();
  int nconv = geigs.compute(Spectra::SortRule::LargestMagn); // UTILISER LARGESTMAGN ET PAS SMALLESTMAGN CAR ON EST EN SHIFT-INVERSE MODE

  // Retrieve results
  Eigen::VectorXd evalues;
  Eigen::MatrixXd evecs;
  if (geigs.info() == Spectra::CompInfo::Successful)
  {
      evalues = geigs.eigenvalues();
      evecs = geigs.eigenvectors();
  }

  // Store in m_eigenvalues and m_eigenfunctions
  // (and reverse the order to order from lowest to higest)
  int dim = evecs.rows();

  for(int i = 0; i < m_nev; i++){
    m_eigenvalues.push_back(evalues[m_nev-i-1]);

    std::unique_ptr<double[]> data(new double[dim]);
    for(int j=0; j<dim; j++){
      data[j] = evecs.col(m_nev-i-1)[j];
    }
    //std::copy(evecs.col(m_nev-i-1).data(), evecs.col(m_nev-i-1).data()+dim, data.get());

    m_eigenfunctions.push_back(new GridFunction<H1>(fes));
    m_eigenfunctions[i]->setData(std::move(data), dim);
  }
}