#include <layer/layer.h>
#include <layer/math.h>
#include <layer/hg.h>
#include <layer/microfacet.h>
#include <layer/fresnel.h>
#include <layer/spline.h>
#include <layer/log.h>
#include <filesystem/path.h>
#include <Eigen/SparseLU>
#include <Eigen/Geometry>
#include <tbb/tbb.h>

#if defined(HAVE_FFTW)
    #include <fftw3.h>
#endif

NAMESPACE_BEGIN(layer)

namespace {
    template <typename VectorType> MatrixS sparseDiagonal(const VectorType &vec) {
        MatrixS result(vec.size(), vec.size());
        for (MatrixS::Index i = 0; i < vec.size(); ++i)
            result.insert(i, i) = vec[i];
        result.makeCompressed();
        return result;
    }

    void sparsify(const MatrixX &dense, MatrixS &sparse) {
        sparse.setZero();

        for (MatrixX::Index j = 0; j < dense.cols(); ++j) {
            for (MatrixX::Index i = 0; i < dense.rows(); ++i) {
                Float value = dense.coeff(i, j);
                if (value != 0)
                    sparse.insert(i, j) = value;
            }
        }
        sparse.makeCompressed();
    }

    void scaleColumns(LayerMode &mode, const VectorX &d) {
        if ((size_t) d.size() != mode.resolution())
            throw std::runtime_error("scaleColumns(): size mismatch!");
        MatrixS scale = sparseDiagonal(d.head(d.size()/2));
        mode.transmissionBottomTop = mode.transmissionBottomTop * scale;
        mode.reflectionBottom = mode.reflectionBottom * scale;
        scale = sparseDiagonal(d.tail(d.size()/2));
        mode.transmissionTopBottom = mode.transmissionTopBottom * scale;
        mode.reflectionTop = mode.reflectionTop * scale;
    }

    void applySurfaceIntegrationWeights(Layer &layer) {
        for (size_t l=0; l<layer.fourierOrders(); ++l)
            scaleColumns(layer[l], layer.weights().cwiseProduct(
                                       layer.nodes().cwiseAbs()) *
                                       math::Pi * (l == 0 ? 2 : 1));
    }

    void applyMediumIntegrationWeights(Layer &layer) {
        for (size_t l=0; l<layer.fourierOrders(); ++l)
            scaleColumns(layer[l], layer.weights() * math::Pi * (l == 0 ? 2 : 1));
    }
};

Layer::Layer(const VectorX &nodes, const VectorX &weights, size_t nFourierOrders)
    : m_modes(nFourierOrders, LayerMode(nodes.size())), m_nodes(nodes), m_weights(weights) {
    if (nodes.size() < 2)
        throw std::runtime_error("Need at least 2 integration nodes!");
    else if (nodes.size() % 2 == 1)
        throw std::runtime_error("The number of integration nodes must be even!");
    for (int i=0; i<nodes.size(); ++i)
        if (nodes[i] == 0)
            throw std::runtime_error("The set of integrations includes mu=0 -- this is not allowed.");

    if (nodes[0] < nodes[1]) {
        size_t n = (size_t) nodes.size();
        /* Order integration weights so that they are usable for adding-doubling */
        m_weights.head(n/2).reverseInPlace();
        m_nodes.head(n/2).reverseInPlace();
    }
}

void Layer::setQuartets(const std::vector<Quartet> &quartets) {
    std::vector<std::vector<Eigen::Triplet<Float>>>
        tripletsTbt(fourierOrders()),
        tripletsTtb(fourierOrders()),
        tripletsRb(fourierOrders()),
        tripletsRt(fourierOrders());

    size_t approxSize = quartets.size() / (4 * fourierOrders());

    for (size_t i=0; i<fourierOrders(); ++i) {
        tripletsTbt[i].reserve(approxSize);
        tripletsTtb[i].reserve(approxSize);
        tripletsRb[i].reserve(approxSize);
        tripletsRt[i].reserve(approxSize);
    }

    size_t n = resolution() / 2;
    for (auto const &quartet: quartets) {
        typedef MatrixS::Index Index;
        if (quartet.o < n && quartet.i < n)
            tripletsTbt[quartet.l].emplace_back(Index(quartet.o), Index(quartet.i), quartet.value);
        else if (quartet.o >= n && quartet.i >= n)
            tripletsTtb[quartet.l].emplace_back(Index(quartet.o-n), Index(quartet.i-n), quartet.value);
        else if (quartet.o <n && quartet.i >= n)
            tripletsRt[quartet.l].emplace_back(Index(quartet.o), Index(quartet.i-n), quartet.value);
        else if (quartet.o >=n && quartet.i < n)
            tripletsRb[quartet.l].emplace_back(Index(quartet.o-n), Index(quartet.i), quartet.value);
        else
            throw std::runtime_error("Layer::setFromQuartets(): internal error!");
    }

    tbb::parallel_for(
        tbb::blocked_range<size_t>(0, fourierOrders(), 1),
        [&](const tbb::blocked_range<size_t> &range) {
            for (size_t l = range.begin(); l < range.end(); ++l) {
                m_modes[l].reflectionTop        .setFromTriplets(tripletsRt [l].begin(), tripletsRt [l].end());
                m_modes[l].reflectionBottom     .setFromTriplets(tripletsRb [l].begin(), tripletsRb [l].end());
                m_modes[l].transmissionTopBottom.setFromTriplets(tripletsTtb[l].begin(), tripletsTtb[l].end());
                m_modes[l].transmissionBottomTop.setFromTriplets(tripletsTbt[l].begin(), tripletsTbt[l].end());
            }
        }
    );
}

void Layer::setHenyeyGreenstein(Float albedo, Float g) {
    std::vector<Quartet> quartets;
    tbb::spin_mutex mutex;

    tbb::parallel_for(
        tbb::blocked_range<size_t>(0, resolution()),
        [&](const tbb::blocked_range<size_t> &range) {
            std::vector<Quartet> quartetsLocal;
            quartetsLocal.reserve(fourierOrders() * resolution());
            std::vector<Float> result;
            for (size_t i = range.begin(); i < range.end(); ++i) {
                for (size_t o = 0; o <= i; ++o) {
                    hgFourierSeries(m_nodes[o], m_nodes[i], g, (int) fourierOrders(),
                                    ERROR_GOAL, result);
                    for (size_t l=0; l<std::min(fourierOrders(), result.size()); ++l) {
                        quartetsLocal.emplace_back(l, o, i, result[l] * albedo);
                        if (i != o)
                            quartetsLocal.emplace_back(l, i, o, result[l] * albedo);
                    }
                }
                tbb::spin_mutex::scoped_lock lock(mutex);
                quartets.insert(quartets.end(), quartetsLocal.begin(), quartetsLocal.end());
            }
        }
    );
    setQuartets(quartets);
    applyMediumIntegrationWeights(*this);
}

void Layer::setVonMisesFisher(Float albedo, Float kappa) {
    std::vector<Quartet> quartets;
    tbb::spin_mutex mutex;

    Float scale;
    if (kappa == 0)
        scale = albedo / (4 * math::Pi);
    else
        scale = albedo * kappa / (4 * math::Pi * std::sinh(kappa));

    tbb::parallel_for(
        tbb::blocked_range<size_t>(0, resolution()),
        [&](const tbb::blocked_range<size_t> &range) {
            std::vector<Quartet> quartetsLocal;
            quartetsLocal.reserve(fourierOrders() * resolution());
            std::vector<Float> result;
            for (size_t i = range.begin(); i < range.end(); ++i) {
                Float mu_i = m_nodes[i];
                for (size_t o = 0; o <= i; ++o) {
                    Float mu_o = m_nodes[o];
                    Float A = kappa * mu_i * mu_o;
                    Float B = kappa * math::safe_sqrt((1 - mu_i * mu_i) *
                                                      (1 - mu_o * mu_o));
                    expCosFourierSeries(A, B, ERROR_GOAL, result);

                    for (size_t l=0; l<std::min(fourierOrders(), result.size()); ++l) {
                        quartetsLocal.emplace_back(l, o, i, result[l] * scale);
                        if (i != o)
                            quartetsLocal.emplace_back(l, i, o, result[l] * scale);
                    }
                }
                tbb::spin_mutex::scoped_lock lock(mutex);
                quartets.insert(quartets.end(), quartetsLocal.begin(), quartetsLocal.end());
            }
        }
    );

    setQuartets(quartets);
    applyMediumIntegrationWeights(*this);
}

void Layer::setDiffuse(Float albedo) {
    std::vector<Quartet> quartets;
    quartets.reserve(resolution() * resolution() / 2);

    size_t n = resolution(), h = n/2;
    for (size_t i=0; i<n; ++i) {
        for (size_t o=0; o<n; ++o) {
            if ((i < h && o >= h) || (o < h && i >= h))
                quartets.emplace_back(0, o, i, albedo * math::InvPi);
        }
    }

    setQuartets(quartets);
    applySurfaceIntegrationWeights(*this);
}

void Layer::setIsotropic(Float albedo) {
    std::vector<Quartet> quartets;
    quartets.reserve(resolution() * resolution());

    size_t n = resolution();
    for (size_t i=0; i<n; ++i)
        for (size_t o=0; o<n; ++o)
            quartets.emplace_back(0, o, i, albedo * math::InvFourPi);

    setQuartets(quartets);
    applyMediumIntegrationWeights(*this);
}

void Layer::setMicrofacet(std::complex<Float> eta, Float alpha, bool conserveEnergy,
                          size_t fourierOrdersTarget) {
    size_t n = resolution(), h = n/2;
    std::vector<Quartet> quartets;
    tbb::spin_mutex mutex;

    fourierOrdersTarget = std::max(fourierOrdersTarget, fourierOrders());

    tbb::parallel_for(
        tbb::blocked_range<size_t>(0, resolution()),
        [&](const tbb::blocked_range<size_t> &range) {
            std::vector<Quartet> quartetsLocal;
            quartetsLocal.reserve(fourierOrdersTarget * resolution());
            std::vector<Float> result;
            for (size_t i = range.begin(); i < range.end(); ++i) {
                for (size_t o=0; o<n; ++o) {
                    /* Sign flip due to different convention (depth values
                     * increase opposite to the normal direction) */
                    microfacetFourierSeries(-m_nodes[o], -m_nodes[i], eta,
                                            alpha, fourierOrdersTarget,
                                            ERROR_GOAL, result);

                    for (size_t l=0; l<std::min(fourierOrders(), result.size()); ++l)
                        quartetsLocal.emplace_back(l, o, i, result[l]);
                }
            }
            tbb::spin_mutex::scoped_lock lock(mutex);
            quartets.insert(quartets.end(), quartetsLocal.begin(), quartetsLocal.end());
        }
    );

    setQuartets(quartets);

    /* Add a pseudo-diffuse term to capture lost energy */
    if (conserveEnergy && eta.imag() == 0) {
        /* Case 1: Dielectrics */
        VectorX W = m_weights.tail(h).cwiseProduct(m_nodes.tail(h)) * 2 * math::Pi;
        LayerMode &l = m_modes[0];

        VectorX Mb  = (W.asDiagonal() * MatrixX(l.reflectionBottom)).colwise().sum();
        VectorX Mt  = (W.asDiagonal() * MatrixX(l.reflectionTop)).colwise().sum();
        VectorX Mtb = (W.asDiagonal() * MatrixX(l.transmissionTopBottom)).colwise().sum();
        VectorX Mbt = (W.asDiagonal() * MatrixX(l.transmissionBottomTop)).colwise().sum();

        /* Determine how much energy we'd like to put into the transmission component
           (proportional to the current reflection/reflaction split) */
        VectorX Atb = (VectorX::Ones(h) - Mt - Mtb).cwiseProduct(Mtb.cwiseQuotient(Mt + Mtb));
        VectorX Abt = (VectorX::Ones(h) - Mb - Mbt).cwiseProduct(Mbt.cwiseQuotient(Mb + Mbt));
        Atb = Atb.cwiseMax(VectorX::Zero(h));
        Abt = Abt.cwiseMax(VectorX::Zero(h));

        /* Create a correction matrix which contains as much of the desired
           energy as possible, while maintaining symmetry and energy conservation */
        MatrixX Ctb = Abt*Atb.transpose() / std::max(W.dot(Abt), W.dot(Atb) / (eta.real()*eta.real()));
        MatrixX Cbt = Ctb.transpose() / (eta.real()*eta.real());

        sparsify(MatrixX(l.transmissionTopBottom) + Ctb, l.transmissionTopBottom);
        sparsify(MatrixX(l.transmissionBottomTop) + Cbt, l.transmissionBottomTop);

        /* Update missing energy terms */
        Mtb = (W.asDiagonal() * MatrixX(l.transmissionTopBottom)).colwise().sum();
        Mbt = (W.asDiagonal() * MatrixX(l.transmissionBottomTop)).colwise().sum();

        /* Put the rest of the missing energy into the reflection component */
        VectorX At = VectorX::Ones(h) - Mt - Mtb;
        VectorX Ab = VectorX::Ones(h) - Mb - Mbt;
        At = At.cwiseMax(VectorX::Zero(h));
        Ab = Ab.cwiseMax(VectorX::Zero(h));

        sparsify(MatrixX(l.reflectionTop)    + At*At.transpose() / W.dot(At), l.reflectionTop);
        sparsify(MatrixX(l.reflectionBottom) + Ab*Ab.transpose() / W.dot(Ab), l.reflectionBottom);
    } else if (conserveEnergy && eta.imag() != 0) {
        /* Case 2: Conductors */
        VectorX W = m_weights.tail(h).cwiseProduct(m_nodes.tail(h)) * 2 * math::Pi;

        /* Compute a reference matrix for a material *without* Fresnel effects */
        MatrixX refMatrix(n, n);

        tbb::parallel_for(
            tbb::blocked_range<size_t>(0, resolution()),
            [&](const tbb::blocked_range<size_t> &range) {
                std::vector<Float> result;
                for (size_t i = range.begin(); i < range.end(); ++i) {
                    /* Parallel loop over 'i' */
                    for (size_t o = 0; o < n; ++o) {
                        microfacetFourierSeries(
                            -m_nodes[o], -m_nodes[i], std::complex<Float>(0.0f, 1.0f), alpha,
                            fourierOrdersTarget, ERROR_GOAL, result);
                        refMatrix(o, i) = result.size() > 0 ? result[0] : 0.0f;
                    }
                }
            }
        );

        MatrixX reflectionTopRef = MatrixX(refMatrix).block(0, h, h, h);

        VectorX Mt = VectorX::Ones(h) - (W.asDiagonal() * reflectionTopRef).colwise().sum().transpose();
        Mt = Mt.cwiseMax(VectorX::Zero(h));

        Float F = fresnelConductorIntegral(eta);
        Float E = 1 - W.dot(Mt) * math::InvPi;

        Float factor = F*E / (1-F*(1-E));

        MatrixX C = Mt * Mt.transpose() * (factor/ W.dot(Mt));

        sparsify(MatrixX(m_modes[0].reflectionTop) + C, m_modes[0].reflectionTop);
        sparsify(MatrixX(m_modes[0].reflectionBottom) + C, m_modes[0].reflectionBottom);
    }

    applySurfaceIntegrationWeights(*this);
}

void Layer::add(const Layer &layer1, const Layer &layer2, Layer &output, bool homogeneous) {
    if (output.resolution() != layer1.resolution() &&
        output.resolution() != layer2.resolution() &&
        output.fourierOrders() != layer1.fourierOrders() &&
        output.fourierOrders() != layer2.fourierOrders())
        throw std::runtime_error("Layer::addLayer(): incompatible sizes!");

    size_t n = output.resolution() / 2;
    MatrixS I((int) n, (int) n);
    I.setIdentity();

    /* Special case: it is possible to save quite a bit of computation when we
       know that both layers are homogeneous and of the same type */
    if (homogeneous) {
        tbb::parallel_for(
            tbb::blocked_range<size_t>(0, layer1.fourierOrders(), 1),
            [&](const tbb::blocked_range<size_t> &range) {
                MatrixS Rb, Ttb;
                for (size_t i = range.begin(); i < range.end(); ++i) {
                    const LayerMode &l1 = layer1[i], &l2 = layer2[i];
                    LayerMode &lo = output[i];

                    /* Gain for downward radiation */
                    Eigen::SparseLU<MatrixS, Eigen::AMDOrdering<int>> G_tb;
                    G_tb.compute(I - l1.reflectionBottom * l2.reflectionTop);

                    /* Transmission at the bottom due to illumination at the top */
                    MatrixS result = G_tb.solve(l1.transmissionTopBottom);
                    Ttb = l2.transmissionTopBottom * result;

                    /* Reflection at the bottom */
                    MatrixS temp = l1.reflectionBottom * l2.transmissionBottomTop;
                    result = G_tb.solve(temp);
                    Rb = l2.reflectionBottom + l2.transmissionTopBottom * result;

                    #if defined(DROP_THRESHOLD)
                        Ttb.prune((Float) 1, (Float) DROP_THRESHOLD);
                        Rb.prune((Float) 1, (Float) DROP_THRESHOLD);
                    #endif

                    lo.transmissionTopBottom = Ttb;
                    lo.transmissionBottomTop = Ttb;
                    lo.reflectionTop = Rb;
                    lo.reflectionBottom = Rb;
                }
            }
        );
    } else {
        tbb::parallel_for(
            tbb::blocked_range<size_t>(0, layer1.fourierOrders(), 1),
            [&](const tbb::blocked_range<size_t> &range) {
                for (size_t i = range.begin(); i < range.end(); ++i) {
                    const LayerMode &l1 = layer1[i], &l2 = layer2[i];
                    LayerMode &lo = output[i];

                    /* Gain for downward radiation */
                    Eigen::SparseLU<MatrixS, Eigen::AMDOrdering<int>> G_tb;
                    G_tb.compute(I - l1.reflectionBottom * l2.reflectionTop);

                    /* Gain for upward radiation */
                    Eigen::SparseLU<MatrixS, Eigen::AMDOrdering<int>> G_bt;
                    G_bt.compute(I - l2.reflectionTop * l1.reflectionBottom);

                    /* Transmission at the bottom due to illumination at the top */
                    MatrixS result = G_tb.solve(l1.transmissionTopBottom);
                    MatrixS Ttb = l2.transmissionTopBottom * result;

                    /* Reflection at the bottom */
                    MatrixS temp = l1.reflectionBottom * l2.transmissionBottomTop;
                    result = G_tb.solve(temp);
                    MatrixS Rb = l2.reflectionBottom + l2.transmissionTopBottom * result;

                    /* Transmission at the top due to illumination at the bottom */
                    result = G_bt.solve(l2.transmissionBottomTop);
                    MatrixS Tbt = l1.transmissionBottomTop * result;

                    /* Reflection at the top */
                    temp = l2.reflectionTop * l1.transmissionTopBottom;
                    result = G_bt.solve(temp);
                    MatrixS Rt = l1.reflectionTop + l1.transmissionBottomTop * result;

                    #if defined(DROP_THRESHOLD)
                        Ttb.prune((Float) 1, (Float) DROP_THRESHOLD);
                        Tbt.prune((Float) 1, (Float) DROP_THRESHOLD);
                        Rb.prune((Float) 1, (Float) DROP_THRESHOLD);
                        Rt.prune((Float) 1, (Float) DROP_THRESHOLD);
                    #endif

                    lo.transmissionTopBottom = Ttb;
                    lo.transmissionBottomTop = Tbt;
                    lo.reflectionTop = Rt;
                    lo.reflectionBottom = Rb;
                }
            }
        );
    }
}

#if !defined(HAVE_FFTW)
void Layer::setMatusik(const fs::path &, int, int) {
    throw std::runtime_error("setMatusik(): You need to recompile with support for FFTW!");
}
#else
void Layer::setMatusik(const fs::path &path, int ch, int order) {
    if (ch < 0 || ch >= 3)
        throw std::runtime_error("Channel must be between 1 and 3");
    double scale[3] = { 1.0/1500.0, 1.15/1500.0, 1.66/1500.0 };

    FILE *f = fopen(path.str().c_str(), "rb");
    if (f == nullptr)
        throw std::runtime_error("I/O error: could not open file " + path.str());

    struct {
        int res_theta_h;
        int res_theta_d;
        int res_phi_d;
    } header;

    if (fread(&header, sizeof(int), 3, f) != 3)
        throw std::runtime_error("I/O error while loading header");

    Log("Loading Matusik-style BRDF data file \"%s\" (%ix%ix%i)",
        path.str(), header.res_theta_h, header.res_theta_d, header.res_phi_d);

    size_t nValues = 3 * header.res_theta_h * header.res_theta_d * header.res_phi_d;

    double *storage = new double[nValues];
    if (fread(storage, sizeof(double), nValues, f) != nValues)
        throw std::runtime_error("I/O error while loading file contents");

    fftw_plan_with_nthreads(1);

    order = std::max(order, (int) fourierOrders());
    int fftSize = order * 4;
    fftw_plan plan = fftw_plan_r2r_1d(fftSize, NULL, NULL, FFTW_REDFT00, FFTW_ESTIMATE);

    size_t n = resolution(), nEntries = n*n;
    tbb::spin_mutex mutex;
    std::vector<Quartet> quartets;

    tbb::parallel_for(
        tbb::blocked_range<size_t>(0, nEntries, 1),
        [&](const tbb::blocked_range<size_t> &range) {
            for (size_t entry = range.begin(); entry < range.end(); ++entry) {
                int i = entry / n, o = entry % n;

                Float cosThetaI = m_nodes[i],
                      sinThetaI = std::sqrt(1-cosThetaI*cosThetaI),
                      cosThetaO = m_nodes[o],
                      sinThetaO = std::sqrt(1-cosThetaO*cosThetaO);

                Vector wi(sinThetaI, 0, cosThetaI);

                if (cosThetaI * cosThetaO > 0 || cosThetaI < 0)
                    continue;

                double *data = (double *) fftw_malloc(fftSize * sizeof(double));

                for (int j=0; j<fftSize; ++j) {
                    Float phi_d = M_PI * j/(Float) (fftSize-1),
                          cosPhi = std::cos(phi_d),
                          sinPhi = std::sin(phi_d);
                    Vector wo(-sinThetaO*cosPhi, -sinThetaO*sinPhi, -cosThetaO);

                    Vector half = (wi + wo).normalized();

                    Float theta_half = std::acos(half.z());
                    Float phi_half = std::atan2(half.y(), half.x());

                    Vector diff =
                          Eigen::AngleAxis<Float>(-theta_half, Vector(0, 1, 0)) *
                         (Eigen::AngleAxis<Float>(-phi_half, Vector(0, 0, 1)) * wi);

                    int theta_half_idx = std::min(std::max(0, (int) std::sqrt(
                        ((theta_half / (M_PI/2.0))*header.res_theta_h) * header.res_theta_h)), header.res_theta_h-1);

                    Float theta_diff = std::acos(diff.z());
                    Float phi_diff = std::atan2(diff.y(), diff.x());

                    if (phi_diff < 0)
                        phi_diff += M_PI;

                    int phi_diff_idx = std::min(std::max(0, int(phi_diff / M_PI * header.res_phi_d)), header.res_phi_d - 1);

                    int theta_diff_idx = std::min(std::max(0, int(theta_diff / (M_PI * 0.5) * header.res_theta_d)),
                        header.res_theta_d - 1);

                    int ind = phi_diff_idx +
                        theta_diff_idx * header.res_phi_d +
                        theta_half_idx * header.res_phi_d * header.res_theta_d;

                    data[j] = storage[ind+ch*header.res_theta_h*header.res_theta_d*header.res_phi_d] * scale[ch] * 2; /// XXX too dark?
                }
                double *spectrum = (double *) fftw_malloc(fftSize * sizeof(double));
                fftw_execute_r2r(plan, data, spectrum);

                for (int j=0; j<fftSize; ++j)
                    spectrum[j] /= (double) (fftSize-1);
                spectrum[0] /= 2;
                spectrum[fftSize-1] /= 2;

                double ref = std::abs(spectrum[0]);
                size_t sparseSize = 0;
                double partialSum = 0;
                if (ref != 0) {
                    sparseSize = fourierOrders();
                    for (size_t j= fourierOrders()-1; j>=1; --j) {
                        double value = (float) spectrum[j];
                        partialSum += std::abs(value);
                        if (partialSum <= ref * ERROR_GOAL)
                            sparseSize = j;
                    }
                }

                tbb::spin_mutex::scoped_lock lock(mutex);
                for (size_t l=0; l<sparseSize; ++l)
                    quartets.push_back(Quartet(l, o, i, (Float) spectrum[l]));
                fftw_free(spectrum);
                fftw_free(data);
            }
        }
    );

    fftw_destroy_plan(plan);
    delete[] storage;

    setQuartets(quartets);
    applySurfaceIntegrationWeights(*this);
}
#endif

void Layer::reverse() {
    for (auto &m: m_modes)
        m.reverse();
}

void Layer::clear() {
    for (auto &m: m_modes)
        m.clear();
}

void Layer::expand(Float target_tau) {
    /* Heuristic for choosing the initial width of a layer based on
       "Discrete Space Theory of Radiative Transfer" by Grant and Hunt
       Proc. R. Soc. London 1969 */
    Float tau = std::min(m_nodes.cwiseAbs().minCoeff() * 2, (Float) std::pow(2.0, -15.0));

    size_t doublings = (size_t) std::ceil(std::log(target_tau / tau) / std::log(2.0));
    tau = target_tau * std::pow((Float) 2.0f, -(Float) doublings);

    size_t n = resolution() / 2;

    MatrixS I((int) n, (int) n);
    I.setIdentity();

    MatrixS rowScale = sparseDiagonal(m_nodes.tail(n).cwiseInverse() * tau);

    tbb::parallel_for(
        tbb::blocked_range<size_t>(0, fourierOrders(), 1),
        [&](const tbb::blocked_range<size_t> &range) {
            for (size_t i = range.begin(); i < range.end(); ++i) {
                LayerMode &mode = m_modes[i];
                MatrixS Rt = rowScale * mode.reflectionTop;
                MatrixS Ttb = I + rowScale * (mode.transmissionTopBottom - I);

                mode.reflectionTop = Rt;
                mode.reflectionBottom = Rt;
                mode.transmissionTopBottom = Ttb;
                mode.transmissionBottomTop = Ttb;
            }
        }
    );

    for (size_t i = 0; i < (size_t) doublings; ++i)
        addToTop(*this, true);
}

std::string Layer::toString() const {
    size_t nz = 0, nz_max = resolution() * resolution() * fourierOrders();
    for (auto const &mode: m_modes)
        nz += mode.nonZeros();
    std::ostringstream oss;
    oss.precision(2);
    oss << "Layer[resolution=" << resolution() << "x" << resolution()
        << ", fourierOrders=" << fourierOrders() << ", nonZeros=" << nz << "/"
        << nz_max << " (" << ((Float) nz / (Float) nz_max * 100) << "%)]";
    return oss.str();
}

MatrixX Layer::matrix(size_t l) const {
    if (l >= m_modes.size())
        throw std::runtime_error("Layer::matrix(): out of bounds!");
    size_t n = resolution(), h = n / 2;
    MatrixX M(n, n);

    M.topLeftCorner(h, h)     = m_modes[l].transmissionBottomTop;
    M.bottomRightCorner(h, h) = m_modes[l].transmissionTopBottom;
    M.topRightCorner(h, h)    = m_modes[l].reflectionTop;
    M.bottomLeftCorner(h, h)  = m_modes[l].reflectionBottom;

    for (size_t i = 0; i < n; ++i)
        M.col(i) /= m_weights[i] * std::abs(m_nodes[i]) * (l == 0 ? 2.f : 1.f) * math::Pi;

    M.block(0, 0, h, n) = M.block(0, 0, h, n).colwise().reverse().eval();
    M.block(0, 0, h, n) = M.block(0, 0, n, h).rowwise().reverse().eval();

    return M;
}

Float Layer::eval(Float mu_o, Float mu_i, Float phi_d) const {
    int n = m_nodes.size(), h = n / 2;
    ssize_t offset_o, offset_i;
    Float weights_o[4], weights_i[4];

    if (mu_o < 0 || (mu_o == 0 && mu_i > 0)) {
        spline::evalSplineWeights(m_nodes.data() + h, h, -mu_o, offset_o, weights_o, true);
    } else {
        spline::evalSplineWeights(m_nodes.data() + h, h, mu_o, offset_o, weights_o, true);
        offset_o += h;
    }

    if (mu_i < 0 || (mu_i == 0 && mu_o > 0)) {
        spline::evalSplineWeights(m_nodes.data() + h, h, -mu_i, offset_i, weights_i, true);
    } else {
        spline::evalSplineWeights(m_nodes.data() + h, h, mu_i, offset_i, weights_i, true);
        offset_i += h;
    }

    Float result = 0;
    for (size_t l=0; l<fourierOrders(); ++l) {
        Float sum = 0;
        for (int o = 0; o < 4; ++o) {
            for (int i = 0; i < 4; ++i) {
                Float weight = weights_o[o] * weights_i[i];
                if (weight == 0)
                    continue;
                weight /=
                    std::abs(m_nodes[offset_i + i]) *
                    m_weights[offset_i + i] *
                    (l == 0 ? 2 : 1);

                sum += m_modes[l].coeff(offset_o + o, offset_i + i) * weight;
            }
        }
        result += sum * std::cos(phi_d * l);
    }

    return std::max((Float) 0.f, result / math::Pi);
}

std::string LayerMode::toString() const {
    size_t nz = nonZeros(), nz_max = resolution() * resolution();
    std::ostringstream oss;
    oss.precision(2);
    oss << "LayerMode[resolution=" << resolution() << "x" << resolution()
        << ", nonZeros=" << nz << "/" << nz_max << " ("
        << ((Float) nz / (Float) nz_max * 100) << "%)]";
    return oss.str();
}

std::pair<int, int> parameterHeuristicMicrofacet(Float alpha, std::complex<Float> &eta) {
    alpha = std::min(alpha, (Float) 1);
    if (eta.real() < 1 && eta.imag() == 0)
        eta = std::complex<Float>(1.f) / eta;

    static const Float c[][9] = {
        /* IOR    A_n      B_n     C_n       D_n      A_m      B_m      C_m      D_m                                 */
        {  0.0, 35.275,  14.136,  29.287,  1.8765,   39.814,  88.992, -98.998,  39.261  },  /* Generic conductor     */
        {  1.1, 256.47, -73.180,  99.807,  37.383,  110.782,  57.576,  94.725,  14.001  },  /* Dielectric, eta = 1.1 */
        {  1.3, 100.264, 28.187,  64.425,  14.850,   45.809,  17.785, -7.8543,  12.892  },  /* Dielectric, eta = 1.3 */
        {  1.5, 74.176,  27.470,  42.454,  9.6437,   31.700,  44.896, -45.016,  19.643  },  /* Dielectric, eta = 1.5 */
        {  1.7, 80.098,  17.016,  50.656,  7.2798,   46.549,  58.592, -73.585,  25.473  },  /* Dielectric, eta = 1.7 */
    };

    int i0 = 0, i1 = 0;

    if (eta.imag() == 0) { /* Dielectric case */
        for (int i=1; i<4; ++i) {
            if (eta.real() >= c[i][0] && eta.real() <= c[i+1][0]) {
                if (std::abs(eta.real()-c[i][0]) < 0.05f) {
                    i1 = i0 = i;
                } else if (std::abs(eta-c[i+1][0]) < 0.05f) {
                    i0 = i1 = i+1;
                } else {
                    i0 = i; i1 = i+1;
                }
            }
        }

        if (!i0)
            throw std::runtime_error("Index of refraction is out of bounds (must be between 1.1 and 1.7)!");
    }

    Float n0 = std::max(c[i0][1] + c[i0][2]*std::pow(std::log(alpha), (Float) 4)*alpha, c[i0][3]+c[i0][4]*std::pow(alpha, (Float) -1.2f));
    Float n1 = std::max(c[i1][1] + c[i1][2]*std::pow(std::log(alpha), (Float) 4)*alpha, c[i1][3]+c[i1][4]*std::pow(alpha, (Float) -1.2f));
    Float m0 = std::max(c[i0][5] + c[i0][6]*std::pow(std::log(alpha), (Float) 4)*alpha, c[i0][7]+c[i0][8]*std::pow(alpha, (Float) -1.2f));
    Float m1 = std::max(c[i1][5] + c[i1][6]*std::pow(std::log(alpha), (Float) 4)*alpha, c[i1][7]+c[i1][8]*std::pow(alpha, (Float) -1.2f));

    int n_i = (int) std::ceil(std::max(n0, n1));
    int m_i = (int) std::ceil(std::max(m0, m1));

    if (n_i % 2 == 1)
        n_i += 1;

    return std::make_pair(n_i, m_i);
}

std::pair<int, int> parameterHeuristicHG(Float g) {
    g = std::abs(g);
    Float m = 5.4f/(1.0f - g) - 1.3f;
    Float n = 8.6f/(1.0f - g) - 0.2f;
    int n_i = (int) std::ceil(n), m_i = (int) std::ceil(m);
    if (n_i % 2 == 1)
        n_i += 1;
    return std::make_pair(n_i, m_i);
}

NAMESPACE_END(layer)


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_FILTERING_FILTER_WLS_HPP
#define PIC_FILTERING_FILTER_WLS_HPP

#include "../filtering/filter.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Sparse"
    #include "../externals/Eigen/src/SparseCore/SparseMatrix.h"
#else
    #include <Eigen/Sparse>
    #include <Eigen/src/SparseCore/SparseMatrix.h>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

class FilterWLS: public Filter
{
protected:
    /**
     * @brief singleChannel applies WLS smoothing filter for gray-scale images.
     * @param imgIn
     * @param imgOut
     * @return
     */
    Image *singleChannel(ImageVec imgIn, Image *imgOut)
    {
        Image *L = imgIn[0];

        int width  = L->width;
        int height = L->height;
        int tot    = height * width;

        Eigen::VectorXd b, x;
        b = Eigen::VectorXd::Zero(tot);

        #ifdef PIC_DEBUG
            printf("Init matrix...");
        #endif

        std::vector< Eigen::Triplet< double > > tL;

        for(int i = 0; i < height; i++) {
            int tmpInd = i * width;

            for(int j = 0; j < width; j++) {

                float Ltmp, tmp;
                int indJ;
                int indI = tmpInd + j;
                float Lref = L->data[indI];

                b[indI] = Lref;

                float sum = 0.0f;

                if((i - 1) >= 0) {
                    indJ = indI - width;
                    Ltmp = L->data[indJ];
                    tmp  = -lambda / (powf(fabsf(Ltmp - Lref), alpha) + epsilon);
                    tL.push_back(Eigen::Triplet< double > (indI, indJ, tmp));
                    sum += tmp;
                }

                if((i + 1) < height) {
                    indJ = indI + width;
                    Ltmp = L->data[indJ];
                    tmp  = -lambda / (powf(fabsf(Ltmp - Lref), alpha) + epsilon);
                    tL.push_back(Eigen::Triplet< double > (indI, indJ, tmp));
                    sum += tmp;
                }

                if((j - 1) >= 0) {
                    indJ = indI - 1;
                    Ltmp = L->data[indJ];
                    tmp  = -lambda / (powf(fabsf(Ltmp - Lref), alpha) + epsilon);
                    tL.push_back(Eigen::Triplet< double > (indI, indJ, tmp));
                    sum += tmp;
                }

                if((j + 1) < width) {
                    indJ = indI + 1;
                    Ltmp = L->data[indJ];
                    tmp  = -lambda / (powf(fabsf(Ltmp - Lref), alpha) + epsilon);
                    tL.push_back(Eigen::Triplet< double > (indI, indJ, tmp));
                    sum += tmp;
                }

                tL.push_back(Eigen::Triplet< double > (indI, indI, 1.0f - sum));
            }
        }

        #ifdef PIC_DEBUG
            printf("Ok\n");
        #endif

        Eigen::SparseMatrix<double> A = Eigen::SparseMatrix<double>(tot, tot);
        A.setFromTriplets(tL.begin(), tL.end());

        Eigen::SimplicialCholesky<Eigen::SparseMatrix<double> > solver(A);
        x = solver.solve(b);

        if(solver.info() != Eigen::Success) {
            #ifdef PIC_DEBUG
                printf("SOLVER FAILED!\n");
            #endif
            return NULL;
        }

        #ifdef PIC_DEBUG
            printf("SOLVER SUCCESS!\n");
        #endif

        #pragma omp parallel for

        for(int i = 0; i < tot; i++) {
            imgOut->data[i] = float(x(i));
        }

        return imgOut;
    }

    /**
     * @brief multiChannel applies WLS filter for color images.
     * @param imgIn
     * @param imgOut
     * @return
     */
    Image *multiChannel(ImageVec imgIn, Image *imgOut)
    {
        Image *img = imgIn[0];

        int width  = img->width;
        int height = img->height;
        int tot    = height * width;

        alpha /= 2.0f;

        int stridex = width * img->channels;

        #ifdef PIC_DEBUG
            printf("Init matrix...");
        #endif

        std::vector< Eigen::Triplet< double > > tL;

        for(int i = 0; i < height; i++) {
            int tmpInd = i * width;

            for(int j = 0; j < width; j++) {

                float sum = 0.0f;
                float tmp;
                int indJ;
                int indI = tmpInd + j;
                int indImg = indI * img->channels;

                if((i - 1) >= 0) {
                    indJ = indImg - stridex;
                    float diff = 0.0f;

                    for(int p = 0; p < img->channels; p++) {
                        float tmpDiff = img->data[indJ + p] - img->data[indImg + p];
                        diff += tmpDiff * tmpDiff;
                    }

                    tmp  = -lambda / (powf(diff, alpha) + epsilon);

                    tL.push_back(Eigen::Triplet< double > (indI, indI - width , tmp));

                    sum += tmp;
                }

                if((i + 1) < height) {
                    indJ = indImg + stridex;
                    float diff = 0.0f;

                    for(int p = 0; p < img->channels; p++) {
                        float tmpDiff = img->data[indJ + p] - img->data[indImg + p];
                        diff += tmpDiff * tmpDiff;
                    }

                    tmp  = -lambda / (powf(diff, alpha) + epsilon);
                    tL.push_back(Eigen::Triplet< double > (indI, indI + width , tmp));
                    sum += tmp;
                }

                if((j - 1) >= 0) {
                    indJ = indImg - img->channels;
                    float diff = 0.0f;

                    for(int p = 0; p < img->channels; p++) {
                        float tmpDiff = img->data[indJ + p] - img->data[indImg + p];
                        diff += tmpDiff * tmpDiff;
                    }

                    tmp  = -lambda / (powf(diff, alpha) + epsilon);
                    tL.push_back(Eigen::Triplet< double > (indI, indI - 1 , tmp));
                    sum += tmp;
                }

                if((j + 1) < width) {
                    indJ = indImg + img->channels;
                    float diff = 0.0f;

                    for(int p = 0; p < img->channels; p++) {
                        float tmpDiff = img->data[indJ + p] - img->data[indImg + p];
                        diff += tmpDiff * tmpDiff;
                    }

                    tmp  = -lambda / (powf(diff, alpha) + epsilon);

                    tL.push_back(Eigen::Triplet< double > (indI, indI + 1 , tmp));
                    sum += tmp;
                }

                tL.push_back(Eigen::Triplet< double > (indI, indI, 1.0f - sum));
            }
        }

        #ifdef PIC_DEBUG
            printf("Ok\n");
        #endif

        Eigen::SparseMatrix<double> A = Eigen::SparseMatrix<double>(tot, tot);

        A.setFromTriplets(tL.begin(), tL.end());

        Eigen::SimplicialCholesky< Eigen::SparseMatrix< double > > solver(A);

        for(int i = 0; i < imgOut->channels; i++) {
            Eigen::VectorXd b, x;

            b = Eigen::VectorXd::Zero(tot);
            #pragma omp parallel for

            for(int j = 0; j < tot; j++) {
                b[j] = img->data[j * img->channels + i];
            }

            x = solver.solve(b);

            if(solver.info() == Eigen::Success) {

                #ifdef PIC_DEBUG
                    printf("SOLVER SUCCESS!\n");
                #endif

                #pragma omp parallel for

                for(int j = 0; j < tot; j++) {
                    imgOut->data[j * imgOut->channels + i] = float(x(j));
                }
            } else {
                #ifdef PIC_DEBUG
                    printf("SOLVER FAILED!\n");
                #endif
            }

        }

        return imgOut;
    }

    float alpha, lambda, epsilon;

public:

    /**
     * @brief FilterWLS
     */
    FilterWLS() : Filter()
    {
        update(1.2f, 1.0f);
    }

    /**
     * @brief FilterWLS
     * @param alpha
     * @param lambda
     */
    FilterWLS(float alpha, float lambda) : Filter()
    {
        update(alpha, lambda);
    }

    /**
     * @brief update
     * @param alpha
     * @param lambda
     */
    void update(float alpha, float lambda)
    {
        epsilon = 0.0001f;

        if(alpha <= 0.0f) {
            alpha = 1.2f;
        }

        if(lambda <= 0.0f) {
            lambda = 1.0f;
        }

        this->alpha = alpha;
        this->lambda = lambda;
    }

    /**
     * @brief Process
     * @param imgIn
     * @param imgOut
     * @return
     */
    Image *Process(ImageVec imgIn, Image *imgOut)
    {
        if(imgIn.empty()){
            return imgOut;
        }

        if(imgIn[0] == NULL) {
            return imgOut;
        }

        imgOut = setupAux(imgIn, imgOut);

        if(imgOut == NULL) {
            return imgOut;
        }

        //convolution
        if(imgIn[0]->channels == 1) {
            return singleChannel(imgIn, imgOut);
        } else {
            return multiChannel(imgIn, imgOut);
        }
    }

    /**
     * @brief main
     * @param argc
     * @param argv
     * @return
     */
    static int main(int argc, char* argv[])
    {
        if(argc < 4) {
            printf("Usage: name_input alpha lambad\n");
            return 0;
        }

        std::string nameIn = argv[1];
        std::string name = removeExtension(nameIn);
        std::string ext = getExtension(nameIn);

        float alpha = float(atof(argv[2]));
        float lambda = float(atof(argv[3]));

        std::string nameOut = name + "_wls." + ext;

        Image img(nameIn);

        FilterWLS *filter = new FilterWLS(alpha, lambda);

        filter->Process(Single(&img), NULL)->Write(nameOut);

        return 0;
    }
};
#endif

} // end namespace pic

#endif /* PIC_FILTERING_FILTER_WLS_HPP */


//
//  Copyright (C) 2020 Brian P. Kelley
//
//   @@ All Rights Reserved @@
//  This file is part of the RDKit.
//  The contents are covered by the terms of the BSD license
//  which is included in the file license.txt, found at the root
//  of the RDKit source tree.
//
#ifdef RDK_HAS_EIGEN3
#include "BCUT.h"
#include "Crippen.h"
#include <Eigen/Dense>
#include <GraphMol/RDKitBase.h>
#include "GraphMol/PartialCharges/GasteigerCharges.h"
#include "GraphMol/PartialCharges/GasteigerParams.h"
#include <RDGeneral/types.h>

namespace RDKit {
namespace Descriptors {
  // diagonal elements are a property (atomic num, charge, etc)
  // off diagonal are 1/sqrt(bond_order)
  //  Original burden matrix was .1, .2, .3, .15 for single,double,triple or aromatic
  //  all other elements are .001
namespace {
std::unique_ptr<Eigen::MatrixXd>  make_burden(const ROMol &m) {
  auto num_atoms = m.getNumAtoms();
  std::unique_ptr<Eigen::MatrixXd> burden(
	 new Eigen::MatrixXd(num_atoms, num_atoms));

  for(unsigned int i=0;i<num_atoms; ++i) {
    for(unsigned int j=0;j<num_atoms; ++j) {
      (*burden)(i,j) = (*burden)(j,i) = 0.001;
    }
  }

  for(auto &bond : m.bonds()) {
    unsigned int i = bond->getBeginAtomIdx();
    unsigned int j = bond->getEndAtomIdx();
    double score = 0.0;
    switch(bond->getBondType()) {
    case Bond::AROMATIC:
      // score = 0.15; orig burden
      score = 0.8164965809277261; // 1/sqrt(1.5)
      break;
    case Bond::SINGLE:
      // score = 0.1;
      score = 1.0; // 1/sqrt(1.0)
      break;
    case Bond::DOUBLE:
      // score = 0.2;
      score = 0.7071067811865475; // 1/sqrt(2.0)
      break;
    case Bond::TRIPLE:
      // score = 0.3;
      score = 0.5773502691896258; // 1/sqrt(3);
      break;
    default:
      CHECK_INVARIANT(0, "Bond order must be Single, Double, Triple or Aromatic");
    }
    (*burden)(i,j) = (*burden)(j,i) = score;
  }
  return burden;
}

std::pair<double,double> BCUT2D(std::unique_ptr<Eigen::MatrixXd> &burden,
				const std::vector<double> &atom_props) {
  for(unsigned int i=0; i<atom_props.size(); ++i) {
    (*burden)(i,i) = atom_props[i];
  }
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(*burden);
  auto eivals = es.eigenvalues();
  double lowest = eivals(0);
  double highest = eivals(atom_props.size()-1);
  return std::pair<double,double>(highest,lowest);
}
}

std::pair<double,double> BCUT2D(const ROMol &m, const std::vector<double> &atom_props) {
  unsigned int num_atoms = m.getNumAtoms();
  PRECONDITION(atom_props.size() == num_atoms, "Number of atom props not equal to number of atoms");

  if (num_atoms == 0) {
    return std::pair<double,double>(0,0);
  }
  auto burden = make_burden(m);
  return BCUT2D(burden, atom_props);
}

std::pair<double,double> BCUT2D(const ROMol &m, const std::string &atom_double_prop) {
  std::vector<double> props;
  props.reserve(m.getNumAtoms());
  for(auto &atom : m.atoms()) {
    props.push_back(atom->getProp<double>(atom_double_prop));
  }
  return BCUT2D(m, props);
}

std::vector<double> BCUT2D(const ROMol &m) {
  std::unique_ptr<ROMol> mol(MolOps::removeHs(m));
  std::vector<double> masses;
  std::vector<double> charges;
  unsigned int num_atoms = mol->getNumAtoms();
  masses.reserve(num_atoms);
  charges.reserve(num_atoms);

  RDKit::computeGasteigerCharges(*mol, 12, true);
  for(auto &atom: mol->atoms()) {
    masses.push_back(atom->getMass());
    charges.push_back(atom->getProp<double>(common_properties::_GasteigerCharge));
  }

  std::vector<double> slogp(num_atoms, 0.0);
  std::vector<double> cmr(num_atoms, 0.0);
  getCrippenAtomContribs(*mol, slogp, cmr);

  // polarizability? - need model
  // slogp?  sasa?
  auto burden = make_burden(m);
  auto atom_bcut = BCUT2D(burden, masses);
  auto gasteiger = BCUT2D(burden, charges);
  auto logp = BCUT2D(burden, slogp);
  auto mr = BCUT2D(burden, cmr);
  std::vector<double> res = {atom_bcut.first, atom_bcut.second,
			     gasteiger.first, gasteiger.second,
			     logp.first, logp.second,
			     mr.first, mr.second
  };
  return res;
}
}
}

#endif


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_FACESFROMTETS_HPP
#define MCL_FACESFROMTETS_HPP 1

#include <Eigen/Dense>
#include <set>

namespace mcl
{

// Given a tet mesh T, compute surface triangles F.
// True on success
template <typename DerivedT, typename DerivedF>
static inline bool faces_from_tets(
	const Eigen::MatrixBase<DerivedT> &T,
	Eigen::PlainObjectBase<DerivedF> &F)
{
	using namespace Eigen;
	struct FaceKey
	{
		FaceKey() : f(Vector3i::Zero()), f_sorted(Vector3i::Zero()) {}
		FaceKey(int f0, int f1, int f2)
		{
			f = Vector3i(f0, f1, f2);
			f_sorted = f;
			mcl::sort3(f_sorted[0], f_sorted[1], f_sorted[2]);
		}
		Vector3i f;
		Vector3i f_sorted;
		bool operator<(const FaceKey& other) const
		{
			for (int i=0; i<3; ++i)
			{
				if (f_sorted[i] < other.f_sorted[i]) { return true; }
				if (f_sorted[i] > other.f_sorted[i]) { return false; }
			}
			return false;
		}
	};

	if (T.rows()==0 || T.cols()!=4) {
		return false;
	}

	std::set<FaceKey> faces;
	std::set<FaceKey> faces_seen_twice;
	int n_tets = T.rows();
	int total_faces = 0;
	for (int t=0; t<n_tets; ++t)
	{
		total_faces += 4;
		int p0 = T(t,0);
		int p1 = T(t,1);
		int p2 = T(t,2);
		int p3 = T(t,3);
		FaceKey curr_faces[4] = {
			FaceKey(p0, p1, p3),
			FaceKey(p0, p2, p1),
			FaceKey(p0, p3, p2),
			FaceKey(p1, p2, p3) };

		for (int f=0; f<4; ++f)
		{
			const FaceKey &curr_face = curr_faces[f];
			// We've already seen the face at least twice.
			if (faces_seen_twice.count(curr_face)>0) { continue; }
			// Check that we don't already have the face
			typename std::set<FaceKey>::iterator it = faces.find(curr_face);
			if (it != faces.end())
			{
				faces.erase(it);
				faces_seen_twice.insert(curr_face);
				continue;
			}
			// Otherwise, add it to faces
			faces.insert(curr_face);
		} // end loop faces

	} // end loop tets

	int nf = faces.size();
	F.resize(nf, 3);
	typename std::set<FaceKey>::const_iterator fit = faces.begin();
	for (int f_idx=0; fit != faces.end(); ++fit, ++f_idx) {
		F.row(f_idx) = fit->f;
	}

	return true;
}

} // ns mcl

#endif


#include <iostream>

#include <cstdlib>
#include <cmath>

#include <armadillo>

#include "Conv2D/Mesh.hpp"
#include "Conv2D/EulerDefaultBase.hpp"

double uniform(double a, double b)
{
    return a + ((b - a) * std::rand() / RAND_MAX);
}

double consistencyTestError(double gamma, const EulerDefaultBase &problem)
{
    // construct random unit normal
    const double nx = uniform(-1.0, 1.0);
    const double ny = uniform(-1.0, 1.0);
    const double nl = std::hypot(nx, ny);
    const arma::rowvec n {nx / nl, ny / nl};

    // construct a state
    const double rho = uniform( 0.0, 1.0);
    const double u = uniform(-1.0, 1.0);
    const double v = uniform(-1.0, 1.0);
    const double e = uniform( 0.0, 1.0);
    const double E = e + 0.5 * (u * u + v * v);
    const arma::vec U {rho, rho * u, rho * v, rho * E};

    // compute analytical flux
    const double p = (gamma - 1.0) * rho * e;
    const double H = E + p / rho;
    const arma::vec F {
            n[0] * (rho * u)         + n[1] * (rho * v),
            n[0] * (rho * u * u + p) + n[1] * (rho * u * v),
            n[0] * (rho * u * v)     + n[1] * (rho * v * v + p),
            n[0] * (rho * u * H)     + n[1] * (rho * v * H)};

    // compute Roe flux
    arma::vec FHat(4);
    double s;
    problem.computeRoeFlux(U, U, n, FHat, s);

    // return error
    return arma::norm(F - FHat);
}

double flipTestError(const EulerDefaultBase &problem)
{
    // construct random oppsing unit normals
    const double nx = uniform(-1.0, 1.0);
    const double ny = uniform(-1.0, 1.0);
    const double nl = std::hypot(nx, ny);
    const arma::rowvec nLR { nx / nl,  ny / nl};
    const arma::rowvec nRL {-nx / nl, -ny / nl};

    // construct left state
    const double rhoL = uniform( 0.0, 1.0);
    const double uL = uniform(-1.0, 1.0);
    const double vL = uniform(-1.0, 1.0);
    const double EL = uniform( 0.0, 1.0) + 0.5 * (uL * uL + vL * vL);
    const arma::vec UL {rhoL, rhoL * uL, rhoL * vL, rhoL * EL};

    // construct right state
    const double rhoR = uniform( 0.0, 1.0);
    const double uR   = uniform(-1.0, 1.0);
    const double vR   = uniform(-1.0, 1.0);
    const double ER   = uniform( 0.0, 1.0) + 0.5 * (uR * uR + vR * vR);
    const arma::vec UR {rhoR, rhoR * uR, rhoR * vR, rhoR * ER};

    // temporary variable to store wave speed, not used here!
    double s;

    // compute Roe flux from left to right
    arma::vec FHatLR(4);
    problem.computeRoeFlux(UL, UR, nLR, FHatLR, s);

    // compute Roe flux from right to left
    arma::vec FHatRL(4);
    problem.computeRoeFlux(UR, UL, nRL, FHatRL, s);

    // return error
    return arma::norm(FHatLR + FHatRL);
}

double supersonicTestError(double gamma, const EulerDefaultBase &problem)
{
    // construct normal that points north-east
    const double nx = uniform(0.0, 1.0);
    const double ny = uniform(0.0, 1.0);
    const double nl = std::hypot(nx, ny);
    const arma::rowvec n {nx / nl, ny / nl};

    // construct supersonic state on the left
    const double rhoL = uniform(0.0, 1.0);
    const double uL   = uniform(0.8, 1.0);
    const double vL   = uniform(0.8, 1.0);
    const double eL   = uniform(0.0, 1.0);
    const double EL   = eL + 0.5 * (uL * uL + vL * vL);

    const arma::vec UL {rhoL, rhoL * uL, rhoL * vL, rhoL * EL};

    // construct supersonic state on the right
    const double rhoR = uniform(0.0, 1.0);
    const double uR   = uniform(0.8, 1.0);
    const double vR   = uniform(0.8, 1.0);
    const double eR   = uniform(0.0, 1.0);
    const double ER   = eR + 0.5 * (uR * uR + vR * vR);

    const arma::vec UR {rhoR, rhoR * uR, rhoR * vR, rhoR * ER};

    // compute Roe flux
    arma::vec FHat(4);
    double s;
    problem.computeRoeFlux(UL, UR, n, FHat, s);

    // compute analytical flux on the left
    const double pL = (gamma - 1.0) * rhoL * eL;
    const double HL = EL + pL / rhoL;

    const arma::vec FL {
            n[0] * (rhoL * uL)           + n[1] * (rhoL * vL),
            n[0] * (rhoL * uL * uL + pL) + n[1] * (rhoL * uL * vL),
            n[0] * (rhoL * uL * vL)      + n[1] * (rhoL * vL * vL + pL),
            n[0] * (rhoL * uL * HL)      + n[1] * (rhoL * vL * HL)};

    // return error
    return arma::norm(FHat - FL);
}

int main()
{
    // random seed
    std::srand(1279);

    // this choice is gamma is very important for supersonic test!
    const double gamma = 1.4;

    // set up problem
    EulerDefaultBase problem;
    problem.setSpecificHeatRatio(gamma);

    // report errors
    std::cout << "L2 errors in" << std::endl
              << "  Consistency test: " << std::scientific
              << consistencyTestError(gamma, problem) << std::endl
              << "  Flip test       : " << std::scientific
              << flipTestError(problem) << std::endl
              << "  Supersonic test : " << std::scientific
              << supersonicTestError(gamma, problem) << std::endl;

    return 0;
}


////////////////////////////////////////////////////////////////////////////////
/// Copyright 2018-present Xinyan DAI<xinyan.dai@outlook.com>
///
/// 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 ofthe 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.

/// @version 0.1
/// @author  Xinyan DAI
/// @contact xinyan.dai@outlook.com
//////////////////////////////////////////////////////////////////////////////

#pragma once

#include <utility>
#include <vector>
#include <random>
#include <unordered_map>
#include <boost/progress.hpp>

#include "../index.hpp"

namespace ss {

    using std::vector;

    template<class DataType>
    class KMeansIndex : public Index<DataType> {
    protected:
        /// _center[i] means the i'th center's coordinate
        vector<vector<DataType > >     _centers;
        /// _points[i] means all points' indexes belong to i'th center
        /// _points[i][j] means the j'th point's index belongs to i'th center
        vector<vector<int > >          _points;
    public:
        explicit KMeansIndex(const parameter& para) :
                Index<DataType >(para),
                _centers(para.kmeans_centers),
                _points(para.kmeans_centers) {}

        const vector<vector<DataType > > & get_centers() const { return _centers; }
        const vector<vector<int > >      & get_points()  const { return _points; }

        void reset(int num_centers) {
            this->_centers = vector<vector<DataType > >(num_centers);
            this->_points = vector<vector<int > >(num_centers);
        }

        void set_centers(const vector<vector<DataType > >& centers) {
            _centers = centers;
        }

        void Train(const Matrix<DataType > & data) override {
            Iterate(Visitor<DataType >(data, 0, data.getDim()));
        }

        void Add(const Matrix<DataType > & data) override {
            Assign(Visitor<DataType >(data, 0, data.getDim()));
        }

        void Search(const DataType *query, const std::function<void (int)>& prober) override {
            const vector<int>& idx = _points[NearestCenter(query, _centers[0].size())];
            for (int id : idx) {
                prober(id);
            }
        }

        const vector<int>& Search(const DataType *query)  {
            return  _points[NearestCenter(query, _centers[0].size())];
        }

        /***
         * iteratively update centers and re-assign points
         * @param data
         */
        void Iterate(const Visitor<DataType> & data) {

            /// initialize centers
            /// TODO(Xinyan): should initialized randomly
            for(int i=0; i<_centers.size(); i++) {
                _centers[i] = vector<DataType >(data[i], data[i]+data.getDim());
            }

            boost::progress_display progress(this->_para.iteration);
            for (int iter = 0; iter < this->_para.iteration; ++iter, ++progress) {

                /// Assignment
                Assign(data);
                /// UpdateCenter
                Update(data);
                /// clear points in each centers
                for (int c = 0; c<_points.size(); c++) {
                    _points[c].clear();
                }
            }
        }

        /**
         * re-calculate center by averaging points' coordinate
         */
        void Update(const Visitor<DataType> & data) {
            for (int c = 0; c < _centers.size(); ++c) {

                vector<DataType > sum(data.getDim(), 0.0f);
                for (int p = 0; p < _points[c].size(); ++p) {

                    for (int d = 0; d < data.getDim(); ++d) {
                        sum[d] += data[_points[c][p]][d];        /// add up
                    }
                }

                for (int d = 0; d < data.getDim(); ++d) {
                    _centers[c][d] = sum[d] / _points[c].size(); /// average
                }
            }
        }

        /**
         * assign each point in {@link data} to nearest center
         */
        void Assign(const Visitor<DataType> & data) {
            vector<int > codes(data.getSize());
#pragma omp parallel for
            for (int i = 0; i < data.getSize(); ++i) {
                codes[i] = NearestCenter(data[i], data.getDim());
            }
            for (int i=0; i<data.getSize(); ++i) {
                _points[codes[i]].push_back(i);
            }
        }

        int NearestCenter(const DataType * vector, int dimension) {
            DataType min_distance = ss::EuclidDistance(vector, _centers[0].data(), dimension);
            int nearest_center = 0;
            for (int c = 1; c < _centers.size(); c++) {
                DataType distance = Distance(vector, dimension, c);
                if (distance < min_distance) {
                    min_distance = distance;
                    nearest_center = c;
                }
            }
            return nearest_center;
        }

        /**
         * calculate distances from {@link vector} to each center
         * @return distances within a vector of pair<distance, center>
         */
        std::vector<std::pair<float, int > > ClusterDistance(const DataType *vector, int dimension) {
            std::vector<std::pair<float, int>> dist_centers(this->_centers.size());
            for (int center = 0; center < (this->_centers.size()); ++center) {
                DataType distance = Distance(vector, dimension, center);
                dist_centers[center] = std::make_pair(distance, center);
            }

            return dist_centers;
        }

        inline DataType Distance(const DataType * vector, int dimension, int center) {
            return ss::EuclidDistance(vector, _centers[center].data(), dimension);
        }

    };

} // namespace ss


// Copyright Louis Dionne 2013-2017
// 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/hana/equal.hpp>
#include <boost/hana/minus.hpp>
#include <boost/hana/plus.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/zip_with.hpp>

#include <functional>
namespace hana = boost::hana;

//
// Example of implementing basic dimensional analysis using Hana
//

// base dimensions                              M  L  T  I  K  J  N
using mass        = decltype(hana::tuple_c<int, 1, 0, 0, 0, 0, 0, 0>);
using length      = decltype(hana::tuple_c<int, 0, 1, 0, 0, 0, 0, 0>);
using time_       = decltype(hana::tuple_c<int, 0, 0, 1, 0, 0, 0, 0>);
using charge      = decltype(hana::tuple_c<int, 0, 0, 0, 1, 0, 0, 0>);
using temperature = decltype(hana::tuple_c<int, 0, 0, 0, 0, 1, 0, 0>);
using intensity   = decltype(hana::tuple_c<int, 0, 0, 0, 0, 0, 1, 0>);
using amount      = decltype(hana::tuple_c<int, 0, 0, 0, 0, 0, 0, 1>);

// composite dimensions
using velocity     = decltype(hana::tuple_c<int, 0, 1, -1, 0, 0, 0, 0>); // M/T
using acceleration = decltype(hana::tuple_c<int, 0, 1, -2, 0, 0, 0, 0>); // M/T^2
using force        = decltype(hana::tuple_c<int, 1, 1, -2, 0, 0, 0, 0>); // ML/T^2

template <typename Dimensions>
struct quantity {
    double value_;

    explicit quantity(double v) : value_(v) { }

    template <typename OtherDimensions>
    explicit quantity(quantity<OtherDimensions> other)
      : value_(other.value_)
    {
      static_assert(Dimensions{} == OtherDimensions{},
        "Constructing quantities with incompatible dimensions!");
    }

    explicit operator double() const { return value_; }
};

template <typename D1, typename D2>
auto operator*(quantity<D1> a, quantity<D2> b) {
    using D = decltype(hana::zip_with(std::plus<>{}, D1{}, D2{}));
    return quantity<D>{static_cast<double>(a) * static_cast<double>(b)};
}

template <typename D1, typename D2>
auto operator/(quantity<D1> a, quantity<D2> b) {
    using D = decltype(hana::zip_with(std::minus<>{}, D1{}, D2{}));
    return quantity<D>{static_cast<double>(a) / static_cast<double>(b)};
}

int main() {
    quantity<mass>         m{10.3};
    quantity<length>       d{3.6};
    quantity<time_>        t{2.4};
    quantity<velocity>     v{d / t};
    quantity<acceleration> a{3.9};
    quantity<force>        f{m * a};
}


/*
 * SphereInitializer.hpp
 *
 *  Created on: Nov 8, 2012
 *      Author: petr
 */
#pragma once
#ifndef SPHEREINITIALIZER_HPP_
#define SPHEREINITIALIZER_HPP_

#include <petscdmadda.h>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/StdVector>

#include "Interface.hpp"

class SphereInitializer{

    // where the sphere is located
    Eigen::Vector3d center;

    // how big is the sphere
    PetscReal              radius;

    size_t                 d;

    // geometry is stores here
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > geometry;

    // select which boundaries need to be initialized
    int selectBoundaries(const Box<double, 3>& procAABB);

    // defines the distance to the sphere
    PetscReal sphereEquation(const Eigen::Vector3d& point);

    // initialize the geometry (in this case creates the sphere)
    void initializeGeometry();

public:

    SphereInitializer(const Eigen::Vector3d& center, PetscReal radius);

    // This is where the magic happens and the initializator puts the data in
    template <typename type, int dim>
    void operator() (Interface<type, dim>& interface, bool initAll);

};

///===========================================
///              Implementation
///===========================================

SphereInitializer::SphereInitializer(const Eigen::Vector3d& center, PetscReal radius) {

    this->center = center;
    this->radius = radius;
    this->d = center.size();
    initializeGeometry();

}

template <typename type, int dim>
void SphereInitializer::operator ()(Interface<type, dim>& interface, bool initAll) {

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > boundaryCells;    // xyz coordinated to compute the distance to

    int                        myBoundaries;     // local boundaries to init

    MPI_Group worldGroup;
    MPI_Comm_group(MPI_COMM_WORLD, &worldGroup);
    // perpendicular distance computed towards the triangle, it is used when we are no longer sure, which normal shall we use

    // initialize the data

    Vec           localData = interface.getLocalData();
    double***     data_ptr;
    int           x, y, z, m, n, p;
    int           vecSize;
    DM            cda;
    Vec           gc;
    DMDACoor3d*** coors;

    VecSet(localData, std::numeric_limits<double>::max());
//    VecSet(localData, -0.5);
    VecGetLocalSize(localData, &vecSize);
    assert( vecSize > 0 );

    int myWorldRank;
    int nProcsInWorld;

    MPI_Comm_rank(MPI_COMM_WORLD, &myWorldRank);
    MPI_Comm_size(MPI_COMM_WORLD, &nProcsInWorld);

    const Grid<type, dim>& gr = interface.getGrid();

    DMDAVecGetArray(gr.getDA(), localData, &data_ptr);

    DMDAGetGhostCorners(gr.getDA(), &x, &y, &z, &m, &n, &p);

    DMGetCoordinatesLocal(gr.getDA() ,&gc);
    DMGetCoordinateDM(gr.getDA(), &cda);
    DMDAVecGetArray(cda, gc, &coors);

    if (initAll) {
        // initialize the whole domain

        for (int i = x; i < x+m; ++i) {
            for (int j = y; j < y+n; ++j) {
                for (int k = z; k < z+p; ++k) {
                    data_ptr[k][j][i] =
                        sphereEquation(
                            Eigen::Vector3d(coors[k][j][i].x,
                                            coors[k][j][i].y,
                                            coors[k][j][i].z)
                        );

                }
            }
        }

    } else {
        // insert sphere boundaries
    	double    narrowBand = gr.getDx(0) * 3;
        for (int i = x; i < x+m; ++i) {
            for (int j = y; j < y+n; ++j) {
                for (int k = z; k < z+p; ++k) {
                    double dist =
                        sphereEquation(
                            Eigen::Vector3d(coors[k][j][i].x,
                                            coors[k][j][i].y,
                                            coors[k][j][i].z)
                        );
                    if (fabs(dist) > narrowBand) {
                        continue;
                    }
                    data_ptr[k][j][i] = dist;
                }
            }
        }

        // boundaries
        int gh = 1;
        int boundaries = 63;//selectBoundaries( gr.getNodeSpan(myWorldRank) );

        if ( boundaries & 1 ) {
            // this means lets initialize left side
            for (int i = x; i < x+gh; ++i) {
                for (int j = y; j < y+n; ++j) {
                    for (int k = z; k < z+p; ++k) {
                        data_ptr[k][j][i+1] = sphereEquation(
                                                Eigen::Vector3d(coors[k][j][i+1].x,
                                                                coors[k][j][i+1].y,
                                                                coors[k][j][i+1].z)
                                                );
                    }
                }
            }
        }

        if ( boundaries & 2 ) {
            // this means lets initialize right side
            for (int i = x+m-1; i > x+m-1-gh; --i) {
                for (int j = y; j < y+n; ++j) {
                    for (int k = z; k < z+p; ++k) {
                        data_ptr[k][j][i-1] = sphereEquation(
                                                Eigen::Vector3d(coors[k][j][i-1].x,
                                                                coors[k][j][i-1].y,
                                                                coors[k][j][i-1].z)
                                                );
                    }
                }
            }
        }

        if ( boundaries & 4 ) {
            // this means initialize the bottom side
            for (int j = y; j < y+gh; ++j) {
                for (int i = x; i < x+m; ++i) {
                    for (int k = z; k < z+p; ++k) {
                        data_ptr[k][j+1][i] = sphereEquation(
                                                Eigen::Vector3d(coors[k][j+1][i].x,
                                                                coors[k][j+1][i].y,
                                                                coors[k][j+1][i].z)
                                                );
                    }
                }
            }
        }

        if ( boundaries & 8 ) {
            // this means initialize the top side
            for (int j = y+n-1; j > y+n-1-gh; --j) {
                for (int i = x; i < x+m; ++i) {
                    for (int k = z; k < z+p; ++k) {
                        data_ptr[k][j-1][i] = sphereEquation(
                                                Eigen::Vector3d(coors[k][j-1][i].x,
                                                                coors[k][j-1][i].y,
                                                                coors[k][j-1][i].z)
                                                );
                    }
                }
            }
        }

        if ( boundaries & 16 ) {
            // this means initialize the front side
            for (int k = z; k < z+gh; ++k) {
                for (int i = x; i < x+m; ++i) {
                    for (int j = y; j < y+n; ++j) {
                        data_ptr[k+1][j][i] = sphereEquation(
                                                Eigen::Vector3d(coors[k+1][j][i].x,
                                                                coors[k+1][j][i].y,
                                                                coors[k+1][j][i].z)
                                                );
                    }
                }
            }
        }

        if ( boundaries & 32 ) {
            // this means initialize the far side
            for (int k = z+p-1; k > z+p-1-gh; --k) {
                for (int i = x; i < x+m; ++i) {
                    for (int j = y; j < y+n; ++j) {
                        data_ptr[k-1][j][i] = sphereEquation(
                                                Eigen::Vector3d(coors[k-1][j][i].x,
                                                                coors[k-1][j][i].y,
                                                                coors[k-1][j][i].z)
                                                );
                    }
                }
            }
        }

    }

    DMDAVecRestoreArray(cda, gc, &coors);
    DMDAVecRestoreArray(gr.getDA(), localData, &data_ptr);

}

PetscReal SphereInitializer::sphereEquation(const Eigen::Vector3d& point) {
    // this function really speaks for itself. It is just sphere equation

    PetscReal dist = 0;

    dist = sqrt( (point[0] - center[0])*(point[0] - center[0]) +
                 (point[1] - center[1])*(point[1] - center[1]) +
                 (point[2] - center[2])*(point[2] - center[2])) - radius;

    return dist;

}

int SphereInitializer::selectBoundaries(const Box<double, 3>& procAABB) {
    // chooses which boundaries of the domain have to be initialized
    // ordering : [left, right, bottom, top, front, back]

    int selectedBoundaries_int = 0;

    for (int i = 0; i < 3; ++i) {
        //gr->getLocalGhostedMax(i) <= dataProvider->getMaxX(i)
        if ( procAABB.maxX(i) <= (center[i]+radius) ) {
            selectedBoundaries_int |= ( 1 << (i*2 + 1) );
        }
        // gr->getLocalGhostedMin(i) >= dataProvider->getMinX(i)
        if ( procAABB.minX(i) >= (center[i]-radius) ) {
            selectedBoundaries_int |= ( 1 << (i*2) );
        }
    }

    return selectedBoundaries_int;

}

void SphereInitializer::initializeGeometry() {
    // creates the sphere points in Nd

    int    numberOfPoints = 300;

    // x(t) = center(1) - radius*cos(angle_step_1 * t)*sin(angle_step_2 * t);
    // y(t) = center(2) - radius*sin(angle_step_1 * t)*cos(angle_step_2 * t);
    // z(t) =             radius*                      cos(angle_step_2 * t);

    double angle_step_1 = (2*3.1415926535897) / numberOfPoints;
    double angle_step_2 = (  3.1415926535897) / numberOfPoints;

    geometry.reserve(numberOfPoints*numberOfPoints);

    for (int i = 0; i < numberOfPoints; ++i) {

        for (int j = 0; j < numberOfPoints; ++j) {

            double x = center[0] + radius*cos(angle_step_1 * i)*sin(angle_step_2*j);
            double y = center[1] + radius*sin(angle_step_1 * i)*sin(angle_step_2*j);
            double z = center[2] + radius*cos(angle_step_2*j);

            geometry.push_back( Eigen::Vector3d(x, y, z) );

        }

    }

}

#endif /* SPHEREINITIALIZER_HPP_ */


// The MIT License
// (c) 2019 Daniel Williams

#include <cmath>
#include <cstdio>
#include <string>
#include <memory>
#include <iostream>
#include <iomanip>

using std::string;
using std::cerr;
using std::endl;

#include <Eigen/Geometry>
using Eigen::Vector2d;

#include <nlohmann/json/json.hpp>
using nlohmann::json;

#include "geometry/layout.hpp"
#include "quadtree/tree.hpp"

using namespace terrain;
using geometry::Layout;
using quadtree::Tree;
using quadtree::Node;

// main method for descending through a tree and returning the appropriate location / node / value
// note: weakly optimized; intended to be a hot path.
void descend( const Vector2d& target, double& x_c, double& y_c, const double start_width, Node* & current_node){
    double current_width = start_width;
    double next_width = start_width*0.5;

    while( ! current_node->is_leaf() )
    {
        current_width = next_width;
        next_width *= 0.5;

        if(target[0] > x_c){
            if( target[1] > y_c){
                x_c += next_width;
                y_c += next_width;
                current_node = current_node->get_northeast();
            }else{
                x_c += next_width;
                y_c -= next_width;
                current_node = current_node->get_southeast();
            }
        }else{
            if( target[1] > y_c){
                x_c -= next_width;
                y_c += next_width;
                current_node = current_node->get_northwest();
            }else{
                x_c -= next_width;
                y_c -= next_width;
                current_node = current_node->get_southwest();
            }
        }
    }
}

Tree::Tree(): Tree(Layout()) {}

Tree::Tree(const Layout& _layout)
    : layout(_layout)
{
    reset();
}

Tree::~Tree(){
    root.release();
}

bool Tree::contains(const Eigen::Vector2d& p) const {
    return layout.contains(p);
}

cell_value_t Tree::classify(const Eigen::Vector2d& p) const {
    // create a R/W copy, initialized at the tree's center.
    Eigen::Vector2d located( layout.get_center() );

    auto current_node = root.get();

    descend( p, located[0], located[1], layout.get_width(), current_node );

    return current_node->get_value();
}

void Tree::debug_tree(const bool show_pointers) const {
    cerr << "====== Quad Tree: ======\n";
    cerr << "##  bounds:     " << layout.to_string() << endl;
    cerr << "##  height:     " << get_height() << endl;
    cerr << "##  precision:  " << layout.get_precision() << endl;

    root->draw(cerr, "    ", "RT", show_pointers);
    cerr << endl;
}

size_t Tree::calculate_complete_tree(const size_t height){
    // see: https://en.wikipedia.org/wiki/M-ary_tree
    //      # properties of M-ary trees
    //
    //    m == branching_factor == 4  ///< for a quadtree, this is trivially 4
    //
    //       (h+1)
    //     m        - 1
    // N = ---------------
    //        m - 1
    //
    return ( pow(4,height+1) - 1 )/3;
}

double Tree::get_load_factor() const {
    const size_t height = root->get_height();
    const size_t count = root->get_count();
    const size_t complete = calculate_complete_tree(height);
    return static_cast<double>(count) / static_cast<double>(complete);
}

size_t Tree::get_memory_usage() const {
    return size() * sizeof(Node);
}

cell_value_t Tree::interp(const Eigen::Vector2d& at) const {

    // cout << "@@" << at << "    near: " << near.get_bounds() << " = " << near.get_value() << endl;

    // if Eigen::Vector2d is outside the tree, entirely
    if( ! contains(at)){
        return cell_default_value;
    }

    // const Node& near = root->search(at, get_bounds());
//     const Eigen::Vector2d& cn = near.get_center();
//     const double dx = std::copysign(1.0, (at.x() - cn.x())) * 2 * near.get_bounds().half_width;
//     const double dy = std::copysign(1.0, (at.y() - cn.y())) * 2 * near.get_bounds().half_width;
//     const Node& n2 = root->search({cn.x() + dx, cn.y()     }, get_bounds());
//     const Node& n3 = root->search({cn.x() + dx, cn.y() + dy}, get_bounds());
//     const Node& n4 = root->search({cn.x()     , cn.y() + dy}, get_bounds());

//     const auto& interp = near.interpolate_bilinear(at, n2, n3, n4);
    // return interp;

    return NAN;
}

void Tree::fill(const cell_value_t fill_value){
    root->fill(fill_value);
}

size_t Tree::get_height() const {
    return root->get_height() - 1;
}

bool Tree::load_tree(const nlohmann::json& doc){
    if(! doc.is_object()){
        cerr << "?? attempted to load unexpected format: no-object json document!\n";
        return false;
    }
    return root->load(doc);
}

void Tree::prune(){
    root->prune();
}

void Tree::reset(){
    root = std::make_unique<Node>(0);
}

void Tree::reset(const Layout& new_layout){
    layout = new_layout;

    root = std::make_unique<Node>(0);
    root->split(layout.get_precision(), layout.get_width());
}

Sample Tree::sample(const Eigen::Vector2d& p) const {
    Vector2d located( layout.get_center() );
    auto current_node = root.get();

    descend( p, located[0], located[1], layout.get_width(), current_node );

    return {located, current_node->get_value()};
}

bool Tree::store(const Vector2d& p, const cell_value_t new_value) {
    Vector2d located( layout.get_center() );
    auto current_node = root.get();

    descend( p, located[0], located[1], layout.get_width(), current_node );

    current_node->set_value(new_value);
    return true;
}

size_t Tree::size() const {
    return root->get_count();
}

json Tree::to_json_tree() const {
    return root->to_json();
}


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

#ifndef BOOST_MATH_SP_FACTORIALS_HPP
#define BOOST_MATH_SP_FACTORIALS_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/detail/unchecked_factorial.hpp>
#include <boost/array.hpp>
#ifdef BOOST_MSVC
#pragma warning(push) // Temporary until lexical cast fixed.
#pragma warning(disable: 4127 4701)
#endif
#include <boost/lexical_cast.hpp>
#ifdef BOOST_MSVC
#pragma warning(pop)
#endif
#include <boost/config/no_tr1/cmath.hpp>

namespace boost { namespace math
{

template <class T, class Policy>
inline T factorial(unsigned i, const Policy& pol)
{
   BOOST_STATIC_ASSERT(!boost::is_integral<T>::value);
   BOOST_MATH_STD_USING // Aid ADL for floor.

   if(i <= max_factorial<T>::value)
      return unchecked_factorial<T>(i);
   T result = boost::math::tgamma(static_cast<T>(i+1), pol);
   if(result > tools::max_value<T>())
      return result; // Overflowed value! (But tgamma will have signalled the error already).
   return floor(result + 0.5f);
}

template <class T>
inline T factorial(unsigned i)
{
   return factorial<T>(i, policies::policy<>());
}
/*
// Can't have these in a policy enabled world?
template<>
inline float factorial<float>(unsigned i)
{
   if(i <= max_factorial<float>::value)
      return unchecked_factorial<float>(i);
   return tools::overflow_error<float>(BOOST_CURRENT_FUNCTION);
}

template<>
inline double factorial<double>(unsigned i)
{
   if(i <= max_factorial<double>::value)
      return unchecked_factorial<double>(i);
   return tools::overflow_error<double>(BOOST_CURRENT_FUNCTION);
}
*/
template <class T, class Policy>
T double_factorial(unsigned i, const Policy& pol)
{
   BOOST_STATIC_ASSERT(!boost::is_integral<T>::value);
   BOOST_MATH_STD_USING  // ADL lookup of std names
   if(i & 1)
   {
      // odd i:
      if(i < max_factorial<T>::value)
      {
         unsigned n = (i - 1) / 2;
         return ceil(unchecked_factorial<T>(i) / (ldexp(T(1), (int)n) * unchecked_factorial<T>(n)) - 0.5f);
      }
      //
      // Fallthrough: i is too large to use table lookup, try the
      // gamma function instead.
      //
      T result = boost::math::tgamma(static_cast<T>(i) / 2 + 1, pol) / sqrt(constants::pi<T>());
      if(ldexp(tools::max_value<T>(), -static_cast<int>(i+1) / 2) > result)
         return ceil(result * ldexp(T(1), (i+1) / 2) - 0.5f);
   }
   else
   {
      // even i:
      unsigned n = i / 2;
      T result = factorial<T>(n, pol);
      if(ldexp(tools::max_value<T>(), -(int)n) > result)
         return result * ldexp(T(1), (int)n);
   }
   //
   // If we fall through to here then the result is infinite:
   //
   return policies::raise_overflow_error<T>("boost::math::double_factorial<%1%>(unsigned)", 0, pol);
}

template <class T>
inline T double_factorial(unsigned i)
{
   return double_factorial<T>(i, policies::policy<>());
}

namespace detail{

template <class T, class Policy>
T rising_factorial_imp(T x, int n, const Policy& pol)
{
   BOOST_STATIC_ASSERT(!boost::is_integral<T>::value);
   if(x < 0)
   {
      //
      // For x less than zero, we really have a falling
      // factorial, modulo a possible change of sign.
      //
      // Note that the falling factorial isn't defined
      // for negative n, so we'll get rid of that case
      // first:
      //
      bool inv = false;
      if(n < 0)
      {
         x += n;
         n = -n;
         inv = true;
      }
      T result = ((n&1) ? -1 : 1) * falling_factorial(-x, n, pol);
      if(inv)
         result = 1 / result;
      return result;
   }
   if(n == 0)
      return 1;
   //
   // We don't optimise this for small n, because
   // tgamma_delta_ratio is alreay optimised for that
   // use case:
   //
   return 1 / boost::math::tgamma_delta_ratio(x, static_cast<T>(n), pol);
}

template <class T, class Policy>
inline T falling_factorial_imp(T x, unsigned n, const Policy& pol)
{
   BOOST_STATIC_ASSERT(!boost::is_integral<T>::value);
   BOOST_MATH_STD_USING // ADL of std names
   if(x == 0)
      return 0;
   if(x < 0)
   {
      //
      // For x < 0 we really have a rising factorial
      // modulo a possible change of sign:
      //
      return (n&1 ? -1 : 1) * rising_factorial(-x, n, pol);
   }
   if(n == 0)
      return 1;
   if(x < n-1)
   {
      //
      // x+1-n will be negative and tgamma_delta_ratio won't
      // handle it, split the product up into three parts:
      //
      T xp1 = x + 1;
      unsigned n2 = itrunc((T)floor(xp1), pol);
      if(n2 == xp1)
         return 0;
      T result = boost::math::tgamma_delta_ratio(xp1, -static_cast<T>(n2), pol);
      x -= n2;
      result *= x;
      ++n2;
      if(n2 < n)
         result *= falling_factorial(x - 1, n - n2, pol);
      return result;
   }
   //
   // Simple case: just the ratio of two
   // (positive argument) gamma functions.
   // Note that we don't optimise this for small n,
   // because tgamma_delta_ratio is alreay optimised
   // for that use case:
   //
   return boost::math::tgamma_delta_ratio(x + 1, -static_cast<T>(n), pol);
}

} // namespace detail

template <class RT>
inline typename tools::promote_args<RT>::type
   falling_factorial(RT x, unsigned n)
{
   typedef typename tools::promote_args<RT>::type result_type;
   return detail::falling_factorial_imp(
      static_cast<result_type>(x), n, policies::policy<>());
}

template <class RT, class Policy>
inline typename tools::promote_args<RT>::type
   falling_factorial(RT x, unsigned n, const Policy& pol)
{
   typedef typename tools::promote_args<RT>::type result_type;
   return detail::falling_factorial_imp(
      static_cast<result_type>(x), n, pol);
}

template <class RT>
inline typename tools::promote_args<RT>::type
   rising_factorial(RT x, int n)
{
   typedef typename tools::promote_args<RT>::type result_type;
   return detail::rising_factorial_imp(
      static_cast<result_type>(x), n, policies::policy<>());
}

template <class RT, class Policy>
inline typename tools::promote_args<RT>::type
   rising_factorial(RT x, int n, const Policy& pol)
{
   typedef typename tools::promote_args<RT>::type result_type;
   return detail::rising_factorial_imp(
      static_cast<result_type>(x), n, pol);
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SP_FACTORIALS_HPP


#ifndef SMOOTH__FEEDBACK__UTILS__SPARSE_HPP_
#define SMOOTH__FEEDBACK__UTILS__SPARSE_HPP_

#include <Eigen/Sparse>

#include <numeric>
#include <optional>

namespace smooth::feedback {

/**
 * @brief Block sparse matrix construction.
 *
 * @param blocks list of lists {{b00, b01}, {b10, b11 ...}} of (optional) sparse matrix blocks
 * @return the blocks as a single sparse matrix
 *
 * Non-present (std::nullopt-valued) blocks are considered zeros.
 *
 * @warning The block sizes must be consistent, i.e. all blocks in the same block-column must have
 * the same number of columns, and similarly for row-columns and rows.
 */
inline Eigen::SparseMatrix<double> sparse_block_matrix(
  const std::initializer_list<std::initializer_list<std::optional<Eigen::SparseMatrix<double>>>> &
    l)
{
  const auto n_rows = l.size();
  const auto n_cols = std::begin(l)->size();

  Eigen::VectorXi dims_rows = Eigen::VectorXi::Constant(n_rows, -1);
  Eigen::VectorXi dims_cols = Eigen::VectorXi::Constant(n_cols, -1);

  // figure block row and col dimensions
  for (auto krow = 0u; const auto & row : l) {
    for (auto kcol = 0u; const auto & item : row) {
      if (item.has_value()) {
        if (dims_cols(kcol) == -1) {
          dims_cols(kcol) = item->cols();
        } else {
          assert(dims_cols(kcol) == item->cols());
        }
        if (dims_rows(krow) == -1) {
          dims_rows(krow) = item->rows();
        } else {
          assert(dims_rows(krow) == item->rows());
        }
      }
      ++kcol;
    }
    ++krow;
  }

  // check that all dimensions are defined by input args
  assert(dims_rows.minCoeff() > -1);
  assert(dims_cols.minCoeff() > -1);

  // figure starting indices
  const auto n_row = std::accumulate(std::cbegin(dims_rows), std::cend(dims_rows), 0u);
  const auto n_col = std::accumulate(std::cbegin(dims_cols), std::cend(dims_cols), 0u);

  Eigen::SparseMatrix<double> ret(n_row, n_col);

  // allocate pattern
  Eigen::Matrix<decltype(ret)::StorageIndex, -1, 1> pattern(n_col);
  pattern.setZero();
  for (const auto & row : l) {
    for (auto kcol = 0u, col0 = 0u; const auto &item : row) {
      if (item.has_value()) {
        for (auto col = 0; col < dims_cols(kcol); ++col) {
          pattern(col0 + col) += item->outerIndexPtr()[col + 1] - item->outerIndexPtr()[col];
        }
      }
      col0 += dims_cols(kcol++);
    }
  }

  ret.reserve(pattern);

  // insert values
  for (auto krow = 0u, row0 = 0u; const auto &row : l) {
    for (auto kcol = 0u, col0 = 0u; const auto &item : row) {
      if (item.has_value()) {
        for (auto col = 0; col < dims_cols(kcol); ++col) {
          for (typename std::decay_t<decltype(*item)>::InnerIterator it(*item, col); it; ++it) {
            ret.insert(row0 + it.index(), col0 + col) = it.value();
          }
        }
      }
      col0 += dims_cols(kcol++);
    }
    row0 += dims_rows(krow++);
  }

  ret.makeCompressed();

  return ret;
}

/**
 * @brief nxn sparse identity matrix
 *
 * @param n matrix square dimension
 */
inline Eigen::SparseMatrix<double> sparse_identity(std::size_t n)
{
  Eigen::SparseMatrix<double> ret(n, n);
  ret.reserve(Eigen::Matrix<int, -1, 1>::Ones(n));
  for (auto i = 0u; i < n; ++i) { ret.insert(i, i) = 1; }
  return ret;
}

/**
 * @brief Compute X ⊗ In where X is sparse.
 *
 * @param X sparse matrix in compressed format
 * @param n identity matrix dimension
 *
 * The result has the same storage order as X.
 */
template<typename Derived>
inline auto kron_identity(const Eigen::SparseCompressedBase<Derived> & X, std::size_t n)
{
  Eigen::
    SparseMatrix<typename Derived::Scalar, Derived::IsRowMajor ? Eigen::RowMajor : Eigen::ColMajor>
      ret(X.rows() * n, X.cols() * n);

  Eigen::Matrix<int, -1, 1> pattern(X.outerSize() * n);

  for (auto i0 = 0u, i = 0u; i < X.outerSize(); ++i) {
    auto nnz_i = X.outerIndexPtr()[i + 1] - X.outerIndexPtr()[i];
    pattern.segment(i0, n).setConstant(nnz_i);
    i0 += n;
  }

  ret.reserve(pattern);

  for (auto i0 = 0u; i0 < X.outerSize(); ++i0) {
    for (typename std::decay_t<decltype(X)>::InnerIterator it(X, i0); it; ++it) {
      for (auto diag = 0u; diag < n; ++diag) {
        ret.insert(n * it.row() + diag, n * it.col() + diag) = it.value();
      }
    }
  }

  ret.makeCompressed();

  return ret;
}

/**
 * @brief Compute X ⊗ In where X is dense.
 *
 * @param X sparse matrix in compressed format
 * @param n identity matrix dimension
 *
 * The result is column-major.
 */
template<typename Derived>
inline auto kron_identity(const Eigen::MatrixBase<Derived> & X, std::size_t n)
{
  Eigen::SparseMatrix<typename Derived::Scalar> ret(X.rows() * n, X.cols() * n);

  ret.reserve(Eigen::Matrix<int, -1, 1>::Constant(ret.cols(), n * X.rows()));

  for (auto row = 0u; row < X.rows(); ++row) {
    for (auto col = 0u; col < X.cols(); ++col) {
      for (auto diag = 0u; diag < n; ++diag) {
        ret.insert(n * row + diag, n * col + diag) = X(row, col);
      }
    }
  }

  ret.makeCompressed();

  return ret;
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__UTILS__SPARSE_HPP_