//  Copyright (c) 2006 Xiaogang Zhang
//  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_BESSEL_IK_HPP
#define BOOST_MATH_BESSEL_IK_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/round.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/sin_pi.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/tools/config.hpp>

// Modified Bessel functions of the first and second kind of fractional order

namespace boost { namespace math {

namespace detail {

template <class T, class Policy>
struct cyl_bessel_i_small_z
{
   typedef T result_type;

   cyl_bessel_i_small_z(T v_, T z_) : k(0), v(v_), mult(z_*z_/4)
   {
      BOOST_MATH_STD_USING
      term = 1;
   }

   T operator()()
   {
      T result = term;
      ++k;
      term *= mult / k;
      term /= k + v;
      return result;
   }
private:
   unsigned k;
   T v;
   T term;
   T mult;
};

template <class T, class Policy>
inline T bessel_i_small_z_series(T v, T x, const Policy& pol)
{
   BOOST_MATH_STD_USING
   T prefix;
   if(v < max_factorial<T>::value)
   {
      prefix = pow(x / 2, v) / boost::math::tgamma(v + 1, pol);
   }
   else
   {
      prefix = v * log(x / 2) - boost::math::lgamma(v + 1, pol);
      prefix = exp(prefix);
   }
   if(prefix == 0)
      return prefix;

   cyl_bessel_i_small_z<T, Policy> s(v, x);
   boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
   T zero = 0;
   T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter, zero);
#else
   T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
#endif
   policies::check_series_iterations<T>("boost::math::bessel_j_small_z_series<%1%>(%1%,%1%)", max_iter, pol);
   return prefix * result;
}

// Calculate K(v, x) and K(v+1, x) by method analogous to
// Temme, Journal of Computational Physics, vol 21, 343 (1976)
template <typename T, typename Policy>
int temme_ik(T v, T x, T* K, T* K1, const Policy& pol)
{
    T f, h, p, q, coef, sum, sum1, tolerance;
    T a, b, c, d, sigma, gamma1, gamma2;
    unsigned long k;

    BOOST_MATH_STD_USING
    using namespace boost::math::tools;
    using namespace boost::math::constants;

    // |x| <= 2, Temme series converge rapidly
    // |x| > 2, the larger the |x|, the slower the convergence
    BOOST_ASSERT(abs(x) <= 2);
    BOOST_ASSERT(abs(v) <= 0.5f);

    T gp = boost::math::tgamma1pm1(v, pol);
    T gm = boost::math::tgamma1pm1(-v, pol);

    a = log(x / 2);
    b = exp(v * a);
    sigma = -a * v;
    c = abs(v) < tools::epsilon<T>() ?
       T(1) : T(boost::math::sin_pi(v) / (v * pi<T>()));
    d = abs(sigma) < tools::epsilon<T>() ?
        T(1) : T(sinh(sigma) / sigma);
    gamma1 = abs(v) < tools::epsilon<T>() ?
        T(-euler<T>()) : T((0.5f / v) * (gp - gm) * c);
    gamma2 = (2 + gp + gm) * c / 2;

    // initial values
    p = (gp + 1) / (2 * b);
    q = (1 + gm) * b / 2;
    f = (cosh(sigma) * gamma1 + d * (-a) * gamma2) / c;
    h = p;
    coef = 1;
    sum = coef * f;
    sum1 = coef * h;

    BOOST_MATH_INSTRUMENT_VARIABLE(p);
    BOOST_MATH_INSTRUMENT_VARIABLE(q);
    BOOST_MATH_INSTRUMENT_VARIABLE(f);
    BOOST_MATH_INSTRUMENT_VARIABLE(sigma);
    BOOST_MATH_INSTRUMENT_CODE(sinh(sigma));
    BOOST_MATH_INSTRUMENT_VARIABLE(gamma1);
    BOOST_MATH_INSTRUMENT_VARIABLE(gamma2);
    BOOST_MATH_INSTRUMENT_VARIABLE(c);
    BOOST_MATH_INSTRUMENT_VARIABLE(d);
    BOOST_MATH_INSTRUMENT_VARIABLE(a);

    // series summation
    tolerance = tools::epsilon<T>();
    for (k = 1; k < policies::get_max_series_iterations<Policy>(); k++)
    {
        f = (k * f + p + q) / (k*k - v*v);
        p /= k - v;
        q /= k + v;
        h = p - k * f;
        coef *= x * x / (4 * k);
        sum += coef * f;
        sum1 += coef * h;
        if (abs(coef * f) < abs(sum) * tolerance)
        {
           break;
        }
    }
    policies::check_series_iterations<T>("boost::math::bessel_ik<%1%>(%1%,%1%) in temme_ik", k, pol);

    *K = sum;
    *K1 = 2 * sum1 / x;

    return 0;
}

// Evaluate continued fraction fv = I_(v+1) / I_v, derived from
// Abramowitz and Stegun, Handbook of Mathematical Functions, 1972, 9.1.73
template <typename T, typename Policy>
int CF1_ik(T v, T x, T* fv, const Policy& pol)
{
    T C, D, f, a, b, delta, tiny, tolerance;
    unsigned long k;

    BOOST_MATH_STD_USING

    // |x| <= |v|, CF1_ik converges rapidly
    // |x| > |v|, CF1_ik needs O(|x|) iterations to converge

    // modified Lentz's method, see
    // Lentz, Applied Optics, vol 15, 668 (1976)
    tolerance = 2 * tools::epsilon<T>();
    BOOST_MATH_INSTRUMENT_VARIABLE(tolerance);
    tiny = sqrt(tools::min_value<T>());
    BOOST_MATH_INSTRUMENT_VARIABLE(tiny);
    C = f = tiny;                           // b0 = 0, replace with tiny
    D = 0;
    for (k = 1; k < policies::get_max_series_iterations<Policy>(); k++)
    {
        a = 1;
        b = 2 * (v + k) / x;
        C = b + a / C;
        D = b + a * D;
        if (C == 0) { C = tiny; }
        if (D == 0) { D = tiny; }
        D = 1 / D;
        delta = C * D;
        f *= delta;
        BOOST_MATH_INSTRUMENT_VARIABLE(delta-1);
        if (abs(delta - 1) <= tolerance)
        {
           break;
        }
    }
    BOOST_MATH_INSTRUMENT_VARIABLE(k);
    policies::check_series_iterations<T>("boost::math::bessel_ik<%1%>(%1%,%1%) in CF1_ik", k, pol);

    *fv = f;

    return 0;
}

// Calculate K(v, x) and K(v+1, x) by evaluating continued fraction
// z1 / z0 = U(v+1.5, 2v+1, 2x) / U(v+0.5, 2v+1, 2x), see
// Thompson and Barnett, Computer Physics Communications, vol 47, 245 (1987)
template <typename T, typename Policy>
int CF2_ik(T v, T x, T* Kv, T* Kv1, const Policy& pol)
{
    BOOST_MATH_STD_USING
    using namespace boost::math::constants;

    T S, C, Q, D, f, a, b, q, delta, tolerance, current, prev;
    unsigned long k;

    // |x| >= |v|, CF2_ik converges rapidly
    // |x| -> 0, CF2_ik fails to converge

    BOOST_ASSERT(abs(x) > 1);

    // Steed's algorithm, see Thompson and Barnett,
    // Journal of Computational Physics, vol 64, 490 (1986)
    tolerance = tools::epsilon<T>();
    a = v * v - 0.25f;
    b = 2 * (x + 1);                              // b1
    D = 1 / b;                                    // D1 = 1 / b1
    f = delta = D;                                // f1 = delta1 = D1, coincidence
    prev = 0;                                     // q0
    current = 1;                                  // q1
    Q = C = -a;                                   // Q1 = C1 because q1 = 1
    S = 1 + Q * delta;                            // S1
    BOOST_MATH_INSTRUMENT_VARIABLE(tolerance);
    BOOST_MATH_INSTRUMENT_VARIABLE(a);
    BOOST_MATH_INSTRUMENT_VARIABLE(b);
    BOOST_MATH_INSTRUMENT_VARIABLE(D);
    BOOST_MATH_INSTRUMENT_VARIABLE(f);

    for (k = 2; k < policies::get_max_series_iterations<Policy>(); k++)     // starting from 2
    {
        // continued fraction f = z1 / z0
        a -= 2 * (k - 1);
        b += 2;
        D = 1 / (b + a * D);
        delta *= b * D - 1;
        f += delta;

        // series summation S = 1 + \sum_{n=1}^{\infty} C_n * z_n / z_0
        q = (prev - (b - 2) * current) / a;
        prev = current;
        current = q;                        // forward recurrence for q
        C *= -a / k;
        Q += C * q;
        S += Q * delta;
        //
        // Under some circumstances q can grow very small and C very
        // large, leading to under/overflow.  This is particularly an
        // issue for types which have many digits precision but a narrow
        // exponent range.  A typical example being a "double double" type.
        // To avoid this situation we can normalise q (and related prev/current)
        // and C.  All other variables remain unchanged in value.  A typical
        // test case occurs when x is close to 2, for example cyl_bessel_k(9.125, 2.125).
        //
        if(q < tools::epsilon<T>())
        {
           C *= q;
           prev /= q;
           current /= q;
           q = 1;
        }

        // S converges slower than f
        BOOST_MATH_INSTRUMENT_VARIABLE(Q * delta);
        BOOST_MATH_INSTRUMENT_VARIABLE(abs(S) * tolerance);
        BOOST_MATH_INSTRUMENT_VARIABLE(S);
        if (abs(Q * delta) < abs(S) * tolerance)
        {
           break;
        }
    }
    policies::check_series_iterations<T>("boost::math::bessel_ik<%1%>(%1%,%1%) in CF2_ik", k, pol);

    if(x >= tools::log_max_value<T>())
       *Kv = exp(0.5f * log(pi<T>() / (2 * x)) - x - log(S));
    else
      *Kv = sqrt(pi<T>() / (2 * x)) * exp(-x) / S;
    *Kv1 = *Kv * (0.5f + v + x + (v * v - 0.25f) * f) / x;
    BOOST_MATH_INSTRUMENT_VARIABLE(*Kv);
    BOOST_MATH_INSTRUMENT_VARIABLE(*Kv1);

    return 0;
}

enum{
   need_i = 1,
   need_k = 2
};

// Compute I(v, x) and K(v, x) simultaneously by Temme's method, see
// Temme, Journal of Computational Physics, vol 19, 324 (1975)
template <typename T, typename Policy>
int bessel_ik(T v, T x, T* I, T* K, int kind, const Policy& pol)
{
    // Kv1 = K_(v+1), fv = I_(v+1) / I_v
    // Ku1 = K_(u+1), fu = I_(u+1) / I_u
    T u, Iv, Kv, Kv1, Ku, Ku1, fv;
    T W, current, prev, next;
    bool reflect = false;
    unsigned n, k;
    int org_kind = kind;
    BOOST_MATH_INSTRUMENT_VARIABLE(v);
    BOOST_MATH_INSTRUMENT_VARIABLE(x);
    BOOST_MATH_INSTRUMENT_VARIABLE(kind);

    BOOST_MATH_STD_USING
    using namespace boost::math::tools;
    using namespace boost::math::constants;

    static const char* function = "boost::math::bessel_ik<%1%>(%1%,%1%)";

    if (v < 0)
    {
        reflect = true;
        v = -v;                             // v is non-negative from here
        kind |= need_k;
    }
    n = iround(v, pol);
    u = v - n;                              // -1/2 <= u < 1/2
    BOOST_MATH_INSTRUMENT_VARIABLE(n);
    BOOST_MATH_INSTRUMENT_VARIABLE(u);

    if (x < 0)
    {
       *I = *K = policies::raise_domain_error<T>(function,
            "Got x = %1% but real argument x must be non-negative, complex number result not supported.", x, pol);
        return 1;
    }
    if (x == 0)
    {
       Iv = (v == 0) ? static_cast<T>(1) : static_cast<T>(0);
       if(kind & need_k)
       {
         Kv = policies::raise_overflow_error<T>(function, 0, pol);
       }
       else
       {
          Kv = std::numeric_limits<T>::quiet_NaN(); // any value will do
       }

       if(reflect && (kind & need_i))
       {
           T z = (u + n % 2);
           Iv = boost::math::sin_pi(z, pol) == 0 ?
               Iv :
               policies::raise_overflow_error<T>(function, 0, pol);   // reflection formula
       }

       *I = Iv;
       *K = Kv;
       return 0;
    }

    // x is positive until reflection
    W = 1 / x;                                 // Wronskian
    if (x <= 2)                                // x in (0, 2]
    {
        temme_ik(u, x, &Ku, &Ku1, pol);             // Temme series
    }
    else                                       // x in (2, \infty)
    {
        CF2_ik(u, x, &Ku, &Ku1, pol);               // continued fraction CF2_ik
    }
    BOOST_MATH_INSTRUMENT_VARIABLE(Ku);
    BOOST_MATH_INSTRUMENT_VARIABLE(Ku1);
    prev = Ku;
    current = Ku1;
    T scale = 1;
    for (k = 1; k <= n; k++)                   // forward recurrence for K
    {
        T fact = 2 * (u + k) / x;
        if((tools::max_value<T>() - fabs(prev)) / fact < fabs(current))
        {
           prev /= current;
           scale /= current;
           current = 1;
        }
        next = fact * current + prev;
        prev = current;
        current = next;
    }
    Kv = prev;
    Kv1 = current;
    BOOST_MATH_INSTRUMENT_VARIABLE(Kv);
    BOOST_MATH_INSTRUMENT_VARIABLE(Kv1);
    if(kind & need_i)
    {
       T lim = (4 * v * v + 10) / (8 * x);
       lim *= lim;
       lim *= lim;
       lim /= 24;
       if((lim < tools::epsilon<T>() * 10) && (x > 100))
       {
          // x is huge compared to v, CF1 may be very slow
          // to converge so use asymptotic expansion for large
          // x case instead.  Note that the asymptotic expansion
          // isn't very accurate - so it's deliberately very hard
          // to get here - probably we're going to overflow:
          Iv = asymptotic_bessel_i_large_x(v, x, pol);
       }
       else if((v > 0) && (x / v < 0.25))
       {
          Iv = bessel_i_small_z_series(v, x, pol);
       }
       else
       {
          CF1_ik(v, x, &fv, pol);                         // continued fraction CF1_ik
          Iv = scale * W / (Kv * fv + Kv1);                  // Wronskian relation
       }
    }
    else
       Iv = std::numeric_limits<T>::quiet_NaN(); // any value will do

    if (reflect)
    {
        T z = (u + n % 2);
        T fact = (2 / pi<T>()) * (boost::math::sin_pi(z) * Kv);
        if(fact == 0)
           *I = Iv;
        else if(tools::max_value<T>() * scale < fact)
           *I = (org_kind & need_i) ? T(sign(fact) * sign(scale) * policies::raise_overflow_error<T>(function, 0, pol)) : T(0);
        else
         *I = Iv + fact / scale;   // reflection formula
    }
    else
    {
        *I = Iv;
    }
    if(tools::max_value<T>() * scale < Kv)
      *K = (org_kind & need_k) ? T(sign(Kv) * sign(scale) * policies::raise_overflow_error<T>(function, 0, pol)) : T(0);
    else
      *K = Kv / scale;
    BOOST_MATH_INSTRUMENT_VARIABLE(*I);
    BOOST_MATH_INSTRUMENT_VARIABLE(*K);
    return 0;
}

}}} // namespaces

#endif // BOOST_MATH_BESSEL_IK_HPP


//
// Created by kevin on 3/12/18.
//
#include "dubins.h"

#include <queue>
#include <boost/math/constants/constants.hpp>

namespace {
    const double twopi = 2. * boost::math::constants::pi<double>();
    const double DUBINS_EPS = 1e-6;
    const double DUBINS_ZERO = -1e-7;

    inline double mod2pi(double x) {
        if (x < 0 && x > DUBINS_ZERO)
            return 0;
        double xm = x - twopi * floor(x / twopi);
        if (twopi - xm < .5 * DUBINS_EPS) xm = 0.;
        return xm;
    }

    DubinsStateSpace::DubinsPath dubinsLSL(double d, double alpha, double beta) {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = 2. + d * d - 2. * (ca * cb + sa * sb - d * (sa - sb));
        if (tmp >= DUBINS_ZERO) {
            double theta = atan2(cb - ca, d + sa - sb);
            double t = mod2pi(-alpha + theta);
            double p = sqrt(std::max(tmp, 0.));
            double q = mod2pi(beta - theta);
            assert(fabs(p * cos(alpha + t) - sa + sb - d) < 2 * DUBINS_EPS);
            assert(fabs(p * sin(alpha + t) + ca - cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha + t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[0], t, p, q);
        }
        return DubinsStateSpace::DubinsPath();
    }

    DubinsStateSpace::DubinsPath dubinsRSR(double d, double alpha, double beta) {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = 2. + d * d - 2. * (ca * cb + sa * sb - d * (sb - sa));
        if (tmp >= DUBINS_ZERO) {
            double theta = atan2(ca - cb, d - sa + sb);
            double t = mod2pi(alpha - theta);
            double p = sqrt(std::max(tmp, 0.));
            double q = mod2pi(-beta + theta);
            assert(fabs(p * cos(alpha - t) + sa - sb - d) < 2 * DUBINS_EPS);
            assert(fabs(p * sin(alpha - t) - ca + cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha - t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[1], t, p, q);
        }
        return DubinsStateSpace::DubinsPath();
    }

    DubinsStateSpace::DubinsPath dubinsRSL(double d, double alpha, double beta) {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = d * d - 2. + 2. * (ca * cb + sa * sb - d * (sa + sb));
        if (tmp >= DUBINS_ZERO) {
            double p = sqrt(std::max(tmp, 0.));
            double theta = atan2(ca + cb, d - sa - sb) - atan2(2., p);
            double t = mod2pi(alpha - theta);
            double q = mod2pi(beta - theta);
            assert(fabs(p * cos(alpha - t) - 2. * sin(alpha - t) + sa + sb - d) < 2 * DUBINS_EPS);
            assert(fabs(p * sin(alpha - t) + 2. * cos(alpha - t) - ca - cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha - t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[2], t, p, q);
        }
        return DubinsStateSpace::DubinsPath();
    }

    DubinsStateSpace::DubinsPath dubinsLSR(double d, double alpha, double beta) {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = -2. + d * d + 2. * (ca * cb + sa * sb + d * (sa + sb));
        if (tmp >= DUBINS_ZERO) {
            double p = sqrt(std::max(tmp, 0.));
            double theta = atan2(-ca - cb, d + sa + sb) - atan2(-2., p);
            double t = mod2pi(-alpha + theta);
            double q = mod2pi(-beta + theta);
            assert(fabs(p * cos(alpha + t) + 2. * sin(alpha + t) - sa - sb - d) < 2 * DUBINS_EPS);
            assert(fabs(p * sin(alpha + t) - 2. * cos(alpha + t) + ca + cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha + t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[3], t, p, q);
        }
        return DubinsStateSpace::DubinsPath();
    }

    DubinsStateSpace::DubinsPath dubinsRLR(double d, double alpha, double beta) {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb + d * (sa - sb)));
        if (fabs(tmp) < 1.) {
            double p = twopi - acos(tmp);
            double theta = atan2(ca - cb, d - sa + sb);
            double t = mod2pi(alpha - theta + .5 * p);
            double q = mod2pi(alpha - beta - t + p);
            assert(fabs(2. * sin(alpha - t + p) - 2. * sin(alpha - t) - d + sa - sb) < 2 * DUBINS_EPS);
            assert(fabs(-2. * cos(alpha - t + p) + 2. * cos(alpha - t) - ca + cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha - t + p - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[4], t, p, q);
        }
        return DubinsStateSpace::DubinsPath();
    }

    DubinsStateSpace::DubinsPath dubinsLRL(double d, double alpha, double beta) {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb - d * (sa - sb)));
        if (fabs(tmp) < 1.) {
            double p = twopi - acos(tmp);
            double theta = atan2(-ca + cb, d + sa - sb);
            double t = mod2pi(-alpha + theta + .5 * p);
            double q = mod2pi(beta - alpha - t + p);
            assert(fabs(-2. * sin(alpha + t - p) + 2. * sin(alpha + t) - d - sa + sb) < 2 * DUBINS_EPS);
            assert(fabs(2. * cos(alpha + t - p) - 2. * cos(alpha + t) + ca - cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha + t - p + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[5], t, p, q);
        }
        return DubinsStateSpace::DubinsPath();
    }

    DubinsStateSpace::DubinsPath dubins(double d, double alpha, double beta) {
        if (d < DUBINS_EPS && fabs(alpha - beta) < DUBINS_EPS)
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[0], 0, d, 0);

        DubinsStateSpace::DubinsPath path(dubinsLSL(d, alpha, beta)), tmp(dubinsRSR(d, alpha, beta));
        double len, minLength = path.length();

        if ((len = tmp.length()) < minLength) {
            minLength = len;
            path = tmp;
        }
        tmp = dubinsRSL(d, alpha, beta);
        if ((len = tmp.length()) < minLength) {
            minLength = len;
            path = tmp;
        }
        tmp = dubinsLSR(d, alpha, beta);
        if ((len = tmp.length()) < minLength) {
            minLength = len;
            path = tmp;
        }
        tmp = dubinsRLR(d, alpha, beta);
        if ((len = tmp.length()) < minLength) {
            minLength = len;
            path = tmp;
        }
        tmp = dubinsLRL(d, alpha, beta);
        if ((len = tmp.length()) < minLength)
            path = tmp;
        return path;
    }
}

const DubinsStateSpace::DubinsPathSegmentType DubinsStateSpace::dubinsPathType[6][3] = {
        {DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_LEFT},
        {DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_RIGHT},
        {DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_LEFT},
        {DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_RIGHT},
        {DUBINS_RIGHT, DUBINS_LEFT, DUBINS_RIGHT},
        {DUBINS_LEFT, DUBINS_RIGHT, DUBINS_LEFT}
};

double DubinsStateSpace::distance(double q0[3], double q1[3]) {
    return rho_ * dubins(q0, q1).length();
}

DubinsStateSpace::DubinsPath DubinsStateSpace::dubins(double q0[3], double q1[3]) {
    double x1 = q0[0], y1 = q0[1], th1 = q0[2];
    double x2 = q1[0], y2 = q1[1], th2 = q1[2];
    double dx = x2 - x1, dy = y2 - y1, d = sqrt(dx * dx + dy * dy) / rho_, th = atan2(dy, dx);
    double alpha = mod2pi(th1 - th), beta = mod2pi(th2 - th);
    return ::dubins(d, alpha, beta);
}

void DubinsStateSpace::interpolate(double q0[3], DubinsPath &path, double seg, double s[3]) {

    if (seg < 0.0) seg = 0.0;
    if (seg > path.length()) seg = path.length();

    double phi, v;

    s[0] = s[1] = 0.0;
    s[2] = q0[2];

    for (unsigned int i = 0; i < 3 && seg > 0; ++i) {
        v = std::min(seg, path.length_[i]);
        seg -= v;
        phi = s[2];
        switch (path.type_[i]) {
            case DUBINS_LEFT:
                s[0] += ( sin(phi+v) - sin(phi));
                s[1] += (-cos(phi+v) + cos(phi));
                s[2] = phi + v;
                break;
            case DUBINS_RIGHT:
                s[0] += (-sin(phi-v) + sin(phi));
                s[1] += ( cos(phi-v) - cos(phi));
                s[2] = phi - v;
                break;
            case DUBINS_STRAIGHT:
                s[0] += (v * cos(phi));
                s[1] += (v * sin(phi));
                break;
        }
    }

    s[0] = s[0] * rho_ + q0[0];
    s[1] = s[1] * rho_ + q0[1];

}

void DubinsStateSpace::sample(double q0[3], double q1[3], double step_size, double &length, std::vector<std::vector<double> > &points) {
    DubinsPath path = dubins(q0, q1);
    length = rho_ * path.length();

    for (double seg=0.0; seg<=length; seg+=step_size){
        double qnew[3] = {};
        interpolate(q0, path, seg/rho_, qnew);
        std::vector<double> v(qnew, qnew + sizeof qnew / sizeof qnew[0]);
        points.push_back(v);
    }
    return;
}


#include <CGAL/Simple_cartesian.h>
#include <iostream>
#include <map>
#include <boost/graph/adjacency_list.hpp>
#include <CGAL/boost/graph/split_graph_into_polylines.h>

typedef CGAL::Simple_cartesian<double> K;
typedef K::Point_2 Point_2;

typedef boost::adjacency_list < boost::listS,
                                boost::vecS,
                                boost::undirectedS,
                                Point_2 > G;

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

typedef std::vector<Point_2> Polyline_2;

struct Is_terminal
{
  template <typename VertexDescriptor, typename Graph>
  bool operator ()(VertexDescriptor vd , const Graph& g )
  {
    return false; // degree(vd,g) != 2; is a bad test in case of parallel edges
  }
};

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

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

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

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

int main()
{
  G g;

  std::list<Polyline_2> polylines;
  Polyline_visitor<G> polyline_visitor(polylines, g);
  std::map<Point_2, vertex_descriptor> p2vd;

  int n;
  std::cin >> n; // number of segments

  Point_2 p, q;
  vertex_descriptor  vdp, vdq;
  for(int i=0; i < n; i++){
    std::cin >> p >> q;

    if(p2vd.find(p) == p2vd.end()){
      vdp = add_vertex(g);
      g[vdp] = p;
      p2vd[p] = vdp;
    } else {
      vdp = p2vd[p];
    }
    if(p2vd.find(q) == p2vd.end()){
      vdq = add_vertex(g);
      g[vdq] = q;
      p2vd[q] = vdq;
    } else {
      vdq = p2vd[q];
    }
    boost::add_edge(vdp, vdq, g);
  }

   CGAL::split_graph_into_polylines( g,
                                     polyline_visitor,
                                     Is_terminal() );
   std::cout.precision(17);

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

  return 0;
}


#include <boost/lexical_cast.hpp>
#include <cctype>
#include <iostream>
#include <iterator>
#include <ostream>
#include <sstream>
#include <stdexcept>
#include <vector>
#include <memory>

struct Token
{
	enum class Type { Literal, Plus, Minus, LParen, RParen, } type;
	std::string text;

	explicit Token(Type type, std::string text)
		:	type{std::move(type)},
			text{std::move(text)}
	{}

	friend std::ostream& operator<<(std::ostream& out, const Token& t)
	{
		return out << "`" << t.text << "`";
	}
};

std::vector<Token> lex(std::string input)
{
	std::vector<Token> result;
	for(auto i = 0ull; i < std::size(input); ++i)
	{
		if (input[i] == '+')
		{
			result.emplace_back(Token::Type::Plus, std::string(1, input[i]));
		}
		else if (input[i] == '-')
		{
			result.emplace_back(Token::Type::Minus, std::string(1, input[i]));
		}
		else if (input[i] == '(')
		{
			result.emplace_back(Token::Type::LParen, std::string(1, input[i]));
		}
		else if (input[i] == ')')
		{
			result.emplace_back(Token::Type::RParen, std::string(1, input[i]));
		}
		else if (std::isdigit(input[i]))
		{
			std::ostringstream out;
			out << input[i++];
			while(i < std::size(input) && std::isdigit(input[i]))
			{
				out << input[i];
				++i;
			}

			result.emplace_back(Token::Type::Literal, out.str());
			--i;
		}
		else if (input[i] == ' ')
		{
			// Do nothing!
			continue;
		}
		else
		{
			std::ostringstream err_msg;
			err_msg << "Invalid token";
			err_msg << " `" << input[i] << "` ";
			err_msg << " found while lexing!";
			throw std::runtime_error{err_msg.str()};
		}
	}

	return result;
}

struct IElement
{
	virtual int eval() const = 0;
};

struct Literal : public IElement
{
	int val;

	explicit Literal(int value)
		:	val{value}
	{}

	int eval() const override
	{
		return val;
	}
};

struct BinaryOperation : public IElement
{
	std::shared_ptr<IElement> lhs, rhs;
	enum class Type { Plus, Minus, NoOp } type;

	explicit BinaryOperation(std::shared_ptr<IElement> lhs = nullptr, std::shared_ptr<IElement> rhs = nullptr, Type t = Type::NoOp)
		:	lhs{std::move(lhs)},
			rhs{std::move(rhs)},
			type{std::move(t)}
	{}

	int eval() const override
	{
		auto lhs_val{lhs->eval()}, rhs_val{rhs->eval()};
		switch(type)
		{
		case Type::Plus:
			return lhs_val + rhs_val;
		case Type::Minus:
			return lhs_val - rhs_val;
		default:
			throw std::runtime_error{"Unsupported operation!"};
		}
	}
};

std::shared_ptr<IElement> parse(const std::vector<Token>& tokens)
{
	auto result{std::make_shared<BinaryOperation>()};
	for(auto i = 0ull; i < std::size(tokens); ++i)
	{
		switch(tokens[i].type)
		{
		case Token::Type::Literal:
			{
				auto lit{std::make_shared<Literal>(boost::lexical_cast<int>(tokens[i].text))};
				if(result->lhs == nullptr)
				{
					result->lhs = std::move(lit);
				}
				else
				{
					result->rhs = std::move(lit);
				}
			}
			break;
		case Token::Type::Plus:
			result->type = BinaryOperation::Type::Plus;
			break;
		case Token::Type::Minus:
			result->type = BinaryOperation::Type::Minus;
			break;
		case Token::Type::LParen:
			{
				auto j{i};
				while(j < std::size(tokens) && tokens[j].type != Token::Type::RParen)
				{
					++j;
				}

				if(j == std::size(tokens))
				{
					throw std::runtime_error{"Mismatched paranthesis!"};
				}

				auto subexpr{std::vector<Token>(tokens.begin() + (i + 1), tokens.begin() + j)};
				auto elem{parse(subexpr)};
				if(result->lhs == nullptr)
				{
					result->lhs = std::move(elem);
				}
				else
				{
					result->rhs = std::move(elem);
				}

				i = j;
			}
			break;
		default:
			break;
		}
	}

	return result;
}

int main()
{
	std::string input{"(13 - 4) - (12 + 1)"};
	auto res{lex(input)};
	std::copy(res.cbegin(), res.cend(), std::ostream_iterator<decltype(res)::value_type>(std::cout, " "));
	auto tree{parse(res)};
	std::cout << input << " = " << tree->eval() << std::endl;

	return 0;
}


/**
 * Copyright (c) 2015 Carnegie Mellon University, Daniel Maturana <dimatura@cmu.edu>
 *
 * For License information please see the LICENSE file in the root directory.
 *
 */

#ifndef RAYCASTING_HPP_OLVFBMND
#define RAYCASTING_HPP_OLVFBMND

#include <Eigen/Core>

#include <pcl_util/point_types.hpp>

#include "scrollgrid/grid_types.hpp"
#include "scrollgrid/box.hpp"
#include "scrollgrid/ray.hpp"
#include "scrollgrid/scrollgrid3.hpp"
#include "scrollgrid/dense_array3.hpp"

namespace ca
{

/**
 * Axis-aligned bounding box intersection test.
 * Reference:
 * An Efficient and Robust Ray–Box Intersection Algorithm, Williams et al. 2004
 * tmin and tmax are updated
 */
template<typename Scalar>
bool aabb_ray_intersect(const ca::scrollgrid::Box<Scalar, 3>& box,
                        ca::scrollgrid::Ray3<Scalar> &r) {
  Scalar tmin = (box.bound(   boost::get<0>(r.sign) ).x() - r.origin.x()) * r.invdir.x();
  Scalar tmax = (box.bound( 1-boost::get<0>(r.sign) ).x() - r.origin.x()) * r.invdir.x();

  Scalar tymin = (box.bound(  boost::get<1>(r.sign) ).y() - r.origin.y()) * r.invdir.y();
  Scalar tymax = (box.bound(1-boost::get<1>(r.sign) ).y() - r.origin.y()) * r.invdir.y();

  if ((tmin > tymax) || (tymin > tmax)) { return false; }
  if (tymin > tmin) { tmin = tymin; }
  if (tymax < tmax) { tmax = tymax; }

  Scalar tzmin = (box.bound(  boost::get<2>(r.sign)).z() - r.origin.z()) * r.invdir.z();
  Scalar tzmax = (box.bound(1-boost::get<2>(r.sign)).z() - r.origin.z()) * r.invdir.z();

  if ((tmin > tzmax) || (tzmin > tmax)) { return false; }
  if (tzmin > tmin) { tmin = tzmin; }
  if (tzmax < tmax) { tmax = tzmax; }
  if (tmin > r.tmin) { r.tmin = tmin; }
  if (tmax < r.tmax) { r.tmax = tmax; }
  return true;
}

/**
 * Trace a straight line from start_pos to end_pos.
 * At each step fun(i, j, k) is called.
 *
 * NOTE start_pos and end_pos should be inside the grid
 *
 * Reference: graphics gems article
 * TODO consider DDA-type raytracing.
 */
template<class TraceFunctor>
void bresenham_trace(const Vec3Ix& start_pos,
                     const Vec3Ix& end_pos,
                     const TraceFunctor& fun) {
  // beware: vec3ix are int64_t
  int x = start_pos[0],
      y = start_pos[1],
      z = start_pos[2];
  int dx = end_pos[0] - start_pos[0],
      dy = end_pos[1] - start_pos[1],
      dz = end_pos[2] - start_pos[2];
  int sx, sy, sz;
  //X
  if ( dx>0 ) {
    sx = 1;
  } else if ( dx<0 ) {
    sx = -1;
    dx = -dx;
  } else {
    sx = 0;
  }

  //Y
  if ( dy>0 ) {
    sy = 1;
  } else if ( dy<0 ) {
    sy = -1;
    dy = -dy;
  } else {
    sy = 0;
  }

  //Z
  if ( dz>0 ) {
    sz = 1;
  } else if ( dz<0 ) {
    sz = -1;
    dz = -dz;
  } else {
    sz = 0;
  }

  int ax = 2*dx,
      ay = 2*dy,
      az = 2*dz;

  if ( ( dy <= dx ) && ( dz <= dx ) ) {
    for (int decy=ay-dx, decz=az-dx;
         ;
         x+=sx, decy+=ay, decz+=az) {
      //SetP ( grid,x,y,z,end_pos, atMax, count);
      if(!fun(x, y, z)) break;
      //Bresenham step
      if ( x==end_pos[0] ) break;
      if ( decy>=0 ) {
        decy-=ax;
        y+=sy;
      }
      if ( decz>=0 ) {
        decz-=ax;
        z+=sz;
      }
    }
  } else if ( ( dx <= dy ) && ( dz <= dy ) ) {
    //dy>=dx,dy
    for (int decx=ax-dy,decz=az-dy;
         ;
         y+=sy,decx+=ax,decz+=az ) {
      // SetP ( grid,x,y,z,end_pos, atMax, count);
      if(!fun(x, y, z)) break;
      //Bresenham step
      if ( y==end_pos[1] ) break;
      if ( decx>=0 ) {
        decx-=ay;
        x+=sx;
      }
      if ( decz>=0 ) {
        decz-=ay;
        z+=sz;
      }
    }
  } else if ( ( dx <= dz ) && ( dy <= dz ) ) {
    //dy>=dx,dy
    for (int decx=ax-dz,decy=ay-dz;
         ;
         z+=sz,decx+=ax,decy+=ay ) {
      //SetP ( grid,x,y,z,end_pos, atMax, count);
      if(!fun(x, y, z))  break;
      //Bresenham step
      if ( z==end_pos[2] ) break;
      if ( decx>=0 ) {
        decx-=az;
        x+=sx;
      } if ( decy>=0 ) {
        decy-=az;
        y+=sy;
      }
    }
  }
}

/**
 * Simply increment a counter in densearray3 for each step along the way.
 */
template<class GridScalar, class ArrayScalar>
void bresenham_trace_simple(const Vec3Ix& start_pos,
                            const Vec3Ix& end_pos,
                            const ca::ScrollGrid3<GridScalar>& grid3,
                            ca::DenseArray3<ArrayScalar>& array3
                            ) {
  //int ray_ctr = 0;
  // beware: vec3ix are int64_t
  int x = start_pos[0],
      y = start_pos[1],
      z = start_pos[2];
  int dx = end_pos[0] - start_pos[0],
      dy = end_pos[1] - start_pos[1],
      dz = end_pos[2] - start_pos[2];
  int sx, sy, sz;
  //X
  if ( dx>0 ) {
    sx = 1;
  } else if ( dx<0 ) {
    sx = -1;
    dx = -dx;
  } else {
    sx = 0;
  }

  //Y
  if ( dy>0 ) {
    sy = 1;
  } else if ( dy<0 ) {
    sy = -1;
    dy = -dy;
  } else {
    sy = 0;
  }

  //Z
  if ( dz>0 ) {
    sz = 1;
  } else if ( dz<0 ) {
    sz = -1;
    dz = -dz;
  } else {
    sz = 0;
  }

  int ax = 2*dx,
      ay = 2*dy,
      az = 2*dz;

  if ( ( dy <= dx ) && ( dz <= dx ) ) {
    for (int decy=ay-dx, decz=az-dx;
         ;
         x+=sx, decy+=ay, decz+=az) {
      mem_ix_t mem_ix = grid3.grid_to_mem(x, y, z);
      array3[mem_ix] += 1;
      //array3[mem_ix] = ray_ctr++;
      //Bresenham step
      if ( x==end_pos[0] ) break;
      if ( decy>=0 ) {
        decy-=ax;
        y+=sy;
      }
      if ( decz>=0 ) {
        decz-=ax;
        z+=sz;
      }
    }
  } else if ( ( dx <= dy ) && ( dz <= dy ) ) {
    //dy>=dx,dy
    for (int decx=ax-dy,decz=az-dy;
         ;
         y+=sy,decx+=ax,decz+=az ) {
      mem_ix_t mem_ix = grid3.grid_to_mem(x, y, z);
      array3[mem_ix] += 1;
      //array3[mem_ix] = ray_ctr++;
      //Bresenham step
      if ( y==end_pos[1] ) break;
      if ( decx>=0 ) {
        decx-=ay;
        x+=sx;
      }
      if ( decz>=0 ) {
        decz-=ay;
        z+=sz;
      }
    }
  } else if ( ( dx <= dz ) && ( dy <= dz ) ) {
    //dy>=dx,dy
    for (int decx=ax-dz,decy=ay-dz;
         ;
         z+=sz,decx+=ax,decy+=ay ) {
      grid_ix_t mem_ix = grid3.grid_to_mem(x, y, z);
      array3[mem_ix] += 1;
      //array3[mem_ix] = ray_ctr++;
      //Bresenham step
      if ( z==end_pos[2] ) break;
      if ( decx>=0 ) {
        decx-=az;
        x+=sx;
      } if ( decy>=0 ) {
        decy-=az;
        y+=sy;
      }
    }
  }
}

template<class TraceFunctor>
void bresenham_trace(const Vec2Ix& start_pos,
                     const Vec2Ix& end_pos,
                     const TraceFunctor& fun) {
    int x = start_pos[0],
      y = start_pos[1];

    int dx = end_pos[0] - start_pos[0],
      dy = end_pos[1] - start_pos[1];
    int sx, sy;
    //X
    if ( dx>0 ) {
    sx = 1;
    } else if ( dx<0 ) {
    sx = -1;
    dx = -dx;
    } else {
    sx = 0;
    }

    //Y
    if ( dy>0 ) {
    sy = 1;
    } else if ( dy<0 ) {
    sy = -1;
    dy = -dy;
    } else {
    sy = 0;
    }

    int ax = 2*dx,
      ay = 2*dy;

    if (dy <= dx){
        for (int decy=ay-dx;;
             x+=sx, decy+=ay) {
          bool end_cell = false;
          //Bresenham step
          if(!fun(x,y,end_cell)){ ROS_INFO("Breaking due to function call"); break;}

          if ( x==end_pos[0] ) break;
          if ( decy>=0 ) {
            decy-=ax;
            y+=sy;
          }
        }
    } else if ( dx <= dy ){
        for (int decx=ax-dy;;
             y+=sy,decx+=ax) {
          bool end_cell = false;
          //Bresenham step
          if(!fun(x,y,end_cell)){ break; ROS_INFO("Breaking due to function call"); break;}

          if ( y==end_pos[1] ) break;
          if ( decx>=0 ) {
            decx-=ay;
            x+=sx;
          }
        }
    }
}

}

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


//
// Created by philipp on 26.12.19.
//

#ifndef FUNNELS_CPP_DYNAMICS_HH
#define FUNNELS_CPP_DYNAMICS_HH

#include <Eigen/Core>
#include <cmath>
#include <cassert>

using namespace Eigen;

class kinematic_2d_sys_t{
public:
  constexpr static const size_t _dimx=4, _dimv=2, _dimu=2;

  static void compute(MatrixXd &x, const VectorXd &u, const VectorXd &t){
    assert(x.cols()==t.size());
    assert(u.rows()==_dimu && x.rows()==_dimx && u.rows()==_dimv);
    // State vector is [x,y,dx,dy]
    const Vector2d x0 = x.block<_dimx-_dimv,1>(0,0);
    size_t n = t.size();
    // Set velocity
    // For a kinematic sys, the velocity can change instantly
    x.block(_dimx-_dimv,0,_dimv,n).colwise() = u;
    // Compute state
    for (size_t i=0; i<n; i++){
      x.block<_dimx-_dimv,1>(0,i) = x0 + u*t(i);
    }
    // Done
  }
};
#endif //FUNNELS_CPP_DYNAMICS_HH


#include "common.h"
#include "ConformalBlock.h"
#include "gegenbauer_polynomial.hpp"
#include <iostream>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/pow.hpp>
#include <boost/math/special_functions/factorials.hpp>

using namespace boost::math;

float_type& RCoefficients::GetCachedRI(int type, int n) {
    return this->cacheRI[type * MaxOrder + n -1];
}

float_type& RCoefficients::GetCachedRII(int type, int l, int n) {
    boost::unordered_map<int, float_type>::iterator it = cacheR.find(type * 64 * 64 + n * 64 + l);
    if (it != cacheR.end()) {
        return it->second;
    } else {
        cacheR[type * 64 * 64 + n * 64 + l] = std::numeric_limits<float_type>::max();
        return cacheR[type * 64 * 64 + n * 64 + l];
    }
}

float_type RCoefficients::RI(int n) {
    float_type& ret = GetCachedRI(0, n);
    if (ret != std::numeric_limits<float_type>::max()) return ret;
    ret = RI_NonDelta(n) * rising_factorial((dlt12 + 1.0 - n) / 2, n) * rising_factorial((dlt34 + 1.0 - n) / 2, n);
    return ret;
}

// the delta independent part of RI.
float_type RCoefficients::RI_NonDelta(int n) {
    float_type fac = factorial<float_type>(n);
    i64 p = ((i64)1) << n;
    float_type ret = p / (fac * fac) * n;

    if (n % 2 == 0) return -ret;
    return ret;
}

float_type RCoefficients::RI_dDelta12(int n) {
    float_type& ret = GetCachedRI(1, n);
    if (ret != std::numeric_limits<float_type>::max()) return ret;
    ret = RI_dDelta(n, dlt12, dlt34);
    return ret;
}

float_type RCoefficients::RI_dDelta34(int n) {
    float_type& ret = GetCachedRI(2, n);
    if (ret != std::numeric_limits<float_type>::max()) return ret;
    ret = RI_dDelta(n, dlt34, dlt12);
    return ret;
}

float_type RCoefficients::RI_dDelta(int n, float_type deltaDiffA, float_type deltaDiffB) {
    if (!IsInteger((deltaDiffA + 1 - n) / 2)) {
        return digammaDiff((deltaDiffA + 1 - n) / 2, n) * RI(n) * .5;
    } else {
        int x = (int)floor((deltaDiffA + 1 - n) / 2 + EPS);
        if (x > 0 || x + n <= 0) {
            return digammaDiff((deltaDiffA + 1 - n) / 2, n) * RI(n) * .5;
        } else {
            // -n + 1 <= x <= 0 need special handling because digammaDiff() has a pole and RI() is zero.
            // now, RI() ~ (x)_n, the (x)_n factor has a pole
            float_type ret = rising_factorial((deltaDiffB + 1.0 - n) / 2, n) * 0.5;
            ret *= factorial<float_type>(-x) * factorial<float_type>(x + n - 1);
            if (x % 2 != 0) ret = -ret;

            return ret * RI_NonDelta(n);
        }
    }
}

float_type RCoefficients::RII(int l, int n) {
    float_type& ret = GetCachedRII(0, l, n);
    if (ret != std::numeric_limits<float_type>::max()) return ret;
    ret = RII_NonDelta(l, n) * rising_factorial((dlt12 + 1.0 - n) / 2, n) * rising_factorial((dlt34 + 1.0 - n) / 2, n);
    return ret;
}

// the delta independent part of RII.
float_type RCoefficients::RII_NonDelta(int l, int n) {
    float_type ret = binomial_coefficient<float_type>(l, n) * n;
    ret /= factorial<float_type>(n);
    ret /= (((i64)1) << n);

    float_type h = d / 2;
    if (abs(h - 1) < EPS && l == n) {
        // if h == 1 and l == n, zero factors of numerator and denominator cancel out.
        ret /= rising_factorial(h + l - n, n);

    } else {
        ret /= rising_factorial(h + l - n, n) * rising_factorial(h + l - n - 1, n);
        ret *= rising_factorial(d + l - n - 2, n);
    }

    if (n % 2 == 0) return -ret;
    return ret;
}

float_type RCoefficients::RII_dDelta12(int l, int n) {
    float_type& ret = GetCachedRII(1, l, n);
    if (ret != std::numeric_limits<float_type>::max()) return ret;
    ret = RII_dDelta(l, n, dlt12, dlt34);
    return ret;
}

float_type RCoefficients::RII_dDelta34(int l, int n) {
    float_type& ret = GetCachedRII(2, l, n);
    if (ret != std::numeric_limits<float_type>::max()) return ret;
    ret = RII_dDelta(l, n, dlt34, dlt12);
    return ret;
}

float_type RCoefficients::RII_dDelta(int l, int n, float_type deltaDiffA, float_type deltaDiffB) {
    if (!IsInteger((deltaDiffA + 1 - n) / 2)) {
        return digammaDiff((deltaDiffA + 1 - n) / 2, n) * RII(l, n) * .5;
    } else {
        int x = (int)floor((deltaDiffA + 1 - n) / 2 + EPS);
        if (x > 0 || x + n <= 0) {
            return digammaDiff((deltaDiffA + 1 - n) / 2, n) * RII(l, n) * .5;
        } else {
            // -n + 1 <= x <= 0 need special handling because digammaDiff() has a pole and RII() is zero.
            // now, RII() ~ (x)_n, the (x)_n factor has a pole
            float_type ret = rising_factorial((deltaDiffB + 1.0 - n) / 2, n) * 0.5;
            ret *= factorial<float_type>(-x) * factorial<float_type>(x + n - 1);
            if (x % 2 != 0) ret = -ret;

            return ret * RII_NonDelta(l, n);
        }
    }
}

float_type RCoefficients::RIII(int l, int n) {
    float_type& ret = GetCachedRII(3, l, n);
    if (ret != std::numeric_limits<float_type>::max()) return ret;
    float_type h = d / 2;
    ret = rising_factorial((dlt12 + h + l - n) / 2, n) * rising_factorial((dlt12 - h - l - n + 2) / 2, n);
    ret *= rising_factorial((dlt34 + h + l - n) / 2, n) * rising_factorial((dlt34 - h - l - n + 2) / 2, n) * RIII_NonDelta(l, n);
    return ret;
}

float_type RCoefficients::RIII_NonDelta(int l, int n) {
    float_type h = d / 2;
    float_type ret;
    // if h is not integer, there is no poles.
    if (!IsInteger(h)) {
        ret = rising_factorial(h - n - 1, 2 * n);
        ret /= rising_factorial(h + l - n - 1, 2 * n) * rising_factorial(h + l - n, 2 * n);
    } else {
        int ih = (int)floor(h + EPS);
        if (n >= ih - 1 && n < ih + l - 1) {
            // in this case, the numerate part is zero, and denominator is nonzero
            return .0;
        } else if (2 * n >= l + 1 && n == ih + l - 1) {
            // in this case, the zeros of numerate and denominate cancel out.
            ret = 1.0 / binomial_coefficient<float_type>(2 * n - 1, l);
            ret /= rising_factorial(h + l - n, 2 * n);
            if (l % 2 == 1) ret = -ret;
        } else {
            return 0.0; // temperary return 0 because the following workaround leads to divergent results.

            // n > h + l - 1 case, numerator is zero but denominator is float_type-zero
            // to avoid infinity, we let h -> h + EPS
/*            float_type hh = h - EPS;
            float_type inf = rising_factorial(hh - n - 1, 2 * n) / rising_factorial(hh + l - n - 1, 2 * n);
            inf /=  rising_factorial(hh + l - n, 2 * n);
            ret *= inf;*/
        }
    }

    float_type fac = factorial<float_type>(n);
    ret *= n / (fac * fac);

    if (n % 2 == 0) return -ret;
    return ret;
}

float_type RCoefficients::RIII_dDelta12(int l, int n) {
    float_type& ret = GetCachedRII(4, l, n);
    if (ret != std::numeric_limits<float_type>::max()) return ret;
    ret = RIII_dDelta(l, n, dlt12, dlt34);
    return ret;
}

float_type RCoefficients::RIII_dDelta34(int l, int n) {
    float_type& ret = GetCachedRII(5, l, n);
    if (ret != std::numeric_limits<float_type>::max()) return ret;
    ret = RIII_dDelta(l, n, dlt34, dlt12);
    return ret;
}

float_type RCoefficients::RIII_dDelta(int l, int n, float_type deltaDiffA, float_type deltaDiffB) {
    float_type h = d / 2;
    float_type ret1;
    if (!IsInteger((deltaDiffA - h - l - n + 2) / 2)) {
        ret1 = digammaDiff((deltaDiffA - h - l - n + 2) / 2, n) * RIII(l, n);
    } else {
        int x = (int)floor((deltaDiffA - h - l - n + 2) / 2 + EPS);
        if (x > 0 || x + n <= 0) {
            ret1 = digammaDiff((deltaDiffA - h - l - n + 2) / 2, n) * RIII(l, n);
        } else {
            // -n + 1 <= x <= 0 need special handling because digammaDiff() has a pole and RII() is zero.
            // now, RII() ~ (x)_n, the (x)_n factor has a pole
            ret1 = rising_factorial((deltaDiffA + h + l - n) / 2, n);
            ret1 *= rising_factorial((deltaDiffB + h + l - n) / 2, n) * rising_factorial((deltaDiffB - h - l - n + 2) / 2, n);
            ret1 *= factorial<float_type>(-x) * factorial<float_type>(x + n - 1);
            if (x % 2 != 0) ret1 = -ret1;

            ret1 *= RIII_NonDelta(l, n);
        }
    }

    float_type ret2;
    if (!IsInteger((deltaDiffA + h + l - n) / 2)) {
        ret2 = digammaDiff((deltaDiffA + h + l - n) / 2, n) * RIII(l, n);
    } else {
        int x = (int)floor((deltaDiffA + h + l - n) / 2 + EPS);
        if (x > 0 || x + n <= 0) {
            ret2 = digammaDiff((deltaDiffA + h + l - n) / 2, n) * RIII(l, n);
        } else {
            // -n + 1 <= x <= 0 need special handling because digammaDiff() has a pole and RII() is zero.
            // now, RII() ~ (x)_n, the (x)_n factor has a pole
            ret2 = rising_factorial((deltaDiffA - h - l - n + 2) / 2, n);
            ret2 *= rising_factorial((deltaDiffB + h + l - n) / 2, n) * rising_factorial((deltaDiffB - h - l - n + 2) / 2, n);
            ret2 *= factorial<float_type>(-x) * factorial<float_type>(x + n - 1);
            if (x % 2 != 0) ret2 = -ret2;

            ret2 *= RIII_NonDelta(l, n);
        }
    }

    return (ret1 + ret2) * 0.5;
}

float_type ConformalBlockScalars::HInfinity(int l, float_type r, float_type eta) {
    float_type ret;
    if (hCache.Get(l, &ret)) return ret;

    float_type h = d / 2;
    ret = pow(1 - r * r, 1 - h);
    ret /= pow(r * r - 2 * r * eta + 1, (1 - dlt12 + dlt34) / 2);
    ret /= pow(r * r + 2 * r * eta + 1, (1 + dlt12 - dlt34) / 2);
    ret *= factorial<float_type>(l);
    ret /= (((i64)1) << l);

    if (abs(h - 1) > EPS) {
        ret /= rising_factorial(h - 1, l);
        float_type x[1];
        x[0] = eta;
        float_type *v = gegenbauer_polynomial_value(l, 1, h - 1, x);
        ret *= v[l + 0 * (l + 1)];
        delete v;
    } else if (l != 0) {
        // for the h == 1 and l == 0 case, (h-1)_l == 1 == gegenbauer(l, h-1);
        // for the h == 1 and l > 0 case, the zeros of (h-1)_l and gegenbauer(l, h-1) cancel out
        ret *= GegenbauerDAlphaAt0(l, eta) / factorial<float_type>(l - 1);
    }

    hCache.Set(l, ret);
    return ret;
}

float_type ConformalBlockScalars::HInfinity_dDelta12(int l, float_type r, float_type eta) {
    return HInfinity_dDelta(l, r, eta);
}

float_type ConformalBlockScalars::HInfinity_dDelta34(int l, float_type r, float_type eta) {
    return -HInfinity_dDelta(l, r, eta);
}

float_type ConformalBlockScalars::HInfinity_dDelta(int l, float_type r, float_type eta) {
    return 0.5 * HInfinity(l, r, eta) * log((r * r - 2 * eta * r + 1)/(r * r + 2 * eta * r + 1));
}

float_type ConformalBlockScalars::HRecursion(float_type delta, int l, float_type r, float_type eta, int order) {
    int idelta = std::numeric_limits<int>::max();
    float_type ret;
    if (IsInteger(delta)) {
        idelta = (int)floor(delta + EPS);
        if (hCache.Get(idelta, l, order, &ret)) return ret;
    }

    ret = HInfinity(l, r, eta);
    if (order == 0) return ret;

    float_type deltaAs;
    int la, na;

    // type I
    for (int n = 1; n <= order; n++) {
        deltaAs = 1 - l - n;
        la = l + n;
        na = n;
        // this if statement is only for the following case:
        // when the exchange operator is identity, i.e, delta==l==0,
        // then delta - deltaAs can be zero.
        if (abs(delta - deltaAs) < EPS) continue;
        ret += pow(4 * r, na) * rCoef->RI(n) / (delta - deltaAs) * HRecursion(deltaAs + na, la, r, eta, order - na);
    }

    // type II
    for (int n = 1; n <= order && n <= l; n++) {
        deltaAs = l + d - 1 - n;
        la = l - n;
        na = n;
        if (abs(delta - deltaAs) < EPS) continue;
        ret += pow(4 * r, na) * rCoef->RII(l, n) / (delta - deltaAs) * HRecursion(deltaAs + na, la, r, eta, order - na);
    }

    // type III
    for (int n = 1; n + n <= order; n++) {
        deltaAs = d / 2 - n;
        la = l;
        na = 2 * n;
        if (abs(delta - deltaAs) < EPS) continue;
        ret += pow(4 * r, na) * rCoef->RIII(l, n) / (delta - deltaAs) * HRecursion(deltaAs + na, la, r, eta, order - na);
    }

    if (idelta != std::numeric_limits<int>::max()) {
        hCache.Set(idelta, l, order, ret);
    }

    return ret;
}

#if CONFORMAL_BLOCK_NORMALIZATION == 1

float_type ConformalBlockScalars::NormalizationFactor(int l) {
    assert(l >= 0);
    assert(l < 63);

    float_type ret = (float_type)(((i64)1)<<l);
    if (l % 2 == 1) ret = -ret;
    if (abs(d - 2) < EPS) {
        return ret;
    } else {
        return ret * rising_factorial(d/2 - 1.0, l) / rising_factorial(d - 2.0, l);
    }
}

#endif

float_type ConformalBlockScalars::evaluate(float_type delta, int l, float_type r, float_type eta, int order) {
    hCache.ClearIfNew(r, eta);
#if CONFORMAL_BLOCK_NORMALIZATION == 1
    float_type prefactor = pow(r, delta) * NormalizationFactor(l);
    float_type hr = HRecursion(delta, l, r, eta, order);
    return  prefactor * hr;
#else
    return pow(4 * r, delta) * HRecursion(delta, l, r, eta, order);
#endif

}

float_type ConformalBlockScalars::dDelta(float_type delta, int l, float_type r, float_type eta, int order) {
#if CONFORMAL_BLOCK_NORMALIZATION == 1
    float_type ret = log(r) * evaluate(delta, l, r, eta, order);
    ret += pow(r, delta) * NormalizationFactor(l) * dHdDelta(delta, l, r, eta, order);
#else
    float_type ret = log(4 * r) * evaluate(delta, l, r, eta, order);
    ret += pow(4 * r, delta) * dHdDelta(delta, l, r, eta, order);
#endif
    return ret;
}

float_type ConformalBlockScalars::dHdDelta(float_type delta, int l, float_type r, float_type eta, int order) {
    float_type deltaAs, ret = 0.0;
    int la, na;

    // type I
    for (int n = 1; n <= order; n++) {
        deltaAs = 1 - l - n;
        la = l + n;
        na = n;
        // this if statement is only for the following case:
        // when the exchange operator is identity, i.e, delta==l==0,
        // then delta - deltaAs can be zero.
        if (abs(delta - deltaAs) < EPS) continue;
        ret -= pow(4 * r, na) * rCoef->RI(n) / ((delta - deltaAs) * (delta - deltaAs)) * HRecursion(deltaAs + na, la, r, eta, order - na);
    }

    // type II
    for (int n = 1; n <= order && n <= l; n++) {
        deltaAs = l + d - 1 - n;
        la = l - n;
        na = n;
        if (abs(delta - deltaAs) < EPS) continue;
        ret -= pow(4 * r, na) * rCoef->RII(l, n) / ((delta - deltaAs) * (delta - deltaAs)) * HRecursion(deltaAs + na, la, r, eta, order - na);
    }

    // type III
    for (int n = 1; n + n <= order; n++) {
        deltaAs = d / 2 - n;
        la = l;
        na = 2 * n;
        if (abs(delta - deltaAs) < EPS) continue;
        ret -= pow(4 * r, na) * rCoef->RIII(l, n) / ((delta - deltaAs) * (delta - deltaAs)) * HRecursion(deltaAs + na, la, r, eta, order - na);
    }

    return ret;
}

float_type ConformalBlockScalars::dDelta12(float_type delta, int l, float_type r, float_type eta, int order) {
    hDiff12Cache.ClearIfNew(r, eta);
    hCache.ClearIfNew(r, eta);
#if CONFORMAL_BLOCK_NORMALIZATION == 1
    return pow(r, delta) * NormalizationFactor(l) * dHdDelta12(delta, l, r, eta, order);
#else
    return pow(4 * r, delta) * dHdDelta12(delta, l, r, eta, order);
#endif

}

float_type ConformalBlockScalars::dHdDelta12(float_type delta, int l, float_type r, float_type eta, int order) {
    int idelta = std::numeric_limits<int>::max();
    float_type ret;
    if (IsInteger(delta)) {
        idelta = (int)floor(delta + EPS);
        if (hDiff12Cache.Get(idelta, l, order, &ret)) return ret;
    }

    ret = HInfinity_dDelta12(l, r, eta);
    if (order == 0) return ret;

    float_type deltaAs;
    int la, na;

    // type I
    for (int n = 1; n <= order; n++) {
        deltaAs = 1 - l - n;
        la = l + n;
        na = n;
        // this if statement is only for the following case:
        // when the exchange operator is identity, i.e, delta==l==0,
        // then delta - deltaAs can be zero.
        if (abs(delta - deltaAs) < EPS) continue;
        ret += pow(4 * r, na) / (delta - deltaAs) * (dHdDelta12(deltaAs + na, la, r, eta, order - na) * rCoef->RI(n) + HRecursion(deltaAs + na, la, r, eta, order - na) * rCoef->RI_dDelta12(n));
    }

    // type II
    for (int n = 1; n <= order && n <= l; n++) {
        deltaAs = l + d - 1 - n;
        la = l - n;
        na = n;
        if (abs(delta - deltaAs) < EPS) continue;
        ret += pow(4 * r, na) / (delta - deltaAs) * (dHdDelta12(deltaAs + na, la, r, eta, order - na) * rCoef->RII(l, n)+ HRecursion(deltaAs + na, la, r, eta, order - na) * rCoef->RII_dDelta12(l,n));
    }

    // type III
    for (int n = 1; n + n <= order; n++) {
        deltaAs = d / 2 - n;
        la = l;
        na = 2 * n;
        if (abs(delta - deltaAs) < EPS) continue;
        ret += pow(4 * r, na) / (delta - deltaAs) * (dHdDelta12(deltaAs + na, la, r, eta, order - na) * rCoef->RIII(l, n)+ HRecursion(deltaAs + na, la, r, eta, order - na) * rCoef->RIII_dDelta12(l,n));
    }

    if (idelta != std::numeric_limits<int>::max()) {
        hDiff12Cache.Set(idelta, l, order, ret);
    }

    return ret;
}

float_type ConformalBlockScalars::dDelta34(float_type delta, int l, float_type r, float_type eta, int order) {
    hDiff34Cache.ClearIfNew(r, eta);
    hCache.ClearIfNew(r, eta);
#if CONFORMAL_BLOCK_NORMALIZATION == 1
    return pow(r, delta) * NormalizationFactor(l) * dHdDelta34(delta, l, r, eta, order);
#else
    return pow(4 * r, delta) * dHdDelta34(delta, l, r, eta, order);
#endif
}

float_type ConformalBlockScalars::dHdDelta34(float_type delta, int l, float_type r, float_type eta, int order) {
    int idelta = std::numeric_limits<int>::max();
    float_type ret;
    if (IsInteger(delta)) {
        idelta = (int)floor(delta + EPS);
        if (hDiff34Cache.Get(idelta, l, order, &ret)) return ret;
    }

    ret = HInfinity_dDelta34(l, r, eta);
    if (order == 0) return ret;
    float_type deltaAs;
    int la, na;

    // type I
    for (int n = 1; n <= order; n++) {
        deltaAs = 1 - l - n;
        la = l + n;
        na = n;
        // this if statement is only for the following case:
        // when the exchange operator is identity, i.e, delta==l==0,
        // then delta - deltaAs can be zero.
        if (abs(delta - deltaAs) < EPS) continue;
        ret += pow(4 * r, na) / (delta - deltaAs) * (dHdDelta34(deltaAs + na, la, r, eta, order - na) * rCoef->RI(n) + HRecursion(deltaAs + na, la, r, eta, order - na) * rCoef->RI_dDelta34(n));
    }

    // type II
    for (int n = 1; n <= order && n <= l; n++) {
        deltaAs = l + d - 1 - n;
        la = l - n;
        na = n;
        if (abs(delta - deltaAs) < EPS) continue;
        ret += pow(4 * r, na) / (delta - deltaAs) * (dHdDelta34(deltaAs + na, la, r, eta, order - na) * rCoef->RII(l, n)+ HRecursion(deltaAs + na, la, r, eta, order - na) * rCoef->RII_dDelta34(l,n));
    }

    // type III
    for (int n = 1; n + n <= order; n++) {
        deltaAs = d / 2 - n;
        la = l;
        na = 2 * n;
        if (abs(delta - deltaAs) < EPS) continue;
        ret += pow(4 * r, na) / (delta - deltaAs) * (dHdDelta34(deltaAs + na, la, r, eta, order - na) * rCoef->RIII(l, n)+ HRecursion(deltaAs + na, la, r, eta, order - na) * rCoef->RIII_dDelta34(l,n));
    }

    if (idelta != std::numeric_limits<int>::max()) {
        hDiff34Cache.Set(idelta, l, order, ret);
    }

    return ret;
}


#include <iostream>
#include <fstream>
#include <sstream>
#include <stdio.h>
#include <assert.h>
#include <math.h>
#include <cmath>
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/SparseQR>
#include <Eigen/SparseCholesky>
#include <Eigen/OrderingMethods>
#include "mmio.hpp"
#include "cxxopts.hpp"
#include "tree.h"
#include "partition.h"
#include "util.h"
#include "is.h"

using namespace Eigen;
using namespace std;

typedef SparseMatrix<double, 0, int> SpMat;

int main(int argc, char* argv[]){

     cxxopts::Options options("spaQR", "Sparsified QR for general sparse matrices");
     options.add_options()
        ("help", "Print help")
        ("m,matrix", "Matrix file in martrix market format", cxxopts::value<string>())
        ("l,lvl","Number of levels", cxxopts::value<int>())
        // Geometry
        ("coordinates", "Coordinates MM array file. If provided, will do a geometric partitioning.", cxxopts::value<string>())
        ("n,coordinates_n", "If provided with -n, will use a tensor n^d & geometric partitioning. Overwrites --coordinates", cxxopts::value<int>()->default_value("-1"))
        ("d,coordinates_d", "If provided with -d, will use a tensor n^d & geometric partitioning. Overwrites --coordinates", cxxopts::value<int>()->default_value("-1"))
        // Partition
        ("hsl","Use bipartite matching routine from HSL to perform row ordering. Use only if compiled with USE_HSL=1 flag. Default false.", cxxopts::value<int>()->default_value("0"))
        // Sparsification
        ("t,tol", "Tolerance", cxxopts::value<double>()->default_value("1e-1"))
        ("skip", "Skip sparsification", cxxopts::value<int>()->default_value("0"))
        ("order", "Specify order of scheme (1, 1.5) Default order=1. Use 1.5 to get a more accurate scheme but uses more memory.", cxxopts::value<float>()->default_value("1"))
        ("scale", "Do scaling. Default true.", cxxopts::value<int>()->default_value("1"))
        // Iterative method
        // ("solver","Wether to use CG or GMRES or CGLS. Default GMRES for square matrices and CGLS for rectangular.", cxxopts::value<string>()->default_value("GMRES"))
        ("i,iterations","Iterative solver iterations", cxxopts::value<int>()->default_value("300"))
        ("rhs", "Provide RHS to solve in matrix market format", cxxopts::value<string>())
        ("res", "Desired relative residual for the iterative solver. Default 1e-12", cxxopts::value<double>()->default_value("1e-12"))

        // Use solvers from Eigen library
        ("useEigenLSCG","If true, run CGLS scheme with standard diagonal preconditioner from Eigen library. Default false.", cxxopts::value<int>()->default_value("0"))
        ("useEigenQR","If true, run SparseQR with default ordering from Eigen library. Default false.", cxxopts::value<int>()->default_value("0"))
        ("useCholesky","If true, run SimplicialLDL^T with AMDOrdering from Eigen library. Default false.", cxxopts::value<int>()->default_value("0"));

    auto result = options.parse(argc, argv);
    if (result.count("help")) {
        cout << options.help({"", "Group"}) << endl;
        exit(0);
    }

    if ( (!result.count("matrix"))  ) {
        cout << "--matrix is mandatory" << endl;
        exit(0);
    }

    string matrix = result["matrix"].as<string>();
    int nlevels;

    // Geometry
    string coordinates;
    int cn = result["coordinates_n"].as<int>();
    int cd = result["coordinates_d"].as<int>();
    bool geo_file = (result.count("coordinates") > 0);
    if ( (cn == -1 && cd >= 0) || (cd == -1 && cn >= 0) ) {
        cout << "cn and cd should be both provided, or none should be provided" << endl;
        return 1;
    }
    bool geo_tensor = (cn >= 0 && cd >= 0);
    bool geo = geo_file || geo_tensor;
    if(geo_file) {
        coordinates = result["coordinates"].as<string>();
    }

    // Load matrix
    SpMat A = mmio::sp_mmread<double, int>(matrix);

    if (A.rows() < A.cols()){
        cout << " <<< Warning!!! nrows < ncols. Finding QR on A.transpose() instead. " << endl;
        SpMat T = A.transpose();
        A = T;
    }

    int nrows = A.rows();
    int ncols = A.cols();
    cout << "Matrix " << matrix << " with " << nrows << " rows,  " << ncols << " columns loaded" << endl;

    // Iterative method
    bool useGMRES = (nrows == ncols) ? true : false;
    bool useCGLS = (nrows > ncols) ? true : false;
    int iterations = result["iterations"].as<int>();

    bool useEigenLSCG = result["useEigenLSCG"].as<int>();
    bool useEigenQR = result["useEigenQR"].as<int>();
    bool useCholesky = result["useCholesky"].as<int>();

    double residual = result["res"].as<double>();

    if ( (!result.count("lvl"))  ) {
        cout << "--Levels not provided" << endl;
        nlevels = ceil(log2(ncols/64));
        cout << " Levels set to ceil(log2(ncols/64)) =  " << nlevels << endl;
    }
    else{
        nlevels = result["lvl"].as<int>();
    }

    // Partition
    int hsl = result["hsl"].as<int>();

    // Sparsification parameters
    int scale = result["scale"].as<int>();
    if (nrows != ncols && scale == 0){
        cout << "Scaling necessary for rectangular matrices" << endl;
        cout << "Setting scale to 1" << endl;
        scale = 1;
    }
    double tol = result["tol"].as<double>();
    float order = result["order"].as<float>();

    // Pre-process matrix to have columns of unit norm (diagonal scaling)
    VectorXd Dentries(A.cols());
    DiagonalMatrix<double, Eigen::Dynamic> D(A.cols());

    auto pstart = wctime();
    for (int i=0; i < A.cols(); ++i) {
        double sum = 0;
       for (SpMat::InnerIterator it(A,i); it; ++it){
            sum += it.value()*it.value();
       }
       Dentries[i] = (double)(1.0/sqrt(sum));
    }

    D = Dentries.asDiagonal();
    A = A*D*10;
    auto pend = wctime();
    cout << "Pre-process time: " << elapsed(pstart, pend) << endl;

    int skip = (tol == 0 ? nlevels-1 : result["skip"].as<int>());

    // Load coordinates ?
    MatrixXd X;
    if(geo_tensor) {
        if(pow(cn, cd) != ncols) {
            cout << "Error: cn and cd where both provided, but cn^cd != N where A is NxN" << endl;
            return 1;
        }
        X = linspace_nd(cn, cd);
        cout << "Tensor coordinate matrix of size " << cn << "^" << cd << " built" << endl;
    } else if(geo_file) {
        X = mmio::dense_mmread<double>(coordinates);
        cout << "Coordinate file " << X.rows() << "x" << X.cols() << " loaded from " << coordinates << endl;
        if(X.cols() != ncols) {
            cout << "Error: coordinate file should hold a matrix of size d x N" << endl;
        }
    }

    // Tree
    Tree t(nlevels, skip);
    t.set_scale(scale);
    t.set_tol(tol);
    t.set_order(order);
    t.set_hsl(hsl);

    if (nrows == ncols) t.set_square(1); // default 0
    if(geo) t.set_Xcoo(&X);

    if (useEigenLSCG) {
        VectorXd x = VectorXd::Zero(ncols);
        VectorXd b = random(nrows,2021);

        LeastSquaresConjugateGradient<SpMat, LeastSquareDiagonalPreconditioner<double>> lscg;
        lscg.compute(A);

        LeastSquareDiagonalPreconditioner<double> diag_precond = lscg.preconditioner();

        timer cgls0 = wctime();
        auto iter = lscg_eigen(A, b, x, diag_precond, iterations, residual, true);
        timer cgls1 = wctime();

        cout << "CGLS error: " << scientific <<  (A.transpose()*(A*x-b)).norm() / (A.transpose()*b).norm() << endl;
        cout << "  CGLS: " << elapsed(cgls0, cgls1) << " s." << endl;
        cout << "<<<<CGLS=" << iter << endl;
        return 0;
    }

    if (useEigenQR){
        VectorXd x = VectorXd::Zero(ncols);
        VectorXd b = random(nrows,2021);

        A.makeCompressed();
        SparseQR<SpMat, COLAMDOrdering<int>> eigenQR;

        eigenQR.setPivotThreshold(1e-14);
        cout << "\n <<<< Using Eigen SparseQR routine..." << endl;
        timer qr0 = wctime();
        eigenQR.compute(A);
        timer qr1 = wctime();
        cout << "Time to factorize: " << elapsed(qr0, qr1) << " s." << endl;

        timer qrs0 = wctime();
        x = eigenQR.solve(b);
        timer qrs1 = wctime();
        cout << "Time to solve: " << elapsed(qrs0, qrs1) << " s." << endl;
        cout << "Error: " << scientific << (A.transpose()*(A*x-b)).norm() / (A.transpose()*b).norm() << endl;

        return 0;
    }

    if (useCholesky){
        VectorXd x = VectorXd::Zero(ncols);
        VectorXd b = random(nrows,2021);

        VectorXd Atb = A.transpose()*b;
        SpMat AtA = A.transpose()*A;

        AtA.makeCompressed();
        SimplicialLLT<SpMat, Lower, AMDOrdering<int> > eigenCholesky;
        cout << "\n <<<< Using Eigen's SimplicalLL^T routine..." << endl;

        timer qr0 = wctime();
        eigenCholesky.compute(AtA);
        timer qr1 = wctime();
        cout << "Time to factorize: " << elapsed(qr0, qr1) << " s." << endl;

        timer qrs0 = wctime();
        x = eigenCholesky.solve(Atb);
        timer qrs1 = wctime();

        cout << "Time to solve: " << elapsed(qrs0, qrs1) << " s." << endl;
        cout << "Error: " << scientific << (A.transpose()*(A*x-b)).norm() / (A.transpose()*b).norm() << endl;

        return 0;
    }

    // Partition
    t.partition(A);
    // Setup
    t.assemble(A);

    // Factorize
    int err = t.factorize();

    if (!err)
    // Run one solve
    {
         // Random b
        {
            VectorXd b = random(nrows, 2021);
            VectorXd bcopy = b;
            VectorXd x(ncols, 1);
            x.setZero();
            timer tsolv_0 = wctime();

            if (nrows == ncols){
                t.solve(bcopy, x);
                timer tsolv = wctime();
                cout << "<<<<tsolv=" << elapsed(tsolv_0, tsolv) << endl;
                cout << "One-time solve (Random b):" << endl;
                cout << "<<<<|(Ax-b)|/|b| : " << scientific <<  ((A*x-b)).norm() / (b).norm() << endl;
            }
            else {
                t.solve_nrml(A.transpose()*bcopy, x);
                timer tsolv = wctime();
                cout << "<<<<tsolv=" << elapsed(tsolv_0, tsolv) << endl;
                cout << "One-time solve (Random b):" << endl;
                cout << "<<<<|A'(Ax-b)|/|A'b| : " << scientific <<  (A.transpose()*(A*x-b)).norm() / (A.transpose()*b).norm() << endl;
            }
        }
    }

    bool verb = false;
    int iter = 0;
    if (!err)
    {
        VectorXd x = VectorXd::Zero(ncols);
        VectorXd b;
        if ((!result.count("rhs"))){
            b = random(nrows,2021);
        }
        else {
            string rhs_file = result["rhs"].as<string>();
            b = mmio::vector_mmread<double>(rhs_file);
        }
        VectorXd bcopy = b;

        if(useGMRES) {
            timer gmres0 = wctime();
            iter = gmres(A, b, x, t, iterations, iterations, residual, verb);
            timer gmres1 = wctime();
            cout << "GMRES: #iterations: " << iter << ", residual |Ax-b|/|b|: " << (A*x-b).norm() / b.norm() << endl;
            cout << "  GMRES: " << elapsed(gmres0, gmres1) << " s." << endl;
            cout << "<<<<GMRES=" << iter << endl;
        }
        else if(useCGLS){
            timer cg0 = wctime();

            Index max_iters = (long)iterations;
            iter = cgls(A, b, x, t, max_iters, residual, verb);
            cout << "CGLS: #iterations: " << iter << ", residual |A'(Ax-b)|/|A'(b)|: " << (A.transpose()*(A*x-b)).norm() / (A.transpose()*b).norm() << endl;
            timer cg1 = wctime();
            cout << "  CGLS: " << elapsed(cg0, cg1) << " s." << endl;
            cout << "<<<<CGLS=" << iter << endl;
        }
    }

  return 0;
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2016 Michael Rabinovich
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "flip_avoiding_line_search.h"
#include "line_search.h"
#include "PI.h"

#include <Eigen/Dense>
#include <vector>

namespace igl
{
  namespace flip_avoiding
  {
    //---------------------------------------------------------------------------
    // x - array of size 3
    // In case 3 real roots: => x[0], x[1], x[2], return 3
    //         2 real roots: x[0], x[1],          return 2
    //         1 real root : x[0], x[1] ± i*x[2], return 1
    // http://math.ivanovo.ac.ru/dalgebra/Khashin/poly/index.html
    IGL_INLINE int SolveP3(std::vector<double>& x,double a,double b,double c)
    { // solve cubic equation x^3 + a*x^2 + b*x + c
      using namespace std;
      double a2 = a*a;
        double q  = (a2 - 3*b)/9;
      double r  = (a*(2*a2-9*b) + 27*c)/54;
        double r2 = r*r;
      double q3 = q*q*q;
      double A,B;
        if(r2<q3)
        {
          double t=r/sqrt(q3);
          if( t<-1) t=-1;
          if( t> 1) t= 1;
          t=acos(t);
          a/=3; q=-2*sqrt(q);
          x[0]=q*cos(t/3)-a;
          x[1]=q*cos((t+(2*igl::PI))/3)-a;
          x[2]=q*cos((t-(2*igl::PI))/3)-a;
          return(3);
        }
        else
        {
          A =-pow(fabs(r)+sqrt(r2-q3),1./3);
          if( r<0 ) A=-A;
          B = A==0? 0 : q/A;

          a/=3;
          x[0] =(A+B)-a;
          x[1] =-0.5*(A+B)-a;
          x[2] = 0.5*sqrt(3.)*(A-B);
          if(fabs(x[2])<1e-14)
          {
            x[2]=x[1]; return(2);
          }
          return(1);
        }
    }

    IGL_INLINE double get_smallest_pos_quad_zero(double a,double b, double c)
    {
      using namespace std;
      double t1, t2;
      if(std::abs(a) > 1.0e-10)
      {
        double delta_in = pow(b, 2) - 4 * a * c;
        if(delta_in <= 0)
        {
          return INFINITY;
        }

        double delta = sqrt(delta_in); // delta >= 0
        if(b >= 0) // avoid subtracting two similar numbers
        {
          double bd = - b - delta;
          t1 = 2 * c / bd;
          t2 = bd / (2 * a);
        }
        else
        {
          double bd = - b + delta;
          t1 = bd / (2 * a);
          t2 = (2 * c) / bd;
        }

        assert (std::isfinite(t1));
        assert (std::isfinite(t2));

        if(a < 0) std::swap(t1, t2); // make t1 > t2
        // return the smaller positive root if it exists, otherwise return infinity
        if(t1 > 0)
        {
          return t2 > 0 ? t2 : t1;
        }
        else
        {
          return INFINITY;
        }
      }
      else
      {
        if(b == 0) return INFINITY; // just to avoid divide-by-zero
        t1 = -c / b;
        return t1 > 0 ? t1 : INFINITY;
      }
    }

    IGL_INLINE double get_min_pos_root_2D(const Eigen::MatrixXd& uv,
                                          const Eigen::MatrixXi& F,
                                          Eigen::MatrixXd& d,
                                          int f)
    {
      using namespace std;
    /*
          Finding the smallest timestep t s.t a triangle get degenerated (<=> det = 0)
          The following code can be derived by a symbolic expression in matlab:

          Symbolic matlab:
          U11 = sym('U11');
          U12 = sym('U12');
          U21 = sym('U21');
          U22 = sym('U22');
          U31 = sym('U31');
          U32 = sym('U32');

          V11 = sym('V11');
          V12 = sym('V12');
          V21 = sym('V21');
          V22 = sym('V22');
          V31 = sym('V31');
          V32 = sym('V32');

          t = sym('t');

          U1 = [U11,U12];
          U2 = [U21,U22];
          U3 = [U31,U32];

          V1 = [V11,V12];
          V2 = [V21,V22];
          V3 = [V31,V32];

          A = [(U2+V2*t) - (U1+ V1*t)];
          B = [(U3+V3*t) - (U1+ V1*t)];
          C = [A;B];

          solve(det(C), t);
          cf = coeffs(det(C),t); % Now cf(1),cf(2),cf(3) holds the coefficients for the polynom. at order c,b,a
        */

      int v1 = F(f,0); int v2 = F(f,1); int v3 = F(f,2);
      // get quadratic coefficients (ax^2 + b^x + c)
      const double& U11 = uv(v1,0);
      const double& U12 = uv(v1,1);
      const double& U21 = uv(v2,0);
      const double& U22 = uv(v2,1);
      const double& U31 = uv(v3,0);
      const double& U32 = uv(v3,1);

      const double& V11 = d(v1,0);
      const double& V12 = d(v1,1);
      const double& V21 = d(v2,0);
      const double& V22 = d(v2,1);
      const double& V31 = d(v3,0);
      const double& V32 = d(v3,1);

      double a = V11*V22 - V12*V21 - V11*V32 + V12*V31 + V21*V32 - V22*V31;
      double b = U11*V22 - U12*V21 - U21*V12 + U22*V11 - U11*V32 + U12*V31 + U31*V12 - U32*V11 + U21*V32 - U22*V31 - U31*V22 + U32*V21;
      double c = U11*U22 - U12*U21 - U11*U32 + U12*U31 + U21*U32 - U22*U31;

      return get_smallest_pos_quad_zero(a,b,c);
    }

    IGL_INLINE double get_min_pos_root_3D(const Eigen::MatrixXd& uv,
                                          const Eigen::MatrixXi& F,
                                          Eigen::MatrixXd& direc,
                                          int f)
    {
      using namespace std;
      /*
          Searching for the roots of:
            +-1/6 * |ax ay az 1|
                    |bx by bz 1|
                    |cx cy cz 1|
                    |dx dy dz 1|
          Every point ax,ay,az has a search direction a_dx,a_dy,a_dz, and so we add those to the matrix, and solve the cubic to find the step size t for a 0 volume
          Symbolic matlab:
            syms a_x a_y a_z a_dx a_dy a_dz % tetrahedera point and search direction
            syms b_x b_y b_z b_dx b_dy b_dz
            syms c_x c_y c_z c_dx c_dy c_dz
            syms d_x d_y d_z d_dx d_dy d_dz
            syms t % Timestep var, this is what we're looking for

            a_plus_t = [a_x,a_y,a_z] + t*[a_dx,a_dy,a_dz];
            b_plus_t = [b_x,b_y,b_z] + t*[b_dx,b_dy,b_dz];
            c_plus_t = [c_x,c_y,c_z] + t*[c_dx,c_dy,c_dz];
            d_plus_t = [d_x,d_y,d_z] + t*[d_dx,d_dy,d_dz];

            vol_mat = [a_plus_t,1;b_plus_t,1;c_plus_t,1;d_plus_t,1]
            //cf = coeffs(det(vol_det),t); % Now cf(1),cf(2),cf(3),cf(4) holds the coefficients for the polynom
            [coefficients,terms] = coeffs(det(vol_det),t); % terms = [ t^3, t^2, t, 1], Coefficients hold the coeff we seek
      */
      int v1 = F(f,0); int v2 = F(f,1); int v3 = F(f,2); int v4 = F(f,3);
      const double& a_x = uv(v1,0);
      const double& a_y = uv(v1,1);
      const double& a_z = uv(v1,2);
      const double& b_x = uv(v2,0);
      const double& b_y = uv(v2,1);
      const double& b_z = uv(v2,2);
      const double& c_x = uv(v3,0);
      const double& c_y = uv(v3,1);
      const double& c_z = uv(v3,2);
      const double& d_x = uv(v4,0);
      const double& d_y = uv(v4,1);
      const double& d_z = uv(v4,2);

      const double& a_dx = direc(v1,0);
      const double& a_dy = direc(v1,1);
      const double& a_dz = direc(v1,2);
      const double& b_dx = direc(v2,0);
      const double& b_dy = direc(v2,1);
      const double& b_dz = direc(v2,2);
      const double& c_dx = direc(v3,0);
      const double& c_dy = direc(v3,1);
      const double& c_dz = direc(v3,2);
      const double& d_dx = direc(v4,0);
      const double& d_dy = direc(v4,1);
      const double& d_dz = direc(v4,2);

      // Find solution for: a*t^3 + b*t^2 + c*d +d = 0
      double a = a_dx*b_dy*c_dz - a_dx*b_dz*c_dy - a_dy*b_dx*c_dz + a_dy*b_dz*c_dx + a_dz*b_dx*c_dy - a_dz*b_dy*c_dx - a_dx*b_dy*d_dz + a_dx*b_dz*d_dy + a_dy*b_dx*d_dz - a_dy*b_dz*d_dx - a_dz*b_dx*d_dy + a_dz*b_dy*d_dx + a_dx*c_dy*d_dz - a_dx*c_dz*d_dy - a_dy*c_dx*d_dz + a_dy*c_dz*d_dx + a_dz*c_dx*d_dy - a_dz*c_dy*d_dx - b_dx*c_dy*d_dz + b_dx*c_dz*d_dy + b_dy*c_dx*d_dz - b_dy*c_dz*d_dx - b_dz*c_dx*d_dy + b_dz*c_dy*d_dx;

      double b = a_dy*b_dz*c_x - a_dy*b_x*c_dz - a_dz*b_dy*c_x + a_dz*b_x*c_dy + a_x*b_dy*c_dz - a_x*b_dz*c_dy - a_dx*b_dz*c_y + a_dx*b_y*c_dz + a_dz*b_dx*c_y - a_dz*b_y*c_dx - a_y*b_dx*c_dz + a_y*b_dz*c_dx + a_dx*b_dy*c_z - a_dx*b_z*c_dy - a_dy*b_dx*c_z + a_dy*b_z*c_dx + a_z*b_dx*c_dy - a_z*b_dy*c_dx - a_dy*b_dz*d_x + a_dy*b_x*d_dz + a_dz*b_dy*d_x - a_dz*b_x*d_dy - a_x*b_dy*d_dz + a_x*b_dz*d_dy + a_dx*b_dz*d_y - a_dx*b_y*d_dz - a_dz*b_dx*d_y + a_dz*b_y*d_dx + a_y*b_dx*d_dz - a_y*b_dz*d_dx - a_dx*b_dy*d_z + a_dx*b_z*d_dy + a_dy*b_dx*d_z - a_dy*b_z*d_dx - a_z*b_dx*d_dy + a_z*b_dy*d_dx + a_dy*c_dz*d_x - a_dy*c_x*d_dz - a_dz*c_dy*d_x + a_dz*c_x*d_dy + a_x*c_dy*d_dz - a_x*c_dz*d_dy - a_dx*c_dz*d_y + a_dx*c_y*d_dz + a_dz*c_dx*d_y - a_dz*c_y*d_dx - a_y*c_dx*d_dz + a_y*c_dz*d_dx + a_dx*c_dy*d_z - a_dx*c_z*d_dy - a_dy*c_dx*d_z + a_dy*c_z*d_dx + a_z*c_dx*d_dy - a_z*c_dy*d_dx - b_dy*c_dz*d_x + b_dy*c_x*d_dz + b_dz*c_dy*d_x - b_dz*c_x*d_dy - b_x*c_dy*d_dz + b_x*c_dz*d_dy + b_dx*c_dz*d_y - b_dx*c_y*d_dz - b_dz*c_dx*d_y + b_dz*c_y*d_dx + b_y*c_dx*d_dz - b_y*c_dz*d_dx - b_dx*c_dy*d_z + b_dx*c_z*d_dy + b_dy*c_dx*d_z - b_dy*c_z*d_dx - b_z*c_dx*d_dy + b_z*c_dy*d_dx;

      double c = a_dz*b_x*c_y - a_dz*b_y*c_x - a_x*b_dz*c_y + a_x*b_y*c_dz + a_y*b_dz*c_x - a_y*b_x*c_dz - a_dy*b_x*c_z + a_dy*b_z*c_x + a_x*b_dy*c_z - a_x*b_z*c_dy - a_z*b_dy*c_x + a_z*b_x*c_dy + a_dx*b_y*c_z - a_dx*b_z*c_y - a_y*b_dx*c_z + a_y*b_z*c_dx + a_z*b_dx*c_y - a_z*b_y*c_dx - a_dz*b_x*d_y + a_dz*b_y*d_x + a_x*b_dz*d_y - a_x*b_y*d_dz - a_y*b_dz*d_x + a_y*b_x*d_dz + a_dy*b_x*d_z - a_dy*b_z*d_x - a_x*b_dy*d_z + a_x*b_z*d_dy + a_z*b_dy*d_x - a_z*b_x*d_dy - a_dx*b_y*d_z + a_dx*b_z*d_y + a_y*b_dx*d_z - a_y*b_z*d_dx - a_z*b_dx*d_y + a_z*b_y*d_dx + a_dz*c_x*d_y - a_dz*c_y*d_x - a_x*c_dz*d_y + a_x*c_y*d_dz + a_y*c_dz*d_x - a_y*c_x*d_dz - a_dy*c_x*d_z + a_dy*c_z*d_x + a_x*c_dy*d_z - a_x*c_z*d_dy - a_z*c_dy*d_x + a_z*c_x*d_dy + a_dx*c_y*d_z - a_dx*c_z*d_y - a_y*c_dx*d_z + a_y*c_z*d_dx + a_z*c_dx*d_y - a_z*c_y*d_dx - b_dz*c_x*d_y + b_dz*c_y*d_x + b_x*c_dz*d_y - b_x*c_y*d_dz - b_y*c_dz*d_x + b_y*c_x*d_dz + b_dy*c_x*d_z - b_dy*c_z*d_x - b_x*c_dy*d_z + b_x*c_z*d_dy + b_z*c_dy*d_x - b_z*c_x*d_dy - b_dx*c_y*d_z + b_dx*c_z*d_y + b_y*c_dx*d_z - b_y*c_z*d_dx - b_z*c_dx*d_y + b_z*c_y*d_dx;

      double d = a_x*b_y*c_z - a_x*b_z*c_y - a_y*b_x*c_z + a_y*b_z*c_x + a_z*b_x*c_y - a_z*b_y*c_x - a_x*b_y*d_z + a_x*b_z*d_y + a_y*b_x*d_z - a_y*b_z*d_x - a_z*b_x*d_y + a_z*b_y*d_x + a_x*c_y*d_z - a_x*c_z*d_y - a_y*c_x*d_z + a_y*c_z*d_x + a_z*c_x*d_y - a_z*c_y*d_x - b_x*c_y*d_z + b_x*c_z*d_y + b_y*c_x*d_z - b_y*c_z*d_x - b_z*c_x*d_y + b_z*c_y*d_x;

      if (std::abs(a)<=1.e-10)
      {
        return get_smallest_pos_quad_zero(b,c,d);
      }
      b/=a; c/=a; d/=a; // normalize it all
      std::vector<double> res(3);
      int real_roots_num = SolveP3(res,b,c,d);
      switch (real_roots_num)
      {
        case 1:
          return (res[0] >= 0) ? res[0]:INFINITY;
        case 2:
        {
          double max_root = std::max(res[0],res[1]); double min_root = std::min(res[0],res[1]);
          if (min_root > 0) return min_root;
          if (max_root > 0) return max_root;
          return INFINITY;
        }
        case 3:
        default:
        {
          std::sort(res.begin(),res.end());
          if (res[0] > 0) return res[0];
          if (res[1] > 0) return res[1];
          if (res[2] > 0) return res[2];
          return INFINITY;
        }
      }
    }

    IGL_INLINE double compute_max_step_from_singularities(const Eigen::MatrixXd& uv,
                                                          const Eigen::MatrixXi& F,
                                                          Eigen::MatrixXd& d)
    {
      using namespace std;
      double max_step = INFINITY;

      // The if statement is outside the for loops to avoid branching/ease parallelizing
      if (uv.cols() == 2)
      {
        for (int f = 0; f < F.rows(); f++)
        {
          double min_positive_root = get_min_pos_root_2D(uv,F,d,f);
          max_step = std::min(max_step, min_positive_root);
        }
      }
      else
      { // volumetric deformation
        for (int f = 0; f < F.rows(); f++)
        {
          double min_positive_root = get_min_pos_root_3D(uv,F,d,f);
          max_step = std::min(max_step, min_positive_root);
        }
      }
      return max_step;
    }
  }
}

IGL_INLINE double igl::flip_avoiding_line_search(
  const Eigen::MatrixXi F,
  Eigen::MatrixXd& cur_v,
  Eigen::MatrixXd& dst_v,
  std::function<double(Eigen::MatrixXd&)> energy,
  double cur_energy)
{
  using namespace std;
  Eigen::MatrixXd d = dst_v - cur_v;

  double min_step_to_singularity = igl::flip_avoiding::compute_max_step_from_singularities(cur_v,F,d);
  double max_step_size = std::min(1., min_step_to_singularity*0.8);

  return igl::line_search(cur_v,d,max_step_size, energy, cur_energy);
}

#ifdef IGL_STATIC_LIBRARY
#endif


#define _CRT_SECURE_NO_WARNINGS
#include <iostream>
#include <boost/program_options.hpp>
#include "IsingModel.h"

namespace opt = boost::program_options;

int main(int argc, char** argv) {
    int rows, cols, nT, nH, nR, nS, frames, skip;
    double Ti, Tf, Hi, Hf, J, mu, kB;
    bool sim, vid;

    opt::options_description params("Ising Model Parameters");

    params.add_options()
        ("help,h", "show usage")
        ("rows,r", opt::value< int >(&rows)->default_value(15), "number of rows")
        ("cols,c", opt::value< int >(&cols)->default_value(15), "number of columns")
        ("nR", opt::value< int >(&nR)->default_value(1000), "number of relaxation steps")
        ("nS", opt::value< int >(&nS)->default_value(1000), "sample size")
        ("Ti,i", opt::value<double>(&Ti)->default_value(1.5), "initial temperature")
        ("Tf", opt::value<double>(&Tf)->default_value(3.5), "final temperature")
        ("Hi", opt::value<double>(&Hi)->default_value(0.), "initial magnetic field")
        ("Hf", opt::value<double>(&Hf)->default_value(1.), "final magnetic field")
        ("nT,n", opt::value< int >(&nT)->default_value(1000), "number of time steps")
        ("nH", opt::value< int >(&nH)->default_value(1), "number of magnetic field steps")
        ("frames,f", opt::value< int >(&frames)->default_value(1000), "number of frames")
        ("skip,s", opt::value< int >(&skip)->default_value(1), "number of updates between frames")
        ("J", opt::value<double>(&J)->default_value(1.), "ferromagnetic coupling constant")
        ("kB", opt::value<double>(&kB)->default_value(1.), "Boltzmann's constant")
        ("mu", opt::value<double>(&mu)->default_value(1.), "magnetic moment")
        ("vid", opt::value<bool>(&vid)->default_value(true), "create a visualization of the relaxation algorithm")
        ("sim", opt::value<bool>(&sim)->default_value(true), "run simulation and save results to a compressed numpy (npz) file")
        ;

    opt::variables_map vm;
    opt::store(opt::parse_command_line(argc, argv, params), vm);

    if (vm.count("help")) {
        std::cout << params << std::endl;
        return 1;
    }
    else {
        opt::notify(vm);

        std::cout << "\n2D Ising Model Simulation" << std::endl;
        std::cout << "=========================" << std::endl;

        if (sim) { // sim and vid are mutually exclusive, with sim taking precedence

            std::cout << "Running Simulation:" << std::endl;
            IsingModel ising(rows, cols);
            ising.simulate(Ti, Tf, nT, Hi, Hf, nH, nR, nS);

        }
        else {
            std::cout << "Creating Visualization:" << std::endl;
            IsingModel ising(rows, cols);
            ising.visualize(frames, skip, Ti, Hi);
        }

        std::cout << "\n\nDone!" << std::endl;
        std::cout << "=========================" << std::endl;
        return 0;
    }
}