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

/// @file
/// Implements full BTE calculations defined in nanos.hpp.

#include <iostream>
#include <map>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <unordered_map>
#include <Eigen/IterativeLinearSolvers>
#include <unsupported/Eigen/src/IterativeSolvers/Scaling.h>
#include <unsupported/Eigen/src/IterativeSolvers/GMRES.h>

#include <utilities.hpp>
#include <nanos.hpp>
#include <vasp_io.hpp>

/// TBB stuff
#include <mutex>
#include <tbb/parallel_for.h>
#include <tbb/concurrent_vector.h>
#include <tbb/concurrent_unordered_map.h>

namespace alma {
namespace nanos {

double scale_tau_nanowire(double tau0,
                          const Eigen::Ref<const Eigen::Vector3d>& uaxis,
                          const Eigen::Ref<const Eigen::Vector3d>& vel,
                          double R) {
    /// Calculate the velocity vector projected onto a
    /// plane with normal vector equal to uaxis
    Eigen::Vector3d vrho_xyz = vel - vel.dot(uaxis) * uaxis;

    double vrho = vrho_xyz.norm();
    double mfp = vrho * tau0;

    if (alma::almost_equal(mfp, 0.)) {
        return tau0;
    }

    return tau0 *
           (1 - 2 / (R * R) * mfp * (mfp * (std::exp(-R / mfp) - 1) + R));
}

double scale_tau_nanoribbon(double tau0,
                            const Eigen::Ref<const Eigen::Vector3d>& uaxis,
                            const Eigen::Ref<const Eigen::Vector3d>& vel,
                            double L) {
    /// It is supposed that the z axis is the one discarded

    /// Change-of-base matrices to xy coordinates with
    /// y-axis equal to uaxis
    static Eigen::Matrix2d B = Eigen::Matrix2d::Zero();
    static Eigen::Matrix2d Binv;

    /// If not inited fill B and its inverse
    if (alma::almost_equal(B.sum(), 0.)) {
        if (!alma::almost_equal(uaxis(2), 0.)) {
            std::cout << "ERROR: Nanoribbons are defined in xy plane"
                      << std::endl;
            exit(1);
        }

        B << uaxis(1), uaxis(0), -uaxis(0), uaxis(1);
        Binv = B.inverse();
    }

    /// Calculate velocity in new coordinates
    Eigen::Vector2d v_cart, v_newcord;
    v_cart << vel(0), vel(1);
    v_newcord = Binv * v_cart;

    double mfp = std::abs(tau0 * v_newcord(0));

    if (alma::almost_equal(mfp, 0.)) {
        return tau0;
    }

    return tau0 * (mfp / L * (std::exp(-L / mfp) - 1.0) + 1.0);
}

double calc_kappa_RTA(const alma::Crystal_structure& poscar,
                      const alma::Gamma_grid& grid,
                      const Eigen::Ref<const Eigen::ArrayXXd>& w,
                      const Eigen::Ref<const Eigen::Vector3d>& uaxis,
                      const std::string& system_name,
                      const double limiting_length,
                      double T) {
    auto Nbranches =
        static_cast<std::size_t>(grid.get_spectrum_at_q(0).omega.size());

    if ((static_cast<std::size_t>(w.rows()) != Nbranches) ||
        (static_cast<std::size_t>(w.cols()) != grid.nqpoints))
        throw alma::value_error("inconsistent dimensions");

    double kappa = 0.;

    // The Gamma point is ignored.
    for (decltype(Nbranches) iq = 1; iq < grid.nqpoints; iq++) {
        auto sp = grid.get_spectrum_at_q(iq);

        for (decltype(Nbranches) im = 0; im < Nbranches; im++) {
            double tau = (w(im, iq) == 0.) ? 0. : (1. / w(im, iq));

            Eigen::Vector3d vg = sp.vg.col(im);
            double vgproj = vg.dot(uaxis);

            /// Scale the lifetimes
            if (system_name == "nanowire") {
                tau = scale_tau_nanowire(tau, uaxis, vg, limiting_length);
            }
            else if (system_name == "nanoribbon") {
                tau = scale_tau_nanoribbon(tau, uaxis, vg, limiting_length);
            }
            else {
                std::cout << "ERROR: calc_k not recognized system_name"
                          << std::endl;
                exit(1);
            }

            kappa += alma::bose_einstein_kernel(sp.omega[im], T) * tau *
                     vgproj * vgproj;
        }
    }
    return (1e21 * alma::constants::kB / poscar.V / grid.nqpoints) * kappa;
}

void get_fullBZ_processes(
    const alma::Gamma_grid& grid,
    const alma::Crystal_structure& cell,
    std::string& anhIFCfile,
    std::unordered_map<std::array<std::size_t, 3>, alma::Threeph_process>&
        emission_processes,
    std::unordered_map<std::array<std::size_t, 3>, alma::Threeph_process>&
        absorption_processes,
    std::unordered_map<std::pair<std::size_t, std::size_t>, double>&
        isotopic_processes,
    boost::mpi::communicator& world,
    double scalebroad_three) {
    /// Get anharmonic IFC
    auto anhIFC = alma::load_FORCE_CONSTANTS_3RD(anhIFCfile.c_str(), cell);

    /// First we are getting sizes
    std::size_t nbands = grid.get_spectrum_at_q(0).omega.size();

    std::size_t my_id = world.rank();
    std::size_t nprocs = world.size();

    /// Create a unique set of Three_phonon processes from irreductible triplets
    /// including the symmetry:
    auto limits = alma::my_jobs(grid.get_nequivalences(), nprocs, my_id);
    std::array<int, 2> signs(
        {{static_cast<int>(alma::threeph_type::emission),
          static_cast<int>(alma::threeph_type::absorption)}});

    std::vector<alma::Threeph_process> myprocesses;

    tbb::concurrent_unordered_map<std::array<std::size_t, 3>,
                                  alma::Threeph_process,
                                  std::hash<std::array<std::size_t, 3>>>
        emission_processes_tbb;
    tbb::concurrent_unordered_map<std::array<std::size_t, 3>,
                                  alma::Threeph_process,
                                  std::hash<std::array<std::size_t, 3>>>
        absorption_processes_tbb;

    /// Calculating the three phonon processes for full BZ. The anhIFC are
    /// needed in case that due to symmetrization we are registering some
    /// transition that was not available in the already calculated elements. We
    /// are calculating them under the assumption that the matrix element is
    /// invariant to rotations, inversions and reciprocity but the smearing is
    /// not.
    for (auto ic = limits[0]; ic < limits[1]; ++ic) {
        if (my_id == 0)
            std::cout << "#Recalculating fullBZ triplets "
                      << static_cast<std::size_t>(ic - limits[0]) << " / "
                      << static_cast<std::size_t>(limits[1] - limits[0])
                      << std::endl;
        auto iq1 = grid.get_representative(ic);
        auto coords1 = grid.one_to_three(iq1);
        auto spectrum1 = grid.get_spectrum_at_q(iq1);

        tbb::parallel_for(
            tbb::blocked_range<std::size_t>(0, grid.nqpoints),
            [&](tbb::blocked_range<std::size_t> iq2range) {
                for (std::size_t iq2 = iq2range.begin(); iq2 < iq2range.end();
                     ++iq2) {
                    auto coords2 = grid.one_to_three(iq2);
                    auto spectrum2 = grid.get_spectrum_at_q(iq2);
                    decltype(coords2) coords3;

                    // Emission and absorption processes satisfy different
                    // conservation rules, with the second phonon in
                    // different sides of the equations.
                    for (auto s : signs) {
                        for (auto i = 0; i < 3; ++i)
                            coords3[i] = coords1[i] + s * coords2[i];
                        auto iq3 = grid.three_to_one(coords3);
                        auto spectrum3 = grid.get_spectrum_at_q(iq3);

                        auto eqqtrip =
                            grid.equivalent_qtriplets({iq1, iq2, iq3});

                        for (decltype(nbands) im1 = 0; im1 < nbands; ++im1) {
                            if (alma::almost_equal(spectrum1.omega(im1), 0.))
                                continue;

                            for (decltype(nbands) im2 = 0; im2 < nbands;
                                 ++im2) {
                                if (alma::almost_equal(spectrum2.omega(im2),
                                                       0.))
                                    continue;

                                for (decltype(nbands) im3 = 0; im3 < nbands;
                                     ++im3) {
                                    if (alma::almost_equal(spectrum3.omega(im3),
                                                           0.))
                                        continue;

                                    auto delta =
                                        std::fabs(spectrum1.omega(im1) +
                                                  s * spectrum2.omega(im2) -
                                                  spectrum3.omega(im3));

                                    std::vector<double> vpsXdelta; //, sigmas;
                                    std::vector<alma::Threeph_process> mythree;

                                    for (auto& qt : eqqtrip) {
                                        auto v1 = grid.get_spectrum_at_q(qt[0])
                                                      .vg.col(im1);
                                        auto v2 = grid.get_spectrum_at_q(qt[1])
                                                      .vg.col(im2);
                                        auto v3 = grid.get_spectrum_at_q(qt[2])
                                                      .vg.col(im3);

                                        if (static_cast<alma::threeph_type>(
                                                s) ==
                                            alma::threeph_type::absorption) {
                                            auto sigma =
                                                scalebroad_three *
                                                std::sqrt(
                                                    boost::math::pow<2>(
                                                        grid.base_sigma(v1)) +
                                                    boost::math::pow<2>(
                                                        grid.base_sigma(v2)) +
                                                    boost::math::pow<2>(
                                                        grid.base_sigma(v3)));
                                            //                                                 0.1 * grid.base_sigma(v2 - v3);
                                            alma::Threeph_process
                                                Gamma_of_triplet1(
                                                    ic,
                                                    std::array<std::size_t, 3>(
                                                        {{qt[0],
                                                          qt[1],
                                                          qt[2]}}),
                                                    std::array<std::size_t, 3>(
                                                        {{im1, im2, im3}}),
                                                    static_cast<
                                                        alma::threeph_type>(s),
                                                    delta,
                                                    sigma);
                                            mythree.push_back(
                                                Gamma_of_triplet1);
                                            if (delta <=
                                                alma::constants::nsigma *
                                                    sigma) {
                                                //                                         Gamma_of_triplet1.compute_vp2(cell,grid,anhIFC);
                                                vpsXdelta.push_back(
                                                    Gamma_of_triplet1
                                                        .compute_gaussian());
                                            }
                                            //                                             sigma =
                                            //                                                 0.1 * grid.base_sigma(v1 - v3);
                                            alma::Threeph_process
                                                Gamma_of_triplet2(
                                                    ic,
                                                    std::array<std::size_t, 3>(
                                                        {{qt[1],
                                                          qt[0],
                                                          qt[2]}}),
                                                    std::array<std::size_t, 3>(
                                                        {{im2, im1, im3}}),
                                                    static_cast<
                                                        alma::threeph_type>(s),
                                                    delta,
                                                    sigma);
                                            mythree.push_back(
                                                Gamma_of_triplet2);
                                            if (delta <=
                                                alma::constants::nsigma *
                                                    sigma) {
                                                //                                         Gamma_of_triplet2.compute_vp2(cell,grid,anhIFC);
                                                vpsXdelta.push_back(
                                                    Gamma_of_triplet2
                                                        .compute_gaussian());
                                            }
                                            //                                             sigma =
                                            //                                                 0.1 * grid.base_sigma(v1 - v2);
                                            alma::Threeph_process
                                                Gamma_of_triplet3(
                                                    ic,
                                                    std::array<std::size_t, 3>(
                                                        {{qt[2],
                                                          qt[0],
                                                          qt[1]}}),
                                                    std::array<std::size_t, 3>(
                                                        {{im3, im1, im2}}),
                                                    static_cast<
                                                        alma::threeph_type>(-s),
                                                    delta,
                                                    sigma);
                                            mythree.push_back(
                                                Gamma_of_triplet3);
                                            if (delta <=
                                                alma::constants::nsigma *
                                                    sigma) {
                                                //                                         Gamma_of_triplet3.compute_vp2(cell,grid,anhIFC);
                                                vpsXdelta.push_back(
                                                    Gamma_of_triplet3
                                                        .compute_gaussian());
                                            }
                                            /*                                            sigma
                                               = 0.1 * grid.base_sigma(v2 -
                                               v1)*/
                                            ;
                                            alma::Threeph_process
                                                Gamma_of_triplet4(
                                                    ic,
                                                    std::array<std::size_t, 3>(
                                                        {{qt[2],
                                                          qt[1],
                                                          qt[0]}}),
                                                    std::array<std::size_t, 3>(
                                                        {{im3, im2, im1}}),
                                                    static_cast<
                                                        alma::threeph_type>(-s),
                                                    delta,
                                                    sigma);
                                            mythree.push_back(
                                                Gamma_of_triplet4);
                                            if (delta <=
                                                alma::constants::nsigma *
                                                    sigma) {
                                                //                                         Gamma_of_triplet4.compute_vp2(cell,grid,anhIFC);
                                                vpsXdelta.push_back(
                                                    Gamma_of_triplet4
                                                        .compute_gaussian());
                                            }
                                        }
                                        else {
                                            auto sigma =
                                                1.0 *
                                                std::sqrt(
                                                    boost::math::pow<2>(
                                                        grid.base_sigma(v1)) +
                                                    boost::math::pow<2>(
                                                        grid.base_sigma(v2)) +
                                                    boost::math::pow<2>(
                                                        grid.base_sigma(v3)));
                                            //                                                 0.1 * grid.base_sigma(v2 - v3);
                                            alma::Threeph_process
                                                Gamma_of_triplet1(
                                                    ic,
                                                    std::array<std::size_t, 3>(
                                                        {{qt[0],
                                                          qt[1],
                                                          qt[2]}}),
                                                    std::array<std::size_t, 3>(
                                                        {{im1, im2, im3}}),
                                                    static_cast<
                                                        alma::threeph_type>(s),
                                                    delta,
                                                    sigma);
                                            mythree.push_back(
                                                Gamma_of_triplet1);
                                            if (delta <=
                                                alma::constants::nsigma *
                                                    sigma) {
                                                //                                         Gamma_of_triplet1.compute_vp2(cell,grid,anhIFC);
                                                vpsXdelta.push_back(
                                                    Gamma_of_triplet1
                                                        .compute_gaussian());
                                            }
                                            //                                             sigma =
                                            //                                                 0.1 * grid.base_sigma(v3 - v2);
                                            alma::Threeph_process
                                                Gamma_of_triplet2(
                                                    ic,
                                                    std::array<std::size_t, 3>(
                                                        {{qt[0],
                                                          qt[2],
                                                          qt[1]}}),
                                                    std::array<std::size_t, 3>(
                                                        {{im1, im3, im2}}),
                                                    static_cast<
                                                        alma::threeph_type>(s),
                                                    delta,
                                                    sigma);
                                            mythree.push_back(
                                                Gamma_of_triplet2);
                                            if (delta <=
                                                alma::constants::nsigma *
                                                    sigma) {
                                                //                                         Gamma_of_triplet2.compute_vp2(cell,grid,anhIFC);
                                                vpsXdelta.push_back(
                                                    Gamma_of_triplet2
                                                        .compute_gaussian());
                                            }
                                            //                                             sigma =
                                            //                                                 0.1 * grid.base_sigma(v2 - v1);
                                            alma::Threeph_process
                                                Gamma_of_triplet3(
                                                    ic,
                                                    std::array<std::size_t, 3>(
                                                        {{qt[2],
                                                          qt[1],
                                                          qt[0]}}),
                                                    std::array<std::size_t, 3>(
                                                        {{im3, im2, im1}}),
                                                    static_cast<
                                                        alma::threeph_type>(-s),
                                                    delta,
                                                    sigma);
                                            mythree.push_back(
                                                Gamma_of_triplet3);
                                            if (delta <=
                                                alma::constants::nsigma *
                                                    sigma) {
                                                //                                         Gamma_of_triplet3.compute_vp2(cell,grid,anhIFC);
                                                vpsXdelta.push_back(
                                                    Gamma_of_triplet3
                                                        .compute_gaussian());
                                            }
                                            //                                             sigma =
                                            //                                                 0.1 * grid.base_sigma(v3 - v1);
                                            alma::Threeph_process
                                                Gamma_of_triplet4(
                                                    ic,
                                                    std::array<std::size_t, 3>(
                                                        {{qt[1],
                                                          qt[2],
                                                          qt[0]}}),
                                                    std::array<std::size_t, 3>(
                                                        {{im2, im3, im1}}),
                                                    static_cast<
                                                        alma::threeph_type>(-s),
                                                    delta,
                                                    sigma);
                                            mythree.push_back(
                                                Gamma_of_triplet4);
                                            if (delta <=
                                                alma::constants::nsigma *
                                                    sigma) {
                                                // Gamma_of_triplet4.compute_vp2(cell,grid,anhIFC);
                                                vpsXdelta.push_back(
                                                    Gamma_of_triplet4
                                                        .compute_gaussian());
                                            }
                                        }
                                    }

                                    if (vpsXdelta.empty())
                                        continue;
                                    std::sort(mythree.begin(), mythree.end());
                                    double VP2 = mythree[0].compute_vp2(
                                        cell, grid, *anhIFC);

                                    double SymGamma = 0.;
                                    for (auto& vD : vpsXdelta) {
                                        SymGamma += VP2 * vD;
                                    }

                                    SymGamma /= 4 * eqqtrip.size();

                                    /// If not 0 all register in map, we account
                                    /// for a state decaying into two of same
                                    /// index
                                    if (!almost_equal(
                                            SymGamma, 0., 1.0e-12, 1.0e-9)) {
                                        for (auto& peq : mythree) {
                                            peq.set_vp2(SymGamma);
                                            std::array<std::size_t, 3>
                                                triplet_indexes;
                                            for (std::size_t idx_ = 0; idx_ < 3;
                                                 idx_++)
                                                triplet_indexes[idx_] =
                                                    peq.q[idx_] * nbands +
                                                    peq.alpha[idx_];
                                            if (peq.type == alma::threeph_type::
                                                                absorption) {
                                                if (triplet_indexes[0] ==
                                                    triplet_indexes[1]) {
                                                    double effective_vp2 =
                                                        2 * peq.get_vp2();
                                                    peq.set_vp2(effective_vp2);
                                                }

                                                if (absorption_processes_tbb
                                                        .count(
                                                            triplet_indexes) ==
                                                    0) {
                                                    absorption_processes_tbb
                                                        .emplace(std::make_pair(
                                                            triplet_indexes,
                                                            peq));
                                                }
                                            }
                                            else {
                                                // We account for those states
                                                // by multiplying by 2
                                                if (triplet_indexes[1] ==
                                                    triplet_indexes[2]) {
                                                    double effective_vp2 =
                                                        2 * peq.get_vp2();
                                                    peq.set_vp2(effective_vp2);
                                                }
                                                if (emission_processes_tbb
                                                        .count(
                                                            triplet_indexes) ==
                                                    0) {
                                                    emission_processes_tbb
                                                        .emplace(std::make_pair(
                                                            triplet_indexes,
                                                            peq));
                                                }
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            });
    }

    /// Passing tbb map to stl map
    absorption_processes.insert(absorption_processes_tbb.begin(),
                                absorption_processes_tbb.end());
    emission_processes.insert(emission_processes_tbb.begin(),
                              emission_processes_tbb.end());

    /// We are now working on isotopic/mass-disoreder scattering
    if (my_id == 0)
        std::cout << "#Working on 2ph processes" << std::endl;

    auto twoph_processes = alma::find_allowed_twoph(grid, world, 1.0);

    double scalebroad_iso = 1.0;

    /// We need sigma full list to symmetrize:
    Eigen::ArrayXXd broadening_sigmas(nbands, grid.nqpoints);
    for (std::size_t iq = 0; iq < grid.nqpoints; ++iq) {
        for (std::size_t im = 0; im < nbands; ++im) {
            auto spectrum = grid.get_spectrum_at_q(iq);
            broadening_sigmas(im, iq) =
                scalebroad_iso * grid.base_sigma(spectrum.vg.col(im));
        }
    }
    // And refine them by removing outliers.
    auto percent = calc_percentiles_log(broadening_sigmas);
    double lbound = std::exp(percent[0] - 1.5 * (percent[1] - percent[0]));
    broadening_sigmas =
        (broadening_sigmas < lbound).select(lbound, broadening_sigmas);

    for (auto& twoph : twoph_processes) {
        auto b0 = twoph.alpha[0];
        auto b1 = twoph.alpha[1];

        auto sp0 = grid.get_spectrum_at_q(twoph.q[0]);
        auto sp1 = grid.get_spectrum_at_q(twoph.q[1]);

        auto delta = std::abs(sp0.omega(b0) - sp1.omega(b1));

        /// If some of them is Gamma-point and acoustic, ignore
        if (twoph.q[0] * nbands + b0 < 3 or twoph.q[1] * nbands + b1 < 3)
            continue;

        /// The scattering to itselt has no effect in scattering operator
        if (twoph.q[0] * nbands + b0 == twoph.q[1] * nbands + b1)
            continue;

        auto qpairs = grid.equivalent_qpairs({twoph.q[0], twoph.q[1]});

        std::vector<alma::Twoph_process> mytwo;

        double gamma = 0.;

        for (auto& qi : qpairs) {
            auto sigma = std::hypot(broadening_sigmas(b0, qi[0]),
                                    broadening_sigmas(b1, qi[1]));
            alma::Twoph_process t1(0,
                                   std::array<std::size_t, 2>({{qi[0], qi[1]}}),
                                   std::array<std::size_t, 2>({{b0, b1}}),
                                   delta,
                                   sigma);
            alma::Twoph_process t2(0,
                                   std::array<std::size_t, 2>({{qi[1], qi[0]}}),
                                   std::array<std::size_t, 2>({{b1, b0}}),
                                   delta,
                                   sigma);
            mytwo.push_back(t1);
            mytwo.push_back(t2);
            if (delta <= alma::constants::nsigma * sigma) {
                gamma += t1.compute_gamma(cell, grid);
                gamma += t2.compute_gamma(cell, grid);
            }
        }
        gamma /= 2 * qpairs.size();
        /// If some equivalent process is not null
        if (!almost_equal(gamma, 0., 1.0e-12, 1.0e-9)) {
            for (auto& peq : mytwo) {
                std::pair<std::size_t, std::size_t> pair_mode_ids =
                    std::make_pair(peq.q[0] * nbands + peq.alpha[0],
                                   peq.q[1] * nbands + peq.alpha[1]);
                if (isotopic_processes.count(pair_mode_ids) == 0)
                    isotopic_processes.emplace(
                        std::make_pair(pair_mode_ids, gamma));
            }
        }
    }
}

double calc_kappa_nanos(
    const alma::Crystal_structure& poscar,
    const alma::Gamma_grid& grid,
    const alma::Symmetry_operations& syms,
    std::unordered_map<std::array<std::size_t, 3>, alma::Threeph_process>&
        emission_processes,
    std::unordered_map<std::array<std::size_t, 3>, alma::Threeph_process>&
        absorption_processes,
    std::unordered_map<std::pair<std::size_t, std::size_t>, double>&
        isotopic_processes,
    const Eigen::Ref<const Eigen::ArrayXXd>& w0,
    const Eigen::Ref<const Eigen::Vector3d>& uaxis,
    const std::string& system_name,
    const double limiting_length,
    double T,
    bool iterative,
    boost::mpi::communicator& world) {
    // GATHER INFORMATION FOR FULL BTE SYSTEM AND DECLARE SOME VARIABLES

    int Ngridpoints = grid.nqpoints;

    int Nbranches = grid.get_spectrum_at_q(0).omega.size();
    int Ntot = Ngridpoints * Nbranches - 3;

    // Unknowns in the linear system
    Eigen::VectorXd H(Ntot);

    // stores heat capacities of the irreducible points
    Eigen::VectorXd C(Ntot);
    C.setConstant(0.0);

    // stores relaxation times of the irreducible points
    Eigen::VectorXd tau(Ntot);
    tau.setConstant(0.0);

    // stores phonon frequencies of the irreducible points
    Eigen::VectorXd omega(Ntot);
    omega.setConstant(0.0);

    // stores group velocity vectors of the irreducible points
    Eigen::MatrixXd vg(Ntot, 3);
    Eigen::VectorXd vgproj(Ntot);
    vg.fill(0.0);

    // GATHER PHONON PROPERTIES FOR IRREDUCIBLE Q-POINTS

    double C_factor = 1e27 * alma::constants::kB / Ngridpoints / poscar.V;

    // scan over all points in the grid

    for (int nq = 0; nq < Ngridpoints; nq++) {
        // spectrum at point

        auto spectrum = grid.get_spectrum_at_q(nq);

        for (int nbranch = 0; nbranch < Nbranches; nbranch++) {
            int imode = nq * Nbranches + nbranch - 3;

            if (imode < 0)
                continue;

            // store heat capacity
            C(imode) = C_factor *
                       alma::bose_einstein_kernel(spectrum.omega[nbranch], T);

            // store group velocity vector and projected one
            Eigen::Vector3d myvg = spectrum.vg.col(nbranch);
            vg(imode, 0) = myvg(0);
            vg(imode, 1) = myvg(1);
            vg(imode, 2) = myvg(2);

            vgproj(imode) = myvg.dot(uaxis);

            // store relaxation time
            double mytau =
                (w0(nbranch, nq) == 0.) ? 0. : (1. / w0(nbranch, nq));

            if (system_name == "nanowire") {
                mytau = scale_tau_nanowire(mytau, uaxis, myvg, limiting_length);
            }
            else if (system_name == "nanoribbon") {
                mytau =
                    scale_tau_nanoribbon(mytau, uaxis, myvg, limiting_length);
            }
            else {
                std::cout
                    << "ERROR: calc_kappa_nanos not recognized system_name"
                    << std::endl;
                exit(1);
            }

            tau(imode) = mytau;

            // store phonon frequency
            omega(imode) = spectrum.omega(nbranch);
        }
    }

    typedef std::array<int, 2> idx_pair;

    std::unordered_map<idx_pair, double> A_elements;
    A_elements.reserve(std::ceil(0.15 * Ntot * Ntot));

    // CALCULATE NON-RTA CONDUCTIVITY BY SOLVING LINEAR SYSTEM

    // build the list of Gamma values for 2-phonon processes
    for (auto& process_block : isotopic_processes) {
        auto I = process_block.first.first;
        auto J = process_block.first.second;
        A_elements[{static_cast<int>(I - 3), static_cast<int>(J - 3)}] -=
            process_block.second * tau(I - 3);
    } // done scanning over all 2-phonon processes

    for (auto& process_block : absorption_processes) {
        auto process(process_block.second);
        auto I = process.q[0] * Nbranches + process.alpha[0];
        auto J = process.q[1] * Nbranches + process.alpha[1];
        auto K = process.q[2] * Nbranches + process.alpha[2];

        double Gamma = process.compute_gamma(grid, T, true);

        A_elements[{static_cast<int>(I - 3), static_cast<int>(J - 3)}] +=
            Gamma * tau(I - 3);
        A_elements[{static_cast<int>(I - 3), static_cast<int>(K - 3)}] -=
            Gamma * tau(I - 3);
    }

    for (auto& process_block : emission_processes) {
        auto process(process_block.second);
        auto I = process.q[0] * Nbranches + process.alpha[0];
        auto J = process.q[1] * Nbranches + process.alpha[1];
        auto K = process.q[2] * Nbranches + process.alpha[2];

        double Gamma = process.compute_gamma(grid, T, true);

        A_elements[{static_cast<int>(I - 3), static_cast<int>(J - 3)}] -=
            0.5 * Gamma * tau(I - 3);
        A_elements[{static_cast<int>(I - 3), static_cast<int>(K - 3)}] -=
            0.5 * Gamma * tau(I - 3);
    }

    // diagonal elements in the system
    for (int diag_idx = 0; diag_idx < Ntot; diag_idx++) {
        A_elements[{diag_idx, diag_idx}] += 1.0;
    }

    /// Get tripletList
    std::vector<Eigen::Triplet<double>> tripletList;
    for (auto& [key, val] : A_elements) {
        auto idx1 = key[0];
        auto idx2 = key[1];
        tripletList.push_back(Eigen::Triplet<double>(idx1, idx2, val));
    }

    // build system of equations "A*H = B"

    Eigen::SparseMatrix<double> A(Ntot, Ntot);

    // Fill sparse matrix
    A.setFromTriplets(tripletList.begin(), tripletList.end());

    Eigen::VectorXd B(Ntot);

    // projected-components of omega*MFP_RTA over transport axis
    B = omega.array() * tau.array() * vgproj.array();

    // Transform to compressed format

    A.makeCompressed();

    std::cout << "#Linear system information\n";
    std::cout << "**A sparsity "
              << static_cast<double>(A.nonZeros()) / (Ntot * Ntot) << std::endl;
    std::cout.flush();

    // Scale system
    Eigen::IterScaling<Eigen::SparseMatrix<double>> scal;
    scal.computeRef(A);
    B = scal.LeftScaling().cwiseProduct(B);

    // SOLVE SYSTEM

    if (iterative) {
        Eigen::BiCGSTAB<Eigen::SparseMatrix<double>> solver;
        // Eigen::GMRES<Eigen::SparseMatrix<double>> solver;
        solver.compute(A);

        // RTA solution to be used as initial guess
        Eigen::VectorXd H_guess(Ntot);

        H_guess = omega.array() * tau.array() * vgproj.array();

        H_guess = scal.RightScaling().cwiseInverse().cwiseProduct(H_guess);

        H = solver.solveWithGuess(B, H_guess);

        std::cout << "# Iterative solver information:\n" << std::endl;
        std::cout << "# -iterations:     " << solver.iterations() << std::endl;
        std::cout << "# -estimated error: " << solver.error() << std::endl;
    }

    else {
        H = omega.array() * tau.array() * vgproj.array();
    }

    double rel_err = (A * H - B).norm() / B.norm();

    if (rel_err > 1e-3) {
        std::cout << "alma::beyondRTA::calc_kappa_nanos > WARNING:"
                  << std::endl;
        std::cout << "solution of linear system might be unstable."
                  << std::endl;
        std::cout << "Relative error metric = " << rel_err << std::endl;
    }

    // Scale back the solution
    H = scal.RightScaling().cwiseProduct(H);

    // PROCESS THE LINEAR SYSTEM SOLUTION

    // construct "generalised MFPs"
    Eigen::VectorXd MFP_nonRTA(Ntot);
    MFP_nonRTA = H.array() / omega.array();

    // fix potential NaN/Inf problems
    for (int n = 0; n < Ntot; n++) {
        if (omega(n) <= 0.0) {
            MFP_nonRTA(n) = 0.0;
        }
    }

    // CALCULATE NON-RTA CONDUCTIVITY

    double kappa_nano = 0.0;
    for (int nq = 0; nq < Ngridpoints; nq++) {
        for (int nbranch = 0; nbranch < Nbranches; nbranch++) {
            int imode = nq * Nbranches + nbranch - 3;

            if (imode < 0)
                continue;

            kappa_nano += 1e-6 * C(imode) * MFP_nonRTA(imode) * vgproj(imode);
        }
    }

    // RETURN RESULT
    return kappa_nano;

} // end of calc_kappa_nanos
} // namespace nanos
} // end of namespace alma


/*
 * ****** Graph Algorithms using Boost Graph Library ********
 *
 * Variable elimination ordering heuristics developed by Cyril Terrioux <cyril.terrioux@lsis.org>
 */

#include "core/tb2wcsp.hpp"
#include "core/tb2binconstr.hpp"
#include "core/tb2knapsack.hpp"
#include "core/tb2naryconstr.hpp"

#ifdef BOOST
#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/minimum_degree_ordering.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/cuthill_mckee_ordering.hpp>

using namespace boost;

namespace boost {
struct edge_component_t {
    enum { num = 555 };
    typedef edge_property_tag kind;
} edge_component;
}

typedef adjacency_list<setS, vecS, undirectedS> Graph;
typedef adjacency_list<setS, vecS, directedS> DirectedGraph;
typedef adjacency_list<setS, vecS, undirectedS, no_property,
    property<edge_weight_t, int, property<edge_component_t, std::size_t>>>
    IntWeightedGraph;
typedef adjacency_list<setS, vecS, undirectedS, no_property,
    property<edge_weight_t, double, property<edge_component_t, std::size_t>>>
    DoubleWeightedGraph;
typedef adjacency_list<setS, vecS, undirectedS, property<vertex_color_t, default_color_type, property<vertex_degree_t, int>>> ColoredGraph;

template <typename T>
static void addConstraint(Constraint* c, T& g)
{
    int a = c->arity();
    for (int i = 0; i < a; i++) {
        for (int j = i + 1; j < a; j++) {
            Variable* vari = c->getVar(i);
            Variable* varj = c->getVar(j);
            add_edge(vari->wcspIndex, varj->wcspIndex, g);
        }
    }
}

static void addConstraint(Constraint* c, DirectedGraph& g)
{
    int a = c->arity();
    for (int i = 0; i < a; i++) {
        for (int j = i + 1; j < a; j++) {
            Variable* vari = c->getVar(i);
            Variable* varj = c->getVar(j);
            add_edge(vari->wcspIndex, varj->wcspIndex, g);
            add_edge(varj->wcspIndex, vari->wcspIndex, g);
        }
    }
}

static void addConstraint(Constraint* c, IntWeightedGraph& g, int weight = 1)
{
    property_map<IntWeightedGraph, edge_weight_t>::type weights = get(edge_weight, g);
    int a = c->arity();
    for (int i = 0; i < a; i++) {
        for (int j = i + 1; j < a; j++) {
            Variable* vari = c->getVar(i);
            Variable* varj = c->getVar(j);
            weights[add_edge(vari->wcspIndex, varj->wcspIndex, g).first] = weight;
        }
    }
}

static void addConstraint(Constraint* c, DoubleWeightedGraph& g, double maxweight = 1000000)
{
    property_map<DoubleWeightedGraph, edge_weight_t>::type weights = get(edge_weight, g);
    int a = c->arity();
    for (int i = 0; i < a; i++) {
        for (int j = i + 1; j < a; j++) {
            Variable* vari = c->getVar(i);
            Variable* varj = c->getVar(j);
            weights[add_edge(vari->wcspIndex, varj->wcspIndex, g).first] = maxweight - c->getTightness();
        }
    }
}

int WCSP::connectedComponents()
{
    Graph G;
    for (unsigned int i = 0; i < vars.size(); i++)
        add_vertex(G);
    for (unsigned int i = 0; i < constrs.size(); i++)
        if (constrs[i]->connected() && !constrs[i]->universal())
            addConstraint(constrs[i], G);
    for (int i = 0; i < elimBinOrder; i++)
        if (elimBinConstrs[i]->connected())
            addConstraint(elimBinConstrs[i], G);
    for (int i = 0; i < elimTernOrder; i++)
        if (elimTernConstrs[i]->connected())
            addConstraint(elimTernConstrs[i], G);
    vector<int> component(num_vertices(G));
    int num = connected_components(G, &component[0]);
    vector<int> cctruesize(num, 0);
    for (size_t i = 0; i < num_vertices(G); ++i) {
        assert(component[i] >= 0 && component[i] < num);
        if (unassigned(i))
            cctruesize[component[i]]++;
    }
    int res = 0;
    char c = '(';
    for (int i = 0; i < num; ++i) {
        if (cctruesize[i] >= 1) {
            res++;
            cout << c << cctruesize[i];
            c = ' ';
        }
    }
    cout << ")";

    return res;
}

int WCSP::biConnectedComponents()
{
    IntWeightedGraph G;
    for (unsigned int i = 0; i < vars.size(); i++)
        add_vertex(G);
    for (unsigned int i = 0; i < constrs.size(); i++)
        if (constrs[i]->connected())
            addConstraint(constrs[i], G);
    for (int i = 0; i < elimBinOrder; i++)
        if (elimBinConstrs[i]->connected())
            addConstraint(elimBinConstrs[i], G);
    for (int i = 0; i < elimTernOrder; i++)
        if (elimTernConstrs[i]->connected())
            addConstraint(elimTernConstrs[i], G);
    property_map<IntWeightedGraph, edge_component_t>::type component = get(edge_component, G);

    int num = biconnected_components(G, component);

    vector<int> art_points;
    articulation_points(G, back_inserter(art_points));
    cout << "Articulation points: " << art_points.size() << endl;
    if (art_points.size() > 0) {
        for (unsigned int i = 0; i < art_points.size(); i++)
            cout << " " << art_points[i];
        cout << endl;
    }
    return num;
}

int WCSP::diameter()
{
    if (vars.size() >= LARGE_NB_VARS)
        return -1;

    IntWeightedGraph G;
    for (unsigned int i = 0; i < vars.size(); i++)
        add_vertex(G);
    for (unsigned int i = 0; i < constrs.size(); i++)
        if (constrs[i]->connected())
            addConstraint(constrs[i], G);
    for (int i = 0; i < elimBinOrder; i++)
        if (elimBinConstrs[i]->connected())
            addConstraint(elimBinConstrs[i], G);
    for (int i = 0; i < elimTernOrder; i++)
        if (elimTernConstrs[i]->connected())
            addConstraint(elimTernConstrs[i], G);

    typedef int* int_ptr;
    int** D;
    D = new int_ptr[num_vertices(G)];
    for (unsigned int i = 0; i < num_vertices(G); ++i)
        D[i] = new int[num_vertices(G)];
    johnson_all_pairs_shortest_paths(G, D);

    if (ToulBar2::verbose >= 2) {
        cout << "     ";
        for (unsigned int i = 0; i < num_vertices(G); ++i) {
            cout << i << " -> ";
            for (unsigned int j = 0; j < num_vertices(G); ++j) {
                cout << " " << D[i][j];
            }
            cout << endl;
        }
    }

    int maxd = 0;
    double meand = 0;
    for (unsigned int i = 0; i < num_vertices(G); ++i) {
        for (unsigned int j = 0; j < num_vertices(G); ++j) {
            if (D[i][j] > maxd)
                maxd = D[i][j];
            meand += D[i][j];
        }
    }
    meand /= num_vertices(G) * num_vertices(G);
    if (ToulBar2::verbose >= 1) {
        cout << "Mean diameter: " << meand << endl;
    }

    for (unsigned int i = 0; i < num_vertices(G); ++i)
        delete[] D[i];
    delete[] D;

    return maxd;
}

inline bool cmp_vars(Variable* v1, Variable* v2) { return (v1->wcspIndex < v2->wcspIndex); }

/// \brief Minimum Degree Ordering algorithm
/// \warning Output order usually worse than WCSP::minimumDegreeOrdering ???
void WCSP::minimumDegreeOrderingBGL(vector<int>& order_inv)
{
    DirectedGraph G;
    for (unsigned int i = 0; i < vars.size(); i++)
        add_vertex(G);
    for (unsigned int i = 0; i < constrs.size(); i++)
        if (constrs[i]->connected())
            addConstraint(constrs[i], G);
    for (int i = 0; i < elimBinOrder; i++)
        if (elimBinConstrs[i]->connected())
            addConstraint(elimBinConstrs[i], G);
    for (int i = 0; i < elimTernOrder; i++)
        if (elimTernConstrs[i]->connected())
            addConstraint(elimTernConstrs[i], G);

    int n = num_vertices(G);
    int delta = 0;
    typedef vector<int> Vector;
    Vector inverse_perm(n, 0);
    Vector perm(n, 0);

    Vector supernode_sizes(n, 1); // init has to be 1

    property_map<DirectedGraph, vertex_index_t>::type id = get(vertex_index, G);

    Vector degree(n, 0);

    minimum_degree_ordering(G,
        make_iterator_property_map(&degree[0], id, degree[0]),
        &inverse_perm[0],
        &perm[0],
        make_iterator_property_map(&supernode_sizes[0], id, supernode_sizes[0]),
        delta,
        id);

    order_inv = inverse_perm;
    if (ToulBar2::verbose >= 1) {
        cout << "Minimum degree ordering:";
        for (size_t i = 0; i < num_vertices(G); ++i) {
            cout << " " << order_inv[i];
        }
        cout << endl;
    }

    // // \bug reordering of vars array is dubious!!! (invalidates further use of variable indexes)
    // for (size_t i=0; i < num_vertices(G); ++i) {
    //    vars[i]->wcspIndex = num_vertices(G) - perm[i] - 1;
    //  }
    //  stable_sort(vars.begin(), vars.end(), cmp_vars);
    //  for (size_t i=0; i < num_vertices(G); ++i) {
    //    assert(vars[i]->wcspIndex == (int) i);
    //  }

    assert(order_inv.size() == numberOfVariables());
}

void WCSP::spanningTreeOrderingBGL(vector<int>& order_inv)
{
    double alltight = 0;
    double maxt = 0;
    for (unsigned int i = 0; i < constrs.size(); i++)
        if (constrs[i]->connected()) {
            double t = constrs[i]->getTightness();
            alltight += t;
            if (t > maxt)
                maxt = t;
        }
    for (int i = 0; i < elimBinOrder; i++)
        if (elimBinConstrs[i]->connected()) {
            double t = elimBinConstrs[i]->getTightness();
            alltight += t;
            if (t > maxt)
                maxt = t;
        }
    for (int i = 0; i < elimTernOrder; i++)
        if (elimTernConstrs[i]->connected()) {
            double t = elimTernConstrs[i]->getTightness();
            alltight += t;
            if (t > maxt)
                maxt = t;
        }

    DoubleWeightedGraph G;
    for (unsigned int i = 0; i < vars.size(); i++)
        add_vertex(G);
    for (unsigned int i = 0; i < constrs.size(); i++)
        if (constrs[i]->connected())
            addConstraint(constrs[i], G, maxt);
    for (int i = 0; i < elimBinOrder; i++)
        if (elimBinConstrs[i]->connected())
            addConstraint(elimBinConstrs[i], G, maxt);
    for (int i = 0; i < elimTernOrder; i++)
        if (elimTernConstrs[i]->connected())
            addConstraint(elimTernConstrs[i], G, maxt);

    int n = num_vertices(G);

    vector<graph_traits<DoubleWeightedGraph>::vertex_descriptor> p(n);
    prim_minimum_spanning_tree(G, &p[0]);

    double tight = 0;
    bool tightok = true;
    vector<int> roots;
    vector<vector<int>> listofsuccessors(n, vector<int>());
    if (ToulBar2::verbose >= 0)
        cout << "Maximum spanning tree ordering"; // << endl;
    for (size_t i = 0; i != p.size(); ++i) {
        if (p[i] != i) {
            BinaryConstraint* bctr = getVar(i)->getConstr(getVar(p[i]));
            if (bctr) {
                //      cout << "parent[" << i << "] = " << p[i] << " (" << bctr->getTightness() << ")" << endl;
                tight += bctr->getTightness();
            } else {
                tightok = false;
            }
            listofsuccessors[p[i]].push_back(i);
        } else {
            roots.push_back(i);
            //      cout << "parent[" << i << "] = no parent" << endl;
        }
    }
    if (ToulBar2::verbose >= 0) {
        if (tightok)
            cout << " (" << 100.0 * tight / alltight << "%)";
        cout << endl;
    }

    vector<bool> marked(n, false);
    for (int i = roots.size() - 1; i >= 0; i--) {
        visit(roots[i], order_inv, marked, listofsuccessors);
    }
    for (int i = n - 1; i >= 0; i--) {
        if (!marked[i]) {
            visit(i, order_inv, marked, listofsuccessors);
        }
    }

    if (ToulBar2::verbose >= 1) {
        cout << "Maximum spanning tree ordering:";
        for (int i = 0; i < n; i++) {
            cout << " " << order_inv[i];
        }
        cout << endl;
    }

    assert(order_inv.size() == numberOfVariables());
}

void WCSP::reverseCuthillMcKeeOrderingBGL(vector<int>& order_inv)
{
    ColoredGraph G;
    for (unsigned int i = 0; i < vars.size(); i++)
        add_vertex(G);
    for (unsigned int i = 0; i < constrs.size(); i++)
        if (constrs[i]->connected())
            addConstraint(constrs[i], G);
    for (int i = 0; i < elimBinOrder; i++)
        if (elimBinConstrs[i]->connected())
            addConstraint(elimBinConstrs[i], G);
    for (int i = 0; i < elimTernOrder; i++)
        if (elimTernConstrs[i]->connected())
            addConstraint(elimTernConstrs[i], G);

    int n = num_vertices(G);
    vector<int> inverse_perm(n, 0);

    cuthill_mckee_ordering(G, inverse_perm.rbegin(), get(vertex_color, G), make_degree_map(G));
    order_inv = inverse_perm;
    if (ToulBar2::verbose >= 1) {
        cout << "Reverse Cuthill-McKee ordering:";
        for (size_t i = 0; i < num_vertices(G); ++i) {
            cout << " " << order_inv[i];
        }
        cout << endl;
    }

    assert(order_inv.size() == numberOfVariables());
}

/// \brief Maximum Cardinality Search algorithm (Tarjan & Yannakakis)
/// \note code from Cyril Terrioux
void WCSP::maximumCardinalitySearch(vector<int>& order_inv)
{
    Graph G;
    for (unsigned int i = 0; i < vars.size(); i++)
        add_vertex(G);
    for (unsigned int i = 0; i < constrs.size(); i++)
        if (constrs[i]->connected())
            addConstraint(constrs[i], G);
    for (int i = 0; i < elimBinOrder; i++)
        if (elimBinConstrs[i]->connected())
            addConstraint(elimBinConstrs[i], G);
    for (int i = 0; i < elimTernOrder; i++)
        if (elimTernConstrs[i]->connected())
            addConstraint(elimTernConstrs[i], G);

    int n = num_vertices(G);
    vector<int> inverse_perm(n, 0);

    vector<vector<int>> sets(n, vector<int>(n));
    vector<int> size(n);
    vector<int> card(n);
    vector<int> degree(n);

    Graph::adjacency_iterator neighbourIt, neighbourEnd;

    /* initialize sets, card and size */
    for (int v = 0; v < n; v++) {
        size[v] = 0;

        for (int i = 0; i < n; i++)
            sets[v][i] = 0;

        sets[0][v] = 1;
        card[v] = 0;
        degree[v] = boost::degree(v, G);
    }

    card[0] = n;
    int i = n - 1;
    int j = 0;
    int v = 0;

    while (i >= 0) {
        /* choose a vertex */
        int deg = -1;
        for (int x = 0; x < n; x++)
            if ((sets[j][x] == 1) && (degree[x] > deg)) {
                v = x;
                deg = degree[x];
            }
        sets[j][v] = 0;
        card[j]--;

        /* build the order */
        inverse_perm[i] = v;
        size[v] = -1;

        /* update sets and size */
        boost::tie(neighbourIt, neighbourEnd) = adjacent_vertices(v, G);
        for (; neighbourIt != neighbourEnd; ++neighbourIt) {
            if (size[*neighbourIt] >= 0) {
                sets[size[*neighbourIt]][*neighbourIt] = 0;
                card[size[*neighbourIt]]--;

                size[*neighbourIt]++;

                sets[size[*neighbourIt]][*neighbourIt] = 1;
                card[size[*neighbourIt]]++;
            }
        }

        i--;
        j++;
        while ((j >= 0) && (card[j] == 0))
            j--;
    }

    order_inv = inverse_perm;
    if (ToulBar2::verbose >= 1) {
        cout << "MCS ordering:";
        for (size_t i = 0; i < num_vertices(G); ++i) {
            cout << " " << order_inv[i];
        }
        cout << endl;
    }
    assert(order_inv.size() == numberOfVariables());
}

/// \brief Minimum Fill-In Ordering algorithm
/// \note code from Cyril Terrioux
void WCSP::minimumFillInOrdering(vector<int>& order_inv)
{
    Graph G;
    for (unsigned int i = 0; i < vars.size(); i++)
        add_vertex(G);
    for (unsigned int i = 0; i < constrs.size(); i++)
        if (constrs[i]->connected())
            addConstraint(constrs[i], G);
    for (int i = 0; i < elimBinOrder; i++)
        if (elimBinConstrs[i]->connected())
            addConstraint(elimBinConstrs[i], G);
    for (int i = 0; i < elimTernOrder; i++)
        if (elimTernConstrs[i]->connected())
            addConstraint(elimTernConstrs[i], G);

    int n = num_vertices(G);
    vector<int> order(n, -1);
    order_inv = order;

    vector<int> nb_fillin(n, 0);
    vector<int> degree(n, 0);

    Graph::adjacency_iterator neighbourIt, neighbourEnd;

    for (int v = 0; v < n; v++) {
        degree[v] = boost::degree(v, G);
        /* compute initial number of edges to add for each vertex */
        boost::tie(neighbourIt, neighbourEnd) = adjacent_vertices(v, G);
        for (; neighbourIt != neighbourEnd; ++neighbourIt) {
            Graph::adjacency_iterator neighbourIt2 = neighbourIt;
            for (++neighbourIt2; neighbourIt2 != neighbourEnd; ++neighbourIt2) {
                if (!edge(*neighbourIt, *neighbourIt2, G).second)
                    nb_fillin[v]++;
            }
        }
    }
    for (int i = 0; i < n - 1; i++) {
        /* compute number of fill-in edges to add for each unprocessed vertex */
        /* choose vertex with minimum fill-in */
        int v = 0;
        Long minfill = (Long)n * n;
        int deg = -1;
        for (int x = 0; x < n; x++) {
            if ((order[x] == -1) && ((nb_fillin[x] < minfill) || ((nb_fillin[x] == minfill) && (degree[x] > deg)))) {
                v = x;
                minfill = nb_fillin[x];
                deg = degree[x];
            }
        }
        order[v] = i;
        order_inv[i] = v;

        /* remove vertex v from nb_fillin */
        boost::tie(neighbourIt, neighbourEnd) = adjacent_vertices(v, G);
        for (; neighbourIt != neighbourEnd; ++neighbourIt) {
            if (order[*neighbourIt] == -1) {
                Graph::adjacency_iterator neighbourIt2, neighbourEnd2;
                boost::tie(neighbourIt2, neighbourEnd2) = adjacent_vertices(*neighbourIt, G);
                for (; neighbourIt2 != neighbourEnd2; ++neighbourIt2) {
                    if (order[*neighbourIt2] == -1 && !edge(v, *neighbourIt2, G).second)
                        nb_fillin[*neighbourIt]--;
                }
            }
        }
        /* add fill-in edges to G */
        boost::tie(neighbourIt, neighbourEnd) = adjacent_vertices(v, G);
        for (; neighbourIt != neighbourEnd; ++neighbourIt) {
            if (order[*neighbourIt] == -1) {
                Graph::adjacency_iterator neighbourIt2 = neighbourIt;
                for (++neighbourIt2; neighbourIt2 != neighbourEnd; ++neighbourIt2) {
                    if ((order[*neighbourIt2] == -1) && !edge(*neighbourIt, *neighbourIt2, G).second) {
                        add_edge(*neighbourIt, *neighbourIt2, G);
                        degree[*neighbourIt]++;
                        degree[*neighbourIt2]++;

                        unsigned int x = *neighbourIt;
                        unsigned int y = *neighbourIt2;
                        /* update nb_fillin with missing edges between x and neighbors of y */
                        Graph::adjacency_iterator neighbourItX, neighbourEndX;
                        boost::tie(neighbourItX, neighbourEndX) = adjacent_vertices(x, G);
                        for (; neighbourItX != neighbourEndX; ++neighbourItX) {
                            if ((order[*neighbourItX] == -1) && (*neighbourItX != y)) {
                                if (!edge(y, *neighbourItX, G).second)
                                    nb_fillin[x]++;
                                else
                                    nb_fillin[*neighbourItX]--; /* new added edge between x and y has to be removed from nb_fillin  */
                            }
                        }
                        /* update nb_fillin with missing edges between y and neighbors of x */
                        Graph::adjacency_iterator neighbourItY, neighbourEndY;
                        boost::tie(neighbourItY, neighbourEndY) = adjacent_vertices(y, G);
                        for (; neighbourItY != neighbourEndY; ++neighbourItY) {
                            if ((order[*neighbourItY] == -1) && (*neighbourItY != x) && !edge(x, *neighbourItY, G).second)
                                nb_fillin[y]++;
                        }
                    }
                }
            }
        }
    }

    int v = 0;
    while (order[v] != -1)
        v++;
    order[v] = n - 1;
    order_inv[n - 1] = v;
    if (ToulBar2::verbose >= 1) {
        cout << "Min-fill ordering:";
        for (int j = 0; j < n; ++j) {
            cout << " " << getName(order_inv[j]);
        }
        cout << endl;
    }
    assert(order_inv.size() == numberOfVariables());
}

/// \brief Minimum Degree Ordering algorithm
/// \note code from Cyril Terrioux
void WCSP::minimumDegreeOrdering(vector<int>& order_inv)
{
    Graph G;
    for (unsigned int i = 0; i < vars.size(); i++)
        add_vertex(G);
    for (unsigned int i = 0; i < constrs.size(); i++)
        if (constrs[i]->connected())
            addConstraint(constrs[i], G);
    for (int i = 0; i < elimBinOrder; i++)
        if (elimBinConstrs[i]->connected())
            addConstraint(elimBinConstrs[i], G);
    for (int i = 0; i < elimTernOrder; i++)
        if (elimTernConstrs[i]->connected())
            addConstraint(elimTernConstrs[i], G);
    int n = num_vertices(G);
    vector<int> order(n, -1);
    order_inv = order;
    vector<int> degree(n, 0);

    //    vector<int> preorder;
    //    reverseCuthillMcKeeOrderingBGL(preorder);

    Graph::adjacency_iterator neighbourIt, neighbourEnd;

    for (int v = 0; v < n; v++) {
        degree[v] = boost::degree(v, G);
    }
    for (int i = 0; i < n - 1; i++) {
        /* find vertex with minimum degree */
        int v = 0;
        int deg_min = n + 1;
        for (int x = 0; x < n; x++) {
            //           int x = preorder[xx];
            if ((order[x] == -1) && (degree[x] < deg_min)) {
                v = x;
                deg_min = degree[x];
            }
        }
        order[v] = i;
        order_inv[i] = v;

        boost::tie(neighbourIt, neighbourEnd) = adjacent_vertices(v, G);
        for (; neighbourIt != neighbourEnd; ++neighbourIt) {
            if (order[*neighbourIt] == -1) {
                degree[*neighbourIt]--;
                Graph::adjacency_iterator neighbourIt2 = neighbourIt;
                for (++neighbourIt2; neighbourIt2 != neighbourEnd; ++neighbourIt2) {
                    if ((order[*neighbourIt2] == -1) && !edge(*neighbourIt, *neighbourIt2, G).second) {
                        add_edge(*neighbourIt, *neighbourIt2, G);
                        degree[*neighbourIt]++;
                        degree[*neighbourIt2]++;
                    }
                }
            }
        }
    }
    int v = 0;
    while (order[v] != -1)
        v++;
    order[v] = n - 1;
    order_inv[n - 1] = v;
    if (ToulBar2::verbose >= 1) {
        cout << "Minimum degree ordering:";
        for (int j = 0; j < n; ++j) {
            cout << " " << order_inv[j];
        }
        cout << endl;
    }
    assert(order_inv.size() == numberOfVariables());
}

int cmpValueCost3(const void* p1, const void* p2)
{
    Cost c1 = ((ValueCost*)p1)->cost;
    Cost c2 = ((ValueCost*)p2)->cost;
    Value v1 = ((ValueCost*)p1)->value;
    Value v2 = ((ValueCost*)p2)->value;
    if (c1 < c2)
        return -1;
    else if (c1 > c2)
        return 1;
    else if (v1 < v2)
        return -1;
    else if (v1 > v2)
        return 1;
    else
        return 0;
}
template <typename T>
static vector<vector<pair<int, int>>> FindClique(vector<int> scope, T& g)
{
    Graph G;
    for (unsigned int i = 0; i < scope.size(); ++i) {
        add_vertex(G);
        add_vertex(G);
    }
    for (unsigned int i = 0; i < scope.size(); i++) {
        for (unsigned int j = i + 1; j < scope.size(); j++) {
            if (edge(2 * scope[i], 2 * scope[j], g).second)
                add_edge(2 * i, 2 * j, G);
            if (edge(2 * scope[i] + 1, 2 * scope[j], g).second)
                add_edge(2 * i + 1, 2 * j, G);
            if (edge(2 * scope[i], 2 * scope[j] + 1, g).second)
                add_edge(2 * i, 2 * j + 1, G);
            if (edge(2 * scope[i] + 1, 2 * scope[j] + 1, g).second)
                add_edge(2 * i + 1, 2 * j + 1, G);
        }
    }
    vector<int> Temp;
    vector<vector<int>> Tempclq;
    vector<int> order;
    for (unsigned int i = 0; i < scope.size(); ++i) {
        order.push_back(i);
    }
    if (G.m_vertices[2 * order[0]].m_out_edges.size() > G.m_vertices[2 * order[0] + 1].m_out_edges.size())
        Temp.push_back(0);
    else
        Temp.push_back(1);
    Tempclq.push_back(Temp);
    bool ok;
    unsigned j, k, curr;
    for (int i = 1; i < (int)order.size(); i++) {
        ok = false;
        j = 0;
        curr = 2 * order[i] + 1;
        if (G.m_vertices[2 * order[i]].m_out_edges.size() > G.m_vertices[2 * order[i] + 1].m_out_edges.size())
            curr = 2 * order[order[i]];
        while (!ok && j < Tempclq.size()) {
            k = 0;
            ok = true;
            while (ok && k < Tempclq[j].size()) {
                if (!edge(curr, Tempclq[j][k], G).second && !edge(Tempclq[j][k], curr, G).second)
                    ok = false;
                k++;
            }
            j++;
        }
        if (ok) {
            Tempclq[j - 1].push_back(curr);
        } else {
            Temp.clear();
            Temp.push_back(curr);
            Tempclq.push_back(Temp);
        }
    }
    vector<vector<pair<int, int>>> clq;
    vector<pair<int, int>> clq2;
    for (unsigned int i = 0; i < Tempclq.size(); ++i) {
        clq2.clear();
        for (unsigned int l = 0; l < Tempclq[i].size(); ++l) {
            clq2.push_back(pair(scope[(int)floor(Tempclq[i][l] / 2.0 + 0.1)], Tempclq[i][l] % 2));
        }
        if (clq2.size() > 1)
            clq.push_back(clq2);
    }
    return clq;
}

void WCSP::addAMOConstraints()
{
    if (ToulBar2::verbose >= 1)
        cout << "Add AMO constraints to knapsack contraints." << endl;
    double startCpuTime = cpuTime();
    double startRealTime = realTime();

    vector<int> Var;
    Graph G;
    int count = 0;
    if (ToulBar2::verbose >= 1)
        cout << "Construct Graph of size " << vars.size() << endl;
    for (int i = 0; i < (int)vars.size(); ++i) {
        if (vars[i]->unassigned()) {
            Var.push_back(i);
            assert(i == vars[i]->wcspIndex);
        }
        add_vertex(G);
        add_vertex(G);
    }
    for (unsigned int varIndex = 0; varIndex < Var.size(); varIndex++) {
        int size = getDomainSize(Var[varIndex]);
        ValueCost sorted[size];
        getEnumDomainAndCost(Var[varIndex], sorted);
        qsort(sorted, size, sizeof(ValueCost), cmpValueCost3);
        for (int a = 0; a < size; a++) {
            int storedepth = Store::getDepth();
            try {
                Store::store();
                assign(Var[varIndex], sorted[a].value);
            } catch (const Contradiction&) {
            }
            for (unsigned int i = 0; i < Var.size(); ++i) {
                if (vars[Var[i]]->assigned() && i != varIndex) {
                    add_edge(2 * Var[varIndex] + sorted[a].value, 2 * Var[i] + 1 - vars[Var[i]]->getValue(), G);
                }
            }
            Store::restore(storedepth);
        }
    }
    if (ToulBar2::verbose >= 1)
        cout << "Graph done." << endl;
    count = 0;
    vector<int> scope;
    vector<int> scope2;
    vector<vector<pair<int, int>>> clq;
    int MaxAMO = 0;
    int total = 0;
    unsigned int nbconstrs = constrs.size();
    for (unsigned int i = 0; i < nbconstrs; ++i) {
        auto* k = dynamic_cast<KnapsackConstraint*>(constrs[i]);
        if (!k)
            continue;
        else {
            if (constrs[i]->arity() > 3 && constrs[i]->connected()) {
                scope.clear();
                scope2.clear();
                clq.clear();
                //scope=k->GetOrder();
                for (int j = 0; j < constrs[i]->arity(); j++) {
                    if (constrs[i]->getVar(j)->unassigned()) {
                        scope2.push_back(constrs[i]->getVar(j)->wcspIndex);
                    }
                }
                if (scope2.size() > 3) {
                    clq = FindClique(scope2, G);
                    if (clq.size() > 0) {
                        for (unsigned int j = 0; j < clq.size(); ++j) {
                            if ((int)clq[j].size() > MaxAMO)
                                MaxAMO = clq[j].size();
                            total += clq[j].size();
                        }
                        count++;
                        k->addAMOConstraints(clq, vars, this);
                    }
                }
            }
        }
    }
    if (ToulBar2::verbose >= 0) {
        if (count>0) cout << count << " AMO constraint" << ((count>1)?"s":"") << " added with max size " << MaxAMO;
        else cout << "No AMO constraint added";
        cout << " in " << ((ToulBar2::parallel) ? (realTime() - startRealTime) : (cpuTime() - startCpuTime)) << " seconds." << endl;
    }
}

#endif

/* Local Variables: */
/* c-basic-offset: 4 */
/* tab-width: 4 */
/* indent-tabs-mode: nil */
/* c-default-style: "k&r" */
/* End: */


/* Copyright (C) 2011 Ion Torrent Systems, Inc. All Rights Reserved */
#include <cassert>
#include <cmath>
#include <algorithm>
#include <deque>
#include <functional>
#include <iostream>
#include <iomanip>
#include <numeric>
#include <set>
#include <string>
#include <vector>
#include <fenv.h>
#include <armadillo>
#include "Mask.h"
#include "RawWells.h"
#include "mixed.h"
#include "bivariate_gaussian.h"
#include "armadillo_utils.h"
#include "MaskSample.h"
#include "BeadTracker.h"

using namespace std;
using namespace arma;

typedef pair<int,int>   well_coord;
typedef set<well_coord> well_set;

static void count_sample(filter_counts& counts, deque<float>& ppf, deque<float>& ssq, Mask& mask, RawWells& wells, const vector<int>& key_ionogram, const PolyclonalFilterOpts & opts);
well_set sample_lib(Mask& mask, int nsamp);
void calcMeanCovariance(mat new_sgma[2], vec new_mean[2], vec &new_alpha, const vec mean[2], const mat sgma[2], const vec& alpha, const deque<float>& ppf, const deque<float>& ssq, int option);

void make_filter(clonal_filter& filter, filter_counts& counts, Mask& mask, RawWells& wells, const vector<int>& key_ionogram, const PolyclonalFilterOpts & opts)
{
    // Make a clonality filter from a sample of reads from a RawWells file.
    // Record number of reads in sample that are caught by each filter.
    deque<float>  ppf;
    deque<float>  ssq;
    count_sample(counts, ppf, ssq, mask, wells, key_ionogram, opts);
    make_filter(filter, counts, ppf, ssq, opts);
}

void make_filter(clonal_filter& filter, filter_counts& counts, const deque<float>& ppf, const deque<float>& ssq, const PolyclonalFilterOpts & opts)
{
    // Make a clonality filter from ppf and ssq for a sample of reads.
    // Record number of putative clonal and mixed reads in the sample.
    vec  mean[2];
    mat  sigma[2];
    vec  alpha;
    bool converged = fit_normals(mean, sigma, alpha, ppf, ssq, opts);

    if(converged){
        bivariate_gaussian clonal(mean[0], sigma[0]);
        bivariate_gaussian mixed( mean[1], sigma[1]);
        filter = clonal_filter(clonal, mixed, mixed_ppf_cutoff(), converged);
    }

    if(converged){
        deque<float>::const_iterator p = ppf.begin();
        for(deque<float>::const_iterator s=ssq.begin(); s!=ssq.end(); ++p, ++s){
            if(filter.is_clonal(*p,*s, opts.mixed_stringency))
                ++counts._nclonal;
        }
        counts._nmixed = ppf.size() - counts._nclonal;
    }
}

static void count_sample(filter_counts& counts, deque<float>& ppf, deque<float>& ssq, Mask& mask, RawWells& wells, const vector<int>& key_ionogram, const PolyclonalFilterOpts & opts)
{
    // Take sample of reads from a RawWells file, and apply some simple
    // filters to identify problem reads.
    // Record number of reads in sample, and number of reads caught by
    // each filter.
    well_set sample = sample_lib(mask, counts._nsamp);
    WellData data;
    unsigned int nflows = wells.NumFlows();
    vector<float> nrm(nflows);
    int flow0 = opts.mixed_first_flow;
    int flow1 = opts.mixed_last_flow;
    wells.ResetCurrentRegionWell();

    // Some temporary code for comparing clonal filter in background model:
    ofstream out("basecaller_ppf_ssq.txt");
    assert(out);

    while(!wells.ReadNextRegionData(&data)){
        // Skip if this is not in the sample:
        well_coord wc(data.y, data.x);
        if(sample.find(wc) == sample.end())
            continue;

        // Skip wells with infinite signal:
        bool finite = all_finite(data.flowValues, data.flowValues+nflows);
        if(not finite){
            ++counts._ninf;
            continue;
        }

        // Key-normalize:
        float normalizer = ComputeNormalizerKeyFlows(data.flowValues, &key_ionogram[0], key_ionogram.size());
        transform(data.flowValues, data.flowValues+nflows, nrm.begin(), bind2nd(divides<float>(),normalizer));

        // Skip wells with bad key:
        bool good_key = key_is_good(nrm.begin(), key_ionogram.begin(), key_ionogram.end());
        if(not good_key){
            ++counts._nbad_key;
            continue;
        }

        // Skip possible super-mixed beads:
        float perc_pos = percent_positive(nrm.begin()+flow0, nrm.begin()+flow1);;
        if(perc_pos > mixed_ppf_cutoff()){
            ++counts._nsuper;
            continue;
        }

        // Record ppf and ssq:
        float sum_frac = sum_fractional_part(nrm.begin()+flow0, nrm.begin()+flow1);
        ppf.push_back(perc_pos);
        ssq.push_back(sum_frac);

        // Some temporary code for comparing clonal filter in background model:
        out << setw(6) << data.y
            << setw(6) << data.x
            << setw(8) << setprecision(2) << fixed << perc_pos
            << setw(8) << setprecision(2) << fixed << sum_frac
            << setw(8) << setprecision(2) << fixed << normalizer
            << endl;
    }
    assert(ppf.size() == ssq.size());
}

well_set sample_lib(Mask& mask, int nsamp)
{
    // Return a random sample of wells with library beads.
    MaskSample<uint32_t> lib_sample(mask, MaskLib, nsamp);

    well_set sample;
    int chip_width = mask.W();
    for(vector<uint32_t>::iterator i=lib_sample.Sample().begin(); i!=lib_sample.Sample().end(); ++i){
        int row = *i / chip_width;
        int col = *i % chip_width;
        sample.insert(make_pair(row,col));
    }

    return sample;
}

inline double square(double x)
{
    return x * x;
}

static bool test_convergence(const vec mean[2], const mat sgma[2], const vec& alpha, const vec new_mean[2], const mat new_sgma[2], const vec& new_alpha)
{
    double eps        = 1e-4;
    double mean_diff0  = max(max(new_mean[0] - mean[0]));
    double mean_diff1  = max(max(new_mean[1] - mean[1]));
    double sgma_diff0  = max(max(new_sgma[0]    - sgma[0]));
    double sgma_diff1  = max(max(new_sgma[1]    - sgma[1]));
    double sgma_diff   = max(sgma_diff0, sgma_diff1);
    double alpha_diff  = max(max(new_alpha   - alpha));
    double max_diff    = max(max(mean_diff0, mean_diff1), max(sgma_diff, alpha_diff));

    return max_diff < eps;
}

static void print_dist(vec mean[2], mat sgma[2], vec& alpha, bool converged, int iter, int option)
{
    cout << "Clonal Filter: fit_normals with option " << option << " at iteration" << setw(4) << iter << endl;
    cout << "convergence status: " << boolalpha << converged << endl;
    cout << "Mean of first cluster:" << endl;
    cout << mean[0] << endl;
    cout << "Mean of mixed cluster:"<< endl;
    cout << mean[1] << endl;
    cout << "Covariance of clonal cluster:" <<endl;
    cout << sgma[0] << endl;
    cout << "Covariance of mixed cluster:" <<endl;
    cout << sgma[1] << endl;
    cout << "fraction clonal vs mixed: " << alpha[0] << " & " << alpha[1] << endl;
    cout << endl;
}

static void init(vec mean[2], mat sgma[2], vec& alpha)
{
    for (int i = 0; i < 2; i++) {
        mean[i].set_size(2);
        sgma[i].set_size(2,2);
    }

    alpha.set_size(2);
    // Following intializations are based on average over largne number of runs.
    // Average parameters for clonal population:
    mean[0] << 0.48811  << 1.61692;
    sgma[0] << 0.004836 << 0.032828 << endr
            << 0.032828 << 0.548305 << endr;

    // Average parameters for mixed population:
    mean[1] << 0.693371 << 4.397405;
    sgma[1] = sgma[0];

    // Start by assuming that clonal and mixed populations are equinumerous:
    alpha.fill(0.5);
}

bool fit_normals(vec mean[2], mat sgma[2], vec& alpha, const deque<float>& ppf, const deque<float>& ssq, const PolyclonalFilterOpts & opts)
{
    bool converged = false;

    try {
        // Initial guesses for two normal distributions:
        init(mean, sgma, alpha);
        if (opts.verbose)
          print_dist(mean, sgma, alpha, converged, 0, opts.mixed_model_option);  // XXX

        // increased stablity of iterative method:
        // Add two points to the ppf, ssp deqeue corresponding to the a-priori cluster centers
        std::deque<float> my_ppf(ppf);
        my_ppf.push_front(mean[0][0]);
        my_ppf.push_front(mean[1][0]);
        std::deque<float> my_ssq(ssq);
        my_ssq.push_front(mean[0][1]);
        my_ssq.push_front(mean[1][1]);

        int  max_iters = opts.max_iterations;
        int iteration = 1;
        cout << "max_iters: " << max_iters << endl;
        bool not_pos_def = false;
        bool not_finite_params = false;
        for(; iteration<=max_iters and not converged; ++iteration){

            vec  new_mean[2];
            mat  new_sgma[2];
            vec  new_alpha;

            calcMeanCovariance(new_sgma, new_mean, new_alpha, mean, sgma, alpha, my_ppf, my_ssq, opts.mixed_model_option);

            if(not is_pos_def(sgma[0]) or not is_pos_def(sgma[1])) {
                not_pos_def = true;
                break;
            }

            // Test for convergence:
            if(not new_mean[0].is_finite() or not new_mean[1].is_finite() or not new_sgma[0].is_finite() or not new_sgma[1].is_finite() or not new_alpha.is_finite()) {
                not_finite_params = true;
                break;
            }

            converged = test_convergence(mean, sgma, alpha, new_mean, new_sgma, new_alpha);//ignore new_sigma2 comparison

            // Update parameters, forcing covariances to be the same for both distributions:
            alpha   = new_alpha;
            mean[0] = new_mean[0];
            mean[1] = new_mean[1];
            sgma[0] = new_sgma[0];
            sgma[1] = new_sgma[1];

            if (opts.verbose)
              print_dist(mean, sgma, alpha, converged, iteration, opts.mixed_model_option);
        }

        // Fallback position if failed to converge:
        if(not converged){
            // in case of singular covariance matrices on infinite means, don't use the mean and covariances at the last iteration
            if (not_pos_def || not_finite_params) {
              cout << "failed to converge to an acceptable filter with improper covariance and mean parameters for the clusters: default filtering used" << endl;
              init(mean, sgma, alpha);
            }
            else {
              cout << "failed to converge to an acceptable filter in " << max_iters << " iterations";
              if (opts.use_last_iter_params) {
                cout << ": using last iteration params for filtering" << endl;
              }
              else {
                cout << ": default filtering used" << endl;
                init(mean, sgma, alpha);
              }
            }
            converged = true;
        } else {
          cout << "converged to acceptable filter: using adapted filter at iteration " << (iteration-1) << endl;
        }
    }catch(const exception& ex){
        converged = false;
        cerr << "exception thrown during fit_normals()" << endl;
        cerr << ex.what() << endl;
    }catch(...){
        converged = false;
        cerr << "unknown exception thrown during fit_normals()" << endl;
    }

    return converged;
}

void calcMeanCovariance(mat new_sgma[2], vec new_mean[2], vec &new_alpha, const vec mean[2], const mat sgma[2], const vec& alpha, const deque<float>& ppf, const deque<float>& ssq, int option)
{
  switch(option){
    case 0:{//common covariance
        bivariate_gaussian clone_dist(mean[0], sgma[0]);
        bivariate_gaussian mixed_dist(mean[1], sgma[1]);

        // Re-estimate parameters for each distribution:
        int nsamp = ppf.size();
        vec sumw(2);
        sumw.fill(0.5);

        mat sum2(2,2);
        sum2.fill(0.0);

        vec   sum1[2];
        sum1[0].set_size(2);
        sum1[1].set_size(2);
        sum1[0].fill(0.0);
        sum1[1].fill(0.0);

        // Accumulate weighted sums for re-estimating moments:
        int savedExceptionFlags = fegetexcept();
        fedisableexcept(FE_DIVBYZERO | FE_INVALID | FE_OVERFLOW | FE_INEXACT | FE_UNDERFLOW); //possible division by zero in probability normalization for outliers
        for(int j=0; j<nsamp; ++j){
            // Skip reads outside the poisson range for ppf:
            if(mixed_ppf_cutoff() < ppf[j])
                continue;

            // Each read gets two weights, reflecting the likelyhoods of that
            // read being clonal or mixed.
            vec x(2);
            x << ppf[j] << ssq[j];
            vec q(2);
            q[0] = alpha[0] * clone_dist.pdf(x);
            q[1] = alpha[1] * mixed_dist.pdf(x);

            vec w = q / sum(q);

            // Skip outliers:
            if( not w.is_finite() )
              continue;

            // Running sums for moments are weighted:
            sumw         += w;
            sum1[0]      += w[0] * x;
            sum1[1]      += w[1] * x;
            sum2.at(0,0) += w[0] * square(ppf[j] - mean[0][0]);
            sum2.at(0,1) += w[0] * (ppf[j] - mean[0][0]) * (ssq[j] - mean[0][1]);
            sum2.at(1,1) += w[0] * square(ssq[j] - mean[0][1]);
        }
        feenableexcept(savedExceptionFlags);

        // New means:
        for(int j=0; j<2; ++j) {
            new_mean[j].set_size(2);
            new_mean[j] = sum1[j] / sumw[j];
        }

        // New covariance:
        mat new_sgma1(2,2);
        new_sgma1.at(0,0) = sum2.at(0,0) / sumw[0];
        new_sgma1.at(0,1) = sum2.at(0,1) / sumw[0];
        new_sgma1.at(1,0) = new_sgma1.at(0,1);
        new_sgma1.at(1,1) = sum2.at(1,1) / sumw[0];

        new_sgma[0] = new_sgma1;
        new_sgma[1] = new_sgma1;

        // New prior:
        new_alpha = sumw / nsamp;
    }
        break;
    case 7: {
        //same volume and shape, different orientation: lambda*D_k*A*(D_k)'
      bivariate_gaussian clone_dist(mean[0], sgma[0]);
      bivariate_gaussian mixed_dist(mean[1], sgma[1]);
      // Re-estimate parameters for each distribution:
      int nsamp = ppf.size();
      vec sumw(2);
      sumw.fill(0.0);

      mat sum2(2,2);
      sum2.fill(0.0);

      mat sum3(2,2);
      sum3.fill(0.0);

      vec   sum1[2];
      sum1[0].set_size(2);
      sum1[1].set_size(2);
      sum1[0].fill(0.0);
      sum1[1].fill(0.0);

      // Accumulate weighted sums for re-estimating moments:
      for(int j=0; j<nsamp; ++j){
          // Skip reads outside the poisson range for ppf:
          if(mixed_ppf_cutoff() < ppf[j])
              continue;

          // Each read gets two weights, reflecting the likelyhoods of that
          // read being clonal or mixed.
          vec x(2);
          x << ppf[j] << ssq[j];
          vec q(2);
          q[0] = alpha[0] * clone_dist.pdf(x);
          q[1] = alpha[1] * mixed_dist.pdf(x);
          vec w = q / sum(q);

          // Skip outliers:
          if(not w.is_finite())
              continue;

          // Running sums for moments are weighted:
          sumw         += w;
          sum1[0]      += w[0] * x;
          sum1[1]      += w[1] * x;
          sum2.at(0,0) += w[0] * square(ppf[j] - mean[0][0]);
          sum2.at(0,1) += w[0] * (ppf[j] - mean[0][0]) * (ssq[j] - mean[0][1]);

          sum2.at(1,1) += w[0] * square(ssq[j] - mean[0][1]);
          sum3.at(0,0) += w[1] * square(ppf[j] - mean[1][0]);
          sum3.at(0,1) += w[1] * (ppf[j] - mean[1][0]) * (ssq[j] - mean[1][1]);

          sum3.at(1,1) += w[1] * square(ssq[j] - mean[1][1]);
      }
      sum2.at(1,0) =  sum2.at(0,1);
      sum3.at(1,0) =  sum3.at(0,1);

      // New means:
      for(int j=0; j<2; ++j) {
          new_mean[j].set_size(2);
          new_mean[j] = sum1[j] / sumw[j];
      }

      //calculate eigen values and vectors for sum2 and sum3
      vec eigval_clonal;
      mat eigvec_clonal;
      eig_sym(eigval_clonal, eigvec_clonal, sum2);

      vec eigval_mixed;
      mat eigvec_mixed;
      eig_sym(eigval_mixed, eigvec_mixed, sum3);

      //maintain same ascending order between clonal and mixed: ascending order by default and should be no op
      //should we bail out if eigenvalue is complex
      if(eigval_clonal.at(0) > eigval_clonal.at(1)){
        double temp = eigval_clonal.at(0);
        eigval_clonal.at(0) = eigval_clonal.at(1);
        eigval_clonal.at(1) = temp;
        temp = eigvec_clonal.at(0, 0);
        eigvec_clonal.at(0, 0) = eigvec_clonal.at(0, 1);
        eigvec_clonal.at(0, 1) = temp;
        temp = eigvec_clonal.at(1, 0);
        eigvec_clonal.at(1, 0) = eigvec_clonal.at(1, 1);
        eigvec_clonal.at(1, 1) = temp;
      }
      if(eigval_mixed.at(0) > eigval_mixed.at(1)){
        double temp = eigval_mixed.at(0);
        eigval_mixed.at(0) = eigval_mixed.at(1);
        eigval_mixed.at(1) = temp;
        temp = eigvec_mixed.at(0, 0);
        eigvec_mixed.at(0, 0) = eigvec_mixed.at(0, 1);
        eigvec_mixed.at(0, 1) = temp;
        temp = eigvec_mixed.at(1, 0);
        eigvec_mixed.at(1, 0) = eigvec_mixed.at(1, 1);
        eigvec_mixed.at(1, 1) = temp;
      }
      //average out the eigenvalues
      vec eigval_sum = eigval_clonal + eigval_mixed;
      //calculate the new covariance
      new_sgma[0] = eigvec_clonal * diagmat(eigval_sum) * trans(eigvec_clonal) / (sumw[0] + sumw[1]);
      new_sgma[1] = eigvec_mixed * diagmat(eigval_sum) * trans(eigvec_mixed) / (sumw[0] + sumw[1]);

      // New prior:
      new_alpha = sumw / nsamp;

    }
        break;

    case 8:{//common volume
        bivariate_gaussian clone_dist(mean[0], sgma[0]);
        bivariate_gaussian mixed_dist(mean[1], sgma[1]);
        // Re-estimate parameters for each distribution:
        int nsamp = ppf.size();
        vec sumw(2);
        sumw.fill(0.0);

        mat sum2(2,2);
        sum2.fill(0.0);

        mat sum3(2,2);
        sum3.fill(0.0);

        vec   sum1[2];
        sum1[0].set_size(2);
        sum1[1].set_size(2);
        sum1[0].fill(0.0);
        sum1[1].fill(0.0);

        // Accumulate weighted sums for re-estimating moments:
        for(int j=0; j<nsamp; ++j){
            // Skip reads outside the poisson range for ppf:
            if(mixed_ppf_cutoff() < ppf[j])
                continue;

            // Each read gets two weights, reflecting the likelyhoods of that
            // read being clonal or mixed.
            vec x(2);
            x << ppf[j] << ssq[j];
            vec q(2);
            q[0] = alpha[0] * clone_dist.pdf(x);
            q[1] = alpha[1] * mixed_dist.pdf(x);
            vec w = q / sum(q);

            // Skip outliers:
            if(not w.is_finite())
                continue;

            // Running sums for moments are weighted:
            sumw         += w;
            sum1[0]      += w[0] * x;
            sum1[1]      += w[1] * x;
            sum2.at(0,0) += w[0] * square(ppf[j] - mean[0][0]);
            sum2.at(0,1) += w[0] * (ppf[j] - mean[0][0]) * (ssq[j] - mean[0][1]);

            sum2.at(1,1) += w[0] * square(ssq[j] - mean[0][1]);
            sum3.at(0,0) += w[1] * square(ppf[j] - mean[1][0]);
            sum3.at(0,1) += w[1] * (ppf[j] - mean[1][0]) * (ssq[j] - mean[1][1]);
            sum3.at(1,1) += w[1] * square(ssq[j] - mean[1][1]);
        }
        sum2.at(1,0) =  sum2.at(0,1);
        sum3.at(1,0) =  sum3.at(0,1);

        // New means:
        for(int j=0; j<2; ++j) {
            new_mean[j].set_size(2);
            new_mean[j] = sum1[j] / sumw[j];
        }

        vec det(2);
        det[0] = sqrt(sum2.at(0, 0)*sum2.at(1,1) - sum2.at(0, 1)*sum2.at(1,0));
        det[1] = sqrt(sum3.at(0, 0)*sum3.at(1,1) - sum3.at(0, 1)*sum3.at(1,0));
        double det_factor = (det[0] + det[1])/(sumw[0] + sumw[1]);

        // New covariance:
        mat new_sgma1(2,2);
        new_sgma1.at(0,0) = sum2.at(0,0) * det_factor / det[0];
        new_sgma1.at(0,1) = sum2.at(0,1) * det_factor / det[0];
        new_sgma1.at(1,0) = new_sgma1.at(0,1);
        new_sgma1.at(1,1) = sum2.at(1,1) * det_factor / det[0];

        mat new_sgma2(2,2);
        new_sgma2.at(0,0) = sum3.at(0,0) * det_factor / det[1];
        new_sgma2.at(0,1) = sum3.at(0,1) * det_factor / det[1];
        new_sgma2.at(1,0) = new_sgma2.at(0,1);
        new_sgma2.at(1,1) = sum3.at(1,1) * det_factor / det[1];

        new_sgma[0] = new_sgma1;
        new_sgma[1] = new_sgma2;

        // New prior:
        new_alpha = sumw / nsamp;
    }
        break;

    case 9:{//independent covariance
        bivariate_gaussian clone_dist(mean[0], sgma[0]);
        bivariate_gaussian mixed_dist(mean[1], sgma[1]);
        // Re-estimate parameters for each distribution:
        int nsamp = ppf.size();
        vec sumw(2);
        sumw.fill(0.0);

        mat sum2(2,2);
        sum2.fill(0.0);

        mat sum3(2,2);
        sum3.fill(0.0);

        vec   sum1[2];
        sum1[0].set_size(2);
        sum1[1].set_size(2);
        sum1[0].fill(0.0);
        sum1[1].fill(0.0);

        // Accumulate weighted sums for re-estimating moments:
        for(int j=0; j<nsamp; ++j){
            // Skip reads outside the poisson range for ppf:
            if(mixed_ppf_cutoff() < ppf[j])
                continue;

            // Each read gets two weights, reflecting the likelyhoods of that
            // read being clonal or mixed.
            vec x(2);
            x << ppf[j] << ssq[j];
            vec q(2);
            q[0] = alpha[0] * clone_dist.pdf(x);
            q[1] = alpha[1] * mixed_dist.pdf(x);
            vec w = q / sum(q);

            // Skip outliers:
            if(not w.is_finite())
                continue;

            // Running sums for moments are weighted:
            sumw         += w;
            sum1[0]      += w[0] * x;
            sum1[1]      += w[1] * x;
            sum2.at(0,0) += w[0] * square(ppf[j] - mean[0][0]);
            sum2.at(0,1) += w[0] * (ppf[j] - mean[0][0]) * (ssq[j] - mean[0][1]);
            sum2.at(1,1) += w[0] * square(ssq[j] - mean[0][1]);
            sum3.at(0,0) += w[1] * square(ppf[j] - mean[1][0]);
            sum3.at(0,1) += w[1] * (ppf[j] - mean[1][0]) * (ssq[j] - mean[1][1]);
            sum3.at(1,1) += w[1] * square(ssq[j] - mean[1][1]);
        }

        // New means:
        for(int j=0; j<2; ++j) {
            new_mean[j].set_size(2);
            new_mean[j] = sum1[j] / sumw[j];
        }

        // New covariance:
        mat new_sgma1(2,2);
        new_sgma1.at(0,0) = sum2.at(0,0) / sumw[0];
        new_sgma1.at(0,1) = sum2.at(0,1) / sumw[0];
        new_sgma1.at(1,0) = new_sgma1.at(0,1);
        new_sgma1.at(1,1) = sum2.at(1,1) / sumw[0];

        mat new_sgma2(2,2);
        new_sgma2.at(0,0) = sum3.at(0,0) / sumw[1];
        new_sgma2.at(0,1) = sum3.at(0,1) / sumw[1];
        new_sgma2.at(1,0) = new_sgma2.at(0,1);
        new_sgma2.at(1,1) = sum3.at(1,1) / sumw[1];
        new_sgma[0] = new_sgma1;
        new_sgma[1] = new_sgma2;

        // New prior:
        new_alpha = sumw / nsamp;
    }
        break;
  }

}

ostream& operator<<(ostream& out, const filter_counts& c)
{
    out << setw(8) << "infinite" << setw(12) << c. _ninf    << endl
        << setw(8) << "bad-key"  << setw(12) << c._nbad_key << endl
        << setw(8) << "high-ppf" << setw(12) << c._nsuper   << endl
        << setw(8) << "mixed"    << setw(12) << c._nmixed   << endl
        << setw(8) << "clonal"   << setw(12) << c._nclonal  << endl
        << setw(8) << "samples"  << setw(12) << c._nsamp    << endl;

    return out;
}

//lamda * D_k * A_k * (D_k)'

//alternative: using lamda from clonal

//Is it possible to try different models and find the one fitting the data best?