#include <mitsuba/render/fresnel.h>
#include <mitsuba/layer/microfacet.h>
#include <mitsuba/layer/fourier.h>
#include <mitsuba/core/frame.h>
#include <mitsuba/core/math.h>
#include <enoki/special.h>
#include <Eigen/SVD>
#include <Eigen/LU>
#include <cmath>
#include <chrono>
#include <atomic>

using namespace std::chrono;

#if defined HAVE_FFTW
    #include <fftw3.h>
    // #define USE_FFTW
#endif

NAMESPACE_BEGIN(mitsuba)

std::atomic<double> microfacet_timer;

double microfacet(double mu_o, double mu_i, double phi_s, double phi_d,
                  double alpha_u, double alpha_v,
                  std::complex<double> eta_,
                  bool isotropic_g) {
    double phi_i = 0.5 * (phi_s - phi_d);
    double phi_o = 0.5 * (phi_s + phi_d);

    double sin_theta_i = safe_sqrt(1-mu_i*mu_i),
           sin_theta_o = safe_sqrt(1-mu_o*mu_o),
           cos_phi_i = std::cos(phi_i),
           sin_phi_i = std::sin(phi_i),
           cos_phi_o = std::cos(phi_o),
           sin_phi_o = std::sin(phi_o);

    Vector3f wi(-sin_theta_i*cos_phi_i,
                -sin_theta_i*sin_phi_i,
                -mu_i);
    Vector3f wo(sin_theta_o*cos_phi_o,
                sin_theta_o*sin_phi_o,
                mu_o);

    bool reflect = -mu_i*mu_o > 0;

    if (mu_o == 0 || mu_i == 0)
        return 0;

    bool conductor = (std::imag(eta_) != 0);
    if (conductor && !reflect)
        return 0;
    std::complex<double> eta = (-mu_i > 0 || conductor) ? eta_ : std::complex<double>(1) / eta_;

    Vector3f wh = normalize(wi + wo * (reflect ? 1.0 : std::real(eta)));
    wh = enoki::mulsign(wh, Frame3f::cos_theta(wh));

    /* The following two visibility checks are usually part of the smith_G1
       implementations, but are considered to be part of the "Fresnel + Remainder"
       term in this framework. */
    if (dot(wi, wh) * Frame3f::cos_theta(wi) <= 0 || dot(wo, wh) * Frame3f::cos_theta(wo) <= 0)
        return 0;

    double cos_theta_h = (double)Frame3f::cos_theta(wh),
           cos_theta_h_2 = cos_theta_h * cos_theta_h;

    double exponent = ((double) (wh.x() * wh.x()) / (alpha_u * alpha_u) +
                       (double) (wh.y() * wh.y()) / (alpha_v * alpha_v)) / cos_theta_h_2;

    /* For the isotropic shadowing-masking function, take max{alpha_u, alpha_v} to ensure energy conservation. */
    double alpha_u_g = isotropic_g ? std::max(alpha_u, alpha_v) : alpha_u;
    double alpha_v_g = isotropic_g ? alpha_u_g : alpha_v;

    double eta_real = std::real(eta),
           eta_imag = std::imag(eta);

    double D = cos_theta_h <= 0 ? 0.0
                                : std::exp(-exponent) / (math::Pi<double> * alpha_u * alpha_v * cos_theta_h_2 * cos_theta_h_2),
           F = !conductor ? (double) std::get<0>(fresnel(float(dot(wi, wh)), float(std::real(eta_))))
                          : (double) fresnel_conductor(float(dot(wi, wh)), enoki::Complex<float>(eta_real, eta_imag)),
           G = smith_G1(wi, wh, alpha_u_g, alpha_v_g) * smith_G1(wo, wh, alpha_u_g, alpha_v_g);

    if (reflect) {
        return F * D * G / (4 * std::abs(mu_i*mu_o));
    } else {
        double sqrt_denom = (double) dot(wi, wh) + std::real(eta) * (double) dot(wo, wh);

        return std::abs(((1 - F) * D * G * std::real(eta) * std::real(eta) * (double) dot(wi, wh)
                         * (double) dot(wo, wh)) / (mu_i*mu_o * sqrt_denom * sqrt_denom));
    }
}

double microfacet_exp(double mu_o, double mu_i, double phi_s, double phi_d,
                      double alpha_u, double alpha_v,
                      std::complex<double> eta_) {
    double phi_i = 0.5 * (phi_s - phi_d);
    double phi_o = 0.5 * (phi_s + phi_d);

    double sin_theta_i = safe_sqrt(1-mu_i*mu_i),
           sin_theta_o = safe_sqrt(1-mu_o*mu_o),
           cos_phi_i = std::cos(phi_i),
           sin_phi_i = std::sin(phi_i),
           cos_phi_o = std::cos(phi_o),
           sin_phi_o = std::sin(phi_o);

    Vector3f wi(-sin_theta_i*cos_phi_i,
                -sin_theta_i*sin_phi_i,
                -mu_i);
    Vector3f wo(sin_theta_o*cos_phi_o,
                sin_theta_o*sin_phi_o,
                mu_o);

    bool reflect = -mu_i*mu_o > 0;

    if (mu_o == 0 || mu_i == 0)
        return 0;

    bool conductor = (std::imag(eta_) != 0);
    if (conductor && !reflect)
        return 0;
    std::complex<double> eta = (-mu_i > 0 || conductor) ? eta_ : std::complex<double>(1) / eta_;

    Vector3f wh = normalize(wi + wo * (reflect ? 1.0 : std::real(eta)));
    wh = enoki::mulsign(wh, Frame3f::cos_theta(wh));

    double cos_theta_h_2 = (double) Frame3f::cos_theta_2(wh);

    double exponent = ((double) (wh.x() * wh.x()) / (alpha_u * alpha_u) +
                       (double) (wh.y() * wh.y()) / (alpha_v * alpha_v)) / cos_theta_h_2;

    return std::exp(-exponent);
}

double microfacet_fresnel(double mu_o, double mu_i, double phi_s, double phi_d,
                          double alpha_u, double alpha_v,
                          std::complex<double> eta_,
                          bool fresnel_only) {
    (void)phi_s; // Unused

    double sin_theta_i = safe_sqrt(1-mu_i*mu_i),
           sin_theta_o = safe_sqrt(1-mu_o*mu_o),
           cos_phi = std::cos(phi_d),
           sin_phi = std::sin(phi_d);

    Vector3f wi(-sin_theta_i, 0, -mu_i);
    Vector3f wo(sin_theta_o*cos_phi, sin_theta_o*sin_phi, mu_o);

    bool reflect = -mu_i*mu_o > 0;

    if (mu_o == 0 || mu_i == 0)
        return 0;

    bool conductor = (std::imag(eta_) != 0);
    if (conductor && !reflect)
        return 0;
    std::complex<double> eta = (-mu_i > 0 || conductor) ? eta_ : std::complex<double>(1) / eta_;

    Vector3f wh = normalize(wi + wo * (reflect ? 1.0 : std::real(eta)));
    wh = enoki::mulsign(wh, Frame3f::cos_theta(wh));

    /* The following two visibility checks are usually part of the smith_G1
       implementations, but are considered to be part of the "Fresnel + Remainder"
       term in this framework. */
    if (dot(wi, wh) * Frame3f::cos_theta(wi) <= 0 || dot(wo, wh) * Frame3f::cos_theta(wo) <= 0)
        return 0;

    double cos_theta_h = (double) Frame3f::cos_theta(wh),
           cos_theta_h_2 = cos_theta_h * cos_theta_h;

    double eta_real = std::real(eta),
           eta_imag = std::imag(eta);

    double D = cos_theta_h == 0 ? 0.0
                                : 1.0 / (math::Pi<double> * alpha_u * alpha_v * cos_theta_h_2 * cos_theta_h_2),
           F = !conductor ? (double) std::get<0>(fresnel(float(dot(wi, wh)), float(std::real(eta_))))
                          : (double) fresnel_conductor(float(dot(wi, wh)), enoki::Complex<float>(eta_real, eta_imag));

    if (fresnel_only) {
        if (reflect) {
            return F;
        } else {
            return std::abs(1-F);
        }
    }

    if (reflect) {
        return F * D / (4.0 * std::abs(mu_i*mu_o));
    } else {
        double sqrt_denom = (double) dot(wi, wh) + std::real(eta) * (double) dot(wo, wh);
        return std::abs(((1 - F) * D * std::real(eta) * std::real(eta) * (double) dot(wi, wh)
                        * (double) dot(wo, wh)) / (mu_i*mu_o * sqrt_denom * sqrt_denom));
    }
}

double microfacet_G(double mu_o, double mu_i, double phi_s, double phi_d,
                    double alpha_u, double alpha_v,
                    std::complex<double> eta_,
                    bool isotropic_g ) {
    double phi_i = 0.5 * (phi_s - phi_d);
    double phi_o = 0.5 * (phi_s + phi_d);

    double sin_theta_i = safe_sqrt(1-mu_i*mu_i),
           sin_theta_o = safe_sqrt(1-mu_o*mu_o),
           cos_phi_i = std::cos(phi_i),
           sin_phi_i = std::sin(phi_i),
           cos_phi_o = std::cos(phi_o),
           sin_phi_o = std::sin(phi_o);

    Vector3f wi(-sin_theta_i*cos_phi_i,
                -sin_theta_i*sin_phi_i,
                -mu_i);
    Vector3f wo(sin_theta_o*cos_phi_o,
                sin_theta_o*sin_phi_o,
                mu_o);

    bool reflect = -mu_i*mu_o > 0;

    if (mu_o == 0 || mu_i == 0)
        return 0;

    bool conductor = (std::imag(eta_) != 0);
    if (conductor && !reflect)
        return 0;
    std::complex<double> eta = (-mu_i > 0 || conductor) ? eta_ : std::complex<double>(1.0) / eta_;

    Vector3f wh = normalize(wi + wo * (reflect ? 1.0 : std::real(eta)));
    wh = enoki::mulsign(wh, Frame3f::cos_theta(wh));

    /* For the isotropic shadowing-masking function, take max{alpha_u, alpha_v}
       to ensure energy conservation. */
    double alpha_u_g = isotropic_g ? std::max(alpha_u, alpha_v) : alpha_u;
    double alpha_v_g = isotropic_g ? alpha_u_g : alpha_v;

    return smith_G1(wi, wh, alpha_u_g, alpha_v_g) *
           smith_G1(wo, wh, alpha_u_g, alpha_v_g);
}

double smith_G1(const Vector3f &v, const Vector3f &m, double alpha_u, double alpha_v) {
    (void)m; // Unused

    /* The following visibility check usually found in smith_G1 implementations is
       treated in the "Fresnel + Remainder" term in this framework. */
    // if (dot(v, m) * Frame3f::cos_theta(v) <= 0)
    //     return 0;

    const double tan_theta = (double) std::abs(Frame3f::tan_theta(v));
    if (tan_theta == 0)
        return 1;

    double alpha = project_roughness(v, alpha_u, alpha_v);

    double a = 1.0 / (alpha * tan_theta);
    if (a < 1.6) {
        /* Use a fast and accurate (<0.35% rel. error) rational
           approximation to the shadowing-masking function */
        const double a_sqr = a * a;
        return (3.535 * a + 2.181 * a_sqr)
             / (1.0 + 2.276 * a + 2.577 * a_sqr);
    }

    return 1.0;
}

VectorX smith_G1_fourier_series(double mu, double alpha_u, double alpha_v,
                                size_t order, size_t n_samples) {
    /* Here we compute the Fourier series of the Smith Shadowing-Masking component,
       for one of the directions phi_i, phi_o via Filon integration.
       Note that we need to sample the function in the domain [0, pi] and perform
       the integration over [0, 2pi] s.t. the "diagonal" versions of these series
       end up being 2pi-periodic in phi_s, phi_d. */
    double cos_theta     = std::abs(mu),
           sin_theta     = std::sqrt(1.0 - cos_theta*cos_theta),
           tan_theta     = sin_theta / cos_theta;

    double phi_step     =  math::Pi<double> / (n_samples - 1),
           cos_phi_prev =  std::cos(phi_step), cos_phi_cur = 1.0,
           sin_phi_prev = -std::sin(phi_step), sin_phi_cur = 0.0,
           two_cos_phi  =  2.0 * cos_phi_prev;

    VectorXc values(n_samples);

    for (size_t i = 0; i < n_samples; ++i) {
        double cos_phi_next = two_cos_phi*cos_phi_cur - cos_phi_prev,
               sin_phi_next = two_cos_phi*sin_phi_cur - sin_phi_prev;

        double cos_phi_2 = cos_phi_cur*cos_phi_cur;
        double sin_phi_2 = sin_phi_cur*sin_phi_cur;

        double alpha = std::sqrt(cos_phi_2 * alpha_u * alpha_u +
                                 sin_phi_2 * alpha_v * alpha_v);

        double result = 1.0;
        double a = 1.0 / (alpha * tan_theta);
        if (a < 1.6) {
            /* Use a fast and accurate (<0.35% rel. error) rational
               approximation to the shadowing-masking function */
            double a_sqr = a * a;
            result = (3.535 * a + 2.181 * a_sqr)
                 / (1.0 + 2.276 * a + 2.577 * a_sqr);
        }
        values[i] = result;

        cos_phi_prev = cos_phi_cur; cos_phi_cur = cos_phi_next;
        sin_phi_prev = sin_phi_cur; sin_phi_cur = sin_phi_next;
    }

    VectorXc coeffs(order);
    coeffs.setZero();
    filon_integrate_exp(values.data(), n_samples, coeffs.data(), order, 0, 2*math::Pi<double>);

    return coeffs.real();
}

double project_roughness(const Vector3f &v, double alpha_u, double alpha_v) {
    double inv_sin_theta_2 = 1.0 / (double) Frame3f::sin_theta_2(v);

    if ((alpha_u == alpha_v) || inv_sin_theta_2 < 0)
        return alpha_u;

    double cos_phi_2 = (double) (v.x() * v.x()) * inv_sin_theta_2;
    double sin_phi_2 = (double) (v.y() * v.y()) * inv_sin_theta_2;

    return std::sqrt(cos_phi_2 * alpha_u * alpha_u +
                     sin_phi_2 * alpha_v * alpha_v);
}

static int expcos_coefficient_count(double B, double relerr) {
    double prod = 1, invB = 1 / B;
    if (B == 0)
        return 1;

    for (int i = 0; ; ++i) {
        prod /= 1 + i * invB;

        if (prod < relerr)
            return 2*i+1;
    }
}

static double mod_bessel_ratio(double z, double k) {
    const double eps = std::numeric_limits<double>::epsilon(),
                 inv_two_b = 2 / z;

    double i  = (double) k,
           D  = 1 / (inv_two_b * i++),
           Cd = D,
           C  = Cd;

    while (std::abs(Cd) > eps * std::abs(C)) {
        double coeff = inv_two_b * i++;
        D = 1 / (D + coeff);
        Cd *= coeff*D - 1;
        C += Cd;
    }

    return C;
}

static void bessel_functions(double c, double z, int n, VectorX &bessel) {
    bessel.resize(n);

    /* Determine the last ratio and work downwards */
    bessel[n-1] = mod_bessel_ratio(z, n - 1);
    for (int i = n-2; i > 0; --i)
        bessel[i] = z / (2*i + z*bessel[i+1]);

    /* Evaluate the exponentially scaled I0 and correct scaling */
    bessel[0] = enoki::i0e(z) * std::exp(z + c);

    /* Apply the ratios upwards */
    double prod = bessel[0];
    for (int i = 1; i < n; ++i) {
        prod *= bessel[i];
        bessel[i] = prod;
    }
}

static double max_B_heuristic(size_t m, double relerr) {
    if (relerr >= 1e-1)
        return 0.1662*std::pow((double) m, 2.05039);
    else if (relerr >= 1e-2)
        return 0.0818*std::pow((double) m, 2.04982);
    else if (relerr >= 1e-3)
        return 0.0538*std::pow((double) m, 2.05001);
    else if (relerr >= 1e-4)
        return 0.0406*std::pow((double) m, 2.04686);
    else if (relerr >= 1e-5)
        return 0.0337*std::pow((double) m, 2.03865);
    else if (relerr >= 1e-6)
        return 0.0299*std::pow((double) m, 2.02628);
    else {
        Log(Warn, "max_B(): unknown relative error bound!");
        return math::Infinity<double>;
    }
}

void microfacet_reflection_exp_coeffs(double mu_o, double mu_i, double alpha_u, double alpha_v, double phi_s, VectorX &c) {
    c.resize(3);

    double denom = 1.0 / (alpha_u * alpha_v * (mu_i - mu_o)),
           alpha_sqr_sum = (alpha_u * alpha_u + alpha_v * alpha_v),
           alpha_sqr_dif = (alpha_u * alpha_u - alpha_v * alpha_v),
           root = safe_sqrt((1 - mu_i * mu_i) * (1 - mu_o * mu_o)),
           tmp = (mu_i * mu_i + mu_o * mu_o - 2);

    double c0 = 0.5 * denom * denom * alpha_sqr_sum * tmp,
           c1 = denom * denom * alpha_sqr_sum * root,
           c2 = denom * denom * -alpha_sqr_dif * root,
           c3 = 0.5 * denom * denom * -alpha_sqr_dif * (mu_o * mu_o - 1),
           c4 = 0.5 * denom * denom * -alpha_sqr_dif * (mu_i * mu_i - 1);

    std::complex<double> z(c1 + (c3 + c4) * std::cos(phi_s),
                                (c3 - c4) * std::sin(phi_s));

    double A = c0 + c2 * std::cos(phi_s),
           B = std::abs(z),
           C = std::arg(z);

    c[0] = A;
    c[1] = B;
    c[2] = C;
}

void microfacet_refraction_exp_coeffs(double mu_o, double mu_i, double alpha_u, double alpha_v, double phi_s, double eta, VectorX &c) {
    c.resize(3);

    double denom = 1.0 / (alpha_u * alpha_v * (mu_i - eta * mu_o)),
           alpha_sqr_sum = (alpha_u * alpha_u + alpha_v * alpha_v),
           alpha_sqr_dif = (alpha_u * alpha_u - alpha_v * alpha_v),
           root = safe_sqrt((1 - mu_i * mu_i) * (1 - mu_o * mu_o));

    double c0 = 0.5 * denom * denom * alpha_sqr_sum * (mu_i * mu_i - 1 + eta * eta * (mu_o * mu_o - 1)),
           c1 = denom * denom * alpha_sqr_sum * eta * root,
           c2 = denom * denom * -alpha_sqr_dif * eta * root,
           c3 = 0.5 * denom * denom * -alpha_sqr_dif * eta * eta * (mu_o * mu_o - 1),
           c4 = 0.5 * denom * denom * -alpha_sqr_dif * (mu_i * mu_i - 1);

    std::complex<double> z(c1 + (c3 + c4) * std::cos(phi_s),
                                (c3 - c4) * std::sin(phi_s));

    double A = c0 + c2 * std::cos(phi_s),
           B = std::abs(z),
           C = std::arg(z);

    c[0] = A;
    c[1] = B;
    c[2] = C;
}

VectorXc exp_cos_fourier_series(double A, double B, double C, double relerr) {
    VectorXc result;
    int md = expcos_coefficient_count(B, relerr);
    int mdh = md / 2;

    // Create Fourier Series for exp(A + B * cos(phi_d + 0))
    VectorX bessel;
    bessel_functions(A, B, mdh + 1, bessel);

    int truncated_size = bessel.size();
    for (int i = 1; i < bessel.size(); ++i) {
         if (2*std::abs(bessel[i]) < bessel[0] * relerr) {
            truncated_size = i;
            break;
        }
    }
    VectorX bessel_trunc = bessel.head(truncated_size);
    mdh = bessel_trunc.size() - 1;
    md = 2*mdh+1;

    result.resize(md);

    result.head(mdh) = bessel_trunc.tail(mdh).reverse().cast<std::complex<double>>();
    result.tail(mdh + 1) = bessel_trunc.cast<std::complex<double>>();

    if (std::abs(C) > 0) {
        // Shift Fourier Series by phase C
        std::complex<double> exp_phase_inc = std::exp(1i * C);
        std::complex<double> exp_phase = std::exp(-1i * (double) mdh * C);
        for (int i = 0; i < md; ++i) {
            result[i] *= exp_phase;
            exp_phase *= exp_phase_inc;
        }
    }

    return result;
}

inline void sample_function(const std::function<double(double)> &f,
                            double *values, size_t size,
                            double a, double b) {
    double delta = (b - a) / (size - 1);
    for (size_t i = 0; i < size; ++i) {
        double x = a + i*delta;
        values[i] = f(x);
    }
}

inline MatrixX coefficient_conversion_matrix(int m, double phi_a, double phi_b) {
    /* Precompute some sines and cosines */
    VectorX cos_phi_a(m), sin_phi_a(m), cos_phi_b(m), sin_phi_b(m);
    for (int i = 0; i < m; ++i) {
        std::tie(sin_phi_a[i], cos_phi_a[i]) = enoki::sincos(i*phi_a);
        std::tie(sin_phi_b[i], cos_phi_b[i]) = enoki::sincos(i*phi_b);
    }

    MatrixX A(m, m);
    for (int i = 0; i < m; ++i) {
        for (int j = 0; j <= i; ++j) {
            if (i != j) {
                A(i, j) = A(j, i) = (i * cos_phi_b[j] * sin_phi_b[i] +
                                     j * cos_phi_a[i] * sin_phi_a[j] -
                                     i * cos_phi_a[j] * sin_phi_a[i] -
                                     j * cos_phi_b[i] * sin_phi_b[j]) / (i*i - j*j);
            } else if (i != 0) {
                A(i, i) = (std::sin(2*i * phi_b) -
                           std::sin(2*i * phi_a) +
                           2*i * (phi_b - phi_a)) / (4*i);
            } else {
                A(i, i) = phi_b - phi_a;
            }
        }
    }
    return A;
}

VectorX fresnel_fourier_series(double mu_o, double mu_i,
                                double alpha_u, double alpha_v,
                                std::complex<double> eta_,
                                int md, double phi_max,
                                bool svd_reg, bool fresnel_only) {
    int m = md / 2 + 1; // Number of Cosine series coefficients
    VectorX result;

    bool reflect = -mu_i * mu_o > 0;

    double sin_mu_2 = safe_sqrt((1.0 - mu_i * mu_i) * (1.0 - mu_o * mu_o)),
           phi_critical = 0.0;

    bool conductor = (std::imag(eta_) != 0);
    std::complex<double> eta = (-mu_i > 0 || conductor) ? eta_ : std::complex<double>(1) / eta_;

    if (reflect) {
        if (!conductor) {
            double tmp = (2.0*std::real(eta)*std::real(eta) - mu_i*mu_o - 1.0) / sin_mu_2;
            phi_critical = safe_acos(tmp);
        }
    } else if (!reflect) {
        if (conductor) {
            Throw("microfacet_no_exp_fourier_series(): Encountered refraction case for a conductor!");
        }
        double eta_denser = (std::real(eta) > 1 ? std::real(eta) : 1 / std::real(eta));
        double tmp = (1 - eta_denser * mu_i * mu_o) / (eta_denser * sin_mu_2);
        phi_critical = safe_acos(tmp);
    }

    bool phi_critical_inside_interval;
    if (reflect && mu_i > 0) {
        /* For reflection from bottom, phi_critical = 0 results in a high-frequency feature that should be captured. */
        phi_critical_inside_interval = phi_critical < phi_max - math::Epsilon<double>;
    } else {
        phi_critical_inside_interval = phi_critical > math::Epsilon<double> && phi_critical < phi_max - math::Epsilon<double>;
    }

    if (!conductor && phi_critical_inside_interval) {
        /* Uh oh, some high frequency content (critical angle) leaked in the
           generally low frequency part. Increase the number of coefficients so
           that we can capture it. Fortunately, this happens very rarely. */
        m = std::max(m, 100);
        md = 2*m-1;
    }

    VectorX coeffs;
    if (svd_reg) {
        coeffs.resize(m);
    } else {
        coeffs.resize(2*m); // Allocate two times the space for QR regularization below
    }
    coeffs.setZero();

    const int samples = 200;
    auto integrand = std::bind(&microfacet_fresnel, mu_o, mu_i, 0.0, std::placeholders::_1, alpha_u, alpha_v, eta_, fresnel_only);

    if (reflect) {
        if (phi_critical_inside_interval) {
            filon_integrate_cosine(integrand, samples, coeffs.data(), m, 0, phi_critical);
            filon_integrate_cosine(integrand, samples, coeffs.data(), m, phi_critical, phi_max);
        } else {
            filon_integrate_cosine(integrand, samples, coeffs.data(), m, 0, phi_max);
        }
    } else {
        filon_integrate_cosine(integrand, samples, coeffs.data(), m, 0, std::min(phi_critical, phi_max));
    }

    if (phi_max < math::Pi<double> - math::Epsilon<double>) {
        /* The fit only occurs on a subset [0, phi_max], where the Fourier
           basis functions are not orthogonal anymore! The following then
           does a change of basis to proper Fourier coefficients. */

        auto start = high_resolution_clock::now();

        if (svd_reg) {
            MatrixX A = coefficient_conversion_matrix(m, 0, phi_max);

            auto svd = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
            const MatrixX &U = svd.matrixU();
            const MatrixX &V = svd.matrixV();
            const VectorX &sigma = svd.singularValues();

            if (sigma[0] == 0) {
                result.resize(1);
                result(0) = 0;
                return result;
            }

            VectorX temp = VectorX::Zero(m);
            coeffs[0] *= math::Pi<double>;
            coeffs.tail(m-1) *= 0.5 * math::Pi<double>;
            for (int i = 0; i < m; ++i) {
                if (sigma[i] < 1e-9 * sigma[0])
                    break;
                temp += V.col(i) * U.col(i).dot(coeffs) / sigma[i];
            }
            result = temp;
        } else {
            double eps = 1e-5;
            MatrixX A(2*m, m);

            /* We want the fit to be as close-as-possible on the interval [0, phi_max].
             We then add soft regularization constraints for the series to be small outside. */
            A.block(0, 0, m, m) = coefficient_conversion_matrix(m, 0, phi_max);
            A.block(m, 0, m, m) = coefficient_conversion_matrix(m, phi_max, math::Pi<double>) * eps;

            // Scale coefficients
            coeffs *= 0.5*math::Pi<double>;
            coeffs[0] *= 2;

            // QR factorization
            result = A.colPivHouseholderQr().solve(coeffs);
        }

        auto end = high_resolution_clock::now();
        duration<double> diff = (end-start);

        while (true) {
            double old = microfacet_timer;
            double update = old + diff.count();
            if (microfacet_timer.compare_exchange_strong(old, update))
                break;
        }

    } else {
        result = coeffs.head(m);   // No regularization necessary, just get rid of the zero padding from before again.
    }

    VectorX result_exp(md);
    result_exp.head(m - 1) = 0.5*result.tail(m - 1).reverse();
    result_exp[m-1] = result[0];
    result_exp.tail(m - 1) = 0.5*result.tail(m - 1);

    return result_exp;
}

void microfacet_fourier_series(double mu_o, double mu_i,
                               double alpha_u, double alpha_v,
                               std::complex<double> eta_,
                               int ms, int md, double relerr, MatrixX &result,
                               int component, int n_samples_phi_s, bool svd_reg) {
    bool reflect = -mu_i * mu_o > 0;

    bool conductor = (std::imag(eta_) != 0.0);
    std::complex<double> eta = (-mu_i > 0 || conductor) ? eta_ : std::complex<double>(1) / eta_;

    if (!reflect) {
        if (conductor) {
            /* No refraction in conductors */
            result.resize(1, 1);
            result(0, 0) = 0;
            return;
        }
    }

    if (component == 2) {
        /* Only compute shadowing-masking component */
        VectorX smith_G1_coeffs_i = smith_G1_fourier_series(mu_i, alpha_u, alpha_v, 9);
        VectorX smith_G1_coeffs_o = smith_G1_fourier_series(mu_o, alpha_u, alpha_v, 9);

        MatrixX G1_coeffs(9, 9);
        G1_coeffs.setZero();
        G1_coeffs.diagonal(0) = smith_G1_coeffs_o;

        MatrixX G2_coeffs;
        convolve_fourier_series_antidiagonal(smith_G1_coeffs_i, G1_coeffs, G2_coeffs);

        result = G2_coeffs;
        return;
    }

    /* At grazing angles, microfacet BSDFs can contain arbitrarily high frequencies.
       This value sets an upper bound on exponential cosine series parameter 'B' to
       prevent ringing in such cases. */
    double max_B = max_B_heuristic(md/2+1, relerr);

    /* Here we compute the Fourier coefficients of the Microfacet "remainder term", i.e.
       Fresnel and some normalizations. This is isotropic and thus invariant over phi_s,
       so we only compute it once here.
       This part is later convolved with the exponential component, so it makes sense to
       only compute it accurately for the interval [0, phi_max] in which the exponential
       component takes large enough values.
       We find such a conservative interval (useful for all phi_s) with a golden section search here. */
    VectorX fresnel_coeffs;
    if (component == 0 || component == 3) {
        auto gss = [] (const std::function<double(double)>& f, double a, double b, double tol=1e-3) {
            double gr = 0.5*(std::sqrt(5.0) + 1.0);
            double inv_gr = 1.0 / gr;

            while (true) {
                double x1 = b - (b - a) * inv_gr,
                       x2 = a + (b - a) * inv_gr;

                if (std::abs(b - a) < tol)
                    break;
                else if (f(x1) < f(x2))
                    b = x2;
                else
                    a = x1;
            }

            return 0.5*(a + b);
        };

        auto get_phi = [&] (double phi_s) {
            VectorX c;
            if (reflect) {
                microfacet_reflection_exp_coeffs(mu_o, mu_i, alpha_u, alpha_v, phi_s, c);
            } else {
                microfacet_refraction_exp_coeffs(mu_o, mu_i, alpha_u, alpha_v, phi_s, std::real(eta), c);
            }
            double A = c[0];
            double B = c[1];
            double C = c[2];

            if (B > max_B && (std::abs(mu_i) < 0.2 || std::abs(mu_o) < 0.2)) {
                A = A + B - max_B + std::log(enoki::i0e(B) / enoki::i0e(max_B));
                B = max_B;
            }

            double phi_max = safe_acos(1.0 + std::log(relerr) / B);
            return -phi_max - C;
        };

        double phi_s = gss(get_phi, -math::Pi<double>, math::Pi<double>);
        double phi_max = std::min(-get_phi(phi_s), math::Pi<double>);

        fresnel_coeffs = fresnel_fourier_series(mu_o, mu_i,
                                                alpha_u, alpha_v, eta_,
                                                23, phi_max, svd_reg, component==3);
    }

    /* Sample phi_s dimension with regular samples and for each, create a Fourier series in phi_d. */

#ifdef USE_FFTW
    n_samples_phi_s = ms;   // Need as many samples as Fourier orders requested for FFTW integration
#else
    n_samples_phi_s = std::max(2, n_samples_phi_s); // Need at least 2 samples in order to do Filon integration.
#endif

    bool isotropic = false;
    if (alpha_u == alpha_v) {
        /* Isotropic Microfacet model. Only compute a 1D Fourier series and
           zero pad coefficient matrix to have dimension ms x md later. */
        n_samples_phi_s = 1;
        isotropic = true;
    }

    int md_max = 0;
    std::vector<VectorXc> coeffs;
    for (int s = 0; s < n_samples_phi_s; ++s) {
        #ifdef USE_FFTW
            int n_steps = n_samples_phi_s;
        #else
            int n_steps = n_samples_phi_s - 1;
        #endif

        double phi_s = 2 * math::Pi<double> * s / n_steps;

        if (n_samples_phi_s == 1) phi_s = 0;    // Handle division by zero for isotropic cases where ms == 1

        VectorX c;
        if (reflect) {
            microfacet_reflection_exp_coeffs(mu_o, mu_i, alpha_u, alpha_v, phi_s, c);
        } else {
            microfacet_refraction_exp_coeffs(mu_o, mu_i, alpha_u, alpha_v, phi_s, std::real(eta), c);
        }
        double A = c[0];
        double B = c[1];
        double C = c[2];

        /* Minor optimization: don't even bother computing the Fourier series
           if the contribution to the scattering model is miniscule */
        if (enoki::i0e(B) * std::exp(A + B) < 1e-10 && component == 0) {
            VectorXc final_coeffs(1);
            final_coeffs.setZero();
            md_max = std::max(md_max, (int) final_coeffs.size());
            coeffs.push_back(final_coeffs);
            continue;
        }

        if (B > max_B && (std::abs(mu_i) < 0.2 || std::abs(mu_o) < 0.2)) {
            A = A + B - max_B + std::log(enoki::i0e(B) / enoki::i0e(max_B));
            B = max_B;
        }

        /* Compute Fourier coefficients of the exponential term */
        VectorXc exp_coeffs = exp_cos_fourier_series(A, B, C, relerr);

        /* Perform discrete convolution of the exponential & Fresnel series, if requested */
        VectorXc final_coeffs;
        if (component == 0) {
            /* full model */
            convolve_fourier_series(fresnel_coeffs, exp_coeffs, final_coeffs);
        } else if (component == 1) {
            /* exponential part only */
            final_coeffs = exp_coeffs;
        } else if (component == 3) {
            /* Fresnel + remainder part */
            final_coeffs = fresnel_coeffs.cast<std::complex<double>>();
        }

        /* Already truncate very low coefficients before considering integration over phi_s */
        int md_tmp = final_coeffs.size(),
            mdh_tmp = md_tmp / 2;
        double zero_coeff = std::abs(final_coeffs[mdh_tmp]);
        double ref = zero_coeff * relerr;

        if (zero_coeff < relerr) {
            final_coeffs.resize(1);
            final_coeffs[0] = 0;
        } else {
            int md_trunc = md_tmp;
            for (int d = mdh_tmp; d >= 0; --d) {
                int od = d + mdh_tmp;
                if (std::abs(final_coeffs[od]) >= ref) {
                    md_trunc = 2*d+1;
                    break;
                }
            }
            int mdh_trunc = md_trunc / 2;

            VectorXc truncated = final_coeffs.segment(mdh_tmp - mdh_trunc, md_trunc);
            final_coeffs.resize(truncated.size());
            final_coeffs = truncated;
        }

        md_max = std::max(md_max, (int) final_coeffs.size());
        coeffs.push_back(final_coeffs);
    }

    /* Collect data in matrix */
    int md_max_half = md_max / 2;
    MatrixXc temp(n_samples_phi_s, md_max);
    temp.setZero();
    for (int i = 0; i < n_samples_phi_s; ++i) {
        int md = coeffs[i].size();
        int mdh = md / 2;
        temp.block(i, md_max_half - mdh, 1, md) = coeffs[i].transpose();
    }

    MatrixX result_tmp(ms, md_max);
    result_tmp.setZero();

    /* Integrate the sampled Fourier series over phi_s. */

    if (ms == 1) {
        /* We only have a 1D Fourier series, thus no integration over phi_s needed. */
        result_tmp.row(0) = temp.row(0).real();
    } else {
        #ifdef USE_FFTW
            /* Use FFT to compute Fourier Series of phi_s dimension */
            std::complex<double> *data       = (std::complex<double> *) fftw_malloc(sizeof(std::complex<double>) * ms),
                                 *spectrum   = (std::complex<double> *) fftw_malloc(sizeof(std::complex<double>) * ms);

            fftw_plan_with_nthreads(1);
            fftw_plan plan = fftw_plan_dft_1d(ms, (fftw_complex *) data, (fftw_complex *) spectrum, FFTW_FORWARD, FFTW_ESTIMATE);

            for (int d = 0; d < md_max; ++d) {
                for (int s = 0; s < ms; ++s) {
                    data[s] = temp(s, d);
                }
                fftw_execute(plan);
                for (int s = 0; s < ms; ++s) {
                    temp(s, d) = spectrum[s] / (double) ms;
                }
            }

            fftw_destroy_plan(plan);
            fftw_free(spectrum);
            fftw_free(data);

            /* Reorder Fourier coefficients. And we know that the coefficients must be real. */
            int msh = ms / 2;
            result_tmp.row(msh) = temp.row(0).real();
            result_tmp.block(0, 0, msh, md_max) = temp.block(msh + 1, 0, msh, md_max).real();
            result_tmp.block(msh + 1, 0, msh, md_max) = temp.block(1, 0, msh, md_max).real();
        #else
            if (isotropic || component == 3) {
                /* No integration over phi_s necessary */
                int msh = ms / 2;
                result_tmp.block(msh, 0, 1, md_max) = temp.real();
            } else {
                /* Compute Fourier series over phi_s with Filon integration.
                   We can save some time here by exploiting symmetry. Only integrate
                   one half of the data and then mirror it. */
                MatrixXc result_tmp_complex(ms, md_max);
                result_tmp_complex.setZero();
                for (int d = 0; d <= md_max_half; ++d) {
                    int od = d + md_max_half;
                    filon_integrate_exp(temp.col(od).data(), n_samples_phi_s, result_tmp_complex.col(od).data(), ms);
                }
                result_tmp_complex.block(0, 0, ms, md_max_half) = result_tmp_complex.block(0, md_max_half+1, ms, md_max_half);
                result_tmp_complex.block(0, 0, ms, md_max_half) = result_tmp_complex.block(0, 0, ms, md_max_half).colwise().reverse().eval();
                result_tmp_complex.block(0, 0, ms, md_max_half) = result_tmp_complex.block(0, 0, ms, md_max_half).rowwise().reverse().eval();

                /* We know that the coefficients must be real. */
                result_tmp = result_tmp_complex.real();
            }

        #endif
    }

    for (int s = 0; s < ms; ++s) {
      for (int d = 0; d < md_max; ++d) {
        if (std::abs(result_tmp(s, d)) < 1e-9) result_tmp(s, d) = 0;
      }
    }

    /* Truncate Fourier series */
    int md_trunc = md_max,
        ms_trunc = ms;

    int msh = ms / 2,
        mdh = md_max_half;

    double zero_coeff = std::abs(result_tmp(msh, mdh));
    double ref = zero_coeff * relerr * 0.01;

    // Check in phi_s dimension
    for (int s = 0; s <= msh; ++s) {
        bool keep = false;
        for (int d = -mdh; d <= mdh; ++d) {
            double value = std::abs(result_tmp(s + msh, d + mdh));
            if (value > ref) keep = true;
        }
        if (keep) {
            ms_trunc = 2*s+1;
        } else {
            break;
        }
    }

    // Check in phi_d dimension
    for (int d = 0; d <= mdh; ++d) {
        bool keep = false;
        for (int s = -msh; s <= msh; ++s) {
            double value = std::abs(result_tmp(s + msh, d + mdh));
            if (value > ref) keep = true;
        }
        if (keep) {
            md_trunc = 2*d+1;
        } else {
            break;
        }
    }

    int msh_trunc = ms_trunc / 2,
        mdh_trunc = md_trunc / 2;
    result.resize(ms_trunc, md_trunc);

    result = result_tmp.block(msh - msh_trunc, mdh - mdh_trunc, ms_trunc, md_trunc);

    if (component == 0) {
        /* Requested full model: add Smith Shadowing-masking term via 2 diagonal convolutions */
        VectorX smith_G1_coeffs_i = smith_G1_fourier_series(mu_i, alpha_u, alpha_v, 19);
        VectorX smith_G1_coeffs_o = smith_G1_fourier_series(mu_o, alpha_u, alpha_v, 19);

        MatrixX tmp;
        convolve_fourier_series_diagonal(smith_G1_coeffs_o, result, tmp);
        convolve_fourier_series_antidiagonal(smith_G1_coeffs_i, tmp, result);
    }
}

bool microfacet_inside_lowfreq_interval(double mu_o, double mu_i, double phi_s,
                                        double alpha_u, double alpha_v,
                                        std::complex<double> eta,
                                        double phi_d, double relerr) {
    bool reflect = -mu_i * mu_o > 0;

    VectorX c;
    if (reflect) {
        microfacet_reflection_exp_coeffs(mu_o, mu_i, alpha_u, alpha_v, phi_s, c);
    } else {
        microfacet_refraction_exp_coeffs(mu_o, mu_i, alpha_u, alpha_v, phi_s, std::real(eta), c);
    }
    double B = c[1];
    double C = c[2];

    /* Ideally, we would also remove high frequencies here,
       but then this would have to depend on our chosen md as well. */
    // if (B > max_B) {
    //     A = A + B - max_B + std::log(enoki::i0e(B) / enoki::i0e(max_B));
    //     B = max_B;
    // }

    double phi_max = safe_acos(1.0 + std::log(relerr) / B),
           phi_a = -phi_max - C,
           phi_b = +phi_max - C;

    if (phi_d > phi_a && phi_d < phi_b)
        return true;

    return false;
}

NAMESPACE_END(mitsuba)


#include <string>
#include <sstream>
#include <Eigen/Dense>
#include <boost/array.hpp>
#include <boost/multi_array.hpp>
#include <boost/python.hpp>

using namespace boost::python;
typedef float fptype;

#ifndef TRI_CUBIC_INTERPOLATOR_H
#define TRI_CUBIC_INTERPOLATOR_H

//This code is adapted from https://github.com/deepzot/likely
class TriCubicInterpolator{
  // Performs tri-cubic interpolation within a 3D periodic grid.
  // Based on http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.89.7835
  public:
    TriCubicInterpolator(list data, list nkpoints);
    fptype ip(list xyz);
  private:
    boost::multi_array<fptype,1> _data;
    fptype _spacing;
    int _n1, _n2, _n3;
    int _i1, _i2, _i3;
    bool _initialized;
    Eigen::Matrix<fptype,64,1> _coefs;
    Eigen::Matrix<fptype,64,64> _C;
    inline int _index(int i1, int i2, int i3) const {
        if((i1 %= _n1) < 0) i1 += _n1;
        if((i2 %= _n2) < 0) i2 += _n2;
        if((i3 %= _n3) < 0) i3 += _n3;
        return i1 + _n1*(i2 + _n2*i3);
	}
};

#endif


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_FUNCTION_DIVFLOOR_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DIVFLOOR_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing divfloor capabilities

    Computes the floor of the division.

    @par semantic:
    For any given value @c x,  @c y of type @c T:

    @code
    T r = divfloor(x, y);
    @endcode

    The code is similar to:

    @code
    T r = floor(x/y);
    @endcode

    for integral types, if y is @ref Zero, it returns @ref Valmax or @ref Valmin
    if x is positive (resp. negative) and @ref Zero if x is @ref Zero.
    Take also care that dividing @ref Valmin by -1 for signed integral types has
    undefined behaviour.

    @see  divides, rec, divs, divfix, divround, divround2even

  **/
  const boost::dispatch::functor<tag::divfloor_> divfloor = {};
} }
#endif

#include <boost/simd/function/scalar/divfloor.hpp>
#include <boost/simd/function/simd/divfloor.hpp>

#endif


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

/**
 * @file odeint.cpp ODE numerical integration example.
 */

#include <boost/numeric/odeint.hpp>

#include <matplot/matplot.h>

#include "smooth/bundle.hpp"
#include "smooth/compat/odeint.hpp"
#include "smooth/so3.hpp"
#include "smooth/tn.hpp"

#include "plot_tools.hpp"

using matplot::plot;
using std::views::transform;

/**
 * @brief Numerically solve the following ODE on \f$ \mathbb{SO}(3) \times \mathbb{R}^3 \f$:
 */
int main(int argc, char const * argv[])
{
  using state_t = smooth::Bundle<smooth::SO3d, smooth::SO3d>;
  using deriv_t = typename state_t::Tangent;

  std::srand(2);

  // equilibrium point
  const smooth::SO3d Xc = smooth::SO3d::Identity();

  const smooth::SO3d X1 = smooth::SO3d::Random();
  const smooth::SO3d X2 = smooth::SO3d::Random();
  const smooth::SO3d::Tangent d = 0.1 * smooth::SO3d::Tangent::Random();

  auto ode = [&](const state_t & state, deriv_t & deriv, double) {
    deriv.head<3>() = 0.1 * (state.part<0>() - Xc);
    deriv.tail<3>() = 0.1 * (state.part<1>() - Xc);
  };

  auto stepper = boost::numeric::odeint::
    runge_kutta4<state_t, double, deriv_t, double, boost::numeric::odeint::vector_space_algebra>();

  std::vector<double> tvec;
  std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> v1, v2;

  state_t state1(X1, X1 + d), state2(X2, X2 + d);

  boost::numeric::odeint::integrate_const(
    stepper, ode, state1, 0., 10., 0.01, [&](const state_t & s, double t) {
      tvec.push_back(t);
      v1.push_back(s.part<1>() - s.part<0>());
    });

  boost::numeric::odeint::integrate_const(
    stepper, ode, state2, 0., 10., 0.01, [&](const state_t & s, double t) {
      v2.push_back(s.part<1>() - s.part<0>());
    });

  matplot::figure();
  matplot::hold(matplot::on);
  plot(tvec, r2v(v1 | transform([](auto s) { return s(0); })), "r") ->line_width(2);
  plot(tvec, r2v(v2 | transform([](auto s) { return s(0); })), ":r") ->line_width(2);
  plot(tvec, r2v(v1 | transform([](auto s) { return s(1); })), "g") ->line_width(2);
  plot(tvec, r2v(v2 | transform([](auto s) { return s(1); })), ":g") ->line_width(2);
  plot(tvec, r2v(v1 | transform([](auto s) { return s(2); })), "b") ->line_width(2);
  plot(tvec, r2v(v2 | transform([](auto s) { return s(2); })), ":b") ->line_width(2);
  matplot::title("Difference");

  matplot::show();

  return 0;
}


#include <iostream>
#include <mist/mist.h>
#include <itkSignedMaurerDistanceMapImageFilter.h>
#include <Eigen/Core>
#include "ItkImageIO.h"
#include "dataIO.h"
#include "configs.h"

typedef unsigned char label_t;
typedef unsigned char mask_t;
typedef double feat_t;
typedef double atlas_t;

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

	if(argc != 8) {
		std::cerr << "Usage:" << std::endl;
		std::cerr << argv[0] << " <input feature directory> <input label directory> "
				  << " <input abdominal cavity mask directory> "
				  << " <output directory> <filename list> <feature name list> <distance margin>"
			      << std::endl;
		return EXIT_FAILURE;
	}

	std::cout << "----- Read data list -----" << std::endl;
	const std::string input_feat_dir = std::string(argv[1]) + "/";
	const std::string input_label_dir = std::string(argv[2]) + "/";
	const std::string input_abd_mask_dir = std::string(argv[3]) + "/";
	const std::string output_dir = std::string(argv[4]) + "/";
	const std::string filename_list_path = argv[5];
	const std::string feature_name_list_path = argv[6];
	const double margin = std::stod(argv[7]);

	std::list<std::string> filename_list;
	std::list<std::string> feature_name_list;
	if (!get_data_list(filename_list_path, filename_list) ||
		!get_data_list(feature_name_list_path, feature_name_list)) {
		return EXIT_FAILURE;
	}
	const int num_cases = filename_list.size();
	const int num_features = feature_name_list.size();

	std::cout << "----- Compute initial value for each organs -----" << std::endl;
	for (int l = 0; l < NUM_LABELS; ++l) {
		std::cout << "Label: " << LABEL_NAMES[l] << std::endl;
		Eigen::MatrixXd total_feat_mean = Eigen::MatrixXd::Zero(num_features, 1);
		Eigen::MatrixXd total_feat_covariance = Eigen::MatrixXd::Zero(num_features, num_features);

		for (auto case_itr = filename_list.begin(); case_itr != filename_list.end(); ++case_itr) {
			std::cout << "Case: " << *case_itr << std::endl;

			std::cout << "---- Load feature img ----" << std::endl;
			std::vector<std::vector<feat_t>> feature_img_list(num_features);
			std::vector<ImageIO<NDIMS>> feature_mhd_list(num_features);
			for (auto feat_itr = feature_name_list.begin(); feat_itr != feature_name_list.end(); ++feat_itr) {
				size_t i = std::distance(feature_name_list.begin(), feat_itr);
				feature_mhd_list[i].Read(feature_img_list[i],
											input_feat_dir + *feat_itr + "/" + *case_itr);
			}

			std::cout << "---- Load label img ----" << std::endl;
			std::vector<label_t> label_img;
			ImageIO<NDIMS> label_mhd;
			label_mhd.Read(label_img, input_label_dir + *case_itr);
			const int xe = label_mhd.Size(0);
			const int ye = label_mhd.Size(1);
			const int ze = label_mhd.Size(2);
			const int se = xe*ye*ze;
			const double x_spacing = label_mhd.Spacing(0);
			const double y_spacing = label_mhd.Spacing(1);
			const double z_spacing = label_mhd.Spacing(2);

			std::vector<mask_t> abd_mask_img;
			ImageIO<NDIMS> abd_mask_mhd;
			abd_mask_mhd.Read(abd_mask_img, input_abd_mask_dir + *case_itr);

			std::cout << "----- Preprocessing for label -----" << std::endl;
			if (l == NUM_LABELS - 1) { // for others
				for (int s = 0; s < se; s++) {
					if (label_img.at(s) == REMOVE_LABEL_NUM) label_img.at(s) = 0;
					else if (abd_mask_img.at(s) && !label_img.at(s)) label_img.at(s) = 1;
					else label_img.at(s) = 0;
				}
			}
			else { // for organs
				for (int s = 0; s < se; s++) {
					if (label_img.at(s) == l + 1) label_img.at(s) = 1;
					else label_img.at(s) = 0;
				}
			}

			std::cout << "----- Distance transformation -----" << std::endl;
			using DistPixelType = double;
			using LabelPixelType = label_t;
			using DistImageType = itk::Image<DistPixelType, NDIMS>;
			using LabelImageType = itk::Image<LabelPixelType, NDIMS>;
			using SignedMaurerDistanceMapImageFilterType =
				itk::SignedMaurerDistanceMapImageFilter<LabelImageType, DistImageType>;
			SignedMaurerDistanceMapImageFilterType::Pointer distanceMapImageFilter =
				SignedMaurerDistanceMapImageFilterType::New();
			distanceMapImageFilter->SetInput(label_mhd.ConvertVector2Itk(label_img));
			try {
				distanceMapImageFilter->Update();
			}
			catch (itk::ExceptionObject & error)
			{
				std::cerr << "Error:" << error << std::endl;
				return EXIT_FAILURE;
			}
			std::vector<DistPixelType> dist_img;
			label_mhd.ConvertItk2Vector(distanceMapImageFilter->GetOutput(), dist_img);

			std::cout << "----- Calculate parameter ------" << std::endl;
			int label_counter = 0;
			Eigen::MatrixXd feat_mean = Eigen::MatrixXd::Zero(num_features, 1);
			Eigen::MatrixXd feat_covariance = Eigen::MatrixXd::Zero(num_features, num_features);
			for (int s = 0; s < se; s++) {
				if (dist_img.at(s) < -(double)(margin / x_spacing)) {
					label_counter++;
					for (int f = 0; f < num_features; f++) {
						feat_mean(f, 0) += feature_img_list[f].at(s);
						for (int ff = f; ff < num_features; ff++) {
							feat_covariance(f, ff) += feature_img_list[f].at(s) * feature_img_list[ff].at(s);
						}
					}
				}
			}
			feat_mean /= label_counter;
			for (int f = 0; f < num_features; f++) {
				for (int ff = 0; ff < num_features; ff++) {
					feat_covariance(f, ff) = feat_covariance(f, ff) / label_counter - feat_mean(f, 0) * feat_mean(ff, 0);
				}
			}
			total_feat_mean += feat_mean;
			total_feat_covariance += feat_covariance;

		}/* Case loop */
		total_feat_mean /= (double)num_cases;
		total_feat_covariance /= (double)num_cases;
		std::cout << "----- Save param -----" << std::endl;
		std::string result_dir = output_dir + LABEL_NAMES[l];
		make_dir(result_dir);
		save_param_as_csv(total_feat_mean, result_dir + "/mean_param.csv");
		save_param_as_csv(total_feat_covariance, result_dir + "/covariance_param.csv");
		write_raw_and_txt(total_feat_mean, result_dir + "/mean_param");
		write_raw_and_txt(total_feat_covariance, result_dir + "/covariance_param");

	} /* Label loop */

	return EXIT_SUCCESS;
}


#include <Eigen/Dense>
#include <cmath>
#include <random>
#include "thermostats.h"
#include "constants.h"

std::random_device g_device;
std::mt19937 g_mt_rand(g_device());
std::uniform_real_distribution<double> distribution(0.0, 1.0);

Thermostat::Thermostat(double aim_temp) {
    m_aim_temp = aim_temp;
}

VelocityRescale::VelocityRescale(int steps, double aim_temp) {
    m_steps = steps;
    m_aim_temp = aim_temp;
}

Berendsen::Berendsen(double timescale, double aim_temp) {
    m_timescale = timescale;
    m_aim_temp = aim_temp;
}

Andersen::Andersen(double freq, double timestep, double aim_temp) {
    // The user provides us with a timestep in ps
    // and a collision frequency in ps^-1

    m_cutoff = freq * timestep;
    m_aim_temp = aim_temp;
}

void Berendsen::apply(Eigen::ArrayXXd &velocities, const Eigen::ArrayXd &masses) {
    auto velocities_squared = (velocities * velocities).rowwise().sum();
    auto effective_temp = (0.5 * masses * velocities_squared).sum() / constants::boltzHar;
    auto delta_T = effective_temp - m_aim_temp / m_timescale;
}

void VelocityRescale::apply(Eigen::ArrayXXd &velocities, const Eigen::ArrayXd &masses) {
    auto velocities_squared = (velocities * velocities).rowwise().sum();
    auto translational_energy = (0.5 * masses * velocities_squared).sum();
    double thermal_energy = constants::boltzHar * m_aim_temp;
    double velocity_factor = sqrt(thermal_energy / translational_energy);
    velocities *= velocity_factor;
}

void Andersen::apply(Eigen::ArrayXXd &velocities, const Eigen::ArrayXd &masses) {
    //Implements an Andersen thermostat. This keeps the system at
    //constant temperature by the following method:
    //    1) For each atom, calculate a "collision chance" in the last
    //       timestep. If this chance > collision frequency * timestep,
    //       then progress to 2
    //    2) Simulate a "collision" with an external heat bath. This
    //       randomly selects the velocity from a Maxwell-Boltzmann distribution

    for (int atom = 0; atom < velocities.rows(); ++atom) {
        double random_num = distribution(g_mt_rand);

        if (random_num > m_cutoff) {
            // The lucky atom is hit!
            double mean = 0.0;
            double variance = std::sqrt(constants::boltz * m_aim_temp / masses(atom));
            std::normal_distribution<double> gaussian(mean, variance);
            for (int dim=0; dim < velocities.cols(); ++dim) {
                velocities(atom, dim) = gaussian(g_mt_rand);
            }
        }
    }
}


// Copyright 2020 Erik Teichmann <kontakt.teichmann@gmail.com>

#ifndef INCLUDE_SAM_ANALYSIS_SAVITZKY_GOLAY_HPP_
#define INCLUDE_SAM_ANALYSIS_SAVITZKY_GOLAY_HPP_

#include <Eigen/Dense>

#include <stdexcept>
#include <vector>

namespace sam {

template<typename state_type = std::vector<double>>
std::vector<std::vector<double>> SavitzkyGolayFilter(double stepsize,
                                                     state_type state,
                                                     unsigned int n_points,
                                                     unsigned int derivatives);

// Implementation

Eigen::MatrixXd CalculateConvolutionCoefficients(unsigned int n_points,
                                                 unsigned int derivatives) {
  unsigned int n_left = n_points/2;
  Eigen::MatrixXd J(n_points, derivatives);
  for (unsigned int i = 0; i < n_points; ++i) {
    double z = static_cast<int>(i) - static_cast<int>(n_left);
    for (unsigned int j = 0; j < derivatives; ++j) {
      J(i, j) = pow(z, j);
    }
  }
  return (J.transpose()*J).inverse()*J.transpose();
}

int factorial(unsigned int n) {
  int factorial = 1.;
  for (unsigned int i = 1; i < n+1; ++i) factorial *= i;
  return factorial;
}

template<typename state_type>
std::vector<std::vector<double>> SavitzkyGolayFilter(double stepsize,
                                                     state_type state,
                                                     unsigned int n_points,
                                                     unsigned int derivatives) {
  if (n_points%2 == 0) {
    throw std::invalid_argument("Number of points for Savitzky-Golay Filter "
        "has to be uneven.");
  }
  if (n_points < derivatives) {
    throw std::invalid_argument("Number of points has to be bigger than "
        "number of derivatives in Savitzky-Golay Filter.");
  }
  Eigen::MatrixXd coefficients = CalculateConvolutionCoefficients(n_points,
      derivatives);
  std::vector<std::vector<double>> a(derivatives);
  for (size_t point = n_points/2; point < state.size() - n_points/2; ++point) {
    for (size_t deriv = 0; deriv < derivatives; ++deriv) {
      double sum = 0.;
      for (int i = -static_cast<int>(n_points)/2;
           i <= static_cast<int>(n_points)/2; ++i) {
        sum += coefficients(deriv, i + n_points/2)*state[point + i];
      }
      sum *= factorial(deriv)/pow(stepsize, deriv);
      a[deriv].push_back(sum);
    }
  }
  return a;
}

}  // namespace sam

#endif  // INCLUDE_SAM_ANALYSIS_SAVITZKY_GOLAY_HPP_


// SPDX-License-Identifier: Apache-2.0
//
// Copyright 2015 Conrad Sanderson (http://conradsanderson.id.au)
// Copyright 2015 National ICT Australia (NICTA)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------------

#include <armadillo>
#include "catch.hpp"

using namespace arma;

TEST_CASE("gen_randu_1")
  {
  const uword n_rows = 100;
  const uword n_cols = 101;

  mat A(n_rows,n_cols, fill::randu);

  mat B(n_rows,n_cols); B.randu();

  mat C; C.randu(n_rows,n_cols);

  REQUIRE( (accu(A)/A.n_elem) == Approx(0.5).margin(0.02) );
  REQUIRE( (accu(B)/A.n_elem) == Approx(0.5).margin(0.02) );
  REQUIRE( (accu(C)/A.n_elem) == Approx(0.5).margin(0.02) );

  REQUIRE( (mean(vectorise(A))) == Approx(0.5).margin(0.02) );
  }

TEST_CASE("gen_randu_2")
  {
  mat A(50,60,fill::zeros);

  A(span(1,48),span(1,58)).randu();

  REQUIRE( accu(A.head_cols(1)) == Approx(0.0).margin(0.001) );
  REQUIRE( accu(A.head_rows(1)) == Approx(0.0).margin(0.001) );

  REQUIRE( accu(A.tail_cols(1)) == Approx(0.0).margin(0.001) );
  REQUIRE( accu(A.tail_rows(1)) == Approx(0.0).margin(0.001) );

  REQUIRE( mean(vectorise(A(span(1,48),span(1,58)))) == Approx(double(0.5)).margin(0.02) );
  }


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file BlockMatrixBase.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_BLOCK_MATRIX_BASE_HPP
#define STEAM_BLOCK_MATRIX_BASE_HPP

#include <vector>

#include <Eigen/Core>

#include <steam/blockmat/BlockMatrixHelpers.hpp>

namespace steam {

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Interface for a block matrix
/////////////////////////////////////////////////////////////////////////////////////////////
class BlockMatrixBase
{
 public:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Default constructor, matrix size must still be set before using
  //////////////////////////////////////////////////////////////////////////////////////////////
  BlockMatrixBase();

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Rectangular matrix constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  BlockMatrixBase(const std::vector<unsigned int>& blkRowSizes,
                  const std::vector<unsigned int>& blkColSizes);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Block-size-symmetric matrix constructor, pure scalar symmetry is still optional
  //////////////////////////////////////////////////////////////////////////////////////////////
  BlockMatrixBase(const std::vector<unsigned int>& blkSqSizes, bool symmetric = false);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Interface for zero'ing all entries
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual void zero() = 0;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get indexing object
  //////////////////////////////////////////////////////////////////////////////////////////////
  const BlockMatrixIndexing& getIndexing() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get if matrix is symmetric on a scalar level
  //////////////////////////////////////////////////////////////////////////////////////////////
  bool isSymmetric() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Adds the matrix to the block entry at index (r,c), block dim must match
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual void add(unsigned int r, unsigned int c, const Eigen::MatrixXd& m) = 0;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Returns a reference to the value at (r,c), if it exists
  ///        *Note this throws an exception if matrix is symmetric and you request a lower
  ///         triangular entry. For read operations, use copyAt(r,c).
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual Eigen::MatrixXd& at(unsigned int r, unsigned int c) = 0;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Returns a copy of the entry at index (r,c)
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual Eigen::MatrixXd copyAt(unsigned int r, unsigned int c) const = 0;

 private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Whether matrix is symmetric
  //////////////////////////////////////////////////////////////////////////////////////////////
  bool symmetric_;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Block matrix indexing object
  //////////////////////////////////////////////////////////////////////////////////////////////
  BlockMatrixIndexing indexing_;

};

} // steam

#endif // STEAM_BLOCK_MATRIX_BASE_HPP


#include "Octopus.h"
#include <vector>
#include <Eigen/Core>
#include <tinyxml.h>

using namespace FEM;

Octopus::
Octopus()
	:mMuscleStiffness(2E6),mYoungsModulus(1E7),mPoissonRatio(0.3),mMesh(),mTarget()
{
	// mMesh = new RectangleMesh(0.2,1,2,20);
	mMesh = new OBJLoader("../octocon2D/export/octo_ver4.obj");
}

void
Octopus::
AddTarget(const Target& target) {
	mTarget.push_back(target);
	// std::cout << mTarget.size() << std::endl;
	// std::cout << target.idx << std::endl;
}

std::vector<Target>
Octopus::
GetTarget() {
	return mTarget;
}

void
Octopus::
SolveSoftIK(FEM::World* world) {
	std::cout << "SolveSoftIK" << std::endl;
	Eigen::VectorXd X = world->GetPositions();

	Eigen::VectorXd X_prime(world->GetNumVertices()*2);
	X_prime.setZero();
	for(int i=0; i<mTarget.size(); i++) {
		X_prime.block<2,1>(mTarget[i].idx*2,0) = X.block<2,1>(mTarget[i].idx*2,0) - mTarget[i].coord;
	}

}

void
Octopus::
AddMuscle(
	const std::vector<Eigen::Vector3i> indexList,
	const Eigen::Vector2d& fiber_direction
	)
{
	mMuscles.push_back(new Muscle());
	auto muscle = mMuscles.back();
	const auto& vertices = mMesh->GetVertices();
	const auto& triangles = mMesh->GetTriangles();
	std::vector<FEM::Constraint*> constraints;
	Eigen::VectorXd v(vertices.size()*2);

	for(const auto& idx : indexList)
	{
		int i0,i1,i2;
		Eigen::Vector2d p0,p1,p2;

		i0 = idx[0];
		i1 = idx[2];
		i2 = idx[1];
		p0 = vertices[i0];
		p1 = vertices[i1];
		p2 = vertices[i2];

		Eigen::Matrix2d Dm;

		Dm.block<2,1>(0,0) = p1 - p0;
		Dm.block<2,1>(0,1) = p2 - p0;
		if(Dm.determinant()<0)
		{
			i0 = idx[0];
			i1 = idx[1];
			i2 = idx[2];

			p0 = vertices[i0];
			p1 = vertices[i1];
			p2 = vertices[i2];
			Dm.block<2,1>(0,0) = p1 - p0;
			Dm.block<2,1>(0,1) = p2 - p0;

			// muscle->constraints.push_back(new CorotateFEMConstraint(mYoungsModulus,mPoissonRatio,i0,i1,i2,1.0/6.0*(Dm.determinant()),Dm.inverse()));
			// muscle->constraints.push_back(new SpringConstraint(10000.0,i0,i1,(p0-p1).norm()));
			// muscle->constraints.push_back(new SpringConstraint(10000.0,i1,i2,(p1-p2).norm()));
			// muscle->constraints.push_back(new SpringConstraint(10000.0,i2,i0,(p2-p0).norm()));
			muscle->muscleConstraints.push_back(new LinearMuscleConstraint(mMuscleStiffness,fiber_direction,i0,i1,i2,1.0/6.0*(Dm.determinant()),Dm.inverse()));
			// muscle->muscleConstraints.push_back(new HillTypeMuscleConstraint(mMuscleStiffness,fiber_direction,i0,i1,i2,1.0/6.0*(Dm.determinant()),Dm.inverse()));
		}
	}

	// Add Attachment
	// AttachmentConstraint* ground = new AttachmentConstraint(1E8,0,Eigen::Vector2d(0,-0.5));
	// mAttachementConstraintVector.push_back(ground);
	std::vector<AttachmentConstraint*> ground;
	std::vector<int> groundIdx;

	for(int i=981; i<=995; i++) {
		groundIdx.push_back(i);
	}

	for(int i=0; i<groundIdx.size(); i++) {
		Eigen::Vector2d fixed;
		fixed[0] = i / 20.0 - 0.35;
		fixed[1] = 1.15;
		mAttachementConstraintVector.push_back(new AttachmentConstraint(1E8,groundIdx[i],fixed));
	}

	muscle->activationLevel = 0.0;
}
void
Octopus::
Initialize(FEM::World* world)
{
	const auto& vertices = mMesh->GetVertices();
	const auto& triangles = mMesh->GetTriangles();

	for(const auto& tri : triangles)
	{
		int i0,i1,i2;
		Eigen::Vector2d p0,p1,p2;

		i0 = tri[0];
		i1 = tri[2];
		i2 = tri[1];
		p0 = vertices[i0];
		p1 = vertices[i1];
		p2 = vertices[i2];

		Eigen::Matrix2d Dm;

		Dm.block<2,1>(0,0) = p1 - p0;
		Dm.block<2,1>(0,1) = p2 - p0;
		if(Dm.determinant()<0)
		{
			i0 = tri[0];
			i1 = tri[1];
			i2 = tri[2];
			p0 = vertices[i0];
			p1 = vertices[i1];
			p2 = vertices[i2];
			Dm.block<2,1>(0,0) = p1 - p0;
			Dm.block<2,1>(0,1) = p2 - p0;

			mConstraints.push_back(new CorotateFEMConstraint(mYoungsModulus,mPoissonRatio,i0,i1,i2,1.0/6.0*(Dm.determinant()),Dm.inverse()));
		}
	}

	Eigen::VectorXd v(vertices.size()*2);
	for(int i =0;i<vertices.size();i++)
		v.block<2,1>(i*2,0) = vertices[i];

	world->AddBody(v,mConstraints,1.0);

	for(auto& c: mAttachementConstraintVector)
			world->AddConstraint(c);

	for(int i=0;i<mMuscles.size();i++)
	{
		Muscle* muscle = mMuscles[i];

		for(auto& c: muscle->muscleConstraints)
			world->AddConstraint(c);
	}

	mActivationLevel.resize(mMuscles.size());
	mActivationLevel.setZero();
}

void
Octopus::
SetActivationLevel(const Eigen::VectorXd& a)
{
	// std::cout << "****" << mMuscles.size() << std::endl;
	mActivationLevel = a;
	for(int i=0;i<mMuscles.size();i++)
		for(auto& mc : mMuscles[i]->muscleConstraints)
			mc->SetActivationLevel(a[i]);
}

void
MakeMuscles(const std::string& path,Octopus* ms)
{
	TiXmlDocument doc;
	if(!doc.LoadFile(path))
    {
        std::cout<<"Cant open XML file : "<<path<<std::endl;
        return;
    }

    TiXmlElement* muscles = doc.FirstChildElement("Muscles");

    for(TiXmlElement* leg = muscles->FirstChildElement("leg");leg!=nullptr;leg = leg->NextSiblingElement("leg")) {
    	for(TiXmlElement* fiber = leg->FirstChildElement("fiber");fiber!=nullptr;fiber = fiber->NextSiblingElement("fiber")) {

	    	int start_num, end_num;
	    	int start_idx1, end_idx1;
	    	int start_idx2, end_idx2;
	    	int start_idx3, end_idx3;

	    	for(TiXmlElement* start = fiber->FirstChildElement("start");start!=nullptr;start = start->NextSiblingElement("start")) {
	        	start_num = std::stod(start->Attribute("num"));
	        	start_idx1 = std::stod(start->Attribute("idx1"));
				start_idx2 = std::stod(start->Attribute("idx2"));
				start_idx3 = std::stod(start->Attribute("idx3"));
	   	    }

	   	    for(TiXmlElement* end = fiber->FirstChildElement("end");end!=nullptr;end = end->NextSiblingElement("end")) {
	        	end_num = std::stod(end->Attribute("num"));
	        	end_idx1 = std::stod(end->Attribute("idx1"));
				end_idx2 = std::stod(end->Attribute("idx2"));
				end_idx3 = std::stod(end->Attribute("idx3"));
	   	    }

	   	    int cellNum = end_num - start_num + 1;
	   	    int dist = start_idx3 - start_idx1;

	   	    std::vector<Eigen::Vector3i> muscleIndex;

	   	    for(int i=0; i<cellNum/2; i++) {
	   	    	int idx1 = start_idx1 + dist*i;
	   	    	int idx2 = start_idx2 + dist*i;
	   	    	int idx3 = start_idx3 + dist*i;

	   	    	muscleIndex.push_back(Eigen::Vector3i(idx1,idx2,idx3));
	   	    }

			for(int i=0; i<cellNum/2; i++) {
				int idx1 = end_idx1 - dist*i;
	   	    	int idx2 = end_idx2 - dist*i;
	   	    	int idx3 = end_idx3 - dist*i;

	   	    	muscleIndex.push_back(Eigen::Vector3i(idx1,idx2,idx3));
	   	    }

	   	    ms->AddMuscle(muscleIndex,Eigen::Vector2d::UnitY());
   	    }
    }

    // std::cout << "# (Muscle Fiber) : " << muscleIndex.size() << std::endl;
}


#include "modules/bio_base/dna_sequence.h"
#include "modules/bio_base/kmer.h"
#include <gtest/gtest.h>
#include <bitset>
#include <unordered_set>
#include <deque>

constexpr kmer_t fives = 0x5555555555555555l;

#include <boost/multiprecision/cpp_int.hpp>

typedef boost::multiprecision::uint128_t big_kmer_t;
typedef std::vector<big_kmer_t> big_kmers_t;

/*
class patterns
{
public:
	patterns(const std::vector<dna_sequnce> patterns)
	{
		m_fives.resize(patterns.size());
		m_matches.resize(patterns.size());
		m_masks.resize(patterns.size());
		for(size_t i = 0; i < patterns.size(); i++)
		{
			for(size_t

	big_kmers_t apply_base(const big_kmers_t& errs, int base);
	big_kmers_t m_fives;
	big_kmers_t m_matches;
	big_kmers_t m_masks;
};

big_kmers_t apply_base(const big_kmers_t& errs, const big_kmers_t& patterns, const big_kmers_t& patterns,
*/

kmer_t apply_base(kmer_t errs, kmer_t match, kmer_t mask, kmer_t base)
{
	errs >>= 2;
	kmer_t base_rep = fives * base;
	kmer_t diff = base_rep ^ match;
	kmer_t diff_bit = (diff | (diff >> 1)) & fives;
	kmer_t saturate = errs & (errs >> 1);
	kmer_t to_add = diff_bit & ~saturate;
	errs += to_add;
	errs &= mask;
	return errs;
}

TEST(tiny_align, test_it)
{
	dna_sequence match_seq("CTGTCTCTTATACACATCT");
	dna_sequence seek_seq("ACCGTCTGTCTCTTATTACTGTCTCTTATACACATCTGGGTAGA");
	size_t kmer_size = match_seq.size();
	kmer_t mask = (uint64_t(1) << (2*kmer_size)) - 1;
	kmer_t errs = mask;
	kmer_t match = match_seq.as_kmer();
	for(size_t i = 0; i < seek_seq.size(); i++) {
		kmer_t base = (int) seek_seq[i];
		errs = apply_base(errs, match, mask, base);
		printf("%c: %s\n", char(seek_seq[i]),
			std::bitset<64>(errs).to_string().c_str());
	}
}

TEST(tiny_align, test_combin)
{
	dna_sequence match_seq("CTGTCTCTTATACACATCT");
	size_t kmer_size = match_seq.size();
	kmer_t mask = (uint64_t(1) << (2*kmer_size)) - 1;
	kmer_t match = match_seq.as_kmer();
	std::set<kmer_t> found;
	std::deque<kmer_t> to_do;
	found.emplace(mask);
	to_do.push_back(mask);
	while(!to_do.empty()) {
		if (found.size() % 1000 == 0) {
			printf("Found = %d: to_do = %d", int(found.size()), int(to_do.size()));
		}
		kmer_t errs = to_do.front();
		to_do.pop_front();
		for(kmer_t b = 0; b < 4; b++) {
			kmer_t new_err = apply_base(errs, match, mask, b);
			if (found.count(new_err)) {
				continue;
			}
			found.emplace(new_err);
			to_do.push_back(new_err);
		}
	}
	printf("Total found size: %d\n", int(found.size()));
	for(kmer_t k : found) {
		printf("%s\n", std::bitset<64>(k).to_string().c_str());
	}
}


/*=============================================================================================================
                                                BDSimulations.cpp
===============================================================================================================

 Simulations of genetic rescue

 C++-code accompanying:

		(ms. in prep).

 Written by:
        F.J.H. de Haas
       	Theoretical Biology Group
        University of British Columbia
        the Netherlands

 Program version
		xx/xx/xxxx	:

=============================================================================================================*/

#include <assert.h>
#include <stdlib.h>
#include <iomanip>
#include <string>
#include <iostream>
#include "random.h"
#include "utils.h"
#include <progress.hpp>
#include <mutex>
#include <Rcpp.h>
#include <atomic>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>

#ifdef _OPENMP
    #include <omp.h>
#endif

enum typenames {AB,Ab,aB,ab};
using namespace boost::accumulators;

struct Parameters{
    Parameters(const int &in_AB0, const int &in_Ab0,const int &in_aB0, const int &in_ab0, const double &in_bA, const double &in_ba, double const& in_dA, double const& in_da, double const&in_r){
        AB0 = in_AB0;
        Ab0 = in_Ab0;
        aB0 = in_aB0;
        ab0 = in_ab0;
        bA = in_bA;
        ba = in_ba;
        dA = in_dA;
        da = in_da;
        r = in_r;
    }

    Parameters(Rcpp::List parslist){
        AB0 = parslist["AB0"];
        Ab0 = parslist["Ab0"];
        aB0 = parslist["aB0"];
        ab0 = parslist["ab0"];
        bA = parslist["bA"];
        ba = parslist["ba"];
        dA = parslist["dA"];
        da = parslist["da"];
        r = parslist["r"];
    }

    int AB0,Ab0,aB0,ab0;
    double bA,ba,dA,da;
    double r;
};

class BDPopulation{
    public:
    BDPopulation(const Parameters &pars) {
        type[AB].push_back(pars.AB0);
        type[Ab].push_back(pars.Ab0);
        type[aB].push_back(pars.aB0);
        type[ab].push_back(pars.ab0);
    }
    bool Iterate(const Parameters &pars){
        /*parents become offspring*/
        type[AB].push_back(type[AB].back());
        type[Ab].push_back(type[Ab].back());
        type[aB].push_back(type[aB].back());
        type[ab].push_back(type[ab].back());
        /*offspring changes*/
        static double f[4];
        assert(pars.da>=0.0);
        assert(pars.dA>=0.0);
        if(CalculateFrequencies(f)==false) {extinct = true; return false;};
        const double sumdeathrate = (f[AB]+f[Ab])*pars.dA+(f[aB]+f[ab])*pars.da;
        const double sumbirthrate = (f[AB]+f[Ab])*pars.bA+(f[aB]+f[ab])*pars.ba;
        const double Pdeath = sumdeathrate/(sumbirthrate+sumdeathrate);
        const double Pbirth = 1-Pdeath;
        const double rD = pars.r*(f[AB]*f[ab]-f[Ab]*f[aB]);
        const double PAB_death = f[AB]*pars.dA/sumdeathrate;
        const double PAb_death = f[Ab]*pars.dA/sumdeathrate;
        const double PaB_death = f[aB]*pars.da/sumdeathrate;
        const double Pab_death = f[ab]*pars.da/sumdeathrate;
        const double PAB_birth = f[AB]-rD;
        const double PAb_birth = f[Ab]+rD;
        const double PaB_birth = f[aB]+rD;
        const double Pab_birth = f[ab]-rD;

        rnd::discrete_distribution birthdeathevent(8);
        birthdeathevent[birth_AB] = Pbirth * PAB_birth;
        birthdeathevent[birth_Ab] = Pbirth * PAb_birth;
        birthdeathevent[birth_aB] = Pbirth * PaB_birth;
        birthdeathevent[birth_ab] = Pbirth * Pab_birth;

        birthdeathevent[death_AB] = Pdeath * PAB_death;
        birthdeathevent[death_Ab] = Pdeath * PAb_death;
        birthdeathevent[death_aB] = Pdeath * PaB_death;
        birthdeathevent[death_ab] = Pdeath * Pab_death;

        const int sample = birthdeathevent.sample();
        switch(sample) {
            case birth_AB:      ++type[AB].back(); break;
            case birth_Ab:      ++type[Ab].back(); break;
            case birth_aB:      ++type[aB].back(); break;
            case birth_ab:      ++type[ab].back(); break;
            case death_AB:      --type[AB].back(); break;
            case death_Ab:      --type[Ab].back(); break;
            case death_aB:      --type[aB].back(); break;
            case death_ab:      --type[ab].back(); break;
        }
        return true;
    }
    inline int returnTypes(const int &index, const int &gen){
        return type[index][gen];
    }
    inline bool getextinction(const int &gen){
        return (type[AB][gen]+type[Ab][gen])!=0 ? false : true;
    }
    inline double getFA(const int &gen){
        //assert(type[AB][gen]+type[Ab][gen] != 0);
        return (double)type[AB][gen] / (double)(type[AB][gen]+type[Ab][gen]);
    }

    inline double getFa(const int &gen){
        //assert(type[aB][gen]+type[aB][gen] != 0);
        return (double)type[aB][gen] / (double)(type[aB][gen]+type[ab][gen]);
    }

    private:
    bool extinct = false;
    enum distribution {birth_AB,birth_Ab,birth_aB,birth_ab,death_AB,death_Ab,death_aB,death_ab};
    std::vector<int> type[4];
    bool CalculateFrequencies(double *f){
        const double sum=(double)(type[AB].back()+type[Ab].back()+type[aB].back()+type[ab].back());
        if(sum<=0){return false;}
        f[AB] = (double)type[AB].back()/sum;
        f[Ab] = (double)type[Ab].back()/sum;
        f[aB] = (double)type[aB].back()/sum;
        f[ab] = (double)type[ab].back()/sum;
        return true;
    }
};

struct RcppOutput{
    RcppOutput(const unsigned int &ngen) : ngennr(ngen) {
        nofixcounter = 0;
        fixcounter = 0;
        type[AB].resize(ngen);
        type[Ab].resize(ngen);
        type[aB].resize(ngen);
        type[ab].resize(ngen);
        FA.resize(ngen);
        Fa.resize(ngen);
    };
    void pushback_protect(BDPopulation* pop){
        if(pop->getextinction(ngennr)){
            ++nofixcounter;
        }
        else{
            ++fixcounter;
            mu_acc.lock();
            for(int t = 0; t < ngennr; ++t){
                type[AB][t](pop->returnTypes(AB,t));
                type[Ab][t](pop->returnTypes(Ab,t));
                type[aB][t](pop->returnTypes(aB,t));
                type[ab][t](pop->returnTypes(ab,t));
                FA[t](pop->getFA(t));
                if(pop->getFa(t) >= 0.0){
                    Fa[t](pop->getFa(t));
                }
            }
            distribution[AB].push_back(pop->returnTypes(AB,ngennr));
            distribution[Ab].push_back(pop->returnTypes(AB,ngennr));
            distribution[aB].push_back(pop->returnTypes(AB,ngennr));
            distribution[ab].push_back(pop->returnTypes(AB,ngennr));
            mu_acc.unlock();
        }
    };
    Rcpp::List pushout(){
        /* make a Rcpp Vector for first 4 entries of the Rlist. */
        Rcpp::NumericVector RcppAB(ngennr);
        Rcpp::NumericVector RcppAb(ngennr);
        Rcpp::NumericVector RcppaB(ngennr);
        Rcpp::NumericVector Rcppab(ngennr);
        Rcpp::NumericVector RcppFA(ngennr);
        Rcpp::NumericVector RcppFa(ngennr);
        Rcpp::NumericVector RcppAB_var(ngennr);
        Rcpp::NumericVector RcppAb_var(ngennr);
        Rcpp::NumericVector RcppaB_var(ngennr);
        Rcpp::NumericVector Rcppab_var(ngennr);
        Rcpp::NumericVector RcppFA_var(ngennr);
        Rcpp::NumericVector RcppFa_var(ngennr);
        Rcpp::NumericVector Time(ngennr);

        for(int t = 0; t < ngennr; ++t){
            RcppAB[t] = mean(type[AB][t]);
            RcppAb[t] = mean(type[Ab][t]);
            RcppaB[t] = mean(type[aB][t]);
            Rcppab[t] = mean(type[ab][t]);
            RcppFA[t] = mean(FA[t]);
            RcppFa[t] = mean(Fa[t]);

            RcppAB_var[t] = variance(type[AB][t]);
            RcppAb_var[t] = variance(type[Ab][t]);
            RcppaB_var[t] = variance(type[aB][t]);
            Rcppab_var[t] = variance(type[ab][t]);
            RcppFA_var[t] = variance(FA[t]);
            RcppFa_var[t] = variance(Fa[t]);

            Time[t] = t;
        }

        return Rcpp::List::create(
            Rcpp::_["t"] = Time,
            Rcpp::_["AB_mean"] = RcppAB,
            Rcpp::_["Ab_mean"] = RcppAb,
            Rcpp::_["aB_mean"] = RcppaB,
            Rcpp::_["ab_mean"] = Rcppab,
            Rcpp::_["FA_mean"] = RcppFA,
            Rcpp::_["Fa_mean"] = RcppFa,

            Rcpp::_["AB_var"] = RcppAB_var,
            Rcpp::_["Ab_var"] = RcppAb_var,
            Rcpp::_["aB_var"] = RcppaB_var,
            Rcpp::_["ab_var"] = Rcppab_var,
            Rcpp::_["FA_var"] = RcppFA_var,
            Rcpp::_["Fa_var"] = RcppFa_var,

            Rcpp::_["extinct"] = static_cast<double>(nofixcounter) / (static_cast<double>(nofixcounter)+static_cast<double>(fixcounter))
        );
    }

    private:
    const int ngennr;
    std::mutex mu_acc;
    std::atomic<int> nofixcounter, fixcounter;
    std::vector<accumulator_set<int, stats<tag::mean, tag::variance > > > type[4];
    std::vector<accumulator_set<double, stats<tag::mean, tag::variance > > > FA,Fa; // Genetic rescue AB/(AB+Ab) & aB/(aB+ab) (a will go extinct)
    std::vector<int> distribution[4];
};

// [[Rcpp::export]]
Rcpp::List BDSim(const int &nrep, const int &tend, const Rcpp::List &parslist, int setthreads = 0, bool progressbar = true){
    rnd::set_seed();
    Parameters pars(parslist);
    #ifdef _OPENMP
        const static int maxthreads = omp_get_max_threads();
        if(setthreads>0) omp_set_num_threads(setthreads);
        else omp_set_num_threads(maxthreads);
        //REprintf("Parallel activated : Number of threads=%i\n",omp_get_max_threads());
    #endif
    Progress p(nrep, progressbar);

    /*collect data */
    RcppOutput dataframe(tend);
    BDPopulation* arrayPopulation[nrep];

    #pragma omp parallel for /* threadprivate(OUTPUT)*/
    for(int j = 0; j < nrep; ++j){
        /* run simulations in parallel*/
        arrayPopulation[j] = new BDPopulation(pars);
        for(int i = 0; i < tend; ++i){arrayPopulation[j]->Iterate(pars);}
        dataframe.pushback_protect(arrayPopulation[j]);
        delete arrayPopulation[j];
        p.increment();
    }

    /*rservoire*/
    return dataframe.pushout();
}


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//
// @file Functions to test the algorithms that route on Benes and AS-Waksman networks.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE routing_algorithms_test

#include <boost/test/unit_test.hpp>

#include <cassert>

#include <nil/crypto3/zk/snark/routing/as_waksman.hpp>
#include <nil/crypto3/zk/snark/routing/benes.hpp>

using namespace nil::crypto3::zk::snark;

/**
 * Test Benes network routing for all permutations on 2^static_cast<std::size_t>(std::ceil(std::log2(N))) elements.
 */
void test_benes(const std::size_t N) {
    integer_permutation permutation(1ul << static_cast<std::size_t>(std::ceil(std::log2(N))));

    do {
        const benes_routing routing = get_benes_routing(permutation);
        assert(valid_benes_routing(permutation, routing));
    } while (permutation.next_permutation());
}

/**
 * Test AS-Waksman network routing for all permutations on N elements.
 */
void test_as_waksman(const std::size_t N) {
    integer_permutation permutation(N);

    do {
        const as_waksman_routing routing = get_as_waksman_routing(permutation);
        assert(valid_as_waksman_routing(permutation, routing));
    } while (permutation.next_permutation());
}

BOOST_AUTO_TEST_SUITE(routing_algorithms_test_suite)

BOOST_AUTO_TEST_CASE(routing_algorithms_test) {
    std::size_t bn_size = 8;
    printf("* for all permutations on %zu elements\n", bn_size);
    test_benes(bn_size);

    std::size_t asw_max_size = 9;
    for (std::size_t i = 2; i <= asw_max_size; ++i) {
        test_as_waksman(i);
    }
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/**
  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)
**/
//==================================================================================================
#include <boost/simd/function/exponent.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

namespace bs = boost::simd;
namespace bd = boost::dispatch;

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  using p_t = bs::pack<T, N>;
  using iT =  bd::as_integer_t<T>;
  using ip_t = bs::pack<iT, N>;

  T a1[N];
  iT b[N];

  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : T(37.98*i);
    b[i] = bs::exponent(a1[i]) ;
  }
  p_t aa1(&a1[0], &a1[0]+N);
  ip_t bb (&b[0], &b[0]+N);

  STF_IEEE_EQUAL(bs::exponent(aa1), bb);
}

STF_CASE_TPL("Check exponent on pack" , STF_IEEE_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;
  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

template <typename T, std::size_t N, typename Env>
void test_invalid(Env& $)
{
  using p_t  = bs::pack<T, N>;
  using ip_t = bs::pack<bd::as_integer_t<T>, N>;

  STF_IEEE_EQUAL(bs::exponent(bs::Nan<p_t>()), ip_t(0));
  STF_IEEE_EQUAL(bs::exponent(bs::Inf<p_t>()), ip_t(0));
  STF_IEEE_EQUAL(bs::exponent(bs::Minf<p_t>()), ip_t(0));
}

STF_CASE_TPL("Check exponent on invalid values" , STF_IEEE_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;
  test_invalid<T, N>($);
  test_invalid<T, N/2>($);
  test_invalid<T, N*2>($);
}


#include <boost/hana/cartesian_product.hpp>


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_POLYNOMIALS_FUNCTIONS_SCALAR_LAGUERRE_HPP_INCLUDED
#define NT2_POLYNOMIALS_FUNCTIONS_SCALAR_LAGUERRE_HPP_INCLUDED

#include <nt2/polynomials/functions/laguerre.hpp>
#include <nt2/include/functions/scalar/oneplus.hpp>
#include <nt2/include/constants/one.hpp>
#include <boost/dispatch/meta/as_floating.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A1 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( laguerre_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< integer_<A0> >)(scalar_< arithmetic_<A1> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0,A1>::type result_type;

    NT2_FUNCTOR_CALL(2)
    {
      return nt2::laguerre(a0, result_type(a1));
    }
  };

  /////////////////////////////////////////////////////////////////////////////
  // Implementation when type A1 is floating_
  /////////////////////////////////////////////////////////////////////////////
  BOOST_DISPATCH_IMPLEMENT  (laguerre_, tag::cpu_,
                            (A0)(A1),
                            ((scalar_<integer_<A0> >))((scalar_<floating_<A1> > ))
                            )
  {
    typedef typename boost::dispatch::meta::as_floating<A0,A1>::type result_type;
    NT2_FUNCTOR_CALL(2)
    {
      A1 p0 = One<A1>();
      if(a0 == 0) return p0;
      A1 p1 = p0-a1;
      A0 c = 1;
      while(c < a0)
      {
        std::swap(p0, p1);
        p1 = laguerre_next(c, a1, p0, p1);
        ++c;
      }
      return p1;
    }
  private:
    template <class T, class T1, class T2>
    static inline T
    laguerre_next(const uint32_t& n, const T& x, const T1 &Ln, const T2& Lnm1)
    {
      const T np1 = T(oneplus(n));
      return ((T(n) + np1 - x) * Ln - n *Lnm1) / np1;
    }
  };
} }

#endif


// Copyright (c) 2015
// Author: Chrono Law
#include <std.hpp>
using namespace std;

//#define BOOST_DATE_TIME_POSIX_TIME_STD_CONFIG
#include <boost/date_time/gregorian/gregorian.hpp>
using namespace boost::gregorian;
#include <boost/date_time/posix_time/posix_time.hpp>
using namespace boost::posix_time;

//////////////////////////////////////////

void case1()
{
    {
        time_duration td = duration_from_string("1:10:30:001");
        cout << td << endl;

        time_duration td1(1,10,30,1000);
        time_duration td2(1,60,60,1000*1000* 6 + 1000);
    }

    hours h(1);
    minutes m(10);
    seconds s(30);
    millisec ms(1);

    time_duration td = h + m + s + ms;
    time_duration td2 = hours(2) + seconds(10);

    cout << td << td2 << endl;
}

//////////////////////////////////////////
void case2()
{
    time_duration td(1,10,30,1000);
    assert(td.hours() == 1 && td.minutes() == 10 && td.seconds() == 30);
    assert(td.total_seconds() == 1*3600+ 10*60 + 30);
    assert(td.total_milliseconds() == td.total_seconds()*1000 + 1);
    assert(td.fractional_seconds() == 1000);

    hours h(-10);
    assert(h.is_negative());

    time_duration h2 = h.invert_sign();
    assert(!h2.is_negative() && h2.hours() == 10);

    time_duration td1(not_a_date_time);
    assert(td1.is_special() && td1.is_not_a_date_time());

    time_duration td2(neg_infin);
    assert(td2.is_negative() && td2.is_neg_infinity());

}

//////////////////////////////////////////
void case3()
{
    time_duration td1 = hours(1);
    time_duration td2 = hours(2) + minutes(30);
    assert(td1 < td2);
    assert((td1+td2).hours() == 3);
    assert((td1-td2).is_negative());
    assert(td1 * 5 == td2 * 2);
    assert((td1/2).minutes() == td2.minutes());

    time_duration td(1,10,30,1000);
    cout << to_simple_string(td) << endl;
    cout << to_iso_string(td) << endl;

}

//////////////////////////////////////////
void case4()
{
#ifdef BOOST_DATE_TIME_POSIX_TIME_STD_CONFIG
    time_duration td(1,10,30,1000);
    cout << td;
    assert(td.total_milliseconds() ==
            td.total_seconds()*1000);

    assert(td.fractional_seconds() ==1000);
    assert(time_duration::unit()*1000*1000*1000 == seconds(1));

    assert(td.resolution() == boost::date_time::nano);
    assert(td.num_fractional_digits() == 9);

#endif
}

//////////////////////////////////////////
void case5()
{
    ptime p(date(2014,6,8), hours(1));
    ptime p1 = time_from_string("2014-6-8 01:00:00");
    ptime p2 = from_iso_string("20140608T010000");

    cout << p1 << endl << p2;
    {
        ptime p1 = second_clock::local_time();
        ptime p2 = microsec_clock::universal_time();
        cout << p1 << endl << p2;

    }
}

//////////////////////////////////////////
void case6()
{
    ptime p(date(2010,3,20), hours(12)+minutes(30));

    date d = p.date();
    time_duration td = p.time_of_day();
    assert(d.month() == 3 && d.day() == 20);
    assert(td.total_seconds() == 12*3600 + 30*60);

    ptime p1(date(2010,3,20), hours(12)+minutes(30));
    ptime p2 = p1 + hours(3);

    assert(p1 < p2);
    assert(p2 - p1 == hours(3));
    p2 += months(1);
    assert(p2.date().month() == 4);

    cout << endl;
    {
        ptime p(date(2014,2,14), hours(20));
        cout << to_simple_string(p) << endl;
        cout << to_iso_string(p) << endl;
        cout << to_iso_extended_string(p) << endl;
    }
}

//////////////////////////////////////////
void case7()
{
    ptime p(date(2010,2,14), hours(20));
    tm t = to_tm(p);
    assert(t.tm_year == 110 && t.tm_hour == 20);

    ptime p2 = from_time_t(std::time(0));
    assert(p2.date() == day_clock::local_day());
}

//////////////////////////////////////////
void case8()
{
    ptime p(date(2014,1,1),hours(12)) ;
    time_period tp1(p, hours(8));
    time_period tp2(p + hours(8), hours(1));
    assert(tp1.end() == tp2.begin() && tp1.is_adjacent(tp2));
    assert(!tp1.intersects(tp2));

    tp1.shift(hours(1));
    assert(tp1.is_after(p));
    assert(tp1.intersects(tp2));

    tp2.expand(hours(10));
    assert(tp2.contains(p) && tp2.contains(tp1));
}

//////////////////////////////////////////
void case9()
{
    ptime p(date(2014,11,3),hours(10)) ;
    for (time_iterator t_iter(p, minutes(10));
            t_iter < p + hours(1); ++ t_iter)
    {
            cout << *t_iter << endl;
    }

}

//////////////////////////////////////////
template<typename Clock = microsec_clock>
class basic_ptimer
{
    public:
        basic_ptimer()
        {   restart();}
        void restart()
        {   _start_time = Clock::local_time();  }
        void elapsed() const
        {   cout << Clock::local_time() - _start_time;  }
        ~basic_ptimer()
        {   elapsed();  }
    private:
        ptime _start_time;
};
typedef basic_ptimer<microsec_clock> ptimer;
typedef basic_ptimer<second_clock>   sptimer;

class work_time
{
public:
    typedef map<time_period, string> map_t;
private:
    map_t map_ts;
    void init()
    {
        ptime p(day_clock::local_day());

        map_ts[time_period(p, hours(9))] = "It's too early, just relax.\n";
        p += hours(9);
        map_ts[time_period(p, hours(3)+ minutes(30))] = "It's AM, please work hard.\n";
        p += hours(3)+ minutes(30);
        map_ts[time_period(p, hours(1))] = "It's lunch time, are you hungry?\n";
        p += hours(1);
        map_ts[time_period(p, hours(4)+minutes(30))] = "It's PM, ready to go home.\n";
        p += hours(4)+ minutes(30);
        map_ts[time_period(p, hours(6))] = "Are you still working? you do need a rest.\n";
    }
public:
    work_time()
    {   init(); }

    void greeting(const ptime& t)
    {
        for (auto& x : map_ts)
        {
            if (x.first.contains(t))
            {
                cout << x.second << endl;
                break;
            }
        }
    }
};

void case10()
{
    ptimer t;

    work_time wt;
    wt.greeting(second_clock::local_time());
}

//////////////////////////////////////////

int main()
{
    case1();
    case2();
    case3();
    case4();
    case5();
    case6();
    case7();
    case8();
    case9();
    case10();
}


#ifndef NBT_OCTREE_HPP
#define NBT_OCTREE_HPP

#include <Eigen>
#include "rigidbody.hpp"

/**
 * @brief Node struct for Octree.
 */
struct OctreeNode {
    OctreeNode* children[2][2][2]; //<! 3d Array to store child pointers (Axes in order: z y x)

    bool isEmpty;       //<! True if no objects are in this node.
    bool isExternal;    //<! True if this node is external (has no children).

    double xMin, xMax;  //!< Bounds in x dimension
    double yMin, yMax;  //!< Bounds in y dimension
    double zMin, zMax;  //!< Bounds in z dimension

    double totalMass;               //!< Total mass in the region bounded by the node.
    Eigen::Vector3d centerOfMass;   //!< Center of mass of the objects within this node.

    //!< Constructs an OctreeNode object from x, y, z bounds
    OctreeNode(double xMin, double xMax, double yMin, double yMax, double zMin, double zMax);

    //!< Destroys OctreeNode object by destroying children
    ~OctreeNode();

    //!< Recursively adds an object into the subtree that has this node as root.
    void addObject(double m, const Eigen::Ref<const Eigen::Vector3d> pos);

    //<! Deletes all children below this node.
    void prune();
};

#endif