#include "NoiseAdjustGadget.h"
#include "hoArmadillo.h"
#include "hoMatrix.h"
#include "hoNDArray_elemwise.h"
#include "hoNDArray_linalg.h"
#include "hoNDArray_reductions.h"
#include "io/primitives.h"
#include "io/ismrmrd_types.h"
#include "log.h"
#include <boost/iterator/counting_iterator.hpp>
#ifdef USE_OMP
#include "omp.h"
#endif // USE_OMP

#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/split.hpp>
#include <typeinfo>

using namespace std::string_literals;
namespace bf = boost::filesystem;
namespace Gadgetron {
    namespace {

        template <class T> T value_or(const ISMRMRD::Optional<T>& opt, T default_value) {
            return opt ? *opt : default_value;
        }

        float bandwidth_from_header(const ISMRMRD::IsmrmrdHeader& header) {
            return value_or(header.acquisitionSystemInformation->relativeReceiverNoiseBandwidth, 0.793f);
        }

        void normalize_covariance(NoiseGatherer& ng){
            if (ng.number_of_samples > 1) {
                ng.tmp_covariance /= std::complex<float>(ng.number_of_samples - 1);
                ng.number_of_samples = 1;
            }
        }

        std::string to_string(const std::vector<ISMRMRD::CoilLabel>& coils) {
            std::stringstream sstream;
            for (auto i = 0u; i < coils.size(); i++)
                sstream << "Coil " << i << " - " << coils[i].coilNumber << " - " << coils[i].coilName << std::endl;
            return sstream.str();
        }

        // compare coil labels of noise and data
        // if number of channels are different, return false and order.size()==0
        // if any channels in noise cannot be found in data, return false and order.size()==0
        // if all channels in noise exist in data, but order is incorrect, return false, but  and order.size()==CHA
        // if all channels in nosie match channels in data, return true
        // order gives the matching order for src and dst coils
        // e.g. [2 1 0 3] means coil 0 of src matches coil 2 of dst etc.

        bool compare_coil_label(const std::vector<ISMRMRD::CoilLabel>& src_coils,
            const std::vector<ISMRMRD::CoilLabel>& dst_coils, std::vector<size_t>& order_in_src) {
            auto coil_name_comparer
                = [](const auto& coil1, const auto& coil2) { return coil1.coilName == coil2.coilName; };
            bool labels_match = std::equal(
                src_coils.begin(), src_coils.end(), dst_coils.begin(), dst_coils.end(), coil_name_comparer);

            if (labels_match)
                return labels_match;

            if (!is_permutation(
                    src_coils.begin(), src_coils.end(), dst_coils.begin(), dst_coils.end(), coil_name_comparer))
                return false;

            order_in_src = std::vector<size_t>(dst_coils.size(), 0);
            std::iota(order_in_src.begin(), order_in_src.end(), 0);

            for (size_t d = 0u; d < order_in_src.size(); d++) {
                if (coil_name_comparer(dst_coils[d], src_coils[d]))
                    continue;
                auto coil_it    = std::find_if(src_coils.begin(), src_coils.end(),
                    [&](const auto& coil) { return coil_name_comparer(coil, dst_coils[d]); });
                order_in_src[d] = std::distance(src_coils.begin(), coil_it);
            }

            return labels_match;
        }

        hoNDArray<std::complex<float>> mask_channels(
            hoNDArray<std::complex<float>> noise_prewhitener_matrix, const std::vector<size_t>& scale_only_channels) {
            // Mask out scale  only channels
            size_t c                  = noise_prewhitener_matrix.get_size(0);
            std::complex<float>* dptr = noise_prewhitener_matrix.data();
            for (auto ch : scale_only_channels) {
                for (size_t i = 0; i < c; i++) {
                    for (size_t j = 0; j < c; j++) {
                        if ((i == ch || j == ch) && (i != j)) { // zero if scale only and not on diagonal
                            dptr[i * c + j] = std::complex<float>(0.0, 0.0);
                        }
                    }
                }
            }
            return std::move(noise_prewhitener_matrix);
        }

        hoNDArray<std::complex<float>> computeNoisePrewhitener(
            const hoNDArray<std::complex<float>>& noise_covariance_matrix) {

            auto noise_prewhitener_matrix = noise_covariance_matrix;
            size_t c                      = noise_prewhitener_matrix.get_size(0);
            float v                       = Gadgetron::asum(noise_covariance_matrix);
            if (v <= 0) {
                GDEBUG("Accumulated noise prewhitener is empty\n");
                for (size_t cha = 0; cha < c; cha++) {
                    noise_prewhitener_matrix(cha, cha) = 1;
                }
            } else {
                // Cholesky and invert lower triangular
                arma::cx_fmat noise_covf = as_arma_matrix(noise_prewhitener_matrix);
                noise_covf               = arma::inv(arma::trimatu(arma::chol(noise_covf)));
            }

            return noise_prewhitener_matrix;
        }

        std::vector<size_t> find_scale_only_channels(
            const std::string& scale_only_channels_by_name, const std::vector<ISMRMRD::CoilLabel>& coillabels) {
            if (scale_only_channels_by_name.empty())
                return {};
            // Let's figure out if some channels are "scale_only"
            const std::string& uncomb_str = scale_only_channels_by_name;
            GDEBUG("SCALE ONLY: %s\n", uncomb_str.c_str());
            std::vector<std::string> uncomb;
            boost::split(uncomb, uncomb_str, boost::is_any_of(","));
            std::vector<size_t> scale_only_channels;

            for (unsigned int i = 0; i < uncomb.size(); i++) {
                std::string ch = boost::algorithm::trim_copy(uncomb[i]);
                if (std::find_if(
                        coillabels.begin(), coillabels.end(), [&](const auto& coil) { return ch == coil.coilName; })
                    != coillabels.end())
                    scale_only_channels.push_back(i);
            }
            return scale_only_channels;
        }

        hoNDArray<std::complex<float>> reorder_noise_channels(
            hoNDArray<std::complex<float>> noise_covariance, const std::vector<size_t>& coil_order) {
            using namespace Indexing;
            // check whether to switch channel order
            auto CHA = noise_covariance.get_size(0);
            if ((coil_order.size() != CHA)
                || std::equal(coil_order.begin(), coil_order.end(), boost::counting_iterator<size_t>(0)))
                return std::move(noise_covariance);

            GDEBUG_STREAM("Require to reorder the noise covariance matrix to match the data ... ");
            hoNDArray<std::complex<float>> noise_covariance_reordered = noise_covariance;

            // switch row
            for (size_t n = 0; n < CHA; n++) {
                //hoNDArrayView<std::complex<float>,1,false> f = noise_covariance_reordered(n,slice);
                noise_covariance_reordered(n, slice) = noise_covariance(coil_order[n], slice);
            }

            // switch column
            for (size_t m = 0; m < CHA; m++) {
                noise_covariance(slice, m) = noise_covariance_reordered(slice, coil_order[m]);
            }

            return std::move(noise_covariance);
        }

        float calculate_scale_factor(
            float acquisition_dwell_time_us, float noise_dwell_time_us, float receiver_noise_bandwidth) {
            float noise_bw_scale_factor;
            if ((noise_dwell_time_us == 0.0f) || (acquisition_dwell_time_us == 0.0f)) {
                noise_bw_scale_factor = 1.0f;
            } else {
                noise_bw_scale_factor
                    = std::sqrt(2.0f * acquisition_dwell_time_us / noise_dwell_time_us * receiver_noise_bandwidth);
            }
            return noise_bw_scale_factor;
        }
    }

    NoiseAdjustGadget::NoiseAdjustGadget(const Core::Context& context, const Core::GadgetProperties& props)
        : Core::ChannelGadget<Core::Acquisition>(context, props)
        , current_ismrmrd_header(context.header)
        , receiver_noise_bandwidth{ bandwidth_from_header(context.header) }
        , measurement_id{ value_or(context.header.measurementInformation->measurementID, ""s) }, measurement_storage(context.storage.measurement) {

        if (!perform_noise_adjust)
            return;

        GDEBUG("Folder to store noise dependencies is %s\n", noise_dependency_folder.c_str());
        GDEBUG("NoiseAdjustGadget::perform_noise_adjust_ is %d\n", perform_noise_adjust);
        GDEBUG("NoiseAdjustGadget::pass_nonconformant_data_ is %d\n", pass_nonconformant_data);
        GDEBUG("receiver_noise_bandwidth_ is %f\n", receiver_noise_bandwidth);

#ifdef USE_OMP
        omp_set_num_threads(1);
#endif // USE_OMP

        // find the measurementID of this scan

        noisehandler = load_or_gather();
    }

    NoiseAdjustGadget::NoiseHandler NoiseAdjustGadget::load_or_gather() const {
        GDEBUG("Measurement ID is %s\n", measurement_id.c_str());
        if (!current_ismrmrd_header.measurementInformation) {
            GWARN("ISMRMRD Header is missing measurmentinformation. Skipping noise adjust");
            return NoiseGatherer{};
        }
        const auto& measurementDependency = current_ismrmrd_header.measurementInformation->measurementDependency;
        auto val = std::find_if(measurementDependency.begin(), measurementDependency.end(), [](const auto& dependency) {
            return boost::algorithm::to_lower_copy(dependency.dependencyType) == "noise";
        });

        // find the noise dependencies if any
        if (val == measurementDependency.end())
            return NoiseGatherer{};

        auto noise_dependency = *val;
        GDEBUG("Measurement ID of noise dependency is %s\n", noise_dependency.measurementID.c_str());

        auto noise_covariance = load_noisedata(noise_dependency.measurementID);

        // try to load the precomputed noise prewhitener
        if (!noise_covariance) {
            GDEBUG("Stored noise dependency is NOT found : %s\n", noise_dependency.measurementID.c_str());
            return NoiseGatherer{};
        } else {
            GDEBUG("Stored noise dependency is found : %s\n", noise_dependency.measurementID.c_str());
            GDEBUG("Stored noise dwell time in us is %f\n", noise_covariance->noise_dwell_time_us);
            size_t CHA = noise_covariance->noise_covariance_matrix.get_size(0);
            GDEBUG("Stored noise channel number is %d\n", CHA);

            if (noise_covariance->header.acquisitionSystemInformation) {
                GDEBUG_STREAM("Noise coil info: ");
                GDEBUG_STREAM(to_string(noise_covariance->header.acquisitionSystemInformation->coilLabel));

                GDEBUG_STREAM("Data coil info: ");
                GDEBUG_STREAM(to_string(current_ismrmrd_header.acquisitionSystemInformation->coilLabel));

                std::vector<size_t> coil_order_of_data_in_noise;
                bool labels_match = compare_coil_label(noise_covariance->header.acquisitionSystemInformation->coilLabel,
                    current_ismrmrd_header.acquisitionSystemInformation->coilLabel, coil_order_of_data_in_noise);

                if (!labels_match) {
                    // if number of channels in noise is different than data
                    // or
                    // if any channels in noise do not exist in data
                    if (CHA != current_ismrmrd_header.acquisitionSystemInformation->coilLabel.size()) {
                        GDEBUG("Noise and measurement have different number of coils\n");
                    } else {
                        if (coil_order_of_data_in_noise.size() == CHA) {
                            GWARN_STREAM("Noise and meansurement have different coils, but will be reordered ... ");
                            noise_covariance->noise_covariance_matrix = reorder_noise_channels(
                                noise_covariance->noise_covariance_matrix, coil_order_of_data_in_noise);

                        } else {
                            GWARN_STREAM("Noise and meansurement have different coils and cannot be reordered ... ");
                        }
                    }
                }
                return LoadedNoise{noise_covariance->noise_covariance_matrix,noise_covariance->noise_dwell_time_us};

            } else if (current_ismrmrd_header.acquisitionSystemInformation) {
                GERROR("Noise ismrmrd header does not have acquisition system information but current header "
                       "does\n");
            }

            //                    number_of_noise_samples_ = 1; // When we load the matrix, it is already
            //                    scaled.
        }
        return NoiseGatherer{};
    }

    static bool is_noise(const Core::Acquisition& acq) {
        return std::get<ISMRMRD::AcquisitionHeader>(acq).isFlagSet(ISMRMRD::ISMRMRD_ACQ_IS_NOISE_MEASUREMENT);
    }

    template <class NOISEHANDLER>
    void NoiseAdjustGadget::add_noise(NOISEHANDLER& nh, const Gadgetron::Core::Acquisition&) const {
    }

    template <> void NoiseAdjustGadget::add_noise(NoiseGatherer& ng, const Gadgetron::Core::Acquisition& acq) const {
        auto& data    = std::get<hoNDArray<std::complex<float>>>(acq);
        auto& head    = std::get<ISMRMRD::AcquisitionHeader>(acq);
        if (ng.tmp_covariance.empty()) {
            auto channels = head.active_channels;
            ng.tmp_covariance = hoNDArray<std::complex<float>>(channels, channels);
            std::fill(ng.tmp_covariance.begin(), ng.tmp_covariance.end(), std::complex<float>(0));
        }

        if (ng.noise_dwell_time_us == 0)
            ng.noise_dwell_time_us = head.sample_time_us;

        auto dataM = as_arma_matrix(data);
        auto covariance = as_arma_matrix(ng.tmp_covariance);
        covariance += dataM.t()*dataM;

        ng.number_of_samples += head.number_of_samples;
    }

    template <> void NoiseAdjustGadget::add_noise(NoiseHandler& nh, const Gadgetron::Core::Acquisition& acq) const {
        Core::visit([&](auto& var) { this->add_noise(var, acq); }, nh);
    }
    template <class NOISEHANDLER> void NoiseAdjustGadget::save_noisedata(NOISEHANDLER& nh) {}

    template <> void NoiseAdjustGadget::save_noisedata(NoiseGatherer& ng) {
        if (ng.tmp_covariance.empty())
            return;

        normalize_covariance(ng);

        this->measurement_storage.store("noise_covariance",NoiseCovariance{ this->current_ismrmrd_header, ng.noise_dwell_time_us, ng.tmp_covariance });
    }

    template <> void NoiseAdjustGadget::save_noisedata(NoiseHandler& nh) {
        Core::visit([&](auto& var) { this->save_noisedata(var); }, nh);
    }

    template <class NH>
    NoiseAdjustGadget::NoiseHandler NoiseAdjustGadget::handle_acquisition(NH nh, Core::Acquisition& acq) {
        return std::move(nh);
    };

    template <>
    NoiseAdjustGadget::NoiseHandler NoiseAdjustGadget::handle_acquisition(
        Prewhitener pw, Core::Acquisition& acq) {

        auto& data = std::get<hoNDArray<std::complex<float>>>(acq);
        if (data.get_size(1) == pw.prewhitening_matrix.get_size(0)) {
            auto dataM = as_arma_matrix(data);
            auto pwm = as_arma_matrix(pw.prewhitening_matrix);
            dataM *= pwm;
        } else if (!this->pass_nonconformant_data) {
            throw std::runtime_error("Input data has different number of channels from noise data");
        }
        return std::move(pw);
    }

    template <>
    NoiseAdjustGadget::NoiseHandler NoiseAdjustGadget::handle_acquisition(
        NoiseGatherer ng, Core::Acquisition& acq) {
        auto& head = std::get<ISMRMRD::AcquisitionHeader>(acq);
        if (ng.number_of_samples == 0)
            return std::move(ng);

        normalize_covariance(ng);
        this->save_noisedata(ng);

        auto masked_covariance = mask_channels(ng.tmp_covariance, scale_only_channels);

        auto prewhitening_matrix = computeNoisePrewhitener(masked_covariance);
        prewhitening_matrix
            *= calculate_scale_factor(head.sample_time_us, ng.noise_dwell_time_us, receiver_noise_bandwidth);
        return handle_acquisition(Prewhitener{ prewhitening_matrix }, acq);
    }

    template <>
    NoiseAdjustGadget::NoiseHandler NoiseAdjustGadget::handle_acquisition(
        LoadedNoise ln, Core::Acquisition& acq)  {
        auto& head               = std::get<ISMRMRD::AcquisitionHeader>(acq);
        auto masked_covariance   = mask_channels(std::move(ln.covariance), scale_only_channels);
        auto prewhitening_matrix = computeNoisePrewhitener(masked_covariance);
        prewhitening_matrix
            *= calculate_scale_factor(head.sample_time_us, ln.noise_dwell_time_us, receiver_noise_bandwidth);
        return handle_acquisition(Prewhitener{ prewhitening_matrix }, acq);
    }

    template <>
    NoiseAdjustGadget::NoiseHandler NoiseAdjustGadget::handle_acquisition(
        NoiseHandler nh, Core::Acquisition& acq) {
        return Core::visit([&](auto var) { return this->handle_acquisition<decltype(var)>(std::move(var), acq); }, std::move(nh));
    }

    void NoiseAdjustGadget::process(Core::InputChannel<Core::Acquisition>& input, Core::OutputChannel& output) {

        scale_only_channels = current_ismrmrd_header.acquisitionSystemInformation
                                  ? find_scale_only_channels(scale_only_channels_by_name,
                                      current_ismrmrd_header.acquisitionSystemInformation->coilLabel)
                                  : std::vector<size_t>{};

        for (auto acq : input) {
            if (is_noise(acq)) {
                add_noise(noisehandler, acq);
                continue;
            }
            noisehandler = handle_acquisition(std::move(noisehandler), acq);
            output.push(std::move(acq));
        }

        this->save_noisedata(noisehandler);
    }

    Core::optional<NoiseCovariance> NoiseAdjustGadget::load_noisedata(const std::string &noise_measurement_id) const {
       auto list = this->measurement_storage.fetch<NoiseCovariance>(noise_measurement_id,"noise_covariance");
       if (!list.empty()){
           return list[0];
       }
        return {};

    }

    GADGETRON_GADGET_EXPORT(NoiseAdjustGadget)

} // namespace Gadgetron


#include <sstream>
#include <limits>
#include <cstdlib> /* srand, rand */
#include <cmath>

#include <boost/math/distributions/negative_binomial.hpp>
#include <boost/log/trivial.hpp>

#include "kmergraphwithcoverage.h"
#include "localPRG.h"

using namespace prg;

void KmerGraphWithCoverage::set_exp_depth_covg(const uint32_t edp)
{
    const bool exp_depth_covg_parameter_is_valid = edp > 0;
    if (!exp_depth_covg_parameter_is_valid) {
        fatal_error(
            "Error setting exp_depth_covg: exp_depth_covg is invalid, must be > 0, is ",
            edp);
    }
    exp_depth_covg = edp;
}

void KmerGraphWithCoverage::set_binomial_parameter_p(const float e_rate)
{
    BOOST_LOG_TRIVIAL(debug) << "Set p in kmergraph";

    const bool valid_parameters_to_set_p
        = (kmer_prg->k != 0) && (0 < e_rate and e_rate < 1);
    if (!valid_parameters_to_set_p) {
        fatal_error("Error setting binomial parameter p, invalid parameters: ",
            "kmer_prg->k = ", kmer_prg->k, ", e_rate = ", e_rate);
    }

    binomial_parameter_p = 1 / exp(e_rate * kmer_prg->k);
}

void KmerGraphWithCoverage::increment_covg(
    uint32_t node_id, pandora::Strand strand, uint32_t sample_id)
{
    const bool sample_is_valid
        = this->node_index_to_sample_coverage[node_id].size() > sample_id;
    if (!sample_is_valid) {
        fatal_error(
            "Error incrementing coverage: sample_id is invalid (", sample_id, ")");
    }

    // get a pointer to the value we want to increment
    uint16_t* coverage_ptr = nullptr;
    if (strand == pandora::Strand::Forward) {
        coverage_ptr = &(this->node_index_to_sample_coverage[node_id][sample_id].first);
    } else {
        coverage_ptr
            = &(this->node_index_to_sample_coverage[node_id][sample_id].second);
    }

    const bool safe_to_increase_covg { (*coverage_ptr) < UINT16_MAX };
    if (safe_to_increase_covg) {
        ++(*coverage_ptr);
    }
}

uint32_t KmerGraphWithCoverage::get_covg(
    uint32_t node_id, pandora::Strand strand, uint32_t sample_id) const
{

    if (this->node_index_to_sample_coverage[node_id].size() <= sample_id)
        return 0;

    if (strand == pandora::Strand::Forward) {
        return (uint32_t)(
            this->node_index_to_sample_coverage[node_id][sample_id].first);
    } else {
        return (uint32_t)(
            this->node_index_to_sample_coverage[node_id][sample_id].second);
    }
}

void KmerGraphWithCoverage::set_covg(
    uint32_t node_id, uint16_t value, pandora::Strand strand, uint32_t sample_id)
{
    const bool sample_is_valid
        = this->node_index_to_sample_coverage[node_id].size() > sample_id;
    if (!sample_is_valid) {
        fatal_error("Error setting coverage: sample_id is invalid (", sample_id, ")");
    }

    if (strand == pandora::Strand::Forward) {
        this->node_index_to_sample_coverage[node_id][sample_id].first = value;
    } else {
        this->node_index_to_sample_coverage[node_id][sample_id].second = value;
    }
}

void KmerGraphWithCoverage::set_negative_binomial_parameters(
    const float& nbin_prob, const float& nb_fail)
{
    if (nbin_prob == 0 and nb_fail == 0)
        return;

    const bool negative_binomial_parameters_were_previously_set
        = (negative_binomial_parameter_p > 0 and negative_binomial_parameter_p < 1)
        && (negative_binomial_parameter_r > 0);
    if (!(negative_binomial_parameters_were_previously_set)) {
        fatal_error("Error setting negative_binomial_parameters: "
                    "negative_binomial_parameter_p (",
            negative_binomial_parameter_p, ")", " or negative_binomial_parameter_r (",
            negative_binomial_parameter_r, ") ", "were not correctly set");
    }

    negative_binomial_parameter_p += nbin_prob;
    negative_binomial_parameter_r += nb_fail;
}

float KmerGraphWithCoverage::nbin_prob(uint32_t node_id, const uint32_t& sample_id)
{
    auto k = this->get_forward_covg(node_id, sample_id)
        + this->get_reverse_covg(node_id, sample_id);
    float return_prob
        = log(pdf(boost::math::negative_binomial(
                      negative_binomial_parameter_r, negative_binomial_parameter_p),
            k));
    return_prob = std::max(return_prob, std::numeric_limits<float>::lowest() / 1000);
    return return_prob;
}

float KmerGraphWithCoverage::lin_prob(uint32_t node_id, const uint32_t& sample_id)
{
    const bool reads_were_mapped_to_this_kmer_graph = num_reads != 0;
    if (!reads_were_mapped_to_this_kmer_graph) {
        fatal_error(
            "Impossible to compute lin_prob, no reads were mapped to this kmer graph");
    }
    auto k = this->get_forward_covg(node_id, sample_id)
        + this->get_reverse_covg(node_id, sample_id);
    return log(float(k) / num_reads);
}

float KmerGraphWithCoverage::bin_prob(uint32_t node_id, const uint32_t& sample_id)
{
    const bool reads_were_mapped_to_this_kmer_graph = num_reads != 0;
    if (!reads_were_mapped_to_this_kmer_graph) {
        fatal_error(
            "Impossible to compute bin_prob, no reads were mapped to this kmer graph");
    }
    return bin_prob(node_id, num_reads, sample_id);
}

float KmerGraphWithCoverage::bin_prob(
    const uint32_t& node_id, const uint32_t& num, const uint32_t& sample_id)
{
    const bool binomial_parameter_p_is_set_correctly = binomial_parameter_p != 1;
    if (!binomial_parameter_p_is_set_correctly) {
        fatal_error("Error when computing bin_prob: binomial_parameter_p (",
            binomial_parameter_p, ") is not correctly set");
    }

    const bool node_exists = node_id < kmer_prg->nodes.size();
    if (!node_exists) {
        fatal_error("Error when computing bin_prob: attempt to access inexistent node ",
            node_id);
    }

    uint32_t sum_coverages = this->get_forward_covg(node_id, sample_id)
        + this->get_reverse_covg(node_id, sample_id);

    float return_prob;
    if (node_id == (*(kmer_prg->sorted_nodes.begin()))->id
        or node_id == (*(kmer_prg->sorted_nodes.rbegin()))->id) {
        return_prob = 0; // is really undefined
    } else if (sum_coverages > num) {
        // under model assumptions this can't happen, but it inevitably will, so bodge
        return_prob
            = lognchoosek2(sum_coverages, this->get_forward_covg(node_id, sample_id),
                  this->get_reverse_covg(node_id, sample_id))
            + sum_coverages * log(binomial_parameter_p / 2);
        // note this may give disadvantage to repeat kmers
    } else {
        return_prob = lognchoosek2(num, this->get_forward_covg(node_id, sample_id),
                          this->get_reverse_covg(node_id, sample_id))
            + sum_coverages * log(binomial_parameter_p / 2)
            + (num - sum_coverages) * log(1 - binomial_parameter_p);
    }
    return return_prob;
}

float KmerGraphWithCoverage::get_prob(
    const std::string& prob_model, const uint32_t& node_id, const uint32_t& sample_id)
{
    if (prob_model == "nbin") {
        // is there no parameter check here?
        return nbin_prob(node_id, sample_id);
    } else if (prob_model == "bin") {
        const bool binomial_parameters_are_ok
            = (binomial_parameter_p < 1) && (num_reads > 0);
        if (!binomial_parameters_are_ok) {
            fatal_error("Error when computing kmer prob: binomial parameters are not "
                        "ok (binomial_parameter_p = ",
                binomial_parameter_p, ", ", "num_reads = ", num_reads);
        }
        return bin_prob(node_id, sample_id);
    } else if (prob_model == "lin") {
        // is there no parameter check here?
        return lin_prob(node_id, sample_id);
    } else {
        fatal_error("Invalid probability model for kmer coverage distribution: ",
            prob_model, ". Should be nbin, bin or lin");
    }
}

bool KmerGraphWithCoverage::coverage_is_zeroes(const uint32_t& sample_id)
{
    bool all_zero = true;
    for (const auto& node_ptr : kmer_prg->nodes) {
        const auto covg { this->get_forward_covg(node_ptr->id, sample_id)
            + this->get_reverse_covg(node_ptr->id, sample_id) };
        if (covg > 0) {
            BOOST_LOG_TRIVIAL(debug) << "Found non-zero coverage in kmer graph";
            all_zero = false;
            break;
        }
    }
    if (all_zero) {
        BOOST_LOG_TRIVIAL(debug) << "ALL ZEROES in kmer graph coverages";
    }
    return all_zero;
}

float KmerGraphWithCoverage::find_max_path(std::vector<KmerNodePtr>& maxpath,
    const std::string& prob_model, const uint32_t& max_num_kmers_to_average,
    const uint32_t& sample_id)
{
    // TODO: FIX THIS INNEFICIENCY I INTRODUCED
    const std::vector<KmerNodePtr> sorted_nodes(
        this->kmer_prg->sorted_nodes.begin(), this->kmer_prg->sorted_nodes.end());
    this->kmer_prg->check();

    // also check not all 0 covgs
    auto coverages_all_zero = coverage_is_zeroes(sample_id);
    if (coverages_all_zero)
        return std::numeric_limits<float>::lowest();

    // create vectors to hold the intermediate values
    std::vector<float> max_sum_of_log_probs_from_node(sorted_nodes.size(), 0);
    std::vector<uint32_t> length_of_maxpath_from_node(sorted_nodes.size(), 0);
    std::vector<uint32_t> prev_node_along_maxpath(
        sorted_nodes.size(), sorted_nodes.size() - 1);
    float max_mean;
    int max_length;
    const float tolerance = 0.000001;

    for (uint32_t j = sorted_nodes.size() - 1; j != 0; --j) {
        max_mean = std::numeric_limits<float>::lowest();
        max_length = 0; // tie break with longest kmer path
        const auto& current_node = sorted_nodes[j - 1];
        for (uint32_t i = 0; i != current_node->out_nodes.size(); ++i) {
            const auto& considered_outnode = current_node->out_nodes[i].lock();
            const bool is_terminus_and_most_likely
                = considered_outnode->id == sorted_nodes.back()->id
                and thresh > max_mean + tolerance;
            const bool avg_log_likelihood_is_most_likely
                = max_sum_of_log_probs_from_node[considered_outnode->id]
                    / length_of_maxpath_from_node[considered_outnode->id]
                > max_mean + tolerance;
            const bool avg_log_likelihood_is_close_to_most_likely = max_mean
                    - max_sum_of_log_probs_from_node[considered_outnode->id]
                        / length_of_maxpath_from_node[considered_outnode->id]
                <= tolerance;
            const bool is_longer_path
                = length_of_maxpath_from_node[considered_outnode->id]
                > (uint)max_length;

            if (is_terminus_and_most_likely or avg_log_likelihood_is_most_likely
                or (avg_log_likelihood_is_close_to_most_likely and is_longer_path)) {
                max_sum_of_log_probs_from_node[current_node->id]
                    = get_prob(prob_model, current_node->id, sample_id)
                    + max_sum_of_log_probs_from_node[considered_outnode->id];
                length_of_maxpath_from_node[current_node->id]
                    = 1 + length_of_maxpath_from_node[considered_outnode->id];
                prev_node_along_maxpath[current_node->id] = considered_outnode->id;

                if (length_of_maxpath_from_node[current_node->id]
                    > max_num_kmers_to_average) {
                    uint32_t prev_node = prev_node_along_maxpath[current_node->id];
                    for (uint step = 0; step < max_num_kmers_to_average; step++) {
                        prev_node = prev_node_along_maxpath[prev_node];
                    }
                    max_sum_of_log_probs_from_node[current_node->id]
                        -= get_prob(prob_model, sorted_nodes[prev_node]->id, sample_id);
                    length_of_maxpath_from_node[current_node->id] -= 1;

                    // this remains as an assert, as it is a code check
                    // Note: I think we might even be able to remove this
                    assert(length_of_maxpath_from_node[current_node->id]
                        == max_num_kmers_to_average);
                }

                if (considered_outnode->id != sorted_nodes.back()->id) {
                    max_mean = max_sum_of_log_probs_from_node[considered_outnode->id]
                        / length_of_maxpath_from_node[considered_outnode->id];
                    max_length = length_of_maxpath_from_node[considered_outnode->id];
                } else {
                    max_mean = thresh;
                }
            }
        }
    }

    // extract path
    uint32_t prev_node = prev_node_along_maxpath[sorted_nodes[0]->id];
    while (prev_node < sorted_nodes.size() - 1) {
        maxpath.push_back(this->kmer_prg->nodes[prev_node]);
        prev_node = prev_node_along_maxpath[prev_node];

        if (maxpath.size() > 1000000) {
            fatal_error("I think I've found an infinite loop - is "
                        "something wrong with this kmergraph?");
        }
    }

    const bool path_was_found_through_the_kmer_PRG = length_of_maxpath_from_node[0] > 0;
    if (!path_was_found_through_the_kmer_PRG) {
        fatal_error("Error when finding max path: found no path through kmer prg");
    }

    return prob_path(maxpath, sample_id, prob_model);
}

std::vector<std::vector<KmerNodePtr>> KmerGraphWithCoverage::get_random_paths(
    uint32_t num_paths)
{
    // find a random path through kmergraph picking ~uniformly from the outnodes at each
    // point
    std::vector<std::vector<KmerNodePtr>> rpaths;
    std::vector<KmerNodePtr> rpath;
    uint32_t i;

    time_t now;
    now = time(nullptr);
    srand((unsigned int)now);

    if (!kmer_prg->nodes.empty()) {
        for (uint32_t j = 0; j != num_paths; ++j) {
            i = rand() % kmer_prg->nodes[0]->out_nodes.size();
            rpath.push_back(kmer_prg->nodes[0]->out_nodes[i].lock());
            while (rpath.back() != kmer_prg->nodes[kmer_prg->nodes.size() - 1]) {
                if (rpath.back()->out_nodes.size() == 1) {
                    rpath.push_back(rpath.back()->out_nodes[0].lock());
                } else {
                    i = rand() % rpath.back()->out_nodes.size();
                    rpath.push_back(rpath.back()->out_nodes[i].lock());
                }
            }
            rpath.pop_back();
            rpaths.push_back(rpath);
            rpath.clear();
        }
    }
    return rpaths;
}

float KmerGraphWithCoverage::prob_path(const std::vector<KmerNodePtr>& kpath,
    const uint32_t& sample_id, const std::string& prob_model)
{
    float return_prob_path = 0;
    for (uint32_t i = 0; i != kpath.size(); ++i) {
        return_prob_path += get_prob(prob_model, kpath[i]->id, sample_id);
    }
    uint32_t len = kpath.size();
    if (kpath[0]->path.length() == 0) {
        len -= 1;
    }
    if (kpath.back()->path.length() == 0) {
        len -= 1;
    }
    if (len == 0) {
        len = 1;
    }
    return return_prob_path / len;
}

void KmerGraphWithCoverage::save_covg_dist(const std::string& filepath)
{
    std::ofstream handle;
    handle.open(filepath);

    for (const auto& kmer_node_ptr : kmer_prg->nodes) {
        const KmerNode& kmer_node = *kmer_node_ptr;

        uint32_t sample_id = 0;
        for (const auto& sample_coverage :
            node_index_to_sample_coverage[kmer_node.id]) {
            handle << kmer_node.id << " " << sample_id << " " << sample_coverage.first
                   << " " << sample_coverage.second;

            sample_id++;
        }
    }
    handle.close();
}

// save the KmerGraph as gfa
// TODO: THIS SHOULD BE RECODED, WE ARE DUPLICATING CODE HERE (SEE KmerGraph::save())!!!
void KmerGraphWithCoverage::save(
    const fs::path& filepath, const std::shared_ptr<LocalPRG> localprg) const
{
    uint32_t sample_id = 0;

    fs::ofstream handle(filepath);
    if (handle.is_open()) {
        handle << "H\tVN:Z:1.0\tbn:Z:--linear --singlearr" << std::endl;
        for (const auto& c : kmer_prg->nodes) {
            handle << "S\t" << c->id << "\t";

            if (localprg != nullptr) {
                handle << localprg->string_along_path(c->path);
            } else {
                handle << c->path;
            }

            handle << "\tFC:i:" << this->get_forward_covg(c->id, sample_id)
                   << "\tRC:i:" << this->get_reverse_covg(c->id, sample_id)
                   << std::endl;

            for (uint32_t j = 0; j < c->out_nodes.size(); ++j) {
                handle << "L\t" << c->id << "\t+\t" << c->out_nodes[j].lock()->id
                       << "\t+\t0M" << std::endl;
            }
        }
        handle.close();
    } else {
        fatal_error("Unable to open kmergraph file ", filepath);
    }
}

// TODO: THIS SHOULD BE RECODED, WE ARE DUPLICATING CODE HERE (SEE KmerGraph::load())!!!
// TODO: remove this method?
void KmerGraphWithCoverage::load(const std::string& filepath)
{
    // TODO: this might be dangerous, recode this?
    auto kmer_prg = const_cast<KmerGraph*>(this->kmer_prg);
    kmer_prg->clear();
    uint32_t sample_id = 0;

    std::string line;
    std::vector<std::string> split_line;
    std::stringstream ss;
    uint32_t id = 0, from, to;
    uint16_t covg;
    prg::Path p;
    uint32_t num_nodes = 0;

    std::ifstream myfile(filepath);
    if (myfile.is_open()) {

        while (getline(myfile, line).good()) {
            if (line[0] == 'S') {
                split_line = split(line, "\t");

                const bool line_is_consistent = split_line.size() >= 4;
                if (!line_is_consistent) {
                    fatal_error("Error reading GFA. Offending line: ", line);
                }

                id = std::stoi(split_line[1]);
                num_nodes = std::max(num_nodes, id);
            }
        }
        myfile.clear();
        myfile.seekg(0, myfile.beg);
        kmer_prg->nodes.reserve(num_nodes);
        std::vector<uint16_t> outnode_counts(num_nodes + 1, 0),
            innode_counts(num_nodes + 1, 0);

        while (getline(myfile, line).good()) {
            if (line[0] == 'S') {
                split_line = split(line, "\t");

                const bool line_is_consistent = split_line.size() >= 4;
                if (!line_is_consistent) {
                    fatal_error("Error reading GFA. Offending line: ", line);
                }

                id = stoi(split_line[1]);
                ss << split_line[2];
                char c = ss.peek();

                if (!isdigit(c)) {
                    fatal_error("Error reading GFA: cannot read in this sort of "
                                "kmergraph GFA as it ",
                        "does not label nodes with their PRG path. ",
                        "Offending line: ", line);
                }

                ss >> p;
                ss.clear();
                // add_node(p);
                KmerNodePtr n = std::make_shared<KmerNode>(id, p);

                const bool id_is_consistent = (id == kmer_prg->nodes.size()
                    or num_nodes - id == kmer_prg->nodes.size());
                if (!id_is_consistent) {
                    fatal_error("Error reading GFA: node ID is inconsistent.",
                        "id = ", id, ", ", "nodes.size() = ", kmer_prg->nodes.size(),
                        ", ", "num_nodes = ", num_nodes);
                }

                kmer_prg->nodes.push_back(n);
                kmer_prg->sorted_nodes.insert(n);
                if (kmer_prg->k == 0 and p.length() > 0) {
                    kmer_prg->k = p.length();
                }
                covg = (uint16_t)(stoul(split(split_line[3], "FC:i:")[0]));
                this->set_forward_covg(n->id, covg, sample_id);
                covg = (uint16_t)(stoul(split(split_line[4], "RC:i:")[0]));
                this->set_reverse_covg(n->id, covg, sample_id);
                if (split_line.size() >= 6) {
                    n->num_AT = std::stoi(split_line[5]);
                }
            } else if (line[0] == 'L') {
                split_line = split(line, "\t");

                const bool line_is_consistent = split_line.size() >= 5;
                if (!line_is_consistent) {
                    fatal_error("Error reading GFA. Offending line: ", line);
                }

                const int from_node = stoi(split_line[1]);
                const int to_node = stoi(split_line[3]);
                const bool from_node_in_range = from_node < (int)outnode_counts.size();
                const bool to_node_in_range = to_node < (int)innode_counts.size();
                if (!from_node_in_range) {
                    fatal_error(
                        "Error reading GFA: from_node out of range: ", from_node,
                        ">=", outnode_counts.size(), ". Offending line: ", line);
                }
                if (!to_node_in_range) {
                    fatal_error("Error reading GFA: to_node out of range: ", to_node,
                        ">=", innode_counts.size(), ". Offending line: ", line);
                }

                outnode_counts[stoi(split_line[1])] += 1;
                innode_counts[stoi(split_line[3])] += 1;
            }
        }

        if (id == 0) {
            reverse(kmer_prg->nodes.begin(), kmer_prg->nodes.end());
        }

        id = 0;
        for (const auto& n : kmer_prg->nodes) {
            const bool id_is_consistent = (kmer_prg->nodes[id]->id == id)
                && (n->id < outnode_counts.size()) && (n->id < innode_counts.size());
            if (!id_is_consistent) {
                fatal_error("Error reading GFA: node: ", n,
                    " has inconsistent id, should be ", id);
            }
            id++;
            n->out_nodes.reserve(outnode_counts[n->id]);
            n->in_nodes.reserve(innode_counts[n->id]);
        }

        myfile.clear();
        myfile.seekg(0, myfile.beg);

        while (getline(myfile, line).good()) {
            if (line[0] == 'L') {
                split_line = split(line, "\t");

                const bool line_is_consistent = split_line.size() >= 5;
                if (!line_is_consistent) {
                    fatal_error("Error reading GFA. Offending line: ", line);
                }

                if (split_line[2] == split_line[4]) {
                    from = std::stoi(split_line[1]);
                    to = std::stoi(split_line[3]);
                } else {
                    // never happens
                    from = std::stoi(split_line[3]);
                    to = std::stoi(split_line[1]);
                }
                kmer_prg->add_edge(kmer_prg->nodes[from], kmer_prg->nodes[to]);
            }
        }
    } else {
        fatal_error("Error reading GFA: unable to open kmergraph file: ", filepath);
    }
}


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

  @copyright 2015 NumScale SAS
  @copyright 2015 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_ARCH_COMMON_SCALAR_FUNCTION_INEARBYINT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_INEARBYINT_HPP_INCLUDED

#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( inearbyint_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_<bd::integer_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      return a0;
    }
  };
  BOOST_DISPATCH_OVERLOAD ( inearbyint_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_<bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<A0> operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      return saturated_(toint)(nearbyint(a0));
    }
  };
  BOOST_DISPATCH_OVERLOAD ( inearbyint_
                          , (typename A0)
                          , bd::cpu_
                          , boost::simd::fast_tag
                          , bd::scalar_<bd::integer_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const fast_tag &,  A0 a0
                                    ) const BOOST_NOEXCEPT
    {
      return a0;
    }
  };
  BOOST_DISPATCH_OVERLOAD ( inearbyint_
                          , (typename A0)
                          , bd::cpu_
                          , boost::simd::fast_tag
                          , bd::scalar_<bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<A0> operator() (const fast_tag &,  A0 a0
                                                      ) const BOOST_NOEXCEPT
    {
      return toint(nearbyint(a0));
    }
  };
} } }

#endif


/* Copyright Institute of Sound and Vibration Research - All rights reserved */

#include "signal_flow.hpp"

#include <libpanning/LoudspeakerArray.h>

#include <boost/algorithm/string.hpp> // case-insensitive string compare

#include <algorithm>
#include <vector>

namespace visr
{
namespace apps
{
namespace scene_decoder
{

SignalFlow::SignalFlow( SignalFlowContext & context,
                        char const * name,
                        CompositeComponent * parent,
                        std::size_t numberOfInputs,
                        std::size_t numberOfOutputs,
                        std::size_t interpolationPeriod,
                        std::string const & configFile,
                        std::size_t udpPort )
 : CompositeComponent( context, name, parent )
 , cNumberOfInputs( numberOfInputs )
 , cNumberOfOutputs( numberOfOutputs )
 , cInterpolationSteps( interpolationPeriod )
 , mNetworkPort( udpPort )
 , mInput( "in", *this )
 , mOutput( "out", *this )
 , mSceneReceiver( context, "SceneReceiver", this, mNetworkPort, rcl::UdpReceiver::Mode::Asynchronous )
 , mSceneDecoder( context, "SceneDecoder", this )
 , mSceneEncoder( context, "SceneEncoder", this )
 , mSceneSender( context, "SceneSender", this, 9998, "152.78.243.62", 9999, rcl::UdpSender::Mode::Asynchronous )
 , mGainCalculator( context, "VbapGainCalculator", this,
                    numberOfInputs,
                    panning::LoudspeakerArray( configFile ) )
 , mMatrix( context, "GainMatrix", this )
{
   mInput.setWidth( cNumberOfInputs );
   mOutput.setWidth( cNumberOfOutputs );

   // Initialise and configure audio components

   mMatrix.setup( cNumberOfInputs, cNumberOfOutputs, cInterpolationSteps, 1.0f );

   audioConnection( "this", "in", ChannelRange( 0, cNumberOfInputs ), "GainMatrix", "in", ChannelRange( 0, cNumberOfInputs ) );
   audioConnection( "GainMatrix", "out", ChannelRange( 0, cNumberOfOutputs ), "this", "out", ChannelRange( 0, cNumberOfOutputs ) );

   parameterConnection( "SceneReceiver", "messageOutput", "SceneDecoder", "datagramInput" );
   parameterConnection( "SceneDecoder", "objectVectorOutput", "VbapGainCalculator", "objectVectorInput" );
   parameterConnection( "VbapGainCalculator", "gainOutput", "GainMatrix", "gainInput" );

}

 SignalFlow::~SignalFlow( ) = default;

} // namespace scene_decoder
} // namespace apps
} // namespace visr


///////////////////////////////////////////////////////////////////////////////
// tail_variate_means.hpp
//
//  Copyright 2006 Daniel Egloff, Olivier Gygi. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_TAIL_VARIATE_MEANS_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_TAIL_VARIATE_MEANS_HPP_DE_01_01_2006

#include <numeric>
#include <vector>
#include <limits>
#include <functional>
#include <sstream>
#include <stdexcept>
#include <boost/throw_exception.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/tail.hpp>
#include <boost/accumulators/statistics/tail_variate.hpp>
#include <boost/accumulators/statistics/tail_mean.hpp>
#include <boost/accumulators/statistics/parameters/quantile_probability.hpp>
#include <boost/serialization/vector.hpp>

#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 4127) // conditional expression is constant
#endif

namespace boost { namespace accumulators
{

namespace impl
{
    /**
        @brief Estimation of the absolute and relative tail variate means (for both left and right tails)

        For all \f$j\f$-th variates associated to the \f$\lceil n(1-\alpha)\rceil\f$ largest samples (or the
        \f$\lceil n(1-\alpha)\rceil\f$ smallest samples in case of the left tail), the absolute tail means
        \f$\widehat{ATM}_{n,\alpha}(X, j)\f$ are computed and returned as an iterator range. Alternatively,
        the relative tail means \f$\widehat{RTM}_{n,\alpha}(X, j)\f$ are returned, which are the absolute
        tail means normalized with the (non-coherent) sample tail mean \f$\widehat{NCTM}_{n,\alpha}(X)\f$.

        \f[
            \widehat{ATM}_{n,\alpha}^{\mathrm{right}}(X, j) =
                \frac{1}{\lceil n(1-\alpha) \rceil}
                \sum_{i=\lceil \alpha n \rceil}^n \xi_{j,i}
        \f]

        \f[
            \widehat{ATM}_{n,\alpha}^{\mathrm{left}}(X, j) =
                \frac{1}{\lceil n\alpha \rceil}
                \sum_{i=1}^{\lceil n\alpha \rceil} \xi_{j,i}
        \f]

        \f[
            \widehat{RTM}_{n,\alpha}^{\mathrm{right}}(X, j) =
                \frac{\sum_{i=\lceil n\alpha \rceil}^n \xi_{j,i}}
            {\lceil n(1-\alpha)\rceil\widehat{NCTM}_{n,\alpha}^{\mathrm{right}}(X)}
        \f]

        \f[
            \widehat{RTM}_{n,\alpha}^{\mathrm{left}}(X, j) =
                \frac{\sum_{i=1}^{\lceil n\alpha \rceil} \xi_{j,i}}
            {\lceil n\alpha\rceil\widehat{NCTM}_{n,\alpha}^{\mathrm{left}}(X)}
        \f]
    */

    ///////////////////////////////////////////////////////////////////////////////
    // tail_variate_means_impl
    //  by default: absolute tail_variate_means
    template<typename Sample, typename Impl, typename LeftRight, typename VariateTag>
    struct tail_variate_means_impl
      : accumulator_base
    {
        typedef typename numeric::functional::fdiv<Sample, std::size_t>::result_type float_type;
        typedef std::vector<float_type> array_type;
        // for boost::result_of
        typedef iterator_range<typename array_type::iterator> result_type;

        tail_variate_means_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            std::size_t cnt = count(args);

            std::size_t n = static_cast<std::size_t>(
                std::ceil(
                    cnt * ( ( is_same<LeftRight, left>::value ) ? args[quantile_probability] : 1. - args[quantile_probability] )
                )
            );

            std::size_t num_variates = tail_variate(args).begin()->size();

            this->tail_means_.clear();
            this->tail_means_.resize(num_variates, Sample(0));

            // If n is in a valid range, return result, otherwise return NaN or throw exception
            if (n < static_cast<std::size_t>(tail(args).size()))
            {
                this->tail_means_ = std::accumulate(
                    tail_variate(args).begin()
                  , tail_variate(args).begin() + n
                  , this->tail_means_
                  , numeric::plus
                );

                float_type factor = n * ( (is_same<Impl, relative>::value) ? non_coherent_tail_mean(args) : 1. );

                std::transform(
                    this->tail_means_.begin()
                  , this->tail_means_.end()
                  , this->tail_means_.begin()
#ifdef BOOST_NO_CXX98_BINDERS
                  , std::bind(std::divides<float_type>(), std::placeholders::_1, factor)
#else
                  , std::bind2nd(std::divides<float_type>(), factor)
#endif
                );
            }
            else
            {
                if (std::numeric_limits<float_type>::has_quiet_NaN)
                {
                    std::fill(
                        this->tail_means_.begin()
                      , this->tail_means_.end()
                      , std::numeric_limits<float_type>::quiet_NaN()
                    );
                }
                else
                {
                    std::ostringstream msg;
                    msg << "index n = " << n << " is not in valid range [0, " << tail(args).size() << ")";
                    boost::throw_exception(std::runtime_error(msg.str()));
                }
            }
            return make_iterator_range(this->tail_means_);
        }

        // make this accumulator serializeable
        template<class Archive>
        void serialize(Archive & ar, const unsigned int file_version)
        {
            ar & tail_means_;
        }

    private:

        mutable array_type tail_means_;

    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::absolute_tail_variate_means
// tag::relative_tail_variate_means
//
namespace tag
{
    template<typename LeftRight, typename VariateType, typename VariateTag>
    struct absolute_tail_variate_means
      : depends_on<count, non_coherent_tail_mean<LeftRight>, tail_variate<VariateType, VariateTag, LeftRight> >
    {
        typedef accumulators::impl::tail_variate_means_impl<mpl::_1, absolute, LeftRight, VariateTag> impl;
    };
    template<typename LeftRight, typename VariateType, typename VariateTag>
    struct relative_tail_variate_means
      : depends_on<count, non_coherent_tail_mean<LeftRight>, tail_variate<VariateType, VariateTag, LeftRight> >
    {
        typedef accumulators::impl::tail_variate_means_impl<mpl::_1, relative, LeftRight, VariateTag> impl;
    };
    struct abstract_absolute_tail_variate_means
      : depends_on<>
    {
    };
    struct abstract_relative_tail_variate_means
      : depends_on<>
    {
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::tail_variate_means
// extract::relative_tail_variate_means
//
namespace extract
{
    extractor<tag::abstract_absolute_tail_variate_means> const tail_variate_means = {};
    extractor<tag::abstract_relative_tail_variate_means> const relative_tail_variate_means = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(tail_variate_means)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(relative_tail_variate_means)
}

using extract::tail_variate_means;
using extract::relative_tail_variate_means;

// tail_variate_means<LeftRight, VariateType, VariateTag>(absolute) -> absolute_tail_variate_means<LeftRight, VariateType, VariateTag>
template<typename LeftRight, typename VariateType, typename VariateTag>
struct as_feature<tag::tail_variate_means<LeftRight, VariateType, VariateTag>(absolute)>
{
    typedef tag::absolute_tail_variate_means<LeftRight, VariateType, VariateTag> type;
};

// tail_variate_means<LeftRight, VariateType, VariateTag>(relative) ->relative_tail_variate_means<LeftRight, VariateType, VariateTag>
template<typename LeftRight, typename VariateType, typename VariateTag>
struct as_feature<tag::tail_variate_means<LeftRight, VariateType, VariateTag>(relative)>
{
    typedef tag::relative_tail_variate_means<LeftRight, VariateType, VariateTag> type;
};

// Provides non-templatized extractor
template<typename LeftRight, typename VariateType, typename VariateTag>
struct feature_of<tag::absolute_tail_variate_means<LeftRight, VariateType, VariateTag> >
  : feature_of<tag::abstract_absolute_tail_variate_means>
{
};

// Provides non-templatized extractor
template<typename LeftRight, typename VariateType, typename VariateTag>
struct feature_of<tag::relative_tail_variate_means<LeftRight, VariateType, VariateTag> >
  : feature_of<tag::abstract_relative_tail_variate_means>
{
};

// So that absolute_tail_means can be automatically substituted
// with absolute_weighted_tail_means when the weight parameter is non-void.
template<typename LeftRight, typename VariateType, typename VariateTag>
struct as_weighted_feature<tag::absolute_tail_variate_means<LeftRight, VariateType, VariateTag> >
{
    typedef tag::absolute_weighted_tail_variate_means<LeftRight, VariateType, VariateTag> type;
};

template<typename LeftRight, typename VariateType, typename VariateTag>
struct feature_of<tag::absolute_weighted_tail_variate_means<LeftRight, VariateType, VariateTag> >
  : feature_of<tag::absolute_tail_variate_means<LeftRight, VariateType, VariateTag> >
{
};

// So that relative_tail_means can be automatically substituted
// with relative_weighted_tail_means when the weight parameter is non-void.
template<typename LeftRight, typename VariateType, typename VariateTag>
struct as_weighted_feature<tag::relative_tail_variate_means<LeftRight, VariateType, VariateTag> >
{
    typedef tag::relative_weighted_tail_variate_means<LeftRight, VariateType, VariateTag> type;
};

template<typename LeftRight, typename VariateType, typename VariateTag>
struct feature_of<tag::relative_weighted_tail_variate_means<LeftRight, VariateType, VariateTag> >
  : feature_of<tag::relative_tail_variate_means<LeftRight, VariateType, VariateTag> >
{
};

}} // namespace boost::accumulators

#ifdef _MSC_VER
# pragma warning(pop)
#endif

#endif


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( shift_left_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::fundamental_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator()(A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
      using itype = bd::as_integer_t<A0, signed>;
      return bitwise_cast<A0>(shift_left(bitwise_cast<itype>(a0),bd::as_integer_t<A0>(a1)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( shift_left_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::integer_<A0> >
                          , bd::scalar_< bd::fundamental_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator()(A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG( assert_good_shift<A0>(a1)
                      , "shift_left: a shift is out of range"
                      );

      return a0 << bd::as_integer_t<A0>(a1);
    }
  };
} } }

#endif


/*******************************************************************************

  Copyright (c) 2017, Honda Research Institute Europe GmbH

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are
  met:

  1. Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.

  2. Redistributions in binary form must reproduce the above copyright
   notice, this list of conditions and the following disclaimer in the
   documentation and/or other materials provided with the distribution.

  3. Neither the name of the copyright holder nor the names of its
   contributors may be used to endorse or promote products derived from
   this software without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
  IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
  THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*******************************************************************************/

/******************************************************************************

  http://eigen.tuxfamily.org/dox/TopicPreprocessorDirectives.html:

  EIGEN_NO_MALLOC - if defined, any request from inside the Eigen to allocate
                    memory from the heap results in an assertion failure. This
                    is useful to check that some routine does not allocate
                    memory dynamically. Not defined by default.

  EIGEN_RUNTIME_NO_MALLOC - if defined, a new switch is introduced which can be
                            turned on and off by calling set_is_malloc_allowed
                            (bool). If malloc is not allowed and Eigen tries to
                            allocate memory dynamically anyway, an assertion
                            failure results. Not defined by default.

  Mapping C-Arrays:
   http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html#
          TutorialMapPlacementNew

  Malloc checking:
   Eigen::internal::set_is_malloc_allowed(true, false) will create a fatal
   error if set to true, and disable checking for false. However, this is
   not thread-safe, and therefore should only be used for testing.

******************************************************************************/

#include <Rcs_eigen.h>
#include <Rcs_macros.h>

#define EIGEN_RUNTIME_NO_MALLOC

#include <Eigen/Core>
#include <Eigen/Dense>

#include <Eigen/QR>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <Eigen/LU>

#include <iostream>
#include <cstdio>

using namespace Eigen;

#if defined (MATND_MAX_STACK_VECTOR_SIZE)
#undef MATND_MAX_STACK_VECTOR_SIZE
#endif

#if defined (MATND_MAX_STACK_MATRIX_SIZE)
#undef MATND_MAX_STACK_MATRIX_SIZE
#endif

#define MATND_MAX_STACK_VECTOR_SIZE (25)
#define MATND_MAX_STACK_MATRIX_SIZE (25*25)

// These types have an unknown size at compile time, and therefore are
// allocated on the heap. The storage order of the matrices is set to row
// major, so that the Eigen's Map class can be used to cast them over a MatNd
// type without any additional memory allocation.
typedef Matrix<double, Dynamic, Dynamic, RowMajor> HeapMat;
typedef Matrix<double, Dynamic, 1, ColMajor> HeapVec;

// These types have a known size at compile time, and therefore are allocated
// on the stack. The storage order of the matrices is set to row major, so that
// the Eigen's Map class can be used to cast them over a MatNd type without any
// additional memory allocation.
typedef Matrix<double, Dynamic, Dynamic, RowMajor,
        MATND_MAX_STACK_VECTOR_SIZE,
        MATND_MAX_STACK_VECTOR_SIZE> StackMat;
typedef Matrix<double, Dynamic, 1, ColMajor,
        MATND_MAX_STACK_VECTOR_SIZE, 1> StackVec;

template <typename T>
static inline void MatNd_fromEigen3(MatNd* dst, const T& src,
                                    bool reshape=false)
{
  if (reshape==false)
  {
    RCHECK_EQ((int)dst->m, src.rows());
    RCHECK_EQ((int)dst->n, src.cols());
  }
  else
  {
    MatNd_reshape(dst, src.rows(), src.cols());
  }

  for (size_t m=0; m<dst->m; m++)
  {
    for (size_t n=0; n<dst->n; n++)
    {
      MatNd_set2(dst, m, n, src(m,n));
    }
  }
}

template <typename T>
static inline double CholDet(const T& L)
{
  double det = 1.0;

  for (int i=0; i<L.rows(); i++)
  {
    det *= L(i, i)*L(i, i);
  }

  return det;
}

template <typename MatT>
static inline double CholDC(MatNd* L_, const MatNd* A_)
{
  Map<MatT> A(A_->ele, A_->m, A_->n);
  LLT<MatT> llt;
  llt.compute(A);

  if (llt.info() != Eigen::Success)
  {
    RLOG(1, "Error on LLT!");
    MatNd_setZero(L_);
    return 0.0;
  }

  MatT L(llt.matrixL());
  double det = CholDet<MatT>(L);
  MatNd_fromEigen3<MatT>(L_, L);

  return det;
}

template <typename MatT>
static inline double CholSolve(MatNd* x_, const MatNd* A_, const MatNd* b_)
{
  Map<const MatT> A(A_->ele, A_->m, A_->n);
  LLT<MatT> llt;
  llt.compute(A);

  if (llt.info() != Eigen::Success)
  {
    RLOG(1, "Error on LLT!");
    return 0.0;
  }

  Map<MatT> x(x_->ele, x_->m, x_->n);
  Map<const MatT> b(b_->ele, b_->m, b_->n);
  x = llt.solve(b);

  // Compute determinant
  MatT L(llt.matrixL());

  return CholDet<MatT>(L);
}

template <typename MatT>
static inline double CholInv(MatNd* A_inv_, const MatNd* A_)
{
  size_t n = A_->m;
  Map<const MatT> A(A_->ele, n, n);

  LLT<MatT> llt;
  llt.compute(A);

  if (llt.info() != Eigen::Success)
  {
    RLOG(1, "Error on LLT!");
    MatNd_setZero(A_inv_);
    return 0.0;
  }

  Map<MatT> invA(A_inv_->ele, n, n);
  invA = llt.solve(MatT::Identity(n,n));

  MatT L(llt.matrixL());

  return CholDet<MatT>(L);
}

template <typename MatT>
static inline int SvdDC(MatNd* U, MatNd* S, MatNd* V, const MatNd* J,
                        double eps)
{
  Map<MatT> mapJ(J->ele, J->m, J->n);

  // We need to compute the full SVD, since otherwise there will dynamic
  // memory allocation for the StackMat types due to the QR preconditioning
  // (on Linux)
  JacobiSVD<MatT> svd(mapJ, ComputeFullU | ComputeFullV);

  // Cut off singular values smaller than eps
  int rank = svd.nonzeroSingularValues();

  for (int i=0; i<svd.nonzeroSingularValues(); ++i)
  {
    if (svd.singularValues()(i) < eps)
    {
      rank = i;
      break;
    }
  }

  if (S != NULL)
  {
    MatNd_fromEigen3<MatT>(S, svd.singularValues(), true);
    S->m = rank;
  }

  if (U != NULL)
  {
    MatT svdU = svd.matrixU();
    MatNd_reshape(U, svdU.rows(), rank);

    for (unsigned int m=0; m<U->m; m++)
    {
      for (unsigned int n=0; n<U->n; n++)
      {
        MatNd_set2(U, m, n, svdU(m,n));
      }
    }
  }

  if (V != NULL)
  {
    MatT svdV = svd.matrixV();
    MatNd_reshape(V, svdV.rows(), rank);

    for (unsigned int m=0; m<V->m; m++)
    {
      for (unsigned int n=0; n<V->n; n++)
      {
        MatNd_set2(V, m, n, svdV(m,n));
      }
    }
  }

  return rank;
}

template <typename MatT>
static inline double SvdSolve(MatNd* x_, const MatNd* A_, const MatNd* b_)
{
  Map<MatT> x(x_->ele, x_->m, x_->n);
  Map<MatT> A(A_->ele, A_->m, A_->n);
  Map<MatT> b(b_->ele, b_->m, b_->n);

  // We need to compute the full SVD, since otherwise there will dynamic
  // memory allocation for the StackMat types due to the QR preconditioning
  // (on Linux)
  JacobiSVD<MatT> svd(A, ComputeFullU | ComputeFullV);
  x = svd.solve(b);

  // Compute determinant
  double det = 1.0;
  MatT S = svd.singularValues();

  // Cut off singular values smaller than eps
  for (int i=0; i<S.rows(); ++i)
  {
    det *= S(i);
  }

  return det;
}

template <typename MatT>
static inline size_t SvdInverse(MatNd* A_, double eps=1.0e-12)
{
  Map<MatT> A(A_->ele, A_->m, A_->n);

  // We need to compute the full SVD, since otherwise there will dynamic
  // memory allocation for the StackMat types due to the QR preconditioning
  // (on Linux)
  JacobiSVD<MatT> svd;
  //  svd.setThreshold(eps);
  svd.compute(A, ComputeThinU | ComputeThinV);

  MatT invS = svd.singularValues();

  // Cut off singular values smaller than eps
  size_t rank = 0;
  for (int i=0; i<invS.rows(); ++i)
  {
    if (invS(i) > eps)
    {
      invS(i) = 1.0/invS(i);
      rank++;
    }
  }

  // inv = V diag(S^-1) U^T
  A.noalias() = svd.matrixV()*invS.asDiagonal()*svd.matrixU().transpose();

  return rank;
}

template <typename MatT>
static inline void QRDC(MatNd* Q_, MatNd* R_, const MatNd* A_)
{
  Map<const MatT> A(A_->ele, A_->m, A_->n);
  Map<MatT> Q(Q_->ele, Q_->m, Q_->n);
  Map<MatT> R(R_->ele, R_->m, R_->n);

  HouseholderQR<MatT> qr(A);

  Q = qr.householderQ();
  R = qr.matrixQR().template triangularView<Eigen::Upper>();
}

template <typename MatT>
static inline bool EigenVectors(MatNd* V_, double* d_, const MatNd* A_)
{
  Map<const MatT> A(A_->ele, A_->m, A_->n);

  Eigen::SelfAdjointEigenSolver<MatT> eigensolver(A);

  if (eigensolver.info() != Eigen::Success)
  {
    return false;
  }

  Map<MatT> V(V_->ele, V_->m, V_->n);
  Map<MatT> d(d_, A_->m, 1);
  V = eigensolver.eigenvectors();
  d = eigensolver.eigenvalues();

  return true;
}

template <typename MatT>
static inline void ColPivHhQR(MatNd* X_, const MatNd* A_, const MatNd* B_)
{
  Map<const MatT> A(A_->ele, A_->m, A_->n);
  Map<const MatT> B(B_->ele, B_->m, B_->n);
  Map<MatT> X(X_->ele, X_->m, X_->n);

  // A X = B
  X = A.colPivHouseholderQr().solve(B);
}

#ifdef __cplusplus
extern "C" {
#endif

double MatNd_choleskyDecomposition_E3(MatNd* L, const MatNd* A)
{
  RCHECK_EQ(A->m, A->n);
  RCHECK_EQ(L->m, L->n);
  RCHECK_EQ(A->m, L->m);

  if (A->m > MATND_MAX_STACK_VECTOR_SIZE)
  {
    return CholDC<HeapMat>(L, A);
  }
  else
  {
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(false);
#endif
    double det = CholDC<StackMat>(L, A);
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(true);
#endif
    return det;
  }
}

double MatNd_choleskySolve_E3(MatNd* x, const MatNd* A, const MatNd* b)
{
  RCHECK_EQ(A->m, A->n);
  RCHECK_EQ(x->m, A->m);
  RCHECK_EQ(b->m, x->m);
  RCHECK_EQ(b->n, x->n);

  if (A->m > MATND_MAX_STACK_VECTOR_SIZE)
  {
    return CholSolve<HeapMat>(x, A, b);
  }
  else
  {
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(false);
#endif
    double det = CholSolve<StackMat>(x, A, b);
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(true);
#endif
    return det;
  }
}

double MatNd_choleskyInverse_E3(MatNd* invA, const MatNd* A)
{
  RCHECK_EQ(A->m, A->n);
  RCHECK_EQ(invA->m, invA->n);
  RCHECK_EQ(A->m, invA->m);

  if (A->m > MATND_MAX_STACK_VECTOR_SIZE)
  {
    return CholInv<HeapMat>(invA,A);
  }
  else
  {
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(false);
#endif
    double det = CholInv<StackMat>(invA,A);
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(true);
#endif
    return det;
  }
}

void MatNd_mul_E3(MatNd* C_, const MatNd* A_, const MatNd* B_)
{
  if ((A_->m > MATND_MAX_STACK_VECTOR_SIZE) ||
      (B_->n > MATND_MAX_STACK_VECTOR_SIZE))
  {
    Map<HeapMat> C(C_->ele, C_->m, C_->n);
    Map<HeapMat> A(A_->ele, A_->m, A_->n);
    Map<HeapMat> B(B_->ele, B_->m, B_->n);

    // The noalias function inhibits the creation of a temporary object which
    // would lead to dynamic memory alllocation.
    C.noalias() = A*B;
  }
  else
  {
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(false);
#endif

    Map<StackMat> C(C_->ele, C_->m, C_->n);
    Map<StackMat> A(A_->ele, A_->m, A_->n);
    Map<StackMat> B(B_->ele, B_->m, B_->n);

    C.noalias() = A*B;

#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(true);
#endif
  }
}

unsigned int MatNd_SVD(MatNd* U, MatNd* S, MatNd* V, const MatNd* J,
                       double eps)
{
  if ((J->m > MATND_MAX_STACK_VECTOR_SIZE) ||
      (J->n > MATND_MAX_STACK_VECTOR_SIZE))
  {
    return SvdDC<HeapMat>(U, S, V, J, eps);
  }
  else
  {
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(false);
#endif
    int rank = SvdDC<StackMat>(U, S, V, J, eps);
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(true);
#endif
    return rank;
  }
}

double MatNd_SVDSolve(MatNd* x, const MatNd* A, const MatNd* b)
{
  if ((A->m > MATND_MAX_STACK_VECTOR_SIZE) ||
      (A->n > MATND_MAX_STACK_VECTOR_SIZE))
  {
    return SvdSolve<HeapMat>(x, A, b);
  }
  else
  {
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(false);
#endif
    double det = SvdSolve<StackMat>(x, A, b);
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(true);
#endif
    return det;
  }
}

unsigned int MatNd_SVDInverse(MatNd* inv, const MatNd* src)
{
  MatNd_copy(inv, src);
  return MatNd_SVDInverseSelf(inv);
}

unsigned int MatNd_SVDInverseSelf(MatNd* A)
{
  if ((A->m > MATND_MAX_STACK_VECTOR_SIZE) ||
      (A->n > MATND_MAX_STACK_VECTOR_SIZE))
  {
    return SvdInverse<HeapMat>(A);
  }
  else
  {
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(false);
#endif
    int rank = SvdInverse<StackMat>(A);
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(true);
#endif
    return rank;
  }
}

unsigned int MatNd_rank(const MatNd* J, double eps)
{
  return MatNd_SVD(NULL, NULL, NULL, J, eps);
}

void MatNd_QRDecomposition(MatNd* Q, MatNd* R, const MatNd* A)
{
  if ((A->m > MATND_MAX_STACK_VECTOR_SIZE) ||
      (A->n > MATND_MAX_STACK_VECTOR_SIZE) ||
      (A->n > A->m))   // That's a bug in Eigen3
  {
    QRDC<HeapMat>(Q, R, A);
  }
  else
  {
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(false);
#endif
    QRDC<StackMat>(Q, R, A);
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(true);
#endif
  }
}

bool MatNd_getEigenVectors(MatNd* V, double* d, const MatNd* A)
{
  RCHECK_EQ(A->m, A->n);
  RCHECK_EQ(A->n, V->m);
  RCHECK_EQ(V->m, V->n);

  bool success;

  if (A->m > MATND_MAX_STACK_VECTOR_SIZE)
  {
    success = EigenVectors<HeapMat>(V, d, A);
  }
  else
  {
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(false);
#endif
    success = EigenVectors<StackMat>(V, d, A);
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(true);
#endif
  }

  return success;
}

void MatNd_HouseholderQR(MatNd* X, const MatNd* A, const MatNd* B)
{
  if ((A->m > MATND_MAX_STACK_VECTOR_SIZE) ||
      (A->n > MATND_MAX_STACK_VECTOR_SIZE))
  {
    return ColPivHhQR<HeapMat>(X, A, B);
  }
  else
  {
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(false);
#endif
    return ColPivHhQR<StackMat>(X, A, B);
#ifdef EIGEN_RUNTIME_NO_MALLOC
    Eigen::internal::set_is_malloc_allowed(true);
#endif
  }
}

#ifdef __cplusplus
}
#endif


//==================================================================================================
/*!

  Copyright 2015 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/constant/log10_ehi.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check log10_ehi behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log10_ehi;
  using boost::simd::Log10_ehi;
  T ref = T(0);
  STF_TYPE_IS(decltype(Log10_ehi<T>()), T);
  STF_EQUAL(Log10_ehi<T>(), ref);
  STF_EQUAL(log10_ehi( as(T{}) ),ref);
}

STF_CASE_TPL( "Check log10_ehi behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log10_ehi;
  using boost::simd::Log10_ehi;
  T ref = T(4.3429448190325190e-1);

  STF_TYPE_IS(decltype(Log10_ehi<T>()), T);
  STF_IEEE_EQUAL(Log10_ehi<T>(), ref);
  STF_IEEE_EQUAL(log10_ehi( as(T{}) ), ref);
}

STF_CASE_TPL( "Check log10_ehi behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::log10_ehi;
  using boost::simd::Log10_ehi;
  T ref = T(4.3359375000000000e-1);
  STF_TYPE_IS(decltype(Log10_ehi<T>()), T);
  STF_IEEE_EQUAL(Log10_ehi<T>(), ref);
  STF_IEEE_EQUAL(log10_ehi( as(T{}) ), ref);
}