// 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


#ifndef PARTICLE_HPP
#define PARTICLE_HPP
//Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace perception_oru{
  class particle{

    Eigen::Affine3d pose;
    double r_,p_,t_,x_,y_,z_;

    double likelihood;
  public:

    double probability;
    particle();
    particle(double roll,double pitch,double yaw,double x, double y, double z);
    particle(Eigen::Affine3d pose_);

    double GetLikelihood();
    double GetProbability();
    void GetXYZ(double &x, double &y, double &z);
    void GetRPY(double &r,double &p,double &y);

    void Set(double roll,double pitch,double yaw,double x, double y, double z);
    void SetLikelihood(double l_);
    void SetProbability(double p_);

    Eigen::Affine3d GetAsAffine();
  };
}
#endif


#include <boost/graph/adjacency_list.hpp>
#include <iostream>

int main()
{
  boost::adjacency_list<> g;

  boost::adjacency_list<>::vertex_descriptor v1 = boost::add_vertex(g);
  boost::adjacency_list<>::vertex_descriptor v2 = boost::add_vertex(g);
  boost::adjacency_list<>::vertex_descriptor v3 = boost::add_vertex(g);
  boost::adjacency_list<>::vertex_descriptor v4 = boost::add_vertex(g);

  std::cout << v1 << ", " << v2 << ", " << v3 << ", " << v4 << '\n';
}


#include <iostream>
#include <algorithm>
#include <numeric>
#include <vector>
#include <random>
// #include <Eigen/Dense>
#include <opencv2/opencv.hpp>
// #include <opencv2/matlab/bridge.hpp>
// #include <opencv2/matlab/bridge.hpp>
// #include "opencv2/core.hpp"
// #include "opencv2/imgproc.hpp"
// #include "opencv2/highgui.hpp"
#include "distance.h"
#include "kernel.h"
#include "meanshift.h"
// #include "kernel.hpp"
// #include "meanshift.hpp"

using namespace std;
using namespace ModelFitting;
using namespace cv;

// int main1(int argc, char* argv[])
// {

//     cout<< "Welcome Mean Shift"<<endl;
//     dist_calc = &euclidean_distance<double>;
//     testClass<double> t;
//     t.dist_calc = &euclidean_distance;
//     // Vec3d p_a, p_b;
//     // p_a << 2,3,4;
//     // p_b << 0,0,2;
//     // cout << euclidean_distance(p_a, p_b) << endl;
//     // cout << jaccard_distance(p_a, p_b) << endl;

//     // p_a << 2,3,6;
//     // p_b << 0,0,4;
//     // cout << euclidean_distance(p_a, p_b) << endl;
//     // cout << jaccard_distance(p_a, p_b) << endl;

//     // p_a << 2,3,16;
//     // p_b << 0,0,14;
//     // cout << euclidean_distance(p_a, p_b) << endl;
//     // cout << jaccard_distance(p_a, p_b) << endl;

//     // MatXd x(3,1), y;
//     // MatXd t;
//     // x << 1, 2, 3;
//     // y = x;
//     // y(1) = 18;
//     // cout << t.rows() << endl;
//     // t.conservativeResize(1,1);
//     // cout << t.rows() << endl;
//     // cout << t.cols() << endl;
//     // t.conservativeResize(2,Eigen::NoChange);
//     // cout << t.rows() << endl;
//     // cout << t.cols() << endl;
//     cout << "number of threads: "<< Eigen::nbThreads( ) << endl;

//     cv::Mat A = (cv::Mat_<double>(3,1) << 0.0, 0.0, 0.0);

//     // cv::Mat_<double> const C = A;
//     // cout << &C << endl;
//     cv::Mat D = (cv::Mat_<double>(3,1) << 0.0, 1.0, 0.0);
//     cout << euclidean_distance<double>(A,D,3) << endl;
//     D.row(2) = 2;
//     cout << t.dist_calc(A,D,3) << endl;

//     cv::Mat E = (cv::Mat_<double>(3,2) << 0.0, 1.0, 0.0,
//                                           1.0, 0.0 ,1.0);

//     cout << "E: " << E << endl;
//     cout << "&E: " << &E << endl;

//     cv::Mat E0;
//     cout << "&E0: " << &E0 << endl;
//     E0 = E.row(0);
//     cout << "E.row(0): " << E0 << endl;
//     cout << "&E.row(0): " << &E0 << endl;
//     const double * Ep = reinterpret_cast<double *>(E.data);
//     cout << "Ep: " << Ep << endl;
//     cout << "*Ep: " << *Ep << endl;
//     cout << "Ep+1: " << (Ep+1) << endl;
//     cout << "*Ep+1: " << *(++Ep) << endl;

//     ///
//     ///
//     ///
//     std::vector<int> v = {0,0,0,1,0,1,0, 1,0,1};
//     // v.erase(std::remove(v.begin(), v.end(), 0), v.end());
//     for(auto ele:v)
//         cout << ele << ", ";
//     cout << endl;

//     std::vector<int> init_pts_inds(10); // initial points indices
//     std::iota(init_pts_inds.begin(), init_pts_inds.end(), 0);
//     for(auto ele:init_pts_inds)
//         cout << ele << ", ";
//     cout << endl;
//     init_pts_inds.erase(
//         std::remove_if(
//         init_pts_inds.begin(),
//         init_pts_inds.end(),
//         [v](int x){return v.at(x) == 1;}
//     ),
//     init_pts_inds.end());
//     // init_pts_inds.erase()

//     for(auto ele:init_pts_inds)
//         cout << ele << ", ";
//     cout << endl;

//     // std::vector<std::vector<int>> a;
//     // test_array(a);

//     // init_pts_inds.data();
//     cv::Mat t1 = (cv::Mat_<double>(1,3) << 1.0, 2.0, 3.0);
//     cv::Mat t2 = (cv::Mat_<double>(1,3) << 0.0, 1.0, 0.0);
//     std::vector<cv::Mat> v1 = {t1,t2};

//     cv::Mat m = cv::Mat_<double>(v1.size(),3);

//     std::memcpy(m.data, v1.data(), v1.size() * sizeof(v1.at(0)));
//     cout << m << endl;

//     // C = &D;//(cv::Mat_<double>(3,1) << 0.0, 1.0, 1.0);
//     // cout << &C << endl;
//     // C(0,0) = 5;
//     // int const dim = 3;
//     // dim = 9;
//     // cv::Mat E = C - D;
//     // cout << static_cast<double>(cv::Mat_<double>((D - C).t() * (D - C))) << endl;
//     // cout << (E.t() * E).type() << endl;

//     test_kernels();
//     return 0;
// }

// Testing meanshift
int main(int argc, char* argv[])
{
    const int MAX_CLUSTERS = 5;
    Scalar colorTab[] =
    {
        Scalar(0, 0, 255),
        Scalar(0,255,0),
        Scalar(255,100,100),
        Scalar(255,0,255),
        Scalar(0,255,255)
    };
    Mat img(500, 500, CV_8UC3);
    RNG rng(12345);

    int k, clusterCount = rng.uniform(2, MAX_CLUSTERS+1);
    int i, sampleCount = rng.uniform(1, 1001);
    Mat points(sampleCount, 1, CV_32FC2), labels;
    clusterCount = MIN(clusterCount, sampleCount);
    std::vector<Point2f> centers;
    /* generate random sample from multigaussian distribution */
    for( k = 0; k < clusterCount; k++ )
    {
        Point center;
        center.x = rng.uniform(0, img.cols);
        center.y = rng.uniform(0, img.rows);
        Mat pointChunk = points.rowRange(k*sampleCount/clusterCount,
                                            k == clusterCount - 1 ? sampleCount :
                                            (k+1)*sampleCount/clusterCount);
        rng.fill(pointChunk, RNG::NORMAL, Scalar(center.x, center.y), Scalar(img.cols*0.05, img.rows*0.05));
    }
    randShuffle(points, 1, &rng);

    Mat_<double>
        in_ms = Mat_<double>(points.rows, 2);
    Mat cluster_centers;
    std::vector<std::vector<int>> cluster_points;
    double bandwidth = 40;

    MeanShift alg(jaccard_distance<double>, uniform_kernel);

    for (size_t i = 0; i < points.rows; i++)
    {
        Point2d ipt = static_cast<Point2d>(points.at<Point2f>(i));
        cout << ipt << endl;
        in_ms(i,0) = static_cast<double>(ipt.x);
        in_ms(i,1) = static_cast<double>(ipt.y);
        cout << in_ms.row(i) << endl;
        circle( img, ipt, 2, colorTab[0], FILLED, LINE_AA );
    }

    alg.cluster(
        in_ms,
        cluster_centers,
        cluster_points,
        bandwidth);
    cout << cluster_centers.size() << endl;
    // cout << cluster_centers << endl;
    for (size_t i = 0; i < cluster_centers.rows; i++)
    {
        Point ipt(cluster_centers.at<double>(i, 0), cluster_centers.at<double>(i, 1));
        // in_ms.row(i) = (Mat_<double>(1,2) << ipt.x, ipt.y);

        circle( img, ipt, 20, colorTab[1], 1, LINE_AA );
    }
    imshow("clusters", img);

    waitKey(0);

}
// {
//     cv::Mat img(100, 100, CV_8UC3);
//     std::random_device rd{};
//     std::mt19937 gen{rd()};

//     cv::randn(img, 25, 20);
//     cv::randn(img, 75, 20);

//     cv::namedWindow("random colors image", cv::WINDOW_FREERATIO );
//     cv::imshow("random colors image", img);
//     // cv::namedWindow("Disparity - Naive", cv::WINDOW_AUTOSIZE);
//     // cv::imshow("Disparity - Naive", naive_disparities);

//     cv::waitKey(0);
//     cout << "welcome" << endl;
//     return 0;
// }


/*
	Definition fo the random generatrs
*/

#include "generators.h"

//--------------------------------------------------------------------------------

#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>

//--------------------------------------------------------------------------------

namespace asws
{

namespace common
{

namespace random
{

//--------------------------------------------------------------------------------
/**
* @TODO: Create an engine per thread - this is daft here....
*/

// Number engine
boost::random::mt19937 mt19937_gen;

// Engine mutex
static boost::mutex mt19937_gen_mutex;

//--------------------------------------------------------------------------------

/**
	Thread safe generate random integer
*/
int gen_uniform_int(int min, int max)
{
	return (min + max) / 2;
	boost::random::uniform_int_distribution<> dist(min, max);
	{
		boost::mutex::scoped_lock smlock(mt19937_gen_mutex);
		return dist(mt19937_gen);
	}
}

//--------------------------------------------------------------------------------

} // namespace random
} // namespace common
} // namespace asws

//--------------------------------------------------------------------------------


// File: train_lbfgs.cc
// Author: Karl Moritz Hermann (mail@karlmoritz.com)
// Created: 09-01-2013
// Last Update: Thu 03 Oct 2013 11:48:59 AM BST

// STL
#include <iostream>

// Boost
#include <boost/program_options/variables_map.hpp>
#include <boost/program_options/parsers.hpp>

#include "shared_defs.h"

// L-BFGS
#include <lbfgs.h>

// Local
#include "train_lbfgs.h"
#include "train_update.h"

#include "utils.h"
#include "recursive_autoencoder.h"
#include "finite_grad_check.h"

using namespace std;
namespace bpo = boost::program_options;

int train_lbfgs(Model& model, LineSearchType linesearch, int max_iterations, float epsilon)
{
  /***************************************************************************
   *                              BFGS training                              *
   ***************************************************************************/

  lbfgs_parameter_t param;
  lbfgs_parameter_init(&param);
  param.linesearch = linesearch;
  param.max_iterations = max_iterations;
  param.epsilon = epsilon;
  param.m = 25;

  Real* vars = nullptr;
  int number_vars = 0;

  setVarsAndNumber(vars,number_vars,model);

  const int n = number_vars;
  Real error = 0.0;

  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_int_distribution<int> rand(1, model.to-1);

  model.noise_sample_offset = rand(rd);

  while (model.it_count < max_iterations) {
    param.max_iterations = max_iterations - model.it_count;
    cout << "Starting new L-BFGS optimization" << endl;
    int ret = lbfgs(n, vars, &error, evaluate_, progress, &model, &param);
    cout << "L-BFGS optimization terminated with status code = " << ret << endl;
    cout << "fx=" << error << endl;
  }

  return 0;
}

static int progress(
    void *instance,
    const lbfgsfloatval_t *x,
    const lbfgsfloatval_t *g,
    const lbfgsfloatval_t fx,
    const lbfgsfloatval_t xnorm,
    const lbfgsfloatval_t gnorm,
    const lbfgsfloatval_t step,
    int n,
    int k,
    int ls
    )
{
  printf("Iteration %d:\n", k);
  printf("  xnorm = %f, gnorm = %f, step = %f\n", xnorm, gnorm, step);
  printf("\n");

  Model* model = reinterpret_cast<Model*>(instance);
  ++model->it_count;
  if (k % model->rae.config.dump_freq == 0)
  {
    printf("Dumping model ...\n");
    dumpModel(*model,k);
  }

  return 0;
}

lbfgsfloatval_t evaluate_(
    void *instance,
    const lbfgsfloatval_t *x,   // Variables (theta)
    lbfgsfloatval_t *g,         // Put gradient here
    const int n,                // Number of variables
    const lbfgsfloatval_t step) // line-search step used in this iteration
{
  Model* model = reinterpret_cast<Model*>(instance);
  //finite_grad_check(*model);
  return computeCostAndGrad(*model,x,g,n);
}

int train_lbfgs_minibatch(Model& model, LineSearchType linesearch, int
    max_iterations, float epsilon, int batches)
{
  /***************************************************************************
   *                              BFGS training                              *
   ***************************************************************************/

  lbfgs_parameter_t param;
  lbfgs_parameter_init(&param);
  param.linesearch = linesearch;
  param.max_iterations = 5;
  param.epsilon = epsilon;
  param.m = 5;

  int size = model.corpus.size();
  int num_batches = min(batches,size);
  int batchsize = (size / num_batches) + 1;

  cout << "Batch size: " << batchsize << " on " << num_batches << " batches." << endl;
  Real* vars = nullptr;

  int number_vars = 0;
  setVarsAndNumber(vars,number_vars,model);
  const int n = number_vars;

  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_int_distribution<int> rand(1, model.to-1);

  Real error = 0.0;

  for (auto iteration = 0; iteration < max_iterations; ++iteration)
  {
    cout << "Iteration " << iteration << endl;
    std::random_shuffle ( model.indexes.begin(), model.indexes.end() );
    for (auto batch = 0; batch < num_batches; ++batch)
    {
      cout << "Starting next batch" << endl;
      model.from = batch*batchsize;
      model.to = min((batch+1)*batchsize,size);
      model.noise_sample_offset = rand(rd);
      int ret = lbfgs(n, vars, &error, evaluate_, progress_minibatch, &model, &param);
      cout << "L-BFGSi minibatch optimization terminated with status code = " << ret << endl;
      cout << "fx=" << error << endl;
    }
    if (iteration % model.rae.config.dump_freq == 0)
    {
      printf("Dumping model ...\n");
      dumpModel(model,iteration);
    }
  }
  return 0;
}

static int progress_minibatch(
    void *instance,
    const lbfgsfloatval_t *x,
    const lbfgsfloatval_t *g,
    const lbfgsfloatval_t fx,
    const lbfgsfloatval_t xnorm,
    const lbfgsfloatval_t gnorm,
    const lbfgsfloatval_t step,
    int n,
    int k,
    int ls
    )
{
  printf("Iteration %d:\n", k);
  printf("  xnorm = %f, gnorm = %f, step = %f\n", xnorm, gnorm, step);
  printf("\n");

  Model* model = reinterpret_cast<Model*>(instance);
  ++model->it_count;

  return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_HYPERBOLIC_FUNCTIONS_GENERIC_SINHCOSH_HPP_INCLUDED
#define NT2_HYPERBOLIC_FUNCTIONS_GENERIC_SINHCOSH_HPP_INCLUDED
#include <nt2/hyperbolic/functions/sinhcosh.hpp>
#include <boost/fusion/include/std_pair.hpp>
#include <nt2/trigonometric/functions/scalar/impl/trigo.hpp>
#include <nt2/trigonometric/functions/simd/common/impl/trigo.hpp>
#include <boost/simd/sdk/simd/meta/is_native.hpp>

namespace nt2 { namespace ext
{

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::sinhcosh_
                            , tag::cpu_
                            , (A0)
                            , ((generic_< floating_<A0> >))
                              ((generic_< floating_<A0> >))
                            )
  {
    typedef A0 result_type;
    typedef typename boost::simd::meta::is_native<A0>::type is_native_t;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0,A0 & a2) const
    {
      A0 a1;
      sinhcosh(tofloat(a0),a1, a2);
      return a1;
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::sinhcosh_, tag::cpu_
                            , (A0)
                            , (generic_< floating_<A0> >)
                            )
  {
    typedef std::pair<A0, A0>                      result_type;
    typedef typename boost::simd::meta::is_native<A0>::type     is_native_t;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      result_type res;
      sinhcosh(tofloat(a0), res.first, res.second);
      return res;
    }
  };

//   NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::sinhcosh_, tag::cpu_
//                             , (A0)
//                             , ((generic_< floating_<A0> >))
//                               ((generic_< floating_<A0> >))
//                               ((generic_< floating_<A0> >))
//                             )
//   {
//     typedef void result_type;
//     typedef typename boost::simd::meta::is_native<A0>::type is_native_t;

//     BOOST_FORCEINLINE result_type operator()(A0 const& a0,A0 & a1,A0 & a2) const
//     {
//       typedef typename meta::as_logical<A1>::type ltype;
//       a2 =  nt2::abs(a0);
//       a1 = a0;
//       if (a2 == Inf<A0>()) return;
//       const A0 u = expm1(a2);
//       const A0 up1 = oneplus(u);
//       const A0 tmp =u/up1;
//       a1 = negif(is_negative(a0), Half<A0>()*tmp*(oneplus(up1)));
//       a2 = oneplus(Half<A0>()*tmp*u);
//     }
//   };

} }

#endif


/*
 * ****** 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: */