#include "delsum-poly/include/poly.hh"
#include "delsum-poly/src/lib.rs.h"
#include <cstdint>
#include <climits>
#include <NTL/GF2X.h>
#include <NTL/GF2E.h>
#include <NTL/GF2XFactoring.h>
namespace poly
{
    long i64_to_l(int64_t x) {
        if (x > LONG_MAX || x < LONG_MIN) {
            std::cerr << "Polynomial too big, probably because of big input files" << std::endl
                      << "Try using smaller files or run on a platform with larger long variables" << std::endl;
            abort();
        }
        return (long)x;
    }
    Poly::Poly()
    {
        int_pol = NTL::GF2X();
    }
    Poly::Poly(const Poly &b)
    {
        int_pol = NTL::GF2X(b.int_pol);
    }
    Poly::Poly(Poly &&b)
    {
        int_pol = NTL::GF2X(b.int_pol);
    }
    bool Poly::coeff(int64_t idx) const
    {
        return NTL::IsOne(NTL::coeff(int_pol, i64_to_l(idx)));
    }
    bool Poly::eq(const Poly &b) const
    {
        return int_pol == b.int_pol;
    }
    int64_t deg(const Poly &a)
    {
        return NTL::deg(a.int_pol);
    }
    std::unique_ptr<Poly> copy_poly(const Poly &p)
    {
        return std::make_unique<Poly>(p);
    }
    std::unique_ptr<std::vector<PolyI64Pair>> factor(const Poly &p, int64_t verbosity)
    {
        auto v = std::vector<PolyI64Pair>();
        auto decomp = NTL::CanZass(p.int_pol, i64_to_l(verbosity));
        for (auto x : decomp)
        {
            auto poly = Poly();
            poly.int_pol = x.a;
            v.push_back(PolyI64Pair{.poly = std::make_unique<Poly>(std::move(poly)), .l = (int64_t)x.b});
        }
        return std::make_unique<std::vector<PolyI64Pair>>(std::move(v));
    }

    std::unique_ptr<Poly> new_poly_shifted(rust::Slice<uint8_t> bytes, int64_t shift, bool msb_first)
    {
        auto lshift = i64_to_l(shift);
        auto ret = Poly();
        ret.int_pol = NTL::GF2X();
        ret.int_pol.SetLength(lshift + bytes.length() * 8);
        for (size_t i = 0; i < bytes.length(); i++)
        {
            auto current_byte = bytes.data()[bytes.length() - 1 - i];
            for (int j = 0; j < 8; j++)
            {
                auto bit_pos = msb_first ? j : (7 - j);
                auto current_bit = (current_byte >> bit_pos) & 1;
                auto bit_index = lshift + 8 * i + j;
                ret.int_pol[bit_index] = current_bit;
            }
        }
        ret.int_pol.normalize();
        return std::make_unique<Poly>(ret);
    }

    std::unique_ptr<Poly> new_poly(rust::Slice<uint8_t> bytes)
    {
        return new_poly_shifted(bytes, 0, true);
    }
    std::unique_ptr<Poly> new_zero()
    {
        auto ret = Poly();
        return std::make_unique<Poly>(ret);
    }

    std::unique_ptr<std::vector<uint8_t>> Poly::to_bytes(int64_t min_bytes) const
    {
        auto d = NTL::deg(int_pol);
        auto n_bytes = d / 8 + 1;
        if (d < 0) {
            n_bytes = 0;
        }
        auto v = std::vector<uint8_t>();
        auto lmin = i64_to_l(min_bytes);
        auto amount_of_bytes = n_bytes > lmin ? n_bytes : lmin;
        v.reserve(amount_of_bytes);
        for (long i = 0; i < amount_of_bytes - n_bytes; i++) {
            v.push_back(0);
        }
        uint8_t current_byte = 0;
        for (long i = d; i >= 0; i--)
        {
            current_byte <<= 1;
            current_byte |= (uint8_t)NTL::rep(int_pol[i]);
            if (i % 8 == 0)
            {
                v.push_back(current_byte);
                current_byte = 0;
            }
        }
        return std::make_unique<std::vector<uint8_t>>(v);
    }
    std::unique_ptr<Poly> add(const Poly &b, const Poly &c)
    {
        auto ret = Poly();
        ret.int_pol = b.int_pol + c.int_pol;
        return std::make_unique<Poly>(ret);
    }
    void Poly::add_to(const Poly &b)
    {
        int_pol += b.int_pol;
    }
    std::unique_ptr<Poly> mul(const Poly &b, const Poly &c)
    {
        auto ret = Poly();
        ret.int_pol = b.int_pol * c.int_pol;
        return std::make_unique<Poly>(ret);
    }
    void Poly::mul_to(const Poly &b)
    {
        int_pol *= b.int_pol;
    }
    std::unique_ptr<Poly> div(const Poly &b, const Poly &c)
    {
        auto ret = Poly();
        ret.int_pol = b.int_pol / c.int_pol;
        return std::make_unique<Poly>(ret);
    }
    void Poly::div_to(const Poly &b)
    {
        int_pol /= b.int_pol;
    }
    bool Poly::div_to_checked(const Poly &b)
    {
        auto ret = NTL::divide(int_pol, b.int_pol);
        return ret == 1;
    }
    bool Poly::is_zero() const
    {
        return NTL::IsZero(int_pol);
    }
    std::unique_ptr<Poly> gcd(const Poly &b, const Poly &c)
    {
        auto ret = Poly();
        NTL::GCD(ret.int_pol, b.int_pol, c.int_pol);
        return std::make_unique<Poly>(ret);
    }
    std::unique_ptr<Poly> xgcd(Poly &x, Poly &y, const Poly &b, const Poly &c)
    {
        auto ret = Poly();
        NTL::XGCD(ret.int_pol, x.int_pol, y.int_pol, b.int_pol, c.int_pol);
        return std::make_unique<Poly>(ret);
    }
    void Poly::gcd_to(const Poly &b)
    {
        NTL::GCD(int_pol, int_pol, b.int_pol);
    }
    std::unique_ptr<Poly> rem(const Poly &b, const Poly &c)
    {
        auto ret = Poly();
        NTL::rem(ret.int_pol, b.int_pol, c.int_pol);
        return std::make_unique<Poly>(ret);
    }
    void Poly::rem_to(const Poly &b)
    {
        NTL::rem(int_pol, int_pol, b.int_pol);
    }
    std::unique_ptr<Poly> power(const Poly &p, int64_t n)
    {
        auto q = Poly();
        q.int_pol = NTL::power(p.int_pol, i64_to_l(n));
        return std::make_unique<Poly>(q);
    }
    std::unique_ptr<Poly> shift(const Poly &p, int64_t n)
    {
        auto q = Poly();
        if (n >= 0) {
            q.int_pol = NTL::LeftShift(p.int_pol, i64_to_l(n));
        } else {
            q.int_pol = NTL::RightShift(p.int_pol, i64_to_l(-n));
        }
        return std::make_unique<Poly>(q);
    }
    void Poly::sqr()
    {
        int_pol = NTL::sqr(int_pol);
    }

    // PolyRem stuff

    PolyRem::PolyRem(const Poly &p)
    {
        int_pol = NTL::GF2E();
        int_pol.init(p.int_pol);
    }
    // copy
    PolyRem::PolyRem(const PolyRem &b)
    {
        int_pol = NTL::GF2E(b.int_pol);
    }
    // move
    PolyRem::PolyRem(PolyRem &&b)
    {
        int_pol = NTL::GF2E(b.int_pol);
    }
    PolyRem::PolyRem(const NTL::GF2E &p)
    {
        int_pol = NTL::GF2E(p);
    }

    std::unique_ptr<PolyRem> new_polyrem(const Poly &rem, const Poly &m)
    {
        auto ret = PolyRem(m);
        NTL::conv(ret.int_pol, rem.int_pol);
        return std::make_unique<PolyRem>(ret);
    }
    void PolyRem::add_to(const PolyRem &b)
    {
        int_pol += b.int_pol;
    }
    void PolyRem::mul_to(const PolyRem &b)
    {
        int_pol *= b.int_pol;
    }
    void PolyRem::div_to(const PolyRem &b)
    {
        int_pol /= b.int_pol;
    }
    void PolyRem::sqr()
    {
        int_pol = NTL::sqr(int_pol);
    }
    std::unique_ptr<Poly> PolyRem::rep() const
    {
        auto ret = Poly();
        ret.int_pol = NTL::rep(int_pol);
        return std::make_unique<Poly>(ret);
    }
    std::unique_ptr<PolyRem> powermod(const PolyRem &p, int64_t n)
    {
        auto q = PolyRem(NTL::power(p.int_pol, i64_to_l(n)));
        return std::make_unique<PolyRem>(q);
    }
    std::unique_ptr<PolyRem> copy_polyrem(const PolyRem &p)
    {
        return std::make_unique<PolyRem>(PolyRem(p));
    }
} // namespace poly


/*!
 * @file
 * Defines the @ref Rational datatype.
 *
 *
 * @copyright Louis Dionne 2014
 * Distributed under the Boost Software License, Version 1.0.
 *         (See accompanying file LICENSE.md or copy at
 *             http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_MPL11_RATIONAL_HPP
#define BOOST_MPL11_RATIONAL_HPP

#include <boost/mpl11/fwd/rational.hpp>

#include <boost/mpl11/bool.hpp>
#include <boost/mpl11/comparable.hpp>
#include <boost/mpl11/core.hpp>
#include <boost/mpl11/enumerable.hpp>
#include <boost/mpl11/field.hpp>
#include <boost/mpl11/group.hpp>
#include <boost/mpl11/monoid.hpp>
#include <boost/mpl11/orderable.hpp>
#include <boost/mpl11/ring.hpp>

namespace boost { namespace mpl11 {
    template <typename T, T numerator, T denominator>
    struct rational_c {
        using type = rational_c;
        using mpl_datatype = Rational;

        static constexpr T num = numerator;
        static constexpr T den = denominator;
    };

    template <>
    struct cast<Integer, Rational> {
        using type = cast;
        template <typename i>
        using apply = rational_c<
            typename i::value_type, i::value
        >;
    };

    template <>
    struct Monoid<Rational> : instantiate<Monoid>::with<Rational> {
        template <typename x, typename y>
        using plus_impl = rational_c<
            decltype(true ? x::num : y::num),
            (x::num * y::den) + (x::den * y::num),
            x::den * y::den
        >;

        template <typename ...>
        using zero_impl = rational_c<long long, 0>;
    };

    template <>
    struct Group<Rational> : instantiate<Group>::with<Rational> {
        template <typename x, typename y>
        using minus_impl = rational_c<
            decltype(true ? x::num : y::num),
            (x::num * y::den) - (x::den * y::num),
            x::den * y::den
        >;

        template <typename x>
        using negate_impl = rational_c<
            decltype(-x::num), -x::num, x::den
        >;
    };

    template <>
    struct Ring<Rational> : instantiate<Ring>::with<Rational> {
        template <typename x, typename y>
        using mult_impl = rational_c<
            decltype(true ? x::num : y::num),
            x::num * y::num,
            x::den * y::den
        >;

        template <typename ...>
        using one_impl = rational_c<long long, 1>;
    };

    template <>
    struct Field<Rational> : instantiate<Field>::with<Rational> {
        template <typename x, typename y>
        using quot_impl = rational_c<
            decltype(true ? x::num : y::num),
            x::num * y::den,
            x::den * y::num
        >;

        template <typename x>
        using recip_impl = rational_c<
            decltype(x::num), x::den, x::num
        >;
    };

    template <>
    struct Comparable<Rational> : instantiate<Comparable>::with<Rational> {
        template <typename x, typename y>
        using equal_impl = bool_<
            x::num * y::den == x::den * y::num
        >;

        template <typename x, typename y>
        using not_equal_impl = bool_<
            x::num * y::den != x::den * y::num
        >;
    };

    template <>
    struct Orderable<Rational> : instantiate<Orderable>::with<Rational> {
        template <typename x, typename y>
        using less_impl = bool_<(
            x::num * y::den < x::den * y::num
        )>;

        template <typename x, typename y>
        using less_equal_impl = bool_<(
            x::num * y::den <= x::den * y::num
        )>;

        template <typename x, typename y>
        using greater_impl = bool_<(
            x::num * y::den > x::den * y::num
        )>;

        template <typename x, typename y>
        using greater_equal_impl = bool_<(
            x::num * y::den >= x::den * y::num
        )>;
    };
}} // end namespace boost::mpl11

#endif // !BOOST_MPL11_RATIONAL_HPP


/*
 * Copyright (c) 2013-2016 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef ALLSOL_HPP
#define ALLSOL_HPP

#include <iostream>
#include <list>
#include <limits>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#ifdef _OPENMP
#include <omp.h>
#endif
// #include <boost/random.hpp>
#include <kv/matrix-inversion.hpp>
#include <kv/autodif.hpp>

#ifndef EDGE_RATIO
#define EDGE_RATIO 0.9
#endif

// 0: Do not use TRIM
// 1: use TRIM and use slow trim algorithm
// 2: use TRIM and use fast bat slightly inefficient trim algorithm
// 3: use TRIM and use new trim algorithm

#ifndef USE_TRIM
#define USE_TRIM 3
#endif

#ifndef USE_ZERODIVIDE
#define USE_ZERODIVIDE 2
#endif

#ifndef USE_MULTITRIM
#define USE_MULTITRIM 0
#endif

#ifndef USE_AFFINEABS
#define USE_AFFINEABS 0
#endif

#ifndef USE_MIGSKIP
#define USE_MIGSKIP 1
#endif

#ifndef RECOVER_RATIO
#define RECOVER_RATIO 0.1
#endif

#ifndef USE_SLOWDIVIDE
#define USE_SLOWDIVIDE 1
#endif

#ifndef USE_FI
#define USE_FI 1
#endif

#ifndef USE_SUPERLINEAR
#define USE_SUPERLINEAR 1
#endif

#ifndef UNIFY_REST
#define UNIFY_REST 1
#endif

#ifndef EXISTENCE_RATIO
#define EXISTENCE_RATIO 0.8
#endif

#ifndef ITER_STOP_RATIO
#define ITER_STOP_RATIO 0.9
#endif

#ifndef ENABLE_INFINITY
#define ENABLE_INFINITY 1
#endif

#ifndef WEIGHTED_MAX
#define WEIGHTED_MAX 1
#endif

namespace kv {

namespace bn = boost::numeric;
namespace ub = boost::numeric::ublas;

namespace allsol_sub {

// return index of I_i which has maximum width

template <class T> int search_maxwidth (const ub::vector< interval<T> >& I) {
	int s = I.size();
	int i, mi;
	T m, tmp;

	m = 0.;
	for (i=0; i<s; i++) {
#if WEIGHTED_MAX == 1
		tmp = width(I(i)) / (1. + mag(I(i)) / (std::numeric_limits<T>::max)() * mag(I(i)));
#else
		tmp = width(I(i));
#endif
		if (tmp > m) {
			m = tmp; mi = i;
		}
	}

	return mi;
}

// return max width(I_i) / width(J_i)

template <class T> T widthratio_max (const ub::vector< interval<T> >& I, const ub::vector< interval<T> >& J) {
	int s = I.size();
	int i;
	T tmp, r;

	r = 0.;

	for (i=0; i<s; i++) {
		tmp = width(I(i)) / width(J(i));
		if (tmp > r) r = tmp;
	}

	return r;
}

// return min width(I_i) / width(J_i)

template <class T> T widthratio_min (const ub::vector< interval<T> >& I, const ub::vector< interval<T> >& J) {
	int s = I.size();
	int i;
	T tmp, r;

	r = std::numeric_limits<T>::max();

	for (i=0; i<s; i++) {
		tmp = width(I(i)) / width(J(i));
		if (tmp < r) r = tmp;
	}

	return r;
}

// J: original interval, I: shrinked interval
// If width(I(i)) < ratio * width(J(i)) then inflate I(i)
//  until width(I(i)) = ratio * width(J(i))
// Inflation outside J(i) is "not allowed".

template <class T> void recovery_inflation (ub::vector< interval<T> >& I, const ub::vector< interval<T> >& J, T ratio) {
	int s = I.size();
	int i;
	T tmp, tmp2, l, u;

	for (i=0; i<s; i++) {
		if ( width(I(i)) < ratio * width(J(i)) ) {
			tmp = mid(I(i));
			tmp2 = rad(J(i)) * ratio;
			l = tmp - tmp2;
			u = tmp + tmp2;
			if (l < J(i).lower()) {
				u += (J(i).lower() - l);
				l = J(i).lower();
			} else if (u > J(i).upper()) {
				l -= (u - J(i).upper());
				u = J(i).upper();
			}
			// to be sure that new I(i) includes original I(i)
			I(i) = interval<T>::hull(I(i), interval<T>(l, u));
		}
	}
}

// J: original interval, I: shrinked interval
// If width(I(i)) < ratio * width(J(i)) then inflate I(i)
//  until width(I(i)) = ratio * width(J(i))
// Inflation outside J(i) is "allowed".

template <class T> void recovery_inflation2 (ub::vector< interval<T> >& I, const ub::vector< interval<T> >& J, T ratio) {
	int s = I.size();
	int i;
	T tmp, tmp2, l, u;

	for (i=0; i<s; i++) {
		if ( width(I(i)) < ratio * width(J(i)) ) {
			tmp = mid(I(i));
			tmp2 = rad(J(i)) * ratio;
			l = tmp - tmp2;
			u = tmp + tmp2;
			// to be sure that new I(i) includes original I(i)
			I(i) = interval<T>::hull(I(i), interval<T>(l, u));
		}
	}
}

// **not used**
// return index of division such that norm of M seems to be smallest
// under the scaled norm u = rad(I:divided)

template <class T> int search_optimal_divide (const ub::matrix< interval<T> >& M, const ub::vector< interval<T> >& I) {
	int n = I.size();
	ub::vector<T> u(n), s(n);
	ub::matrix<T> m(n,n);
	int i, j, p, mp;
	T tmp, tmp2, tmp3;

	for (i=0; i<n; i++) u(i) = width(I(i));
	for (i=0; i<n; i++) {
		for (j=0; j<n; j++) {
			m(i,j) = norm(M(i,j));
		}
	}
	for (i=0; i<n; i++) {
		tmp = 0.;
		for (j=0; j<n; j++) {
			tmp += m(i,j) * u(j);
		}
		s(i) = tmp / u(i);
	}

	tmp3 = std::numeric_limits<T>::max();
	for (p=0; p<n; p++) {
		tmp2 = 0.;
		for (i=0; i<n; i++) {
			tmp = s(i) - m(i,p)*u(p)/u(i)/2.;
			if (i == p) tmp *= 2.;
			if (tmp > tmp2) tmp2 = tmp;
		}
		if (tmp2 < tmp3) {
			tmp3 = tmp2;
			mp = p;
		}
	}

	return mp;
}

// **not used**
// return index such that width(K(i)) is relatively smallest
// compared with width(I(i))

template <class T> int search_optimal_divide2 (const ub::vector< interval<T> >& K, const ub::vector< interval<T> >& I) {
	int n = I.size();
	T tmp, tmp2;
	int i, r;

	tmp2 = std::numeric_limits<T>::max();
	for (i=0; i<n; i++) {
		tmp =  width(K(i))/ width(I(i));
		if (tmp < tmp2) {
			tmp2 = tmp;
			r = i;
		}
	}

	return r;
}

#if ENABLE_INFINITY == 1

template <class T> T mid_infinity (const interval<T>& I) {
	if (I.upper() == std::numeric_limits<T>::infinity()) {
		if (I.lower() == -std::numeric_limits<T>::infinity()) {
			return T(0.);
		} else {
			return mid(interval<T>((std::numeric_limits<T>::max)(), I.lower()));
			#if 0
			if (I.lower() < 0.) {
				return T(0.);
			} else if (I.lower() == 0.) {
				return T(1.);
			} else {
				using std::sqrt;
				return sqrt((std::numeric_limits<T>::max)()) * sqrt(I.lower());
			}
			#endif
		}
	} else {
		if (I.lower() == -std::numeric_limits<T>::infinity()) {
			return mid(interval<T>(-(std::numeric_limits<T>::max)(), I.upper()));
			#if 0
			if (I.upper() > 0.) {
				return T(0.);
			} else if (I.upper() == 0.) {
				return T(-1.);
			} else {
				using std::sqrt;
				return -sqrt((std::numeric_limits<T>::max)()) * sqrt(-I.upper());
			}
			#endif
		} else {
			return mid(I);
		}
	}
}

template <class T> ub::vector<T> mid_infinity (const ub::vector< interval<T> >& I) {
	int n = I.size();
	int i;
	ub::vector<T> r(n);

	for (i=0; i<n; i++) {
		r(i) = mid_infinity(I(i));
	}

	return r;
}

template <class T> bool include_infinity (const interval<T>& I) {
	if (I.lower() == -std::numeric_limits<T>::infinity() || I.upper() == std::numeric_limits<T>::infinity()) return true;
	return false;
}

template <class T> bool include_infinity (const ub::vector< interval<T> >& I) {
	int n = I.size();
	int i;
	ub::vector<T> r(n);

	for (i=0; i<n; i++) {
		if (include_infinity(I(i))) return true;
	}

	return false;
}

template <class T> int search_maxwidth_infinity (const ub::vector< interval<T> >& I) {
	int s = I.size();
	int i, mi, r, tr;
	T m, tmp, tmp2;

	m = 0.;
	r = 0.;
	for (i=0; i<s; i++) {
		if (I(i).lower() == -std::numeric_limits<T>::infinity()) {
			if (I(i).upper() == std::numeric_limits<T>::infinity()) {
				return i;
			} else {
				tr = 1;
				tmp = I(i).upper();
			}
		} else {
			if (I(i).upper() == std::numeric_limits<T>::infinity()) {
				tr = 1;
				tmp = -I(i).lower();
			} else {
				tr = 0;
#if WEIGHTED_MAX == 1
				tmp = width(I(i)) / (1. + mag(I(i)) / (std::numeric_limits<T>::max)() * mag(I(i)));
#else
				tmp = width(I(i));
#endif
			}
		}
		if (tr > r || (tr == r &&  tmp > m)) {
			r = tr; m = tmp; mi = i;
		}
	}

	return mi;
}

template <class T> int search_maxwidth_finite (const ub::vector< interval<T> >& I) {
	int s = I.size();
	int i, mi;
	T m, tmp, tmp2;

	mi = -1;
	m = 0.;
	for (i=0; i<s; i++) {
		if (include_infinity(I(i))) continue;
#if WEIGHTED_MAX == 1
		tmp = width(I(i)) / (1. + mag(I(i)) / (std::numeric_limits<T>::max)() * mag(I(i)));
#else
		tmp = width(I(i));
#endif
		if (tmp > m) {
			m = tmp; mi = i;
		}
	}

	return mi;
}

#endif // ENABLE_INFINITY

// generate 1-d vector function from scalar function

template <class F>
struct MakeVec {
	F f;
	MakeVec(F f): f(f) {}

	template <class T> ub::vector<T> operator()(const ub::vector<T>& x) {
		ub::vector<T> r(1);
		r(0) = f(x(0));
		return r;
	}
};

} // namespace allsol_sub

// find all solution of f in I

template <class T, class F>
std::list< ub::vector< interval<T> > >
allsol (
F f,
const ub::vector< interval<T> >& I,
int verbose = 1,
T giveup = T(0.),
std::list< ub::vector < interval<T> > >* rest = NULL
)
{
	std::list< ub::vector < interval<T> > > targets;
	targets.push_back(I);
	return allsol_list(f, targets, verbose, giveup, rest);
}

// find all solution of f in targets (list of intervals)

template <class T, class F>
std::list< ub::vector< interval<T> > >
allsol_list (
F f,
std::list< ub::vector< interval<T> > > targets,
int verbose = 1,
T giveup = T(0.),
std::list< ub::vector < interval<T> > >* rest = NULL
)
{
	int s = (targets.front()).size();
	std::list< ub::vector< interval<T> > > solutions, solutions_big;
	int count_ne_test = 0;
	int count_ex_test = 0;
	int count_unknown = targets.size();
	int count_ne = 0;
	int count_ex = 0;
	int count_giveup = 0;

	#pragma omp parallel
	{

	ub::vector< interval<T> > I, fc, fi, C, CK, K, mvf, I1, I2, IR, Iorg, g;
	ub::vector<T> v;
	ub::matrix< interval<T> > fdi, M, L2;
	ub::matrix<T> L, R, E;
	typename std::list< ub::vector< interval<T> > >::iterator p, p2;
	int i, j, k, mi;
	T tmp, tmp2;
	T wmax;
	bool r, M_calculated, flag, flag2, flag3;
	interval<T> A, B, J, J2, Itmp;
#if USE_TRIM == 3
	ub::vector< interval<T> > A0, A1, A2; // for new trim algorithm
#endif // USE_TRIM == 3

	// boost::variate_generator<boost::mt19937, boost::uniform_int<> > rand (boost::mt19937(time(0)), boost::uniform_int<>(0, s-1));

	E = ub::identity_matrix<T>(s);

	while (true) {
		if (verbose >= 2) {
			#pragma omp critical (cout)
			{
			std::cout << "ne_test: " << count_ne_test << ", ex_test: " << count_ex_test << ", unknown: " << count_unknown << ", ne: " << count_ne << ", ex: " << count_ex << ", giveup: " << count_giveup << "    \r" << std::flush;
			}
		}

		#ifdef _OPENMP

		int iflag = 0;
		#pragma omp critical (targets)
		{
		if (count_unknown == 0) iflag = 2;
		else {
			if (targets.empty()) {
				iflag = 1;
			} else {
				I = targets.front();
				targets.pop_front();
			}
		}
		}
		if (iflag == 2)  break;
		if (iflag == 1) continue;

		#else // _OPENMP

		if (targets.empty()) break;
		I = targets.front();
		targets.pop_front();

		#endif // _OPENMP

		Iorg = I;

		// non-existence test

		#pragma omp atomic
		count_ne_test++;

#if USE_FI == 1
		try {
			fi = f(I);
		}
		catch (std::domain_error& e) {
			goto label;
		}

		if (!zero_in(fi)) {
			#pragma omp atomic
			count_ne++;
			#pragma omp critical (targets)
			{
			count_unknown--;
			}
			continue;
		}
#endif

		try {
			autodif< interval<T> >::split(f(autodif< interval<T> >::init(I)), fi, fdi);
		}
		catch (std::domain_error& e) {
			goto label;
		}

#if USE_FI != 1
		if (!zero_in(fi)) {
			#pragma omp atomic
			count_ne++;
			#pragma omp critical (targets)
			{
			count_unknown--;
			}
			continue;
		}
#endif

#if ENABLE_INFINITY == 1
		C = allsol_sub::mid_infinity(I);
#else
		C = mid(I);
#endif
		try {
			fc = f(C);
		}
		catch (std::domain_error& e) {
			goto label;
		}

		mvf = fc + prod(fdi, I - C);
		if (!zero_in(mvf)) {
			#pragma omp atomic
			count_ne++;
			#pragma omp critical (targets)
			{
			count_unknown--;
			}
			continue;
		}

#if ENABLE_INFINITY == 1
		if (allsol_sub::include_infinity(I)) goto label;
#endif

#if USE_TRIM >= 1
		// interval shrinking
		IR = I;
		flag = false; // non-existence in I turns out or not
		flag2 = false; // shrinking of I occurs or not
#if USE_ZERODIVIDE == 2
		flag3 = false; // division of I occurs or not
#endif

		// maximum width for using TRIM
		wmax = 0.;
		for (i=0; i<s; i++) {
			tmp = width(I(i));
			if (tmp > wmax) wmax = tmp;
		}
		wmax *= RECOVER_RATIO;

#if USE_TRIM == 3
		A0.resize(s);
		A1.resize(s);
		A2.resize(s);
#endif // USE_TRIM == 3

		for (i=0; i<s; i++) {

#if USE_TRIM == 3
			// prepare for new trim algorithm
			for (j=0; j<s; j++) {
				A0(j) = fdi(i,j) * (I(j)-C(j));
			}
			Itmp = 0.;
			for (j=0; j<s; j++) {
				A1(j) = Itmp;
				Itmp += A0(j);
			}
			Itmp = 0.;
			for (j=s-1; j>=0; j--) {
				A2(j) = Itmp;
				Itmp += A0(j);
			}
#endif // USE_TRIM == 3

			for (j=0; j<s; j++) {
				// do not use TRIM for narrow component
				if (width(I(j)) < wmax) continue;

				B = fdi(i, j);

#if USE_TRIM == 1
				// calculate A simply
				// simple but slow
				A = 0.;
				for (k=0; k<s; k++) {
					if (k == j) continue;
					A += fdi(i, k) * (I(k)-C(k));
				}
				A += fc(i);
#endif // USE_TRIM == 1
#if USE_TRIM == 2
				// old trim algorithm
				// calculate back A from mvf
				A = mvf(i);
				Itmp = B * (I(j)-C(j));
				rop<T>::begin();
				tmp = rop<T>::sub_down(A.lower(), Itmp.lower());
				tmp2 = rop<T>::sub_up(A.upper(), Itmp.upper());
				rop<T>::end();
				A.assign(tmp, tmp2);
#endif // USE_TRIM == 2
#if USE_TRIM == 3
				// new trim algorithm
				A = fc(i) + A1(j) + A2(j);
#endif // USE_TRIM == 3

				if (zero_in(B)) {
#if USE_ZERODIVIDE >= 1
					bool bdummy;
					if (rad(B) <= 0.) continue;
					J = C(j) - division_part1(A, B, bdummy);
					J2 = C(j) - division_part2(A, B);
					if (overlap(IR(j), J)) {
						if (overlap(IR(j), J2)) {
#if USE_ZERODIVIDE == 2
							if (overlap(J, J2)) continue;
							// interval division
							I1 = IR;
							I2 = IR;
							I1(j) = intersect(IR(j), J);
							I2(j) = intersect(IR(j), J2);
							allsol_sub::recovery_inflation(I1, Iorg, (T)RECOVER_RATIO);
							allsol_sub::recovery_inflation(I2, Iorg, (T)RECOVER_RATIO);
							#pragma omp critical (targets)
							{
							targets.push_back(I1);
							targets.push_back(I2);
							count_unknown += 1;
							}
							flag3 = true;
							break;
#else
							continue;
#endif
						} else {
							if (!subset(IR(j), J)) {
								IR(j) = intersect(IR(j), J);
								flag2 = true;
							}
						}
					} else {
						if (overlap(IR(j), J2)) {
							if (!subset(IR(j), J2)) {
								IR(j) = intersect(IR(j), J2);
								flag2 = true;
							}
						} else {
							flag = true;
							break;
						}
					}
#else
					continue;
#endif
				} else {
					J = C(j) - A/B;
					if (overlap(IR(j), J)) {
						if (!subset(IR(j), J)) {
							IR(j) = intersect(IR(j), J);
							flag2 = true;
						}
					} else {
						flag = true;
						break;
					}
				}
			} // loop j
			if (flag == true) break;
#if USE_ZERODIVIDE == 2
			if (flag3 == true) break;
#endif
		} // loop i

#if USE_ZERODIVIDE == 2
		// interval division occurs
		if (flag3 == true) continue;
#endif

		// non-existence in I turns out
		if (flag == true) {
			#pragma omp atomic
			count_ne++;
			#pragma omp critical (targets)
			{
			count_unknown--;
			}
			continue;
		}

		if (flag2 == true) {
			allsol_sub::recovery_inflation(IR, Iorg, (T)RECOVER_RATIO);
#if USE_MULTITRIM == 1
			// if radius of I is smaller than half on the
			// "division scheduled" index by interval shrinking,
			// skip the existence test and do interval
			// shrinking again.
			mi = allsol_sub::search_maxwidth(I);
			if (rad(IR(mi)) <= 0.5 * rad(I(mi))) {
				#pragma omp critical (targets)
				{
				targets.push_back(IR);
				}
				continue;
			}
#endif

			// renew I, C and f(C)
			// do not renew f'(I) because of calculation cost
			I = IR;
			C = mid(I);
			try {
				fc = f(C);
			}
			catch (std::domain_error& e) {
				goto label;
			}

			// re-check mvf
			mvf = fc + prod(fdi, I - C);
			if (!zero_in(mvf)) {
				#pragma omp atomic
				count_ne++;
				#pragma omp critical (targets)
				{
				count_unknown--;
				}
				continue;
			}
		}

#endif // USE_TRIM >= 1

#if USE_AFFINEABS == 1
		g.resize(s);
		for (i=0; i<s; i++) {
			g(i) = fc(i) / rad(mvf(i));
		}
		L2 = mid(fdi);
		I1 = fc + prod(fdi - L2, I - C);
		I2 = prod(g, L2);
		if (inner_prod(mag(g), mig(I1)) - inner_prod(mag(I2), rad(I)) > 0.) {
			#pragma omp atomic
			count_ne++;
			#pragma omp critical (targets)
			{
			count_unknown--;
			}
			continue;
		}
#endif

		// existence test

		M_calculated = false;

		// skip existence test using mig
#if USE_MIGSKIP >= 1
#if USE_MIGSKIP == 1
		// use "loose" condition
		// interval<T> is for avoiding compile error
		mvf = fc + prod(mig(fdi), interval<T>(1. + EDGE_RATIO) * (I - C));
#else
		// use "strict" condition
		mvf = fc + prod(mig(fdi), I - C);
#endif
		if (!zero_in(mvf)) goto label;
#endif

		L = mid(fdi);

#if 0
		// **not used**
		// below is same as mig?
		I1 = - fc + prod(L-fdi, I-C);
		I2 = prod(L, I-C);
		if (!subset(I1, I2)) goto label;
#endif

#if 0
		// **not used**
		// prod(mag(L) - mag(L - fdi), rad(I));
		L2 = L - fdi;
		v = prod(mag(L) - mag(L2), rad(I));

		flag = true;
		for (i=0; i<s; i++) {
			if (abs(fc(i)) > v(i)) {flag=false; break;}
		}
		if (flag == false) goto label;
#endif

		#pragma omp atomic
		count_ex_test++;

		r = invert(L, R);
		if (!r) goto label;

		M = E - prod(R, fdi);
		M_calculated = true;
		CK = C - prod(R, fc);
		K = CK +  prod(M, I - C);
		if (!overlap(K, I)) {
			#pragma omp atomic
			count_ne++;
			#pragma omp critical (targets)
			{
			count_unknown--;
			}
			continue;
		}

#if 0
		// **not used**
		// same condition as above !overlap(K, I)
		I1 = Rfc + prod(Rfdi, I - C);
		if (!zero_in(I1)) {
			count_ne++;
			continue;
		}
#endif

		if (proper_subset(K, I) && allsol_sub::widthratio_max(K, I) < EXISTENCE_RATIO ) {
			#pragma omp critical (solutions)
			{
			// check whether the solution is already found or not
			flag = true;
			p = solutions.begin();
			p2 = solutions_big.begin();
			while (p != solutions.end()) {
				if (overlap(K, *p)) {
					if (subset(K, *p2)||subset(*p, I)) {
						flag = false;
						break;
					}
					while (true) {
						C = mid(K);
						I1 = C - prod(R, f(C)) + prod(M, K - C);
						K = intersect(K, I1);
						if (subset(K, *p2)) {
							flag2 = true;
							break;
						}
						if (!overlap(K, *p)) {
							flag2 = false;
							break;
						}
					}
					if (flag2 == true) {
						flag = false;
						break;
					} else {
						/* never reach? */
						std::cout << "two overlap intervals includes different solutions";
					}
				}
				p++;
				p2++;
			}
			if (flag) { // new solution found
				if (verbose >= 1) {
					#pragma omp critical (cout)
					{
					std::cout << I << "(ex)\n";
					}
				}
				solutions_big.push_back(I);
				// iterative refinement
				while (1) {
					C = mid(K);
					#if USE_SUPERLINEAR == 1
					autodif< interval<T> >::split(f(autodif< interval<T> >::init(K)), fi, fdi);
					L = mid(fdi);
					r = invert(L, R);
					M = E - prod(R, fdi);
					#endif
					I1 = C - prod(R, f(C)) + prod(M, K - C);
					I1 = intersect(K, I1);
					tmp = allsol_sub::widthratio_min(I1, K);
					K = I1;
					if (tmp > ITER_STOP_RATIO) break;
				}
				solutions.push_back(K);
				count_ex++;
				if (verbose >= 1) {
					#pragma omp critical (cout)
					{
					std::cout << K << "(ex:improved)\n";
					}
				}
			}
			} // pragma omp critical (solutions)
			#pragma omp critical (targets)
			{
			count_unknown--;
			}
			continue;
		}

		// check the case that solution may exist near boundary.
		// If so, use K as next interval
#if ENABLE_INFINITY == 1
		if (!allsol_sub::include_infinity(I) && allsol_sub::widthratio_max(K, I) < EDGE_RATIO)
#else
		if (allsol_sub::widthratio_max(K, I) < EDGE_RATIO)
#endif
		{
			allsol_sub::recovery_inflation2(K, Iorg, (T)RECOVER_RATIO);
			#pragma omp critical (targets)
			{
			targets.push_back(K);
			}
			continue;
		}

		I = intersect(I, K);
		allsol_sub::recovery_inflation(I, Iorg, (T)RECOVER_RATIO);

		label:

		// divide interval

		// if (M_calculated) mi = search_optimal_divide(M, I);
		// if (M_calculated) mi = search_optimal_divide2(K, I);
		// else mi = allsol_sub::search_maxwidth(I);
		// mi = rand();

#if USE_SLOWDIVIDE == 1
#if ENABLE_INFINITY == 1
		if (!allsol_sub::include_infinity(I) && allsol_sub::widthratio_max(I, Iorg) <= 0.5)
#else
		if (allsol_sub::widthratio_max(I, Iorg) <= 0.5)
#endif
		{
			#pragma omp critical (targets)
			{
			targets.push_back(I);
			}
			continue;
		}
#endif

#ifdef USE_SLOWDIVIDE_OLD

		// if radius of I is smaller than half on the
		// "division scheduled" index by interval shrinking,
		// skip division.

		mi = allsol_sub::search_maxwidth(Iorg);
		if (rad(I(mi)) <= 0.5 * rad(Iorg(mi))) {
			#pragma omp critical (targets)
			{
			targets.push_back(I);
			}
			continue;
		}
#else
#if ENABLE_INFINITY == 1
		mi = allsol_sub::search_maxwidth_infinity(I);
#else
		mi = allsol_sub::search_maxwidth(I);
#endif
#endif

#if ENABLE_INFINITY == 1
		tmp = allsol_sub::mid_infinity(I(mi));
#else
		tmp = mid(I(mi));
#endif
		if (width(I(mi)) < giveup || tmp == I(mi).lower() || tmp == I(mi).upper()) {
			if (verbose >= 2) {
				std::cout << "too small interval (may be multiple root?):\n" << I << "\n";
			}
			if (rest != NULL) {
				#pragma omp critical (rest)
				{
				#if UNIFY_REST == 1
				I1 = I;
				while (true) {
					flag = false;
					p = (*rest).begin();
					while (p != (*rest).end()) {
						if (overlap(*p, I1)) {
							I1 = hull(I1, *p);
							p = (*rest).erase(p);
							// #pragma omp atomic
							// count_giveup--;
							flag = true;
							continue;
						}
						p++;
					}
					if (flag == false) break;
				}
				(*rest).push_back(I1);
				#else // UNIFY_REST == 1
				(*rest).push_back(I);
				#endif // UNIFY_REST == 1
				}
			}
			#pragma omp atomic
			count_giveup++;
			#pragma omp critical (targets)
			{
			count_unknown--;
			}
			continue;
		}

		I1 = I; I2 = I;
		I1(mi).assign(I1(mi).lower(), tmp);
		I2(mi).assign(tmp, I2(mi).upper());
#if ENABLE_INFINITY == 1
		ub::vector< interval<T> > I3;
		int mi2;
		if (allsol_sub::include_infinity(I1(mi))) {
			mi2 = allsol_sub::search_maxwidth_finite(I1);
			if (mi2 != -1) {
				I3 = I1;
				tmp = mid(I1(mi2));
				I1(mi2).assign(I1(mi2).lower(), tmp);
				I3(mi2).assign(tmp, I3(mi2).upper());
				#pragma omp critical (targets)
				{
				targets.push_back(I3);
				count_unknown += 1;
				}
			}
		}
		if (allsol_sub::include_infinity(I2(mi))) {
			mi2 = allsol_sub::search_maxwidth_finite(I2);
			if (mi2 != -1) {
				I3 = I2;
				tmp = mid(I2(mi2));
				I2(mi2).assign(I2(mi2).lower(), tmp);
				I3(mi2).assign(tmp, I3(mi2).upper());
				#pragma omp critical (targets)
				{
				targets.push_back(I3);
				count_unknown += 1;
				}
			}
		}
#endif
		#pragma omp critical (targets)
		{
		targets.push_back(I1);
		targets.push_back(I2);
		count_unknown += 1;
		}
	}

	} // pragma omp parallel

	if (verbose >= 1) {
			std::cout << "ne_test: " << count_ne_test << ", ex_test: " << count_ex_test << ", ne: " << count_ne << ", ex: " << count_ex << ", giveup: " << count_giveup << "    \n";
	}

	return solutions;
}

// allsol for 1-dimentional function

template <class T, class F>
std::list< interval<T> >
allsol (
F f,
const interval<T>& I,
int verbose = 1,
T giveup = T(0.),
std::list< interval<T> >* rest = NULL
)
{
	allsol_sub::MakeVec<F> g(f);
	ub::vector< interval<T> > I2(1);
	std::list< ub::vector< interval<T> > > rest2;
	typename std::list< ub::vector< interval<T> > >::iterator p1;
	std::list< ub::vector< interval<T> > >* rest_p;
	std::list< ub::vector< interval<T> > > r1;
	typename std::list< ub::vector< interval<T> > >::iterator p2;
	std::list< interval<T> > r2;

	I2(0) = I;
	if (rest == NULL) {
		rest_p = NULL;
	} else {
		rest_p = &rest2;
	}

	r1 = allsol(g, I2, verbose, giveup, rest_p);

	p2 = r1.begin();
	while (p2 != r1.end()) {
		r2.push_back((*(p2++))(0));
	}

	if (rest != NULL) {
		p1 = rest2.begin();
		while (p1 != rest2.end()) {
			(*rest).push_back((*(p1++))(0));
		}
	}

	return r2;
}

} // namespace kv

#endif // ALLSOL_HPP


// Eigen tutorial -- Block operations
// SOurce:
// http://eigen.tuxfamily.org/dox/group__TutorialBlockOperations.html
#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;

int main(void)
{
    MatrixXf m(4,4);
    m << 1, 2, 3, 4,
         5, 6, 7, 8,
         9,10,11,12,
        13,14,15,16;
    std::cout << "Block in the middle" << std::endl;
    std::cout << m.block<2,2>(1,1) << '\n' << std::endl;
    for (int i = 0; i <= 3; i++) {
        std::cout << "Block of size " << i << "x" << i << std::endl;
        std::cout << m.block(0,0,i,i) << '\n' << std::endl;
    }

    // block can also be used as lvalues, meaning that you can assign to a block
    Array22f r;
    r << 1,2,
         3,4;
    Array44f a = Array44f::Constant(0.6);
    std::cout << "Here is the array a:\n" << a << '\n' << std::endl;
    a.block<2,2>(1,1) = r;
    std::cout << "Here is now a with m copied into its central 2x2 block:\n"
        << a << '\n' << std::endl;
    a.block(0,0,2,3) = a.block(2,1,2,3);
    std::cout << "Here is now a with bottom-right 2x3 copied into top-left 2x2 block:\n"
        << a << '\n' << std::endl;

    // Columns and rows
    MatrixXf u(3,3);
    u << 1,2,3,
         4,5,6,
         7,8,9;
    std::cout << "Here is thematrix u:\n" << u << std::endl;
    std::cout << "2nd Row: " << u.row(1) << std::endl;
    u.col(2) += 3 * u.col(0);
    std::cout << "After adding 3 times the first column into the third column,"
        << "the matrix u is:\n" << u << std::endl;

    // Corner-related operations
    MatrixXf w(4,4);
    w << 1, 2, 3, 4,
         5, 6, 7, 8,
         9,10,11,12,
        13,14,15,16;
    std::cout << "w.leftCols(2) =\n" << m.leftCols(2) << '\n' << std::endl;
    std::cout << "w.bottomRows<2>() =\n" <<  w.bottomRows<2>() << '\n' << std::endl;
    w.topLeftCorner(1,3) = w.bottomRightCorner(3,1).transpose();
    std::cout << "After assignment, w =\n" << w << std::endl;

    // Block operations for vectors
    ArrayXf y(6);
    y << 1,2,3,4,5,6;
    std::cout << "y.head(3) = \n" << y.head(3) << '\n' << std::endl;
    std::cout << "y.tail<3>() =\n" << y.tail<3>() << "\n\n";
    y.segment(1,4) *= 2;
    std::cout << "after 'y.segment(1,4) *= 2', y =\n" << y << std::endl;

    return 0;
}


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

#ifndef BOOST_MATH_HANKEL_HPP
#define BOOST_MATH_HANKEL_HPP

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/bessel.hpp>

namespace boost{ namespace math{

namespace detail{

template <class T, class Policy>
std::complex<T> hankel_imp(T v, T x, const bessel_no_int_tag&, const Policy& pol, int sign)
{
   BOOST_MATH_STD_USING
   static const char* function = "boost::math::cyl_hankel_1<%1%>(%1%,%1%)";

   if(x < 0)
   {
      bool isint_v = floor(v) == v;
      T j, y;
      bessel_jy(v, -x, &j, &y, need_j | need_y, pol);
      std::complex<T> cx(x), cv(v);
      std::complex<T> j_result, y_result;
      if(isint_v)
      {
         int s = (iround(v) & 1) ? -1 : 1;
         j_result = j * s;
         y_result = T(s) * (y - (2 / constants::pi<T>()) * (log(-x) - log(cx)) * j);
      }
      else
      {
         j_result = pow(cx, v) * pow(-cx, -v) * j;
         T p1 = pow(-x, v);
         std::complex<T> p2 = pow(cx, v);
         y_result = p1 * y / p2
            + (p2 / p1 - p1 / p2) * j / tan(constants::pi<T>() * v);
      }
      // multiply y_result by i:
      y_result = std::complex<T>(-sign * y_result.imag(), sign * y_result.real());
      return j_result + y_result;
   }

   if(x == 0)
   {
      if(v == 0)
      {
         // J is 1, Y is -INF
         return std::complex<T>(1, sign * -policies::raise_overflow_error<T>(function, 0, pol));
      }
      else
      {
         // At least one of J and Y is complex infinity:
         return std::complex<T>(policies::raise_overflow_error<T>(function, 0, pol), sign * policies::raise_overflow_error<T>(function, 0, pol));
      }
   }

   T j, y;
   bessel_jy(v, x, &j, &y, need_j | need_y, pol);
   return std::complex<T>(j, sign * y);
}

template <class T, class Policy>
std::complex<T> hankel_imp(int v, T x, const bessel_int_tag&, const Policy& pol, int sign);

template <class T, class Policy>
inline std::complex<T> hankel_imp(T v, T x, const bessel_maybe_int_tag&, const Policy& pol, int sign)
{
   BOOST_MATH_STD_USING  // ADL of std names.
   int ival = detail::iconv(v, pol);
   if(0 == v - ival)
   {
      return hankel_imp(ival, x, bessel_int_tag(), pol, sign);
   }
   return hankel_imp(v, x, bessel_no_int_tag(), pol, sign);
}

template <class T, class Policy>
inline std::complex<T> hankel_imp(int v, T x, const bessel_int_tag&, const Policy& pol, int sign)
{
   BOOST_MATH_STD_USING
   if((std::abs(v) < 200) && (x > 0))
      return std::complex<T>(bessel_jn(v, x, pol), sign * bessel_yn(v, x, pol));
   return hankel_imp(static_cast<T>(v), x, bessel_no_int_tag(), pol, sign);
}

template <class T, class Policy>
inline std::complex<T> sph_hankel_imp(T v, T x, const Policy& pol, int sign)
{
   BOOST_MATH_STD_USING
   return constants::root_half_pi<T>() * hankel_imp(v + 0.5f, x, bessel_no_int_tag(), pol, sign) / sqrt(std::complex<T>(x));
}

} // namespace detail

template <class T1, class T2, class Policy>
inline std::complex<typename detail::bessel_traits<T1, T2, Policy>::result_type> cyl_hankel_1(T1 v, T2 x, const Policy& pol)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename detail::bessel_traits<T1, T2, Policy>::optimisation_tag tag_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<std::complex<result_type>, Policy>(detail::hankel_imp<value_type>(v, static_cast<value_type>(x), tag_type(), pol, 1), "boost::math::cyl_hankel_1<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline std::complex<typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type> cyl_hankel_1(T1 v, T2 x)
{
   return cyl_hankel_1(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline std::complex<typename detail::bessel_traits<T1, T2, Policy>::result_type> cyl_hankel_2(T1 v, T2 x, const Policy& pol)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename detail::bessel_traits<T1, T2, Policy>::optimisation_tag tag_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<std::complex<result_type>, Policy>(detail::hankel_imp<value_type>(v, static_cast<value_type>(x), tag_type(), pol, -1), "boost::math::cyl_hankel_1<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline std::complex<typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type> cyl_hankel_2(T1 v, T2 x)
{
   return cyl_hankel_2(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline std::complex<typename detail::bessel_traits<T1, T2, Policy>::result_type> sph_hankel_1(T1 v, T2 x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<std::complex<result_type>, Policy>(detail::sph_hankel_imp<value_type>(static_cast<value_type>(v), static_cast<value_type>(x), forwarding_policy(), 1), "boost::math::sph_hankel_1<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline std::complex<typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type> sph_hankel_1(T1 v, T2 x)
{
   return sph_hankel_1(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline std::complex<typename detail::bessel_traits<T1, T2, Policy>::result_type> sph_hankel_2(T1 v, T2 x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<std::complex<result_type>, Policy>(detail::sph_hankel_imp<value_type>(static_cast<value_type>(v), static_cast<value_type>(x), forwarding_policy(), -1), "boost::math::sph_hankel_1<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline std::complex<typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type> sph_hankel_2(T1 v, T2 x)
{
   return sph_hankel_2(v, x, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_HANKEL_HPP


#include "lookIncFromSr.h"

#include <pybind11/eigen.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>

#include <Eigen/Dense>

#include <isce3/core/Ellipsoid.h>
#include <isce3/core/Orbit.h>
#include <isce3/geometry/DEMInterpolator.h>
#include <isce3/geometry/geometry.h>

// Aliases
namespace py = pybind11;
using isce3::geometry::DEMInterpolator;
using namespace isce3::core;

// Functions binding
void addbinding_look_inc_from_sr(pybind11::module& m)
{

    m.def(
            "look_inc_ang_from_slant_range",
            [](double slant_range, const Orbit& orbit,
                    std::optional<double> az_time = {},
                    const DEMInterpolator& dem_interp = {},
                    const Ellipsoid& ellips = {}) {
                return lookIncAngFromSlantRange(
                        slant_range, orbit, az_time, dem_interp, ellips);
            },
            py::arg("slant_range"), py::arg("orbit"),
            py::arg("az_time") = std::nullopt,
            py::arg_v("dem_interp", DEMInterpolator(), "0.0"),
            py::arg_v("ellips", Ellipsoid(), "WGS84"),
            R"(
Estimate look angle (off-nadir angle) and local incidence angle at a desired slant range
from orbit(spacecraft/antenna statevector) and at a certain relative azimuth time.

Parameters
----------
slant_range : float
    true slant range in meters from antenna phase center (or spacecraft position) to the ground.
orbit : isce3.core.orbit
az_time : float, optional
    relative azimuth time in seconds w.r.t reference epoch time of orbit object.
    If not speficied, the mid time of orbit will be used as azimuth time.
dem_interp : isce3.geometry.DEMInterpolator, default=0.0
ellips : isce3.core.Ellipsoid, default=WGS84

Returns
-------
float
    Look angle or off-nadir angle in (rad)
float
    Incidence angle in (rad)

Raises
------
RuntimeError
    for bad-value look angle or incidence angles

Notes
-----
See references [1]_ and [2]_ for the equations to calculate
look angle and incidence angle, respectivelty.

References
----------
..[1] https://en.wikipedia.org/wiki/Law_of_cosines
..[2] https://en.wikipedia.org/wiki/Law_of_sines
)");

    m.def(
            "look_inc_ang_from_slant_range",
            [](const Eigen::Ref<const Eigen::ArrayXd>& slant_range,
                    const Orbit& orbit, std::optional<double> az_time = {},
                    const DEMInterpolator& dem_interp = {},
                    const Ellipsoid& ellips = {}) {
                return lookIncAngFromSlantRange(
                        slant_range, orbit, az_time, dem_interp, ellips);
            },
            py::arg("slant_range"), py::arg("orbit"),
            py::arg("az_time") = std::nullopt,
            py::arg_v("dem_interp", DEMInterpolator(), "0.0"),
            py::arg_v("ellips", Ellipsoid(), "WGS84"),
            R"(
Estimate look angles (off-nadir angle) and local incidence angles at desired slant ranges
from orbit(spacecraft/antenna statevector) and at a certain relative azimuth time.

Parameters
----------
slant_range : numpy.ndarray(float)
    Array of slant ranges in meters from antenna phase center (or spacecraft position) to the ground.
orbit : isce3.core.orbit
az_time : float, optional
    relative azimuth time in seconds w.r.t reference epoch time of orbit object.
    If not speficied, the mid time of orbit will be used as azimuth time.
dem_interp : isce3.geometry.DEMInterpolator, default=0.0
ellips : isce3.core.Ellipsoid, default=WGS84

Returns
-------
numpy.ndarray(float)
    Look angles or off-nadir angles in (rad)
numpy.ndarray(float)
    Incidence angles in (rad)

Raises
------
RuntimeError
    for bad-value look angle or incidence angles

Notes
-----
See references [1]_ and [2]_ for the equations to calculate
look angle and incidence angle, respectivelty.

References
----------
..[1] https://en.wikipedia.org/wiki/Law_of_cosines
..[2] https://en.wikipedia.org/wiki/Law_of_sines
)");
}


#include <NTL/mat_lzz_p.h>

#include <NTL/new.h>

#include <NTL/vec_long.h>
#include <NTL/vec_ulong.h>
#include <NTL/vec_double.h>

NTL_START_IMPL

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

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

   X.SetDims(n, m);

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

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

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

   X.SetDims(n, m);

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

// some local buffers

NTL_THREAD_LOCAL static vec_long mul_aux_vec;
NTL_THREAD_LOCAL static NTL_SPMM_VEC_T precon_vec;

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

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

   X.SetDims(n, m);

   if (m > 1) {  // new preconditioning code

      long p = zz_p::modulus();
      double pinv = zz_p::ModulusInverse();

      vec_long::Watcher watch_mul_aux_vec(mul_aux_vec);
      mul_aux_vec.SetLength(m);
      long *acc = mul_aux_vec.elts();

      long i, j, k;

      for (i = 0; i < n; i++) {
         const zz_p* ap = A[i].elts();

         for (j = 0; j < m; j++) acc[j] = 0;

         for (k = 0;  k < l; k++) {
            long aa = rep(ap[k]);
            if (aa != 0) {
               const zz_p* bp = B[k].elts();
               long T1;
               mulmod_precon_t aapinv = PrepMulModPrecon(aa, p, pinv);

               for (j = 0; j < m; j++) {
        	  T1 = MulModPrecon(rep(bp[j]), aa, p, aapinv);
        	  acc[j] = AddMod(acc[j], T1, p);
               }
            }
         }

         zz_p *xp = X[i].elts();
         for (j = 0; j < m; j++)
            xp[j].LoopHole() = acc[j];
      }
   }
   else {  // just use the old code, w/o preconditioning

      long p = zz_p::modulus();
      double pinv = zz_p::ModulusInverse();

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

      for (i = 1; i <= n; i++) {
	 for (j = 1; j <= m; j++) {
            acc = 0;
            for(k = 1; k <= l; k++) {
               tmp = MulMod(rep(A(i,k)), rep(B(k,j)), p, pinv);
               acc = AddMod(acc, tmp, p);
            }
            X(i,j).LoopHole() = acc;
	 }
      }

   }
}

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

void mul(vec_zz_p& x, const vec_zz_p& a, const mat_zz_p& B)
{
   long l = a.length();
   long m = B.NumCols();

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

   if (m == 0) {

      x.SetLength(0);

   }
   else if (m == 1) {

      long p = zz_p::modulus();
      double pinv = zz_p::ModulusInverse();

      long acc, tmp;
      long k;

      acc = 0;
      for(k = 1; k <= l; k++) {
         tmp = MulMod(rep(a(k)), rep(B(k,1)), p, pinv);
         acc = AddMod(acc, tmp, p);
      }

      x.SetLength(1);
      x(1).LoopHole()  = acc;

   }
   else {  // m > 1.  precondition

      long p = zz_p::modulus();
      double pinv = zz_p::ModulusInverse();

      vec_long::Watcher watch_mul_aux_vec(mul_aux_vec);
      mul_aux_vec.SetLength(m);
      long *acc = mul_aux_vec.elts();

      long j, k;

      const zz_p* ap = a.elts();

      for (j = 0; j < m; j++) acc[j] = 0;

      for (k = 0;  k < l; k++) {
         long aa = rep(ap[k]);
         if (aa != 0) {
            const zz_p* bp = B[k].elts();
            long T1;
            mulmod_precon_t aapinv = PrepMulModPrecon(aa, p, pinv);

            for (j = 0; j < m; j++) {
               T1 = MulModPrecon(rep(bp[j]), aa, p, aapinv);
               acc[j] = AddMod(acc[j], T1, p);
            }
         }
      }

      x.SetLength(m);
      zz_p *xp = x.elts();
      for (j = 0; j < m; j++)
         xp[j].LoopHole() = acc[j];
   }
}

void mul_aux(vec_zz_p& x, const mat_zz_p& A, const vec_zz_p& b)
{
   long n = A.NumRows();
   long l = A.NumCols();

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

   x.SetLength(n);
   zz_p* xp = x.elts();

   long p = zz_p::modulus();
   double pinv = zz_p::ModulusInverse();

   long i, k;
   long acc, tmp;

   const zz_p* bp = b.elts();

   if (n <= 1) {

      for (i = 0; i < n; i++) {
	 acc = 0;
	 const zz_p* ap = A[i].elts();

	 for (k = 0; k < l; k++) {
            tmp = MulMod(rep(ap[k]), rep(bp[k]), p, pinv);
            acc = AddMod(acc, tmp, p);
	 }

	 xp[i].LoopHole() = acc;
      }

   }
   else {

      NTL_SPMM_VEC_T::Watcher watch_precon_vec(precon_vec);
      precon_vec.SetLength(l);
      mulmod_precon_t *bpinv = precon_vec.elts();

      for (k = 0; k < l; k++)
         bpinv[k] = PrepMulModPrecon(rep(bp[k]), p, pinv);

      for (i = 0; i < n; i++) {
	 acc = 0;
	 const zz_p* ap = A[i].elts();

	 for (k = 0; k < l; k++) {
            tmp = MulModPrecon(rep(ap[k]), rep(bp[k]), p, bpinv[k]);
            acc = AddMod(acc, tmp, p);
	 }

	 xp[i].LoopHole() = acc;
      }
   }
}

void mul(vec_zz_p& x, const mat_zz_p& A, const vec_zz_p& b)
{
   if (&b == &x || A.position1(x) != -1) {
      vec_zz_p tmp;
      mul_aux(tmp, A, b);
      x = tmp;
   }
   else
      mul_aux(x, A, b);

}

void mul(mat_zz_p& X, const mat_zz_p& A, zz_p b)
{
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;

   if (n == 0 || m == 0 || (n == 1 && m == 1)) {

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

   }
   else {

      long p = zz_p::modulus();
      double pinv = zz_p::ModulusInverse();
      long bb = rep(b);
      mulmod_precon_t bpinv = PrepMulModPrecon(bb, p, pinv);

      for (i = 0; i < n; i++) {
         const zz_p *ap = A[i].elts();
         zz_p *xp = X[i].elts();

	 for (j = 0; j < m; j++)
            xp[j].LoopHole() = MulModPrecon(rep(ap[j]), bb, p, bpinv);
      }

   }
}

void mul(mat_zz_p& X, const mat_zz_p& A, long b_in)
{
   zz_p b;
   b = b_in;
   mul(X, A, b);
}

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

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

void determinant(zz_p& d, const mat_zz_p& M_in)
{
   long k, n;
   long i, j;
   long pos;
   zz_p t1, t2, t3;
   zz_p *x, *y;

   mat_zz_p M;
   M = M_in;

   n = M.NumRows();

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

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

   zz_p det;

   set(det);

   long p = zz_p::modulus();
   double pinv = zz_p::ModulusInverse();

   for (k = 0; k < n; k++) {
      pos = -1;
      for (i = k; i < n; i++) {
         if (!IsZero(M[i][k])) {
            pos = i;
            break;
         }
      }

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

         mul(det, det, M[k][k]);

         inv(t3, M[k][k]);

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] - M[k]*M[i,k]*t3

            mul(t1, M[i][k], t3);
            negate(t1, t1);

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

            long T1 = rep(t1);
            mulmod_precon_t t1pinv = PrepMulModPrecon(T1, p, pinv); // T1*pinv;
            long T2;

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

               T2 = MulModPrecon(rep(*y), T1, p, t1pinv);
               x->LoopHole() = AddMod(rep(*x), T2, p);
            }
         }
      }
      else {
         clear(d);
         return;
      }
   }

   d = det;
}

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

   long i, j;

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

   return 1;
}

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

   long i, j;

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

void solve(zz_p& d, vec_zz_p& X,
           const mat_zz_p& A, const vec_zz_p& b)

{
   long n = A.NumRows();

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

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

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

   long i, j, k, pos;
   zz_p t1, t2, t3;
   zz_p *x, *y;

   mat_zz_p M;
   M.SetDims(n, n+1);
   for (i = 0; i < n; i++) {
      for (j = 0; j < n; j++)
         M[i][j] = A[j][i];
      M[i][n] = b[i];
   }

   zz_p det;
   set(det);

   long p = zz_p::modulus();
   double pinv = zz_p::ModulusInverse();

   for (k = 0; k < n; k++) {
      pos = -1;
      for (i = k; i < n; i++) {
         if (!IsZero(M[i][k])) {
            pos = i;
            break;
         }
      }

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

         mul(det, det, M[k][k]);

         inv(t3, M[k][k]);
         M[k][k] = t3;

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] - M[k]*M[i,k]*t3

            mul(t1, M[i][k], t3);
            negate(t1, t1);

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

            long T1 = rep(t1);
            mulmod_precon_t  t1pinv = PrepMulModPrecon(T1, p, pinv); // T1*pinv;
            long T2;

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

               T2 = MulModPrecon(rep(*y), T1, p, t1pinv);
               x->LoopHole() = AddMod(rep(*x), T2, p);
            }
         }
      }
      else {
         clear(d);
         return;
      }
   }

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

   d = det;
}

void inv(zz_p& d, mat_zz_p& X, const mat_zz_p& A)
{
   long n = A.NumRows();
   if (A.NumCols() != n)
      LogicError("inv: nonsquare matrix");

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

   long i, j, k, pos;
   zz_p t1, t2, t3;
   zz_p *x, *y;

   mat_zz_p M;
   M.SetDims(n, 2*n);
   for (i = 0; i < n; i++) {
      for (j = 0; j < n; j++) {
         M[i][j] = A[i][j];
         clear(M[i][n+j]);
      }
      set(M[i][n+i]);
   }

   zz_p det;
   set(det);

   long p = zz_p::modulus();
   double pinv = zz_p::ModulusInverse();

   for (k = 0; k < n; k++) {
      pos = -1;
      for (i = k; i < n; i++) {
         if (!IsZero(M[i][k])) {
            pos = i;
            break;
         }
      }

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

         mul(det, det, M[k][k]);

         inv(t3, M[k][k]);
         M[k][k] = t3;

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] - M[k]*M[i,k]*t3

            mul(t1, M[i][k], t3);
            negate(t1, t1);

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

            long T1 = rep(t1);
            mulmod_precon_t t1pinv = PrepMulModPrecon(T1, p, pinv); // T1*pinv;
            long T2;

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

               T2 = MulModPrecon(rep(*y), T1, p, t1pinv);
               x->LoopHole() = AddMod(rep(*x), T2, p);
            }
         }
      }
      else {
         clear(d);
         return;
      }
   }

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

   d = det;
}

long gauss(mat_zz_p& M, long w)
{
   long k, l;
   long i, j;
   long pos;
   zz_p t1, t2, t3;
   zz_p *x, *y;

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

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

   long p = zz_p::modulus();
   double pinv = zz_p::ModulusInverse();
   long T1, T2;

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

      pos = -1;
      for (i = l; i < n; i++) {
         if (!IsZero(M[i][k])) {
            pos = i;
            break;
         }
      }

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

         inv(t3, M[l][k]);
         negate(t3, t3);

         for (i = l+1; i < n; i++) {
            // M[i] = M[i] + M[l]*M[i,k]*t3

            mul(t1, M[i][k], t3);

            T1 = rep(t1);
            mulmod_precon_t T1pinv = PrepMulModPrecon(T1, p, pinv); // ((double) T1)*pinv;

            clear(M[i][k]);

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

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

               T2 = MulModPrecon(rep(*y), T1, p, T1pinv);
               T2 = AddMod(T2, rep(*x), p);
               (*x).LoopHole() = T2;
            }
         }

         l++;
      }
   }

   return l;
}

long gauss(mat_zz_p& M)
{
   return gauss(M, M.NumCols());
}

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

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

   mat_zz_p M;
   long r;

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

   X.SetDims(m-r, m);

   long i, j, k, s;
   zz_p t1, t2;

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

   vec_zz_p inverses;
   inverses.SetLength(m);

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

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

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

            clear(t1);

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

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

void diag(mat_zz_p& X, long n, zz_p d)
{
   X.SetDims(n, n);
   long i, j;

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

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

   long i, j;

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

   return 1;
}

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

   X.SetDims(n, m);

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

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

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

   return 1;
}

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

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

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

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

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

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

vec_zz_p operator*(const vec_zz_p& a, const mat_zz_p& b)
{
   vec_zz_p res;
   mul(res, a, b);
   NTL_OPT_RETURN(vec_zz_p, res);
}

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

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

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

   mat_zz_p T1, T2;
   long i, k;

   k = NumBits(e);
   T1 = A;

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

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

NTL_END_IMPL


/*
  [auto_generated]
  boost/numeric/odeint/stepper/bulirsch_stoer.hpp

  [begin_description]
  Implementation of the Burlish-Stoer method. As described in
  Ernst Hairer, Syvert Paul Norsett, Gerhard Wanner
  Solving Ordinary Differential Equations I. Nonstiff Problems.
  Springer Series in Comput. Mathematics, Vol. 8, Springer-Verlag 1987, Second revised edition 1993.
  [end_description]

  Copyright 2011-2013 Mario Mulansky
  Copyright 2011-2013 Karsten Ahnert
  Copyright 2012 Christoph Koke

  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_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_HPP_INCLUDED

#include <iostream>

#include <algorithm>

#include <boost/config.hpp> // for min/max guidelines

#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>

#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/modified_midpoint.hpp>
#include <boost/numeric/odeint/stepper/controlled_step_result.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/unit_helper.hpp>
#include <boost/numeric/odeint/util/detail/less_with_sign.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template<
    class State ,
    class Value = double ,
    class Deriv = State ,
    class Time = Value ,
    class Algebra = typename algebra_dispatcher< State >::algebra_type ,
    class Operations = typename operations_dispatcher< State >::operations_type ,
    class Resizer = initially_resizer
    >
class bulirsch_stoer {

public:

    typedef State state_type;
    typedef Value value_type;
    typedef Deriv deriv_type;
    typedef Time time_type;
    typedef Algebra algebra_type;
    typedef Operations operations_type;
    typedef Resizer resizer_type;
#ifndef DOXYGEN_SKIP
    typedef state_wrapper< state_type > wrapped_state_type;
    typedef state_wrapper< deriv_type > wrapped_deriv_type;
    typedef controlled_stepper_tag stepper_category;

    typedef bulirsch_stoer< State , Value , Deriv , Time , Algebra , Operations , Resizer > controlled_error_bs_type;

    typedef typename inverse_time< time_type >::type inv_time_type;

    typedef std::vector< value_type > value_vector;
    typedef std::vector< time_type > time_vector;
    typedef std::vector< inv_time_type > inv_time_vector;  //should be 1/time_type for boost.units
    typedef std::vector< value_vector > value_matrix;
    typedef std::vector< size_t > int_vector;
    typedef std::vector< wrapped_state_type > state_table_type;
#endif //DOXYGEN_SKIP
    const static size_t m_k_max = 8;

    bulirsch_stoer(
        value_type eps_abs = 1E-6 , value_type eps_rel = 1E-6 ,
        value_type factor_x = 1.0 , value_type factor_dxdt = 1.0 ,
        time_type max_dt = static_cast<time_type>(0))
        : m_error_checker( eps_abs , eps_rel , factor_x, factor_dxdt ) , m_midpoint() ,
          m_last_step_rejected( false ) , m_first( true ) ,
          m_max_dt(max_dt) ,
          m_interval_sequence( m_k_max+1 ) ,
          m_coeff( m_k_max+1 ) ,
          m_cost( m_k_max+1 ) ,
          m_facmin_table( m_k_max+1 ) ,
          m_table( m_k_max ) ,
          STEPFAC1( 0.65 ) , STEPFAC2( 0.94 ) , STEPFAC3( 0.02 ) , STEPFAC4( 4.0 ) , KFAC1( 0.8 ) , KFAC2( 0.9 )
    {
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        /* initialize sequence of stage numbers and work */
        for( unsigned short i = 0; i < m_k_max+1; i++ )
        {
            m_interval_sequence[i] = 2 * (i+1);
            if( i == 0 )
                m_cost[i] = m_interval_sequence[i];
            else
                m_cost[i] = m_cost[i-1] + m_interval_sequence[i];
            m_coeff[i].resize(i);
            m_facmin_table[i] = pow BOOST_PREVENT_MACRO_SUBSTITUTION( STEPFAC3 , static_cast< value_type >(1) / static_cast< value_type >( 2*i+1 ) );
            for( size_t k = 0 ; k < i ; ++k  )
            {
                const value_type r = static_cast< value_type >( m_interval_sequence[i] ) / static_cast< value_type >( m_interval_sequence[k] );
                m_coeff[i][k] = 1.0 / ( r*r - static_cast< value_type >( 1.0 ) ); // coefficients for extrapolation
            }
        }
        reset();
    }

    /*
     * Version 1 : try_step( sys , x , t , dt )
     *
     * The overloads are needed to solve the forwarding problem
     */
    template< class System , class StateInOut >
    controlled_step_result try_step( System system , StateInOut &x , time_type &t , time_type &dt )
    {
        return try_step_v1( system , x , t, dt );
    }

    /**
     * \brief Second version to solve the forwarding problem, can be used with Boost.Range as StateInOut.
     */
    template< class System , class StateInOut >
    controlled_step_result try_step( System system , const StateInOut &x , time_type &t , time_type &dt )
    {
        return try_step_v1( system , x , t, dt );
    }

    /*
     * Version 2 : try_step( sys , x , dxdt , t , dt )
     *
     * this version does not solve the forwarding problem, boost.range can not be used
     */
    template< class System , class StateInOut , class DerivIn >
    controlled_step_result try_step( System system , StateInOut &x , const DerivIn &dxdt , time_type &t , time_type &dt )
    {
        m_xnew_resizer.adjust_size( x , detail::bind( &controlled_error_bs_type::template resize_m_xnew< StateInOut > , detail::ref( *this ) , detail::_1 ) );
        controlled_step_result res = try_step( system , x , dxdt , t , m_xnew.m_v , dt );
        if( res == success )
        {
            boost::numeric::odeint::copy( m_xnew.m_v , x );
        }
        return res;
    }

    /*
     * Version 3 : try_step( sys , in , t , out , dt )
     *
     * this version does not solve the forwarding problem, boost.range can not be used
     */
    template< class System , class StateIn , class StateOut >
    typename boost::disable_if< boost::is_same< StateIn , time_type > , controlled_step_result >::type
    try_step( System system , const StateIn &in , time_type &t , StateOut &out , time_type &dt )
    {
        typename odeint::unwrap_reference< System >::type &sys = system;
        m_dxdt_resizer.adjust_size( in , detail::bind( &controlled_error_bs_type::template resize_m_dxdt< StateIn > , detail::ref( *this ) , detail::_1 ) );
        sys( in , m_dxdt.m_v , t );
        return try_step( system , in , m_dxdt.m_v , t , out , dt );
    }

    /*
     * Full version : try_step( sys , in , dxdt_in , t , out , dt )
     *
     * contains the actual implementation
     */
    template< class System , class StateIn , class DerivIn , class StateOut >
    controlled_step_result try_step( System system , const StateIn &in , const DerivIn &dxdt , time_type &t , StateOut &out , time_type &dt )
    {
        if( m_max_dt != static_cast<time_type>(0) && detail::less_with_sign(m_max_dt, dt, dt) )
        {
            // given step size is bigger then max_dt
            // set limit and return fail
            dt = m_max_dt;
            return fail;
        }

        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();

        static const value_type val1( 1.0 );

        if( m_resizer.adjust_size( in , detail::bind( &controlled_error_bs_type::template resize_impl< StateIn > , detail::ref( *this ) , detail::_1 ) ) )
        {
            reset(); // system resized -> reset
        }

        if( dt != m_dt_last )
        {
            reset(); // step size changed from outside -> reset
        }

        bool reject( true );

        time_vector h_opt( m_k_max+1 );
        inv_time_vector work( m_k_max+1 );

        time_type new_h = dt;

        /* m_current_k_opt is the estimated current optimal stage number */
        for( size_t k = 0 ; k <= m_current_k_opt+1 ; k++ )
        {
            /* the stage counts are stored in m_interval_sequence */
            m_midpoint.set_steps( m_interval_sequence[k] );
            if( k == 0 )
            {
                m_midpoint.do_step( system , in , dxdt , t , out , dt );
                /* the first step, nothing more to do */
            }
            else
            {
                m_midpoint.do_step( system , in , dxdt , t , m_table[k-1].m_v , dt );
                extrapolate( k , m_table , m_coeff , out );
                // get error estimate
                m_algebra.for_each3( m_err.m_v , out , m_table[0].m_v ,
                                     typename operations_type::template scale_sum2< value_type , value_type >( val1 , -val1 ) );
                const value_type error = m_error_checker.error( m_algebra , in , dxdt , m_err.m_v , dt );
                h_opt[k] = calc_h_opt( dt , error , k );
                work[k] = static_cast<value_type>( m_cost[k] ) / h_opt[k];

                if( (k == m_current_k_opt-1) || m_first )
                { // convergence before k_opt ?
                    if( error < 1.0 )
                    {
                        //convergence
                        reject = false;
                        if( (work[k] < KFAC2*work[k-1]) || (m_current_k_opt <= 2) )
                        {
                            // leave order as is (except we were in first round)
                            m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(k)+1 ) );
                            new_h = h_opt[k];
                            new_h *= static_cast<value_type>( m_cost[k+1] ) / static_cast<value_type>( m_cost[k] );
                        } else {
                            m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(k) ) );
                            new_h = h_opt[k];
                        }
                        break;
                    }
                    else if( should_reject( error , k ) && !m_first )
                    {
                        reject = true;
                        new_h = h_opt[k];
                        break;
                    }
                }
                if( k == m_current_k_opt )
                { // convergence at k_opt ?
                    if( error < 1.0 )
                    {
                        //convergence
                        reject = false;
                        if( (work[k-1] < KFAC2*work[k]) )
                        {
                            m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(m_current_k_opt)-1 );
                            new_h = h_opt[m_current_k_opt];
                        }
                        else if( (work[k] < KFAC2*work[k-1]) && !m_last_step_rejected )
                        {
                            m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max-1) , static_cast<int>(m_current_k_opt)+1 );
                            new_h = h_opt[k];
                            new_h *= static_cast<value_type>(m_cost[m_current_k_opt])/static_cast<value_type>(m_cost[k]);
                        } else
                            new_h = h_opt[m_current_k_opt];
                        break;
                    }
                    else if( should_reject( error , k ) )
                    {
                        reject = true;
                        new_h = h_opt[m_current_k_opt];
                        break;
                    }
                }
                if( k == m_current_k_opt+1 )
                { // convergence at k_opt+1 ?
                    if( error < 1.0 )
                    {   //convergence
                        reject = false;
                        if( work[k-2] < KFAC2*work[k-1] )
                            m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(m_current_k_opt)-1 );
                        if( (work[k] < KFAC2*work[m_current_k_opt]) && !m_last_step_rejected )
                            m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , static_cast<int>(k) );
                        new_h = h_opt[m_current_k_opt];
                    } else
                    {
                        reject = true;
                        new_h = h_opt[m_current_k_opt];
                    }
                    break;
                }
            }
        }

        if( !reject )
        {
            t += dt;
        }

        if( !m_last_step_rejected || boost::numeric::odeint::detail::less_with_sign(new_h, dt, dt) )
        {
            // limit step size
            if( m_max_dt != static_cast<time_type>(0) )
            {
                new_h = detail::min_abs(m_max_dt, new_h);
            }
            m_dt_last = new_h;
            dt = new_h;
        }

        m_last_step_rejected = reject;
        m_first = false;

        if( reject )
            return fail;
        else
            return success;
    }

    /** \brief Resets the internal state of the stepper */
    void reset()
    {
        m_first = true;
        m_last_step_rejected = false;
        // crude estimate of optimal order
        m_current_k_opt = 4;
        /* no calculation because log10 might not exist for value_type!
        const value_type logfact( -log10( max BOOST_PREVENT_MACRO_SUBSTITUTION( eps_rel , static_cast< value_type >(1.0E-12) ) ) * 0.6 + 0.5 );
        m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<value_type>( 1 ) , min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<value_type>( m_k_max-1 ) , logfact ));
        */
    }

    /* Resizer methods */

    template< class StateIn >
    void adjust_size( const StateIn &x )
    {
        resize_m_dxdt( x );
        resize_m_xnew( x );
        resize_impl( x );
        m_midpoint.adjust_size( x );
    }

private:

    template< class StateIn >
    bool resize_m_dxdt( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
    }

    template< class StateIn >
    bool resize_m_xnew( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_xnew , x , typename is_resizeable<state_type>::type() );
    }

    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        bool resized( false );
        for( size_t i = 0 ; i < m_k_max ; ++i )
            resized |= adjust_size_by_resizeability( m_table[i] , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_err , x , typename is_resizeable<state_type>::type() );
        return resized;
    }

    template< class System , class StateInOut >
    controlled_step_result try_step_v1( System system , StateInOut &x , time_type &t , time_type &dt )
    {
        typename odeint::unwrap_reference< System >::type &sys = system;
        m_dxdt_resizer.adjust_size( x , detail::bind( &controlled_error_bs_type::template resize_m_dxdt< StateInOut > , detail::ref( *this ) , detail::_1 ) );
        sys( x , m_dxdt.m_v ,t );
        return try_step( system , x , m_dxdt.m_v , t , dt );
    }

    template< class StateInOut >
    void extrapolate( size_t k , state_table_type &table , const value_matrix &coeff , StateInOut &xest )
    /* polynomial extrapolation, see http://www.nr.com/webnotes/nr3web21.pdf
       uses the obtained intermediate results to extrapolate to dt->0
    */
    {
        static const value_type val1 = static_cast< value_type >( 1.0 );
        for( int j=k-1 ; j>0 ; --j )
        {
            m_algebra.for_each3( table[j-1].m_v , table[j].m_v , table[j-1].m_v ,
                                 typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k][j] , -coeff[k][j] ) );
        }
        m_algebra.for_each3( xest , table[0].m_v , xest ,
                             typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k][0] , -coeff[k][0]) );
    }

    time_type calc_h_opt( time_type h , value_type error , size_t k ) const
    /* calculates the optimal step size for a given error and stage number */
    {
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        using std::pow;
        value_type expo( 1.0/(2*k+1) );
        value_type facmin = m_facmin_table[k];
        value_type fac;
        if (error == 0.0)
            fac=1.0/facmin;
        else
        {
            fac = STEPFAC2 / pow BOOST_PREVENT_MACRO_SUBSTITUTION( error / STEPFAC1 , expo );
            fac = max BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<value_type>(facmin/STEPFAC4) , min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<value_type>(1.0/facmin) , fac ) );
        }
        return h*fac;
    }

    controlled_step_result set_k_opt( size_t k , const inv_time_vector &work , const time_vector &h_opt , time_type &dt )
    /* calculates the optimal stage number */
    {
        if( k == 1 )
        {
            m_current_k_opt = 2;
            return success;
        }
        if( (work[k-1] < KFAC1*work[k]) || (k == m_k_max) )
        {   // order decrease
            m_current_k_opt = k-1;
            dt = h_opt[ m_current_k_opt ];
            return success;
        }
        else if( (work[k] < KFAC2*work[k-1]) || m_last_step_rejected || (k == m_k_max-1) )
        {   // same order - also do this if last step got rejected
            m_current_k_opt = k;
            dt = h_opt[ m_current_k_opt ];
            return success;
        }
        else
        {   // order increase - only if last step was not rejected
            m_current_k_opt = k+1;
            dt = h_opt[ m_current_k_opt-1 ] * m_cost[ m_current_k_opt ] / m_cost[ m_current_k_opt-1 ] ;
            return success;
        }
    }

    bool in_convergence_window( size_t k ) const
    {
        if( (k == m_current_k_opt-1) && !m_last_step_rejected )
            return true; // decrease stepsize only if last step was not rejected
        return ( (k == m_current_k_opt) || (k == m_current_k_opt+1) );
    }

    bool should_reject( value_type error , size_t k ) const
    {
        if( k == m_current_k_opt-1 )
        {
            const value_type d = m_interval_sequence[m_current_k_opt] * m_interval_sequence[m_current_k_opt+1] /
                (m_interval_sequence[0]*m_interval_sequence[0]);
            //step will fail, criterion 17.3.17 in NR
            return ( error > d*d );
        }
        else if( k == m_current_k_opt )
        {
            const value_type d = m_interval_sequence[m_current_k_opt] / m_interval_sequence[0];
            return ( error > d*d );
        } else
            return error > 1.0;
    }

    default_error_checker< value_type, algebra_type , operations_type > m_error_checker;
    modified_midpoint< state_type , value_type , deriv_type , time_type , algebra_type , operations_type , resizer_type > m_midpoint;

    bool m_last_step_rejected;
    bool m_first;

    time_type m_dt_last;
    time_type m_t_last;
    time_type m_max_dt;

    size_t m_current_k_opt;

    algebra_type m_algebra;

    resizer_type m_dxdt_resizer;
    resizer_type m_xnew_resizer;
    resizer_type m_resizer;

    wrapped_state_type m_xnew;
    wrapped_state_type m_err;
    wrapped_deriv_type m_dxdt;

    int_vector m_interval_sequence; // stores the successive interval counts
    value_matrix m_coeff;
    int_vector m_cost; // costs for interval count
    value_vector m_facmin_table; // for precomputed facmin to save pow calls

    state_table_type m_table; // sequence of states for extrapolation

    value_type STEPFAC1 , STEPFAC2 , STEPFAC3 , STEPFAC4 , KFAC1 , KFAC2;
};

/******** DOXYGEN ********/
/**
 * \class bulirsch_stoer
 * \brief The Bulirsch-Stoer algorithm.
 *
 * The Bulirsch-Stoer is a controlled stepper that adjusts both step size
 * and order of the method. The algorithm uses the modified midpoint and
 * a polynomial extrapolation compute the solution.
 *
 * \tparam State The state type.
 * \tparam Value The value type.
 * \tparam Deriv The type representing the time derivative of the state.
 * \tparam Time The time representing the independent variable - the time.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 */

    /**
     * \fn bulirsch_stoer::bulirsch_stoer( value_type eps_abs , value_type eps_rel , value_type factor_x , value_type factor_dxdt )
     * \brief Constructs the bulirsch_stoer class, including initialization of
     * the error bounds.
     *
     * \param eps_abs Absolute tolerance level.
     * \param eps_rel Relative tolerance level.
     * \param factor_x Factor for the weight of the state.
     * \param factor_dxdt Factor for the weight of the derivative.
     */

    /**
     * \fn bulirsch_stoer::try_step( System system , StateInOut &x , time_type &t , time_type &dt )
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed. Also, the internal order of the stepper is adjusted if required.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE.
     * It must fulfill the Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */

    /**
     * \fn bulirsch_stoer::try_step( System system , StateInOut &x , const DerivIn &dxdt , time_type &t , time_type &dt )
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed. Also, the internal order of the stepper is adjusted if required.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE.
     * It must fulfill the Simple System concept.
     * \param x The state of the ODE which should be solved. Overwritten if
     * the step is successful.
     * \param dxdt The derivative of state.
     * \param t The value of the time. Updated if the step is successful.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */

    /**
     * \fn bulirsch_stoer::try_step( System system , const StateIn &in , time_type &t , StateOut &out , time_type &dt )
     * \brief Tries to perform one step.
     *
     * \note This method is disabled if state_type=time_type to avoid ambiguity.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed. Also, the internal order of the stepper is adjusted if required.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE.
     * It must fulfill the Simple System concept.
     * \param in The state of the ODE which should be solved.
     * \param t The value of the time. Updated if the step is successful.
     * \param out Used to store the result of the step.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */

    /**
     * \fn bulirsch_stoer::try_step( System system , const StateIn &in , const DerivIn &dxdt , time_type &t , StateOut &out , time_type &dt )
     * \brief Tries to perform one step.
     *
     * This method tries to do one step with step size dt. If the error estimate
     * is to large, the step is rejected and the method returns fail and the
     * step size dt is reduced. If the error estimate is acceptably small, the
     * step is performed, success is returned and dt might be increased to make
     * the steps as large as possible. This method also updates t if a step is
     * performed. Also, the internal order of the stepper is adjusted if required.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE.
     * It must fulfill the Simple System concept.
     * \param in The state of the ODE which should be solved.
     * \param dxdt The derivative of state.
     * \param t The value of the time. Updated if the step is successful.
     * \param out Used to store the result of the step.
     * \param dt The step size. Updated.
     * \return success if the step was accepted, fail otherwise.
     */

    /**
     * \fn bulirsch_stoer::adjust_size( const StateIn &x )
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */

}
}
}

#endif // BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_HPP_INCLUDED


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Constant \f$\frac4\pi\f$.

    @par Header <boost/simd/constant/fouropi.hpp>

    @par Semantic:

    For type T:

    @code
    T r = Fouropi<T>();
    @endcode

    is similar to:

    @code
    T r = Four<T>()/Pi<T>();
    @endcode

    @return a value of type T

**/
  template<typename T> T Fouropi();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Constant \f$\frac4\pi\f$.

      Generate the  constant fouropi.

      @return The Fouropi constant for the proper type
    **/
    Value Fouropi();
  }
} }
#endif

#include <boost/simd/constant/scalar/fouropi.hpp>
#include <boost/simd/constant/simd/fouropi.hpp>

#endif


//# /Users/arnold/Documents/prg/clang/bin/clang++ -U__STRICT_ANSI__ -std=c++14 -I/Users/arnold/Documents/prg/libraries/boost_1_56_0/ -I/Developer/SDKs/MacOSX10.6.sdk/usr/include -I/Users/arnold/Documents/prg/libraries/cryptopp563/include -Wall -O2 -L/Users/arnold/Documents/prg/libraries/boost_1_56_0/stage/lib -L/Users/arnold/Documents/prg/libraries/cryptopp563/lib -o apfel2 apfel2.cpp -lboost_system -lboost_filesystem -lboost_iostreams -lcryptopp

#include <iostream>
#include <boost/filesystem.hpp>
#include <vector>
#include <string>
#include <array>
#include <algorithm>
#include <complex>
#include <boost/format.hpp>
#include <future>
#include <utility>

using namespace std;

template<typename T>
struct window {
	window(const T& xmin, const T& ymin, const T& xmax, const T& ymax)
		: xmin_{xmin}, ymin_{ymin}, xmax_{xmax}, ymax_{ymax}
	{
		if (xmin_ > xmax_)
			swap(xmin_, xmax_);
		if (ymin_ > ymax_)
			swap(ymin_, ymax_);
	}
	window() = default;
	T width() const { return xmax_ - xmin_; }
	T height() const { return ymax_ - ymin_; }
	void zoom(const double& fx, const double& fy) {
		//double ffx = 1.0/fx;
		//double ffy = 1.0/fy;
		double dx = width() * fx/2.0;
		double dy = height() * fy/2.0;
		xmin_ += dx;
		xmax_ -= dx;
		ymin_ += dy;
		ymax_ -= dy;
	}
	void zoom(const double& f) {
		zoom(f, f);
/*
		double ff = 1.0/f;
		double dx = width() * ff/2.0;
		double dy = height() * ff/2.0;
		xmin_ += dx;
		xmax_ -= dx;
		ymin_ += dy;
		ymax_ -= dy;*/
	}
	void move(const double& h, const double& v) {
		double dx = width() * h;
		double dy = height() * v;
		xmin_ += dx;
		xmax_ += dx;
		ymin_ += dy;
		ymax_ += dy;
	}
	T xmin_ {-2};
	T ymin_ {-1.5};
	T xmax_ {1};
	T ymax_ {1.5};
};

struct fract {
	using it_t = unsigned;
	window<double>     imgrange_;
	window<unsigned>   dim_;

};

template<typename T>
ostream& operator<<(ostream& o, const window<T>& w) {
	o << boost::format{"(%1.15f, %1.15f)-(%1.15f, %1.15f)"} % w.xmin_ % w.ymin_ % w.xmax_ % w.ymax_;
	return o;
}

struct pxmap {
	pxmap(unsigned maxit, unsigned width, unsigned height)
		: maxit_{maxit+1}, width_{width}, height_{height}, px_(width_ * height_)
	{}
	unsigned maxit_;
	unsigned width_;
	unsigned height_;
	vector<unsigned> px_;
	static const array<char, 14> ch_; // = {{ ' ', '.', ',', ':', ';', '+', 'i', 'I', '%', 'H', '8', 'M', '#'}};
	void print() {
		unsigned pn = 0;
		for(const auto& p : px_) {
			cout << ch_[static_cast<double>(p)/maxit_ * pxmap::ch_.size()];
			if (++pn % width_ == 0) {
				cout << endl;
			}
		}
		cout << endl;
	}
};

constexpr const array<char, 14> pxmap::ch_ = {{ ' ', '.', ',', ':', ';', '+', 'i', 'I', '%', 'H', '8', 'M', '#', '*'}};

int main (int argc, char const *argv[])
{
	using cpt = complex<double>;
	double escval = 4.0;
	vector<string> params(argv, argv + argc);
//	cin >> noskipws;
	try {
//		window<unsigned> p(0, 100, 0, 20);
		cpt ca(1,0.5);
/*		cout << ca << endl;
		cout << ca * ca * ca << endl;
		cout << pow(ca,3) << endl;*/
		window<double> w;//(-1.5,0,0.5,1);
		unsigned maxit = 100;
		unsigned rows = 100;
		unsigned lines = rows * 1.0/2.0;
		double exp = 2.0;
		for(unsigned it = 0; true; ++it) {
			if (it > 0) {
				cout << w << " " << maxit << " esc:" << escval << " exp:" << exp << " zoom In, zoom Out, Left, Right, Up, Down, +iterate, -iterate, +Esc, -esc, +Wide, -wide, +Pow, -pow, Quit: ";
				string cc;
				getline(cin, cc);
				char c;
				c = cc.at(0);
				cout << endl;
				switch(c) {
					case 'q' :
						exit(0);
						break;
					case 'l':
						w.move(-0.25, 0);
						break;
					case 'r':
						w.move(0.25, 0);
						break;
					case 'u':
						w.move(0,0.25);
						break;
					case 'd':
						w.move(0,-0.25);
						break;
					case 'i':
						w.zoom(0.5);
						break;
					case 'o':
						w.zoom(-1);
						break;
					case '+':
						maxit *= 2.0;
						maxit = min(1u<<16,maxit);
						break;
					case '-':
						maxit /= 2.0;
						maxit = max(1u,maxit);
						break;
					case 'e':
						escval /= 2.0;
						escval = max(1.0, escval);
						break;
					case 'E':
						escval *= 2.0;
						escval = min(256.0, escval);
						break;
					case 'W':
						rows += rows * 0.25;
						w.zoom(-0.25, 0);
						break;
					case 'w':
						rows -= rows/4.0;
						w.zoom(0.25, 0);
						break;
					case 'P':
						exp += 0.1;
						exp = min(10.0, exp);
						break;
					case 'p':
						exp -= 0.1;
						exp = max(0.1, exp);
						break;
				}
			}
			double stepx = w.width() / static_cast<double>(rows);
			double stepy = w.height() / static_cast<double>(lines);
			cout << stepx << ", " << stepy << endl;
			//cout << endl;
			pxmap pm(maxit, rows, lines);
			auto pmit = pm.px_.begin();
			for(unsigned iy = 0; iy < lines; ++iy) {
				double y = w.ymax_ - stepy * iy;
			//for(double y = w.ymax_; y > w.ymin_; y -= stepy) {
				for(unsigned ix = 0; ix < rows; ++ix) {
					double x = w.xmin_ + stepx * ix;
				//for(double x = w.xmin_; x < w.xmax_; x += stepx) {
					unsigned i = 0;
					//bool isin = true;
					cpt c(0,0);
					cpt cc(x,y);
					for(; i < maxit; ++i) {
						//c = c * c + cc;
						c = pow(c, exp) + cc;
						if (c.real() * c.real() + c.imag() * c.imag() > escval) {
							//isin = false;
							break;
						}
					}
					*pmit = i;
					++pmit;
				}
				cout << endl;
			}
//			cout << "********************" << endl;
			pm.print();
		}
	} catch(...) {
		cerr << "Unknown exception" << endl;
	}
	return 0;
}