#include "fssh.hpp"
#include "electronic.hpp"
#include "constants.hpp"
#include "decoherence_afssh.hpp"
#include "util.hpp"
#include <armadillo>
#include <complex>
#include <cmath>
#include <iostream>

ConfigBlockReader FSSH::setup_reader()
{
    using types = ConfigBlockReader::types;
    ConfigBlockReader reader{"fssh"};
    reader.add_entry("dtc", types::DOUBLE);
    reader.add_entry("delta_e_tol", 1e-4);

    reader.add_entry("amplitude_file", "cs.dat");
    reader.add_entry("decoherence", "");
    // FIXME: make ConfigBlockReader complain if adding the same key twice!

    std::random_device rd; // generate default random seed
    reader.add_entry("random_seed", (size_t) rd());
    {
      std::vector<std::complex<double>> cs {};
      reader.add_entry("amplitudes", cs);
    }
    return reader;
}

void FSSH::get_reader_data(ConfigBlockReader& reader) {
  {
    double in_dtc;
    reader.get_data("dtc", in_dtc);
    // FIXME?: Should input time be in fs as it is (because our
    // interface is general) or in ns to match GMX?
    dtc = in_dtc * (1e-15 / AU2SI_TIME); // fs -> a.u.
  }

  reader.get_data("delta_e_tol", delta_e_tol);
  reader.get_data("amplitude_file", amplitude_file);

  /* added in BOMD::add_qm_keys() */
  reader.get_data("min_state", min_state);

  /*
     Must set rng seed before decoherence because Decoherence will use
     armadillo random number generator
  */
  {
    size_t seed = 0; // a default seed is set in setup_reader(); this value will not be propagated.

    // FIXME: ConfigReader won't throw an error if the wrong type is passed in
    reader.get_data("random_seed", seed);
    arma::arma_rng::set_seed(seed);

    std::cerr << "[FSSH] random_seed = " << seed << std::endl;
  }

  {
    size_t excited_states;
    /* added in BOMD::add_qm_keys() */
    reader.get_data("excited_states", excited_states);

    shstates = excited_states + 1 - min_state;
  }

  if (shstates < 2){
    throw std::logic_error("Cannot run FSSH on a single surface!");
  }

  if (!(min_state <= active_state && active_state <= min_state + shstates)){
    throw std::range_error("Active state not in the range of hopping states!");
  }

  {
    std::string decoherence_in;
    reader.get_data("decoherence", decoherence_in);

    if (decoherence_in == "") {
      // do nothing; already nullptr
    } else if (decoherence_in == "afssh" ||
               decoherence_in == "jain2016" ) {
      decoherence = new AFSSH(&qm, dtc, min_state, shstates, NQM(), NMM());
    }
    else{
      throw std::runtime_error("Decoherence class, '" + decoherence_in + "', not recognized!");
    }
  }

  active_state -= min_state;

  energy.set_size(shstates);

  U.set_size(shstates, shstates);
  T.set_size(shstates, shstates);
  V.set_size(shstates, shstates);

  /*
    FIXME: should be able to configure (multiple) initial electronic
    state(s). Can use DVEC?
  */

  {
    std::vector<std::complex<double>> cs_vec {};
    reader.get_data("amplitudes", cs_vec);
    if (cs_vec.size() > 0){
      if (cs_vec.size() != (arma::uword) shstates){
        throw std::runtime_error("Number of Amplitudes do not match number of hopping surfaces!");
      }
      double norm = 0;
      for (const auto& c: cs_vec){
        norm += std::norm(c);
      }
      std::cerr << "[FSSH] amplitude norm = " << norm << std::endl;
      if (std::abs(1.0 -norm) > 1e-6){
        throw std::runtime_error("Amplitudes are not normed; check your input!");
      }
      arma::cx_vec cs_arma(shstates);
      for (arma::uword i = 0; i < (arma::uword) shstates; i++){
        cs_arma(i) = cs_vec[i];
      }
      c = Electronic(cs_arma);
    }
    else{
      c.reset(shstates, 1, active_state);
    }
  }
}

// For use in the update_gradient() call; Jain step 4
void FSSH::electronic_evolution(void){
  if (hopping){
    throw std::logic_error("Should never be hopping at start of electronic evolution!");
  }

  PropMap props{};
  props.emplace(QMProperty::wfoverlap, &U);
  qm->get_properties(props);

  Electronic::phase_match(U);
  T = arma::real(arma::logmat(U)) / dtc;

  // R.B.: energy was updated in update_gradient()
  V = diagmat(energy);

  /*
    Since the off-diagonal elements of V are always 0 in this
    implementation, we work only with the diagonal of V. Equation 20
    seems to have been developed for a system with positive energy
    eigenvalues. A constant shift of energy scale to the negative (Vii
    = Vii - max(V.diag) can cause Eq. 20 to fail.

    FIXME: construct an alternate to Eq. 20 that is applicable to Vii < 0
  */

  // compute max time step for electronic propagation (eqs. 20, 21)
  {
    double dtq_ = std::min(dtc,
			   std::min(0.02 / T.max(),
				    0.02 / arma::max( V.diag() - arma::mean(V.diag()) )
				    )
			   );

    dtq = dtc / std::round(dtc / dtq_);
  }
  const size_t n_steps = (size_t) dtc / dtq;
  const std::complex<double> I(0,1);

  // Propagate electronic coefficients and compute hopping probabilities for dtc
  for (size_t nt = 0; nt < n_steps; nt++){
    /*
      Propagate all states simultaneously. Recall that
      Electronic::advance(H, dt) uses rk4 to propagate the internally
      held coefficients, c, according the Hamiltonian H for time dt
    */

    // FIXME: should use rk4
    //c.advance_rk4(V - I*T, dtq);
    c.advance_exact(V - I*T, dtq);

    // Check for a hop unless we've already had one
    if (! hopping){
      const arma::uword a = active_state;

      /*
	Compute transition probabilities. Jain (2016) eq. 12 has the
        conjugate on the wrong element; cf. Tully (1990) -- discussion
        with Zeyu Zhou
      */
      arma::vec g = -2 * arma::real(c(a) * arma::conj(c()) % T.col(a)) * dtq / std::norm(c(a));
      // set negative elements to 0
      g.elem( arma::find(g < 0) ).zeros();

      /*
	Ensure that g is normed by adding any residual density to the
	active state. This maintains the correct transition
	probability to all states.
      */
      g(a) += 1.0 - arma::sum(g);

      // randomly select an element from the discrete distribution represented by g
      arma::uword j = util::sample_discrete(g);
      if (a != j){
      	// will update these in the velocity_rescale call
      	hopping = true;
      	target_state = j;
      }
    }  // end hopping check
  }  // end loop over dtq
}

// For use within the velocity_rescale() call; Jain steps 5 & 6
// returns energy gap if the hop succeeds without frustration
double FSSH::hop_and_scale(arma::mat &total_gradient, arma::mat &velocities, const arma::vec &m){
  if (! hopping){
    throw std::logic_error("Should not be attempting a hop right now!");
  }

  bool hop_succeeds = false;

  /*
    Recall that in our implementation, the kinetic energy reservoir to
    balance a hop includes the MM atoms, a region of arbitrary
    size. However, the kinetic energy is only available in proportion
    to the NAC vector, which is properly computed over the MM region
    (Thank you, Ou Qi!)  and, we believe, decays with distance. See
    the calculation of vd below.
  */

  nac.set_size(3, NQM() + NMM());

  arma::mat qmg_new, mmg_new;
  qmg_new.set_size(3,NQM());

  {
    PropMap props{};
    props.emplace(QMProperty::nacvector, {min_state + active_state, min_state + target_state}, &nac);
    props.emplace(QMProperty::qmgradient, {min_state + target_state}, &qmg_new);

    if (NMM() > 0){
      mmg_new.set_size(3, NMM());
      props.emplace(QMProperty::mmgradient, {min_state + target_state}, &mmg_new);
    }

    qm->get_properties(props);
  }

  // Make 3N vector versions of the NAC, velocity, and new
  // gradient. (m comes in as 3N.)
  // FIXME: How do each of these interact with link atoms?
  const arma::vec & nacv = nac.as_col();

  // FIXME: find a safer way to have a writeable velocity view
  arma::vec vel(velocities.memptr(), 3 * (NQM() + NMM()), false, true);

  arma::vec gradv(3 * (NQM() + NMM()));
  gradv.head(3*NQM()) = arma::vectorise(qmg_new);
  if (NMM() > 0){
    gradv.tail(3*NMM()) = arma::vectorise(mmg_new);
  }

  double deltaE = energy(target_state) - energy(active_state);

  /*
    The hopping energy-conservation equations are documented in
    Vale's GQSH notes dated May 14, 2021.
  */

  double vd  = arma::as_scalar(vel.t() * nacv);
  double dmd = arma::as_scalar(nacv.t() * (nacv / m));

  double discriminant = (vd/dmd)*(vd/dmd) - 2*deltaE/dmd;
  if (discriminant > 0){
    hop_succeeds = true;
    std::cerr << "[FSSH] Hop, " << active_state + min_state << "->"
              << target_state + min_state << " succeeds; energy difference = "
              << deltaE << std::endl;

    // test the sign of dmv to pick the root yielding the smallest value of alpha
    double alpha = (vd > 0 ? 1.0 : -1.0) * std::sqrt(discriminant) - (vd/dmd);
    vel = vel + alpha * (nacv / m); // recall the nacv has dimension of momentum
    active_state = target_state;

    // Update the gradient with the new surface so that GMX can take its second step
    auto rows=arma::span(arma::span::all); auto cols = arma::span(0, NQM() - 1);
    total_gradient(rows, cols) += qmg_new - qm_grd;
    qm_grd = qmg_new;

    if (NMM() > 0){
      cols = arma::span(NQM(), NQM() + NMM() - 1);
      total_gradient(rows, cols) += mmg_new - mm_grd;
      mm_grd = mmg_new;
    }
  }
  else{
    hop_succeeds = false;
    std::cerr << "[FSSH] Hop is frustrated---will remain on " << active_state + min_state << "; ";
    /*
      Momentum reversal along nac as per Jasper, A. W.; Truhlar,
      D. G. Chem. Phys. Lett. 2003, 369, 60--67 c.f. eqns. 1 & 2

      In Jain (2016) a second criterion was imposed. But, in January
      2020 A. Jain indicated to JES that this was not necessary. We
      follow the original Jasper-Truhlar prescription in line with
      Jain's updated advice.
    */
    if (arma::as_scalar((-gradv.t() * nacv)*(nacv.t() * (vel % m))) < 0){
      std::cerr << "velocities reversed." << std::endl;
      const arma::vec nacu = arma::normalise(nacv);
      //vel = vel - 2.0 * (nacu * nacu.t() * (vel % m))/m;
      vel = vel - 2.0 * (nacu / m) * (nacu.t() * (vel % m));
    }
    else{
      std::cerr << "velocities, unchanged." << std::endl;
      // Ignore the unsuccessful hop; active_state remains unchanged
    }
  }

  hopping = false;  // update class-level state
  return hop_succeeds ? deltaE : 0;
}

/*
  Call this function when energy fluctuation tolerance in the MD
  driver is exceeded and we've had a hop. Update the current surface
  and to the total gradient, add (new-old). Similarly, take a step in
  velocity space backwards along the old gradient and then forwards
  along the new one.

  N.B.: When working with Gromacs, it is not necessary to do any
  back-propagation in positions, only velocities. Gromacs's
  velocity-Verlet integrator splits velocity integration over 2 steps
  (so that all of the tooling for leap-frog still works) and then does
  position integration in a single step afterwards. See
  gifs/docs/GromacsVVImplementation.jpg for notes.

  Conversation with Amber in Marhc 2021 indicated that it was faster
  and didn't rely on an ad hoc selection of energy tolerance to simply
  converge properties in dtc directly rather than use this scheme.

  void FSSH::backpropagate_gradient_velocities(
    arma::mat &total_gradient,
    arma::mat &velocities,
    arma::vec &masses);
*/

// This is our primary hook into the Gromacs (or other) MD loop
double FSSH::update_gradient(void){
  qm->update();

  // get gradients and energies
  {
    PropMap props{};
    props.emplace(QMProperty::qmgradient, {min_state + active_state}, &qm_grd);
    props.emplace(QMProperty::mmgradient, {min_state + active_state}, &mm_grd);
    props.emplace(QMProperty::energies, util::range(min_state, min_state + shstates), &energy);
    qm->get_properties(props);
  }

  // write amplitudes
  {
    std::ofstream output(amplitude_file, std::ios_base::app);
    output << active_state + min_state << " ";
    c().st().print(output);
    output.close();
  }

  electronic_evolution();

  return energy(active_state);
}

/*
  rescale_velocities() comes after the first MD half-step (and before
  constraint forces are calculated in gromacs)
*/

// FIXME: should alter velocity rescale interface so we take inverse masses as 3N vector
bool FSSH::rescale_velocities(arma::mat &velocities, arma::vec &masses, arma::mat &total_gradient, double total_energy){
  // call parent to update edrift
  BOMD::rescale_velocities(velocities, masses, total_gradient, total_energy);

  // FIXME: don't fire on the first inf
  if (std::abs(edrift) > delta_e_tol){
    std::cerr << "WARNING, energy drift exceeds tolerance!" << std::endl;
  }

  if (masses.has_inf() || arma::any(masses==0)){
    throw std::logic_error("Cannot do surface hopping with massless atoms or momentum sinks!");
  }

  // 3N vector of masses
  arma::vec m (3 * (NQM() + NMM()), arma::fill::zeros);
  for(arma::uword i = 0 ; i < m.n_elem ; i++){
    // no reason to do this without a bounds [] check!
    m(i) =  masses(i/3);
  }

  bool update = hopping; // copy so hopping can be reset
  double deltaE = 0;
  if (hopping){
    std::cerr <<  "[FSSH] Attempting hop: " << active_state + min_state
              << "->" << target_state + min_state << std::endl;

    deltaE = hop_and_scale(total_gradient, velocities, m);

    if (decoherence && deltaE > 0){
      decoherence->hopped(c, active_state);
    }
  }

  if (decoherence){
    decoherence->decohere(c, U, active_state, velocities.as_col(), m);
  }

  // update indicates we need to copy velocity and gradient back to GMX
  return update;
}


#ifndef SHIFT_RC_IMAGE_UTIL_CONVERT_HPP
#define SHIFT_RC_IMAGE_UTIL_CONVERT_HPP

#include <cstdint>
#include <type_traits>
#include <boost/assert.hpp>
#include <shift/core/mpl.hpp>
#include <shift/math/half.hpp>
#include <shift/math/vector.hpp>
#include "shift/rc/image_util/linear_image_view.hpp"
#include "shift/rc/image_util/convert_pixel_component.hpp"

namespace shift::rc::image_util
{
namespace detail
{
  template <typename T, typename PixelChannel>
  struct unorm_converter
  {
    static_assert(std::is_integral_v<T> && std::is_unsigned_v<T>);
    static_assert(!PixelChannel::is_block_format);

    static constexpr float to_float(T value)
    {
      return value / static_cast<float>((1 << PixelChannel::size_in_bits) - 1);
    }

    static constexpr T from_float(float value)
    {
      return static_cast<T>(value * ((1 << PixelChannel::size_in_bits) - 1));
    }
  };

  template <typename T, typename PixelChannel>
  struct snorm_converter
  {
    static_assert(std::is_integral_v<T> && std::is_signed_v<T>);
    static_assert(!PixelChannel::is_block_format);

    static constexpr float to_float(T value)
    {
      static_assert(
        static_cast<T>(std::numeric_limits<std::make_unsigned_t<T>>::max()
                       << 1) == -2);
      // The minimum value of a PixelChannel::size_in_bits sized signed integer,
      // sign extended and stored in a value of type T.
      constexpr T min_value =
        static_cast<T>(std::numeric_limits<std::make_unsigned_t<T>>::max()
                       << (PixelChannel::size_in_bits - 1));
      if (value == min_value)
      {
        return -1.0f;
      }
      else
      {
        return value /
               static_cast<float>((1 << (PixelChannel::size_in_bits - 1)) - 1);
      }
    }

    static constexpr T from_float(float value)
    {
      return static_cast<T>(value *
                            ((1 << (PixelChannel::size_in_bits - 1)) - 1));
    }
  };

  struct srgb_converter
  {
    static constexpr float to_linear(float srgb_value)
    {
      if (srgb_value <= 0.0f)
        return 0.0f;
      else if (srgb_value < 0.04045f)
        return srgb_value / 12.92f;
      else if (srgb_value < 1.0f)
        return std::pow((srgb_value + 0.055f) / 1.055f, 2.4f);
      else
        return 1.0f;
    }

    static constexpr float from_linear(float linear_value)
    {
      if (linear_value <= 0.0f)
        return 0.0f;
      else if (linear_value < 0.0031308f)
        return linear_value * 12.92f;
      else if (linear_value < 1.0f)
        return std::pow(linear_value, 1.0f / 2.4f) * 1.055f - 0.055f;
      else
        return 1.0f;
    }
  };

  template <typename DestinationPixel, typename SourcePixel>
  struct channel_converter
  {
    template <typename DestinationChannel>
    void operator()(const DestinationChannel*,
                    DestinationPixel& destination_pixel,
                    const SourcePixel& source_pixel) const
    {
      using destination_t = typename DestinationPixel::component_t;
      using source_t = typename SourcePixel::component_t;

      constexpr auto destination_channel_index =
        channel_index_v<typename DestinationPixel::channels_t,
                        DestinationChannel>;
      static_assert(destination_channel_index <
                    DestinationPixel::channel_count);
      using destination_channel_t =
        core::get_type_opt_t<destination_channel_index,
                             typename DestinationPixel::channels_t>;

      constexpr auto source_channel_index =
        channel_index_v<typename SourcePixel::channels_t, DestinationChannel>;
      using source_channel_t =
        core::get_type_opt_t<source_channel_index,
                             typename SourcePixel::channels_t>;

      if constexpr (source_channel_index >= SourcePixel::channel_count)
      {
        // There is no matching source channel. We have to fill the destination
        // with a default value.
        destination_pixel[destination_channel_index] = destination_t{};
      }
      else if constexpr ((SourcePixel::data_type ==
                          DestinationPixel::data_type) &&
                         std::is_same_v<source_t, destination_t> &&
                         std::is_same_v<source_channel_t,
                                        destination_channel_t>)
      {
        // Source and destination channels are of the same channel type and
        // data type, so we can perform a direct copy.
        destination_pixel[destination_channel_index] =
          source_pixel[source_channel_index];
      }
      else if constexpr (SourcePixel::data_type == pixel_data_type::unorm)
      {
        if constexpr (DestinationPixel::data_type == pixel_data_type::unorm)
        {
          // Convert unorm to float.
          auto temp = unorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::snorm)
        {
          // Convert unorm to float.
          auto temp = unorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Convert float to snorm.
          destination_pixel[destination_channel_index] =
            snorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                             pixel_data_type::ufloat ||
                           DestinationPixel::data_type ==
                             pixel_data_type::sfloat)
        {
          // Convert unorm to float.
          destination_pixel[destination_channel_index] =
            unorm_converter<source_t, source_channel_t>::to_float(
              source_pixel[source_channel_index]);
        }
        else if constexpr (DestinationPixel::data_type == pixel_data_type::srgb)
        {
          // Convert unorm to float.
          auto temp = unorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Convert color space from linear to sRGB.
          temp = srgb_converter::from_linear(temp);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else
          BOOST_ASSERT(false);
      }
      else if constexpr (SourcePixel::data_type == pixel_data_type::snorm)
      {
        if constexpr (DestinationPixel::data_type == pixel_data_type::unorm)
        {
          // Convert snorm to float.
          auto temp = snorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Clamp values < 0.0f.
          temp = std::max(temp, 0.0f);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::snorm)
        {
          // Convert snorm to float.
          auto temp = snorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Convert float to snorm.
          destination_pixel[destination_channel_index] =
            snorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                             pixel_data_type::ufloat ||
                           DestinationPixel::data_type ==
                             pixel_data_type::sfloat)
        {
          // Convert snorm -> float.
          destination_pixel[destination_channel_index] =
            snorm_converter<source_t, source_channel_t>::to_float(
              source_pixel[source_channel_index]);
        }
        else if constexpr (DestinationPixel::data_type == pixel_data_type::srgb)
        {
          // Convert snorm -> float.
          auto temp = snorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Clamp values < 0.0f.
          temp = std::max(temp, 0.0f);
          // Convert color space from linear to sRGB.
          temp = srgb_converter::from_linear(temp);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else
          BOOST_ASSERT(false);
      }
      else if constexpr (SourcePixel::data_type == pixel_data_type::ufloat)
      {
        if constexpr (DestinationPixel::data_type == pixel_data_type::unorm)
        {
          // Clamp values above 1.0f.
          auto temp = std::min(
            static_cast<float>(source_pixel[source_channel_index]), 1.0f);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::snorm)
        {
          // Clamp values above 1.0f.
          auto temp = std::min(
            static_cast<float>(source_pixel[source_channel_index]), 1.0f);
          // Convert float to snorm.
          destination_pixel[destination_channel_index] =
            snorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::ufloat)
        {
          // Simply copy value.
          destination_pixel[destination_channel_index] =
            static_cast<float>(source_pixel[source_channel_index]);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::sfloat)
        {
          // Simply copy value.
          destination_pixel[destination_channel_index] =
            static_cast<destination_t>(source_pixel[source_channel_index]);
        }
        else if constexpr (DestinationPixel::data_type == pixel_data_type::srgb)
        {
          // Clamp values above 1.0f.
          auto temp = std::min(
            static_cast<float>(source_pixel[source_channel_index]), 1.0f);
          // Convert color space from linear to sRGB.
          temp = srgb_converter::from_linear(temp);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else
          BOOST_ASSERT(false);
      }
      else if constexpr (SourcePixel::data_type == pixel_data_type::sfloat)
      {
        if constexpr (DestinationPixel::data_type == pixel_data_type::unorm)
        {
          // Clamp values between 0.0f and 1.0f.
          auto temp = std::clamp(
            static_cast<float>(source_pixel[source_channel_index]), 0.0f, 1.0f);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::snorm)
        {
          // Clamp values between -1.0f and 1.0f.
          auto temp =
            std::clamp(static_cast<float>(source_pixel[source_channel_index]),
                       -1.0f, 1.0f);
          // Convert float to snorm.
          destination_pixel[destination_channel_index] =
            snorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::ufloat)
        {
          // Clamp values below 0.0f.
          destination_pixel[destination_channel_index] = std::max(
            static_cast<float>(source_pixel[source_channel_index]), 0.0f);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::sfloat)
        {
          // Simply copy value.
          destination_pixel[destination_channel_index] =
            static_cast<destination_t>(source_pixel[source_channel_index]);
        }
        else if constexpr (DestinationPixel::data_type == pixel_data_type::srgb)
        {
          // Clamp values between 0.0f and 1.0f.
          auto temp = std::clamp(
            static_cast<float>(source_pixel[source_channel_index]), 0.0f, 1.0f);
          // Convert color space from linear to sRGB.
          temp = srgb_converter::from_linear(temp);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else
          BOOST_ASSERT(false);
      }
      else if constexpr (SourcePixel::data_type == pixel_data_type::srgb)
      {
        if constexpr (DestinationPixel::data_type == pixel_data_type::unorm)
        {
          // Convert unorm to float.
          auto temp = unorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Convert color space from sRGB to linear.
          temp = srgb_converter::to_linear(temp);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                           pixel_data_type::snorm)
        {
          // Convert unorm to float.
          auto temp = unorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Convert color space from sRGB to linear.
          temp = srgb_converter::to_linear(temp);
          // Convert float to snorm.
          destination_pixel[destination_channel_index] =
            snorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else if constexpr (DestinationPixel::data_type ==
                             pixel_data_type::ufloat ||
                           DestinationPixel::data_type ==
                             pixel_data_type::sfloat)
        {
          // Convert unorm to float.
          auto temp = unorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Convert color space from sRGB to linear.
          destination_pixel[destination_channel_index] =
            srgb_converter::to_linear(temp);
        }
        else if constexpr (DestinationPixel::data_type == pixel_data_type::srgb)
        {
          // Convert unorm to float.
          auto temp = unorm_converter<source_t, source_channel_t>::to_float(
            source_pixel[source_channel_index]);
          // Convert float to unorm.
          destination_pixel[destination_channel_index] =
            unorm_converter<destination_t, destination_channel_t>::from_float(
              temp);
        }
        else
          BOOST_ASSERT(false);
      }
      else
        BOOST_ASSERT(false);
    }
  };
}

/// This class converts a single pixel from one format to another.
template <typename DestinationPixel, typename SourcePixel>
class pixel_converter
{
public:
  using destination_pixel_t = DestinationPixel;
  using source_pixel_t = SourcePixel;

  void operator()(destination_pixel_t& destination,
                  const source_pixel_t& source) const
  {
    if constexpr (std::is_same_v<destination_pixel_t, source_pixel_t>)
      std::memcpy(&destination, &source, destination_pixel_t::size_in_bytes);
    else
    {
      // Convert each destination pixel channel separately.
      core::for_each<typename DestinationPixel::channels_t>(
        detail::channel_converter<DestinationPixel, SourcePixel>{}, destination,
        source);
    }
  }
};
}

#endif


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/global_pose_estimation/linear_position_estimator.h"

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <glog/logging.h>
#include <algorithm>
#include <unordered_map>
#include <unordered_set>
#include <vector>

#include "spectra/include/SymEigsSolver.h"

#include "theia/math/matrix/linear_operator.h"
#include "theia/sfm/find_common_tracks_in_views.h"
#include "theia/sfm/global_pose_estimation/compute_triplet_baseline_ratios.h"
#include "theia/sfm/reconstruction.h"
#include "theia/sfm/types.h"
#include "theia/sfm/view_graph/triplet_extractor.h"
#include "theia/sfm/view_triplet.h"
#include "theia/util/map_util.h"
#include "theia/util/threadpool.h"

namespace theia {

using Eigen::Matrix3d;
using Eigen::Vector3d;

namespace {

// Adds the 3x3 matrix to the triplet list with the top-left corner in the
// sparse matrix corresponding to (row, col).
void Add3x3MatrixToTripletList(
    const Matrix3d& mat,
    const int row,
    const int col,
    std::vector<Eigen::Triplet<double> >* triplet_list) {
  // If col < 0 then we are holding this particular view constant so it should
  // not be added to the linear system.
  if (col < 0) {
    return;
  }

  triplet_list->emplace_back(row + 0, col + 0, mat(0, 0));
  triplet_list->emplace_back(row + 1, col + 0, mat(1, 0));
  triplet_list->emplace_back(row + 2, col + 0, mat(2, 0));
  triplet_list->emplace_back(row + 0, col + 1, mat(0, 1));
  triplet_list->emplace_back(row + 1, col + 1, mat(1, 1));
  triplet_list->emplace_back(row + 2, col + 1, mat(2, 1));
  triplet_list->emplace_back(row + 0, col + 2, mat(0, 2));
  triplet_list->emplace_back(row + 1, col + 2, mat(1, 2));
  triplet_list->emplace_back(row + 2, col + 2, mat(2, 2));
}

inline Matrix3d AngleAxisToRotationMatrix(const Vector3d angle_axis) {
  const double angle = angle_axis.norm();
  const Eigen::AngleAxisd rotation_aa(angle, angle_axis / angle);
  return rotation_aa.toRotationMatrix();
}

// Adds a triplet constraint to the linear system. The weight of the constraint
// (w), the global orientations, baseline (ratios), and view triplet information
// are needed to form the constraint.
void AddTripletConstraint(const ViewTriplet& triplet,
                          const int start_row,
                          const std::vector<int> cols,
                          const double w,
                          const std::vector<Vector3d>& orientations,
                          const Vector3d& baselines,
                          std::vector<Eigen::Triplet<double> >* triplet_list) {
  // Relative camera positions.
  const Matrix3d orientation0 = AngleAxisToRotationMatrix(orientations[0]);
  const Matrix3d orientation1 = AngleAxisToRotationMatrix(orientations[1]);
  const Vector3d t01 =
      -orientation0.transpose() * triplet.info_one_two.position_2;
  const Vector3d t02 =
      -orientation0.transpose() * triplet.info_one_three.position_2;
  const Vector3d t12 =
      -orientation1.transpose() * triplet.info_two_three.position_2;

  // Rotations between the translation vectors.
  const Matrix3d r012 =
      Eigen::Quaterniond::FromTwoVectors(t12, -t01).toRotationMatrix();
  const Matrix3d r201 =
      Eigen::Quaterniond::FromTwoVectors(t01, t02).toRotationMatrix();
  const Matrix3d r120 =
      Eigen::Quaterniond::FromTwoVectors(-t02, -t12).toRotationMatrix();

  // Baselines ratios.
  const double s_012 = baselines[0] / baselines[2];
  const double s_201 = baselines[1] / baselines[0];
  const double s_120 = baselines[2] / baselines[1];

  // Assume that t01 is perfect and solve for c2.
  Matrix3d m1 =
      (-s_201 * r201 + r012.transpose() / s_012 + Matrix3d::Identity()) * w;
  Matrix3d m2 =
      (s_201 * r201 - r012.transpose() / s_012 + Matrix3d::Identity()) * w;
  Matrix3d m3 = -2.0 * w * Matrix3d::Identity();
  Add3x3MatrixToTripletList(m1, start_row, cols[0], triplet_list);
  Add3x3MatrixToTripletList(m2, start_row, cols[1], triplet_list);
  Add3x3MatrixToTripletList(m3, start_row, cols[2], triplet_list);

  // Assume t02 is perfect and solve for c1.
  m1 = (-r201.transpose() / s_201 + s_120 * r120 + Matrix3d::Identity()) * w;
  m2 = -2.0 * w * Matrix3d::Identity();
  m3 = (r201.transpose() / s_201 - s_120 * r120 + Matrix3d::Identity()) * w;
  Add3x3MatrixToTripletList(m1, start_row + 3, cols[0], triplet_list);
  Add3x3MatrixToTripletList(m2, start_row + 3, cols[1], triplet_list);
  Add3x3MatrixToTripletList(m3, start_row + 3, cols[2], triplet_list);

  // Assume t12 is perfect and solve for c0.
  m1 = -2.0  * w * Matrix3d::Identity();
  m2 = (-s_012 * r012 + r120.transpose() / s_120 + Matrix3d::Identity()) * w;
  m3 = (s_012 * r012 - r120.transpose() / s_120 + Matrix3d::Identity()) * w;
  Add3x3MatrixToTripletList(m1, start_row + 6, cols[0], triplet_list);
  Add3x3MatrixToTripletList(m2, start_row + 6, cols[1], triplet_list);
  Add3x3MatrixToTripletList(m3, start_row + 6, cols[2], triplet_list);
}

// Returns true if the vector R1 * (c2 - c1) is in the same direction as t_12.
bool VectorsAreSameDirection(const Vector3d& position1,
                             const Vector3d& position2,
                             const Vector3d& rotation1,
                             const Vector3d& relative_position12) {
  const Vector3d global_relative_position =
      (position2 - position1).normalized();
  Vector3d rotated_relative_position;
  ceres::AngleAxisRotatePoint(rotation1.data(),
                              global_relative_position.data(),
                              rotated_relative_position.data());
  return rotated_relative_position.dot(relative_position12) > 0;
}

}  // namespace

LinearPositionEstimator::LinearPositionEstimator(
    const Options& options,
    const Reconstruction& reconstruction)
    : options_(options), reconstruction_(reconstruction) {
  CHECK_GT(options.num_threads, 0);
}

bool LinearPositionEstimator::EstimatePositions(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
  CHECK_NOTNULL(positions)->clear();

  // Extract triplets from the view pairs. As of now, we only consider the
  // largest connected triplet in the viewing graph.
  // TODO(cmsweeney): Utilize all connected triplet graphs.
  VLOG(2) << "Extracting triplets from the viewing graph.";
  TripletExtractor extractor;
  std::vector<std::vector<ViewTriplet> > triplets_vec;
  CHECK(extractor.ExtractTripletsFromViewPairs(view_pairs, &triplets_vec));
  // Find the largest triplet.
  int largest_triplet_graph = 0;
  for (int i = 1; i < triplets_vec.size(); i++) {
    if (triplets_vec[i].size() > triplets_vec[largest_triplet_graph].size()) {
      largest_triplet_graph = i;
    }
  }
  triplets_ = triplets_vec[largest_triplet_graph];

  // Count the number of times each view is in a triplet.
  for (int i = 0; i < triplets_.size(); i++) {
    num_triplets_for_view_[triplets_[i].view_ids[0]] =
        num_triplets_for_view_[triplets_[i].view_ids[0]] + 1;
    num_triplets_for_view_[triplets_[i].view_ids[1]] =
        num_triplets_for_view_[triplets_[i].view_ids[1]] + 1;
    num_triplets_for_view_[triplets_[i].view_ids[2]] =
        num_triplets_for_view_[triplets_[i].view_ids[2]] + 1;

    // Determine the order of the views in the linear system. We subtract 1 from
    // the linear system index so that the first position added to the system
    // will be set constant (index of -1 is intentionally not evaluated later).
    InsertIfNotPresent(&linear_system_index_,
                       triplets_[i].view_ids[0],
                       linear_system_index_.size() - 1);
    InsertIfNotPresent(&linear_system_index_,
                       triplets_[i].view_ids[1],
                       linear_system_index_.size() - 1);
    InsertIfNotPresent(&linear_system_index_,
                       triplets_[i].view_ids[2],
                       linear_system_index_.size() - 1);
  }

  VLOG(2) << "Determining baseline ratios within each triplet...";
  // Baselines where (x, y, z) corresponds to the baseline of the first, second,
  // and third view pair in the triplet.
  std::vector<Vector3d> baselines(triplets_.size());
  std::unique_ptr<ThreadPool> pool(new ThreadPool(options_.num_threads));
  for (int i = 0; i < triplets_.size(); i++) {
    pool->Add(&LinearPositionEstimator::ComputeBaselineRatioForTriplet,
              this,
              triplets_[i],
              &baselines[i]);
  }
  // Wait for the baseline ratio computation to finish.
  pool.reset(nullptr);

  VLOG(2) << "Building the constraint matrix...";
  // Create the linear system based on triplet constraints.
  Eigen::SparseMatrix<double> constraint_matrix;
  CreateLinearSystem(orientations, baselines, &constraint_matrix);
  // Try to remove any 0 elements that were accidentally added.
  constraint_matrix.makeCompressed();

  const Eigen::SparseMatrix<double> aTa(constraint_matrix.transpose() *
                                        constraint_matrix);

  // Solve for positions by examining the smallest eigenvalues. Since we have
  // set one position constant at the origin, we only need to solve for the
  // eigenvector corresponding to the smallest eigenvalue. This can be done
  // efficiently with inverse power iterations.
  VLOG(2) << "Solving for positions from the sparse eigenvalue problem...";
  SparseSymShiftSolveLLT op(aTa);
  Spectra::SymEigsShiftSolver<double, Spectra::LARGEST_MAGN,
                              SparseSymShiftSolveLLT> eigs(&op, 1, 6, 0.0);
  eigs.init();
  eigs.compute();
  // Compute with power iterations.
  const Eigen::VectorXd solution = eigs.eigenvectors().col(0);

  // Add the solutions to the output. Set the position with an index of -1 to
  // be at the origin.
  for (const auto& view_index : linear_system_index_) {
    if (view_index.second < 0) {
      (*positions)[view_index.first].setZero();
    } else {
      (*positions)[view_index.first] =
          solution.segment<3>(view_index.second * 3);
    }
  }

  // Flip the sign of the positions if necessary.
  FlipSignOfPositionsIfNecessary(orientations, positions);

  return true;
}

void LinearPositionEstimator::ComputeBaselineRatioForTriplet(
    const ViewTriplet& triplet, Vector3d* baseline) {
  baseline->setZero();

  const View& view1 = *reconstruction_.View(triplet.view_ids[0]);
  const View& view2 = *reconstruction_.View(triplet.view_ids[1]);
  const View& view3 = *reconstruction_.View(triplet.view_ids[2]);

  // Find common tracks.
  const std::vector<ViewId> triplet_view_ids = {
      triplet.view_ids[0], triplet.view_ids[1], triplet.view_ids[2]};
  const std::vector<TrackId>& common_tracks =
      FindCommonTracksInViews(reconstruction_, triplet_view_ids);

  // Normalize all features.
  std::vector<Feature> feature1, feature2, feature3;
  feature1.reserve(common_tracks.size());
  feature2.reserve(common_tracks.size());
  feature3.reserve(common_tracks.size());
  for (const TrackId track_id : common_tracks) {
    feature1.emplace_back(GetNormalizedFeature(view1, track_id));
    feature2.emplace_back(GetNormalizedFeature(view2, track_id));
    feature3.emplace_back(GetNormalizedFeature(view3, track_id));
  }

  // Get the baseline ratios.
  ComputeTripletBaselineRatios(triplet, feature1, feature2, feature3, baseline);
}

// Sets up the linear system with the constraints that each triplet adds.
void LinearPositionEstimator::CreateLinearSystem(
    const std::unordered_map<ViewId, Vector3d>& orientations,
    const std::vector<Vector3d>& baselines,
    Eigen::SparseMatrix<double>* constraint_matrix) {
  const int num_views = num_triplets_for_view_.size();

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

  int num_valid_triplets = 0;
  for (int i = 0; i < triplets_.size(); i++) {
    // If we were not able to extract a stable baseline for this triplet then
    // skip this triplet.
    if (baselines[i] == Eigen::Vector3d::Zero()) {
      continue;
    }

    const ViewTriplet& triplet = triplets_[i];
    const std::vector<Vector3d> triplet_orientations = {
      FindOrDie(orientations, triplet.view_ids[0]),
      FindOrDie(orientations, triplet.view_ids[1]),
      FindOrDie(orientations, triplet.view_ids[2])
    };
    // Get the row and columns that we will modify.
    const std::vector<int> cols = {
        static_cast<int>(3 *
                         FindOrDie(linear_system_index_, triplet.view_ids[0])),
        static_cast<int>(3 *
                         FindOrDie(linear_system_index_, triplet.view_ids[1])),
        static_cast<int>(3 *
                         FindOrDie(linear_system_index_, triplet.view_ids[2]))};

    const double w =
        1.0 / sqrt(std::min({num_triplets_for_view_[triplet.view_ids[0]],
                             num_triplets_for_view_[triplet.view_ids[1]],
                             num_triplets_for_view_[triplet.view_ids[2]]}));
    AddTripletConstraint(triplet,
                         9 * num_valid_triplets,
                         cols,
                         w,
                         triplet_orientations,
                         baselines[i],
                         &triplet_list);
    ++num_valid_triplets;
  }
  // One position is set constant, which is why we use (num_views - 1) * 3
  // columns.
  constraint_matrix->resize(num_valid_triplets * 9, (num_views - 1) * 3);
  constraint_matrix->setFromTriplets(triplet_list.begin(), triplet_list.end());
}

Feature LinearPositionEstimator::GetNormalizedFeature(const View& view,
                                                      const TrackId track_id) {
  Feature normalized_feature = *view.GetFeature(track_id);
  const Camera& camera = view.Camera();
  normalized_feature.y() = (normalized_feature.y() - camera.PrincipalPointY()) /
                           (camera.FocalLength() * camera.AspectRatio());
  normalized_feature.x() =
      (normalized_feature.x() - camera.Skew() * normalized_feature.y() -
       camera.PrincipalPointX()) /
      camera.FocalLength();
  return normalized_feature;
}

void LinearPositionEstimator::FlipSignOfPositionsIfNecessary(
    const std::unordered_map<ViewId, Vector3d>& orientation,
    std::unordered_map<ViewId, Vector3d>* positions) {
  // If this value is below zero, then we should flip the sign.
  int correct_sign_votes = 0;

  std::unordered_set<ViewIdPair> pairs_visited;
  for (const ViewTriplet& triplet : triplets_) {
    const ViewIdPair id_pair_12(triplet.view_ids[0], triplet.view_ids[1]);
    const ViewIdPair id_pair_13(triplet.view_ids[0], triplet.view_ids[2]);
    const ViewIdPair id_pair_23(triplet.view_ids[1], triplet.view_ids[2]);

    // It is not guaranteed that these positions are needed, but it should be
    // very fast to fetch them regardless so this will not be a significant
    // performance cost.
    const Vector3d& position1 = FindOrDie(*positions, triplet.view_ids[0]);
    const Vector3d& position2 = FindOrDie(*positions, triplet.view_ids[1]);
    const Vector3d& position3 = FindOrDie(*positions, triplet.view_ids[2]);

    // Check the relative translation of views 1 and 2 in the triplet.
    if (!ContainsKey(pairs_visited, id_pair_12)) {
      pairs_visited.insert(id_pair_12);
      correct_sign_votes +=
          (VectorsAreSameDirection(position1, position2,
                                   FindOrDie(orientation, id_pair_12.first),
                                   triplet.info_one_two.position_2))
              ? 1
              : -1;
    }

    // Check the relative translation of views 1 and 3 in the triplet.
    if (!ContainsKey(pairs_visited, id_pair_13)) {
      pairs_visited.insert(id_pair_13);
      correct_sign_votes +=
          (VectorsAreSameDirection(position1, position3,
                                   FindOrDie(orientation, id_pair_13.first),
                                   triplet.info_one_three.position_2))
              ? 1
              : -1;
    }

    // Check the relative translation of views 2 and 3 in the triplet.
    if (!ContainsKey(pairs_visited, id_pair_23)) {
      pairs_visited.insert(id_pair_23);
      correct_sign_votes +=
          (VectorsAreSameDirection(position2, position3,
                                   FindOrDie(orientation, id_pair_23.first),
                                   triplet.info_two_three.position_2))
              ? 1
              : -1;
    }
  }

  // If the sign of the votes is below zero, we must flip the sign of all
  // position estimates.
  if (correct_sign_votes < 0) {
    const int num_correct_votes =
        (pairs_visited.size() + correct_sign_votes) / 2;
    VLOG(2) << "Sign of the positions was incorrect: " << num_correct_votes
            << " of " << pairs_visited.size()
            << " relative translations had the correct sign. "
               "Flipping the sign of the camera positions.";
    for (auto& position : *positions) {
      position.second *= -1.0;
    }
  }
}

}  // namespace theia


//          Copyright (C) 2012, Michele Caini.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

//          Two Graphs Common Spanning Trees Algorithm
//      Based on academic article of Mint, Read and Tarjan
//     Efficient Algorithm for Common Spanning Tree Problem
// Electron. Lett., 28 April 1983, Volume 19, Issue 9, p.346-347

#ifndef BOOST_GRAPH_TWO_GRAPHS_COMMON_SPANNING_TREES_HPP
#define BOOST_GRAPH_TWO_GRAPHS_COMMON_SPANNING_TREES_HPP

#include <boost/config.hpp>

#include <boost/bimap.hpp>
#include <boost/type_traits.hpp>
#include <boost/concept/requires.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/undirected_dfs.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <vector>
#include <stack>
#include <map>

namespace boost
{

namespace detail
{

    template < typename TreeMap, typename PredMap, typename DistMap,
        typename LowMap, typename Buffer >
    struct bridges_visitor : public default_dfs_visitor
    {
        bridges_visitor(TreeMap tree, PredMap pred, DistMap dist, LowMap low,
            Buffer& buffer)
        : mTree(tree), mPred(pred), mDist(dist), mLow(low), mBuffer(buffer)
        {
            mNum = -1;
        }

        template < typename Vertex, typename Graph >
        void initialize_vertex(const Vertex& u, const Graph& g)
        {
            put(mPred, u, u);
            put(mDist, u, -1);
        }

        template < typename Vertex, typename Graph >
        void discover_vertex(const Vertex& u, const Graph& g)
        {
            put(mDist, u, ++mNum);
            put(mLow, u, get(mDist, u));
        }

        template < typename Edge, typename Graph >
        void tree_edge(const Edge& e, const Graph& g)
        {
            put(mPred, target(e, g), source(e, g));
            put(mTree, target(e, g), e);
        }

        template < typename Edge, typename Graph >
        void back_edge(const Edge& e, const Graph& g)
        {
            put(mLow, source(e, g),
                (std::min)(get(mLow, source(e, g)), get(mDist, target(e, g))));
        }

        template < typename Vertex, typename Graph >
        void finish_vertex(const Vertex& u, const Graph& g)
        {
            Vertex parent = get(mPred, u);
            if (get(mLow, u) > get(mDist, parent))
                mBuffer.push(get(mTree, u));
            put(mLow, parent, (std::min)(get(mLow, parent), get(mLow, u)));
        }

        TreeMap mTree;
        PredMap mPred;
        DistMap mDist;
        LowMap mLow;
        Buffer& mBuffer;
        int mNum;
    };

    template < typename Buffer >
    struct cycle_finder : public base_visitor< cycle_finder< Buffer > >
    {
        typedef on_back_edge event_filter;
        cycle_finder() : mBuffer(0) {}
        cycle_finder(Buffer* buffer) : mBuffer(buffer) {}
        template < typename Edge, typename Graph >
        void operator()(const Edge& e, const Graph& g)
        {
            if (mBuffer)
                mBuffer->push(e);
        }
        Buffer* mBuffer;
    };

    template < typename DeletedMap > struct deleted_edge_status
    {
        deleted_edge_status() {}
        deleted_edge_status(DeletedMap map) : mMap(map) {}
        template < typename Edge > bool operator()(const Edge& e) const
        {
            return (!get(mMap, e));
        }
        DeletedMap mMap;
    };

    template < typename InLMap > struct inL_edge_status
    {
        inL_edge_status() {}
        inL_edge_status(InLMap map) : mMap(map) {}
        template < typename Edge > bool operator()(const Edge& e) const
        {
            return get(mMap, e);
        }
        InLMap mMap;
    };

    template < typename Graph, typename Func, typename Seq, typename Map >
    void rec_two_graphs_common_spanning_trees(const Graph& iG,
        bimap< bimaps::set_of< int >,
            bimaps::set_of< typename graph_traits< Graph >::edge_descriptor > >
            iG_bimap,
        Map aiG_inL, Map diG, const Graph& vG,
        bimap< bimaps::set_of< int >,
            bimaps::set_of< typename graph_traits< Graph >::edge_descriptor > >
            vG_bimap,
        Map avG_inL, Map dvG, Func func, Seq inL)
    {
        typedef graph_traits< Graph > GraphTraits;

        typedef typename GraphTraits::vertex_descriptor vertex_descriptor;
        typedef typename GraphTraits::edge_descriptor edge_descriptor;

        typedef typename Seq::size_type seq_size_type;

        int edges = num_vertices(iG) - 1;
        //
        //  [ Michele Caini ]
        //
        //  Using the condition (edges != 0) leads to the accidental submission
        //  of
        //    sub-graphs ((V-1+1)-fake-tree, named here fat-tree).
        //  Remove this condition is a workaround for the problem of fat-trees.
        //  Please do not add that condition, even if it improves performance.
        //
        //  Here is proposed the previous guard (that was wrong):
        //     for(seq_size_type i = 0; (i < inL.size()) && (edges != 0); ++i)
        //
        {
            for (seq_size_type i = 0; i < inL.size(); ++i)
                if (inL[i])
                    --edges;

            if (edges < 0)
                return;
        }

        bool is_tree = (edges == 0);
        if (is_tree)
        {
            func(inL);
        }
        else
        {
            std::map< vertex_descriptor, default_color_type > vertex_color;
            std::map< edge_descriptor, default_color_type > edge_color;

            std::stack< edge_descriptor > iG_buf, vG_buf;
            bool found = false;

            seq_size_type m;
            for (seq_size_type j = 0; j < inL.size() && !found; ++j)
            {
                if (!inL[j] && !get(diG, iG_bimap.left.at(j))
                    && !get(dvG, vG_bimap.left.at(j)))
                {
                    put(aiG_inL, iG_bimap.left.at(j), true);
                    put(avG_inL, vG_bimap.left.at(j), true);

                    undirected_dfs(
                        make_filtered_graph(iG,
                            detail::inL_edge_status< associative_property_map<
                                std::map< edge_descriptor, bool > > >(aiG_inL)),
                        make_dfs_visitor(detail::cycle_finder<
                            std::stack< edge_descriptor > >(&iG_buf)),
                        associative_property_map<
                            std::map< vertex_descriptor, default_color_type > >(
                            vertex_color),
                        associative_property_map<
                            std::map< edge_descriptor, default_color_type > >(
                            edge_color));
                    undirected_dfs(
                        make_filtered_graph(vG,
                            detail::inL_edge_status< associative_property_map<
                                std::map< edge_descriptor, bool > > >(avG_inL)),
                        make_dfs_visitor(detail::cycle_finder<
                            std::stack< edge_descriptor > >(&vG_buf)),
                        associative_property_map<
                            std::map< vertex_descriptor, default_color_type > >(
                            vertex_color),
                        associative_property_map<
                            std::map< edge_descriptor, default_color_type > >(
                            edge_color));

                    if (iG_buf.empty() && vG_buf.empty())
                    {
                        inL[j] = true;
                        found = true;
                        m = j;
                    }
                    else
                    {
                        while (!iG_buf.empty())
                            iG_buf.pop();
                        while (!vG_buf.empty())
                            vG_buf.pop();
                        put(aiG_inL, iG_bimap.left.at(j), false);
                        put(avG_inL, vG_bimap.left.at(j), false);
                    }
                }
            }

            if (found)
            {

                std::stack< edge_descriptor > iG_buf_copy, vG_buf_copy;
                for (seq_size_type j = 0; j < inL.size(); ++j)
                {
                    if (!inL[j] && !get(diG, iG_bimap.left.at(j))
                        && !get(dvG, vG_bimap.left.at(j)))
                    {

                        put(aiG_inL, iG_bimap.left.at(j), true);
                        put(avG_inL, vG_bimap.left.at(j), true);

                        undirected_dfs(
                            make_filtered_graph(iG,
                                detail::inL_edge_status<
                                    associative_property_map<
                                        std::map< edge_descriptor, bool > > >(
                                    aiG_inL)),
                            make_dfs_visitor(detail::cycle_finder<
                                std::stack< edge_descriptor > >(&iG_buf)),
                            associative_property_map< std::map<
                                vertex_descriptor, default_color_type > >(
                                vertex_color),
                            associative_property_map< std::map< edge_descriptor,
                                default_color_type > >(edge_color));
                        undirected_dfs(
                            make_filtered_graph(vG,
                                detail::inL_edge_status<
                                    associative_property_map<
                                        std::map< edge_descriptor, bool > > >(
                                    avG_inL)),
                            make_dfs_visitor(detail::cycle_finder<
                                std::stack< edge_descriptor > >(&vG_buf)),
                            associative_property_map< std::map<
                                vertex_descriptor, default_color_type > >(
                                vertex_color),
                            associative_property_map< std::map< edge_descriptor,
                                default_color_type > >(edge_color));

                        if (!iG_buf.empty() || !vG_buf.empty())
                        {
                            while (!iG_buf.empty())
                                iG_buf.pop();
                            while (!vG_buf.empty())
                                vG_buf.pop();
                            put(diG, iG_bimap.left.at(j), true);
                            put(dvG, vG_bimap.left.at(j), true);
                            iG_buf_copy.push(iG_bimap.left.at(j));
                            vG_buf_copy.push(vG_bimap.left.at(j));
                        }

                        put(aiG_inL, iG_bimap.left.at(j), false);
                        put(avG_inL, vG_bimap.left.at(j), false);
                    }
                }

                // REC
                detail::rec_two_graphs_common_spanning_trees< Graph, Func, Seq,
                    Map >(iG, iG_bimap, aiG_inL, diG, vG, vG_bimap, aiG_inL,
                    dvG, func, inL);

                while (!iG_buf_copy.empty())
                {
                    put(diG, iG_buf_copy.top(), false);
                    put(dvG,
                        vG_bimap.left.at(iG_bimap.right.at(iG_buf_copy.top())),
                        false);
                    iG_buf_copy.pop();
                }
                while (!vG_buf_copy.empty())
                {
                    put(dvG, vG_buf_copy.top(), false);
                    put(diG,
                        iG_bimap.left.at(vG_bimap.right.at(vG_buf_copy.top())),
                        false);
                    vG_buf_copy.pop();
                }

                inL[m] = false;
                put(aiG_inL, iG_bimap.left.at(m), false);
                put(avG_inL, vG_bimap.left.at(m), false);

                put(diG, iG_bimap.left.at(m), true);
                put(dvG, vG_bimap.left.at(m), true);

                std::map< vertex_descriptor, edge_descriptor > tree_map;
                std::map< vertex_descriptor, vertex_descriptor > pred_map;
                std::map< vertex_descriptor, int > dist_map, low_map;

                detail::bridges_visitor<
                    associative_property_map<
                        std::map< vertex_descriptor, edge_descriptor > >,
                    associative_property_map<
                        std::map< vertex_descriptor, vertex_descriptor > >,
                    associative_property_map<
                        std::map< vertex_descriptor, int > >,
                    associative_property_map<
                        std::map< vertex_descriptor, int > >,
                    std::stack< edge_descriptor > >
                iG_vis(associative_property_map<
                           std::map< vertex_descriptor, edge_descriptor > >(
                           tree_map),
                    associative_property_map<
                        std::map< vertex_descriptor, vertex_descriptor > >(
                        pred_map),
                    associative_property_map<
                        std::map< vertex_descriptor, int > >(dist_map),
                    associative_property_map<
                        std::map< vertex_descriptor, int > >(low_map),
                    iG_buf),
                    vG_vis(associative_property_map<
                               std::map< vertex_descriptor, edge_descriptor > >(
                               tree_map),
                        associative_property_map<
                            std::map< vertex_descriptor, vertex_descriptor > >(
                            pred_map),
                        associative_property_map<
                            std::map< vertex_descriptor, int > >(dist_map),
                        associative_property_map<
                            std::map< vertex_descriptor, int > >(low_map),
                        vG_buf);

                undirected_dfs(
                    make_filtered_graph(iG,
                        detail::deleted_edge_status< associative_property_map<
                            std::map< edge_descriptor, bool > > >(diG)),
                    iG_vis,
                    associative_property_map<
                        std::map< vertex_descriptor, default_color_type > >(
                        vertex_color),
                    associative_property_map<
                        std::map< edge_descriptor, default_color_type > >(
                        edge_color));
                undirected_dfs(
                    make_filtered_graph(vG,
                        detail::deleted_edge_status< associative_property_map<
                            std::map< edge_descriptor, bool > > >(dvG)),
                    vG_vis,
                    associative_property_map<
                        std::map< vertex_descriptor, default_color_type > >(
                        vertex_color),
                    associative_property_map<
                        std::map< edge_descriptor, default_color_type > >(
                        edge_color));

                found = false;
                std::stack< edge_descriptor > iG_buf_tmp, vG_buf_tmp;
                while (!iG_buf.empty() && !found)
                {
                    if (!inL[iG_bimap.right.at(iG_buf.top())])
                    {
                        put(aiG_inL, iG_buf.top(), true);
                        put(avG_inL,
                            vG_bimap.left.at(iG_bimap.right.at(iG_buf.top())),
                            true);

                        undirected_dfs(
                            make_filtered_graph(iG,
                                detail::inL_edge_status<
                                    associative_property_map<
                                        std::map< edge_descriptor, bool > > >(
                                    aiG_inL)),
                            make_dfs_visitor(detail::cycle_finder<
                                std::stack< edge_descriptor > >(&iG_buf_tmp)),
                            associative_property_map< std::map<
                                vertex_descriptor, default_color_type > >(
                                vertex_color),
                            associative_property_map< std::map< edge_descriptor,
                                default_color_type > >(edge_color));
                        undirected_dfs(
                            make_filtered_graph(vG,
                                detail::inL_edge_status<
                                    associative_property_map<
                                        std::map< edge_descriptor, bool > > >(
                                    avG_inL)),
                            make_dfs_visitor(detail::cycle_finder<
                                std::stack< edge_descriptor > >(&vG_buf_tmp)),
                            associative_property_map< std::map<
                                vertex_descriptor, default_color_type > >(
                                vertex_color),
                            associative_property_map< std::map< edge_descriptor,
                                default_color_type > >(edge_color));

                        if (!iG_buf_tmp.empty() || !vG_buf_tmp.empty())
                        {
                            found = true;
                        }
                        else
                        {
                            while (!iG_buf_tmp.empty())
                                iG_buf_tmp.pop();
                            while (!vG_buf_tmp.empty())
                                vG_buf_tmp.pop();
                            iG_buf_copy.push(iG_buf.top());
                        }

                        put(aiG_inL, iG_buf.top(), false);
                        put(avG_inL,
                            vG_bimap.left.at(iG_bimap.right.at(iG_buf.top())),
                            false);
                    }
                    iG_buf.pop();
                }
                while (!vG_buf.empty() && !found)
                {
                    if (!inL[vG_bimap.right.at(vG_buf.top())])
                    {
                        put(avG_inL, vG_buf.top(), true);
                        put(aiG_inL,
                            iG_bimap.left.at(vG_bimap.right.at(vG_buf.top())),
                            true);

                        undirected_dfs(
                            make_filtered_graph(iG,
                                detail::inL_edge_status<
                                    associative_property_map<
                                        std::map< edge_descriptor, bool > > >(
                                    aiG_inL)),
                            make_dfs_visitor(detail::cycle_finder<
                                std::stack< edge_descriptor > >(&iG_buf_tmp)),
                            associative_property_map< std::map<
                                vertex_descriptor, default_color_type > >(
                                vertex_color),
                            associative_property_map< std::map< edge_descriptor,
                                default_color_type > >(edge_color));
                        undirected_dfs(
                            make_filtered_graph(vG,
                                detail::inL_edge_status<
                                    associative_property_map<
                                        std::map< edge_descriptor, bool > > >(
                                    avG_inL)),
                            make_dfs_visitor(detail::cycle_finder<
                                std::stack< edge_descriptor > >(&vG_buf_tmp)),
                            associative_property_map< std::map<
                                vertex_descriptor, default_color_type > >(
                                vertex_color),
                            associative_property_map< std::map< edge_descriptor,
                                default_color_type > >(edge_color));

                        if (!iG_buf_tmp.empty() || !vG_buf_tmp.empty())
                        {
                            found = true;
                        }
                        else
                        {
                            while (!iG_buf_tmp.empty())
                                iG_buf_tmp.pop();
                            while (!vG_buf_tmp.empty())
                                vG_buf_tmp.pop();
                            vG_buf_copy.push(vG_buf.top());
                        }

                        put(avG_inL, vG_buf.top(), false);
                        put(aiG_inL,
                            iG_bimap.left.at(vG_bimap.right.at(vG_buf.top())),
                            false);
                    }
                    vG_buf.pop();
                }

                if (!found)
                {

                    while (!iG_buf_copy.empty())
                    {
                        inL[iG_bimap.right.at(iG_buf_copy.top())] = true;
                        put(aiG_inL, iG_buf_copy.top(), true);
                        put(avG_inL,
                            vG_bimap.left.at(
                                iG_bimap.right.at(iG_buf_copy.top())),
                            true);
                        iG_buf.push(iG_buf_copy.top());
                        iG_buf_copy.pop();
                    }
                    while (!vG_buf_copy.empty())
                    {
                        inL[vG_bimap.right.at(vG_buf_copy.top())] = true;
                        put(avG_inL, vG_buf_copy.top(), true);
                        put(aiG_inL,
                            iG_bimap.left.at(
                                vG_bimap.right.at(vG_buf_copy.top())),
                            true);
                        vG_buf.push(vG_buf_copy.top());
                        vG_buf_copy.pop();
                    }

                    // REC
                    detail::rec_two_graphs_common_spanning_trees< Graph, Func,
                        Seq, Map >(iG, iG_bimap, aiG_inL, diG, vG, vG_bimap,
                        aiG_inL, dvG, func, inL);

                    while (!iG_buf.empty())
                    {
                        inL[iG_bimap.right.at(iG_buf.top())] = false;
                        put(aiG_inL, iG_buf.top(), false);
                        put(avG_inL,
                            vG_bimap.left.at(iG_bimap.right.at(iG_buf.top())),
                            false);
                        iG_buf.pop();
                    }
                    while (!vG_buf.empty())
                    {
                        inL[vG_bimap.right.at(vG_buf.top())] = false;
                        put(avG_inL, vG_buf.top(), false);
                        put(aiG_inL,
                            iG_bimap.left.at(vG_bimap.right.at(vG_buf.top())),
                            false);
                        vG_buf.pop();
                    }
                }

                put(diG, iG_bimap.left.at(m), false);
                put(dvG, vG_bimap.left.at(m), false);
            }
        }
    }

} // namespace detail

template < typename Coll, typename Seq > struct tree_collector
{

public:
    BOOST_CONCEPT_ASSERT((BackInsertionSequence< Coll >));
    BOOST_CONCEPT_ASSERT((RandomAccessContainer< Seq >));
    BOOST_CONCEPT_ASSERT((CopyConstructible< Seq >));

    typedef typename Coll::value_type coll_value_type;
    typedef typename Seq::value_type seq_value_type;

    BOOST_STATIC_ASSERT((is_same< coll_value_type, Seq >::value));
    BOOST_STATIC_ASSERT((is_same< seq_value_type, bool >::value));

    tree_collector(Coll& seqs) : mSeqs(seqs) {}

    inline void operator()(Seq seq) { mSeqs.push_back(seq); }

private:
    Coll& mSeqs;
};

template < typename Graph, typename Order, typename Func, typename Seq >
BOOST_CONCEPT_REQUIRES(
    ((RandomAccessContainer< Order >))((IncidenceGraphConcept< Graph >))(
        (UnaryFunction< Func, void, Seq >))(
        (Mutable_RandomAccessContainer< Seq >))(
        (VertexAndEdgeListGraphConcept< Graph >)),
    (void))
two_graphs_common_spanning_trees(const Graph& iG, Order iG_map, const Graph& vG,
    Order vG_map, Func func, Seq inL)
{
    typedef graph_traits< Graph > GraphTraits;

    typedef typename GraphTraits::directed_category directed_category;
    typedef typename GraphTraits::vertex_descriptor vertex_descriptor;
    typedef typename GraphTraits::edge_descriptor edge_descriptor;

    typedef typename GraphTraits::edges_size_type edges_size_type;
    typedef typename GraphTraits::edge_iterator edge_iterator;

    typedef typename Seq::value_type seq_value_type;
    typedef typename Seq::size_type seq_size_type;

    typedef typename Order::value_type order_value_type;
    typedef typename Order::size_type order_size_type;

    BOOST_STATIC_ASSERT((is_same< order_value_type, edge_descriptor >::value));
    BOOST_CONCEPT_ASSERT((Convertible< order_size_type, edges_size_type >));

    BOOST_CONCEPT_ASSERT((Convertible< seq_size_type, edges_size_type >));
    BOOST_STATIC_ASSERT((is_same< seq_value_type, bool >::value));

    BOOST_STATIC_ASSERT((is_same< directed_category, undirected_tag >::value));

    if (num_vertices(iG) != num_vertices(vG))
        return;

    if (inL.size() != num_edges(iG) || inL.size() != num_edges(vG))
        return;

    if (iG_map.size() != num_edges(iG) || vG_map.size() != num_edges(vG))
        return;

    typedef bimaps::bimap< bimaps::set_of< int >,
        bimaps::set_of< order_value_type > >
        bimap_type;
    typedef typename bimap_type::value_type bimap_value;

    bimap_type iG_bimap, vG_bimap;
    for (order_size_type i = 0; i < iG_map.size(); ++i)
        iG_bimap.insert(bimap_value(i, iG_map[i]));
    for (order_size_type i = 0; i < vG_map.size(); ++i)
        vG_bimap.insert(bimap_value(i, vG_map[i]));

    edge_iterator current, last;
    boost::tuples::tie(current, last) = edges(iG);
    for (; current != last; ++current)
        if (iG_bimap.right.find(*current) == iG_bimap.right.end())
            return;
    boost::tuples::tie(current, last) = edges(vG);
    for (; current != last; ++current)
        if (vG_bimap.right.find(*current) == vG_bimap.right.end())
            return;

    std::stack< edge_descriptor > iG_buf, vG_buf;

    std::map< vertex_descriptor, edge_descriptor > tree_map;
    std::map< vertex_descriptor, vertex_descriptor > pred_map;
    std::map< vertex_descriptor, int > dist_map, low_map;

    detail::bridges_visitor< associative_property_map< std::map<
                                 vertex_descriptor, edge_descriptor > >,
        associative_property_map<
            std::map< vertex_descriptor, vertex_descriptor > >,
        associative_property_map< std::map< vertex_descriptor, int > >,
        associative_property_map< std::map< vertex_descriptor, int > >,
        std::stack< edge_descriptor > >
    iG_vis(associative_property_map<
               std::map< vertex_descriptor, edge_descriptor > >(tree_map),
        associative_property_map<
            std::map< vertex_descriptor, vertex_descriptor > >(pred_map),
        associative_property_map< std::map< vertex_descriptor, int > >(
            dist_map),
        associative_property_map< std::map< vertex_descriptor, int > >(low_map),
        iG_buf),
        vG_vis(associative_property_map<
                   std::map< vertex_descriptor, edge_descriptor > >(tree_map),
            associative_property_map<
                std::map< vertex_descriptor, vertex_descriptor > >(pred_map),
            associative_property_map< std::map< vertex_descriptor, int > >(
                dist_map),
            associative_property_map< std::map< vertex_descriptor, int > >(
                low_map),
            vG_buf);

    std::map< vertex_descriptor, default_color_type > vertex_color;
    std::map< edge_descriptor, default_color_type > edge_color;

    undirected_dfs(iG, iG_vis,
        associative_property_map<
            std::map< vertex_descriptor, default_color_type > >(vertex_color),
        associative_property_map<
            std::map< edge_descriptor, default_color_type > >(edge_color));
    undirected_dfs(vG, vG_vis,
        associative_property_map<
            std::map< vertex_descriptor, default_color_type > >(vertex_color),
        associative_property_map<
            std::map< edge_descriptor, default_color_type > >(edge_color));

    while (!iG_buf.empty())
    {
        inL[iG_bimap.right.at(iG_buf.top())] = true;
        iG_buf.pop();
    }
    while (!vG_buf.empty())
    {
        inL[vG_bimap.right.at(vG_buf.top())] = true;
        vG_buf.pop();
    }

    std::map< edge_descriptor, bool > iG_inL, vG_inL;
    associative_property_map< std::map< edge_descriptor, bool > > aiG_inL(
        iG_inL),
        avG_inL(vG_inL);

    for (seq_size_type i = 0; i < inL.size(); ++i)
    {
        if (inL[i])
        {
            put(aiG_inL, iG_bimap.left.at(i), true);
            put(avG_inL, vG_bimap.left.at(i), true);
        }
        else
        {
            put(aiG_inL, iG_bimap.left.at(i), false);
            put(avG_inL, vG_bimap.left.at(i), false);
        }
    }

    undirected_dfs(
        make_filtered_graph(iG,
            detail::inL_edge_status<
                associative_property_map< std::map< edge_descriptor, bool > > >(
                aiG_inL)),
        make_dfs_visitor(
            detail::cycle_finder< std::stack< edge_descriptor > >(&iG_buf)),
        associative_property_map<
            std::map< vertex_descriptor, default_color_type > >(vertex_color),
        associative_property_map<
            std::map< edge_descriptor, default_color_type > >(edge_color));
    undirected_dfs(
        make_filtered_graph(vG,
            detail::inL_edge_status<
                associative_property_map< std::map< edge_descriptor, bool > > >(
                avG_inL)),
        make_dfs_visitor(
            detail::cycle_finder< std::stack< edge_descriptor > >(&vG_buf)),
        associative_property_map<
            std::map< vertex_descriptor, default_color_type > >(vertex_color),
        associative_property_map<
            std::map< edge_descriptor, default_color_type > >(edge_color));

    if (iG_buf.empty() && vG_buf.empty())
    {

        std::map< edge_descriptor, bool > iG_deleted, vG_deleted;
        associative_property_map< std::map< edge_descriptor, bool > > diG(
            iG_deleted);
        associative_property_map< std::map< edge_descriptor, bool > > dvG(
            vG_deleted);

        boost::tuples::tie(current, last) = edges(iG);
        for (; current != last; ++current)
            put(diG, *current, false);
        boost::tuples::tie(current, last) = edges(vG);
        for (; current != last; ++current)
            put(dvG, *current, false);

        for (seq_size_type j = 0; j < inL.size(); ++j)
        {
            if (!inL[j])
            {
                put(aiG_inL, iG_bimap.left.at(j), true);
                put(avG_inL, vG_bimap.left.at(j), true);

                undirected_dfs(
                    make_filtered_graph(iG,
                        detail::inL_edge_status< associative_property_map<
                            std::map< edge_descriptor, bool > > >(aiG_inL)),
                    make_dfs_visitor(
                        detail::cycle_finder< std::stack< edge_descriptor > >(
                            &iG_buf)),
                    associative_property_map<
                        std::map< vertex_descriptor, default_color_type > >(
                        vertex_color),
                    associative_property_map<
                        std::map< edge_descriptor, default_color_type > >(
                        edge_color));
                undirected_dfs(
                    make_filtered_graph(vG,
                        detail::inL_edge_status< associative_property_map<
                            std::map< edge_descriptor, bool > > >(avG_inL)),
                    make_dfs_visitor(
                        detail::cycle_finder< std::stack< edge_descriptor > >(
                            &vG_buf)),
                    associative_property_map<
                        std::map< vertex_descriptor, default_color_type > >(
                        vertex_color),
                    associative_property_map<
                        std::map< edge_descriptor, default_color_type > >(
                        edge_color));

                if (!iG_buf.empty() || !vG_buf.empty())
                {
                    while (!iG_buf.empty())
                        iG_buf.pop();
                    while (!vG_buf.empty())
                        vG_buf.pop();
                    put(diG, iG_bimap.left.at(j), true);
                    put(dvG, vG_bimap.left.at(j), true);
                }

                put(aiG_inL, iG_bimap.left.at(j), false);
                put(avG_inL, vG_bimap.left.at(j), false);
            }
        }

        int cc = 0;

        std::map< vertex_descriptor, int > com_map;
        cc += connected_components(
            make_filtered_graph(iG,
                detail::deleted_edge_status< associative_property_map<
                    std::map< edge_descriptor, bool > > >(diG)),
            associative_property_map< std::map< vertex_descriptor, int > >(
                com_map));
        cc += connected_components(
            make_filtered_graph(vG,
                detail::deleted_edge_status< associative_property_map<
                    std::map< edge_descriptor, bool > > >(dvG)),
            associative_property_map< std::map< vertex_descriptor, int > >(
                com_map));

        if (cc != 2)
            return;

        // REC
        detail::rec_two_graphs_common_spanning_trees< Graph, Func, Seq,
            associative_property_map< std::map< edge_descriptor, bool > > >(
            iG, iG_bimap, aiG_inL, diG, vG, vG_bimap, aiG_inL, dvG, func, inL);
    }
}

template < typename Graph, typename Func, typename Seq >
BOOST_CONCEPT_REQUIRES(
    ((IncidenceGraphConcept< Graph >))((EdgeListGraphConcept< Graph >)), (void))
two_graphs_common_spanning_trees(
    const Graph& iG, const Graph& vG, Func func, Seq inL)
{
    typedef graph_traits< Graph > GraphTraits;

    typedef typename GraphTraits::edge_descriptor edge_descriptor;
    typedef typename GraphTraits::edge_iterator edge_iterator;

    std::vector< edge_descriptor > iGO, vGO;
    edge_iterator curr, last;

    boost::tuples::tie(curr, last) = edges(iG);
    for (; curr != last; ++curr)
        iGO.push_back(*curr);

    boost::tuples::tie(curr, last) = edges(vG);
    for (; curr != last; ++curr)
        vGO.push_back(*curr);

    two_graphs_common_spanning_trees(iG, iGO, vG, vGO, func, inL);
}

} // namespace boost

#endif // BOOST_GRAPH_TWO_GRAPHS_COMMON_SPANNING_TREES_HPP


/*******************************************************************************
 *
 * MIT License
 *
 * Copyright (c) 2019 Advanced Micro Devices, Inc.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 *******************************************************************************/
#ifndef BFLOAT16_H_
#define BFLOAT16_H_
#include <boost/operators.hpp>
#include <iostream>
#include <miopen/config.h>

class bfloat16 : boost::totally_ordered<bfloat16, boost::arithmetic<bfloat16>>
{
    public:
    bfloat16() : data_{0} {}
    explicit bfloat16(float rhs)
    {
        union
        {
            float float_st;
            std::uint32_t bf16_st;
        } bits_st = {rhs};

        // BF16 round and NaN preservation code matches
        // https://github.com/ROCmSoftwarePlatform/rocBLAS/blob/develop/library/include/rocblas_bfloat16.h
        if((~bits_st.bf16_st & 0x7f800000) == 0) // Inf or NaN
        {
            // When all of the exponent bits are 1, the value is Inf or NaN.
            // Inf is indicated by a zero mantissa. NaN is indicated by any nonzero
            // mantissa bit. Quiet NaN is indicated by the most significant mantissa
            // bit being 1. Signaling NaN is indicated by the most significant
            // mantissa bit being 0 but some other bit(s) being 1. If any of the
            // lower 16 bits of the mantissa are 1, we set the least significant bit
            // of the bfloat16 mantissa, in order to preserve signaling NaN in case
            // the bloat16's mantissa bits are all 0.
            if((bits_st.bf16_st & 0xffff) != 0)
            {
                bits_st.bf16_st |= 0x10000; // Preserve signaling NaN
            }
        }
        else
        {
#if MIOPEN_USE_RNE_BFLOAT16 == 1
            // When the exponent bits are not all 1s, then the value is zero, normal,
            // or subnormal. We round the bfloat16 mantissa up by adding 0x7FFF, plus
            // 1 if the least significant bit of the bfloat16 mantissa is 1 (odd).
            // This causes the bfloat16's mantissa to be incremented by 1 if the 16
            // least significant bits of the float mantissa are greater than 0x8000,
            // or if they are equal to 0x8000 and the least significant bit of the
            // bfloat16 mantissa is 1 (odd). This causes it to be rounded to even when
            // the lower 16 bits are exactly 0x8000. If the bfloat16 mantissa already
            // has the value 0x7f, then incrementing it causes it to become 0x00 and
            // the exponent is incremented by one, which is the next higher FP value
            // to the unrounded bfloat16 value. When the bfloat16 value is subnormal
            // with an exponent of 0x00 and a mantissa of 0x7F, it may be rounded up
            // to a normal value with an exponent of 0x01 and a mantissa of 0x00.
            // When the bfloat16 value has an exponent of 0xFE and a mantissa of 0x7F,
            // incrementing it causes it to become an exponent of 0xFF and a mantissa
            // of 0x00, which is Inf, the next higher value to the unrounded value.
            bits_st.bf16_st +=
                (0x7fff + ((bits_st.bf16_st >> 16) & 1)); // Round to nearest, round to even
#else                                                     // truncation
// do nothing
#endif
        }
        data_ = bits_st.bf16_st >> 16;
    }
    operator float() const
    {
        union
        {
            std::uint32_t bf16_st;
            float float_st;
        } bits_st = {data_};

        bits_st.bf16_st = bits_st.bf16_st << 16;
        return bits_st.float_st;
    }

    bfloat16 operator-() const { return bfloat16(-static_cast<float>(*this)); }
    bfloat16 operator+() const { return *this; }

    bfloat16& operator=(const float rhs)
    {
        *this = bfloat16(rhs);
        return *this;
    }
    bfloat16& operator+=(bfloat16 rhs)
    {
        *this = bfloat16(static_cast<float>(*this) + static_cast<float>(rhs));
        return *this;
    }

    bfloat16& operator+=(float rhs)
    {
        *this = bfloat16(static_cast<float>(*this) + rhs);
        return *this;
    }

    bfloat16& operator-=(bfloat16 rhs)
    {
        *this += -rhs;
        return *this;
    }
    bfloat16& operator*=(bfloat16 rhs)
    {
        *this = bfloat16(static_cast<float>(*this) * static_cast<float>(rhs));
        return *this;
    }
    bfloat16& operator*=(float rhs)
    {
        *this = bfloat16(static_cast<float>(*this) * rhs);
        return *this;
    }

    bfloat16& operator/=(bfloat16 rhs)
    {
        *this = bfloat16(static_cast<float>(*this) / static_cast<float>(rhs));
        return *this;
    }
    bool operator<(bfloat16 rhs) const
    {
        return static_cast<float>(*this) < static_cast<float>(rhs);
    }
    bool operator==(bfloat16 rhs) const { return std::equal_to<float>()(*this, rhs); }

    static constexpr bfloat16 generate(uint16_t val) { return bfloat16{val, true}; }

    private:
    constexpr bfloat16(std::uint16_t val, bool) : data_{val} {}

    std::uint16_t data_;
};

namespace std {
template <>
class numeric_limits<bfloat16>
{
    public:
    static constexpr bool is_specialized = true;
    static constexpr bfloat16 min() noexcept { return bfloat16::generate(0x007F); }
    static constexpr bfloat16 max() noexcept { return bfloat16::generate(0x7F7F); }
    static constexpr bfloat16 lowest() noexcept { return bfloat16::generate(0xFF7F); }
    static constexpr bfloat16 epsilon() noexcept { return bfloat16::generate(0x3C00); }
    static constexpr bfloat16 infinity() noexcept { return bfloat16::generate(0x7F80); }
    static constexpr bfloat16 quiet_NaN() noexcept { return bfloat16::generate(0x7FC0); }
    static constexpr bfloat16 signaling_NaN() noexcept { return bfloat16::generate(0x7FC0); }
    static constexpr bfloat16 denorm_min() noexcept { return bfloat16::generate(0); }
};
} // namespace std
#endif


/**************************************************************************\
|
|    Copyright (C) 2009 Marc Stevens
|
|    This program is free software: you can redistribute it and/or modify
|    it under the terms of the GNU General Public License as published by
|    the Free Software Foundation, either version 3 of the License, or
|    (at your option) any later version.
|
|    This program is distributed in the hope that it will be useful,
|    but WITHOUT ANY WARRANTY; without even the implied warranty of
|    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
|    GNU General Public License for more details.
|
|    You should have received a copy of the GNU General Public License
|    along with this program.  If not, see <http://www.gnu.org/licenses/>.
|
\**************************************************************************/

#ifndef HASHCLASH_BOOLEANFUNCTION_HPP
#define HASHCLASH_BOOLEANFUNCTION_HPP

#include <vector>
#include <string>
#include <set>
#include <map>

#include <boost/function.hpp>

#include "types.hpp"
#include "sdr.hpp"
#include "md5detail.hpp"
#include "conditions.hpp"

namespace hashclash {

	typedef triple<bitcondition, bitcondition, bitcondition> bf_conditions;

	struct bf_outcome {
		unsigned char c;

		static const unsigned char fconstant = 1;
		static const unsigned char fplus = 2;
		static const unsigned char fminus = 4;
		bf_outcome(): c(0) {}
		bf_outcome(unsigned char k): c(k) {}
		bf_outcome& operator=(const bf_outcome& r)
		{ c = r.c; return *this; }
		bf_outcome& operator=(unsigned char k)
		{ c = k; return *this; }

		bool constant() const
		{ return (c&fconstant) != 0; }
		bool plus() const
		{ return (c&fplus) != 0; }
		bool minus() const
		{ return (c&fminus) != 0; }
		unsigned size() const
		{
			unsigned w = 0;
			if (constant()) ++w;
			if (plus()) ++w;
			if (minus()) ++w;
			return w;
		}
		uint32 operator()(unsigned index, unsigned b) const
		{
			if (index == 0) {
				if (constant()) return 0;
				if (plus()) return 0+(1<<b);
				return 0-(1<<b);
			} else if (index == 1) {
				if (constant())	{
					if (plus()) return 0+(1<<b);
					return 0-(1<<b);
				} else
					return 0-(1<<b);
			} else
				return 0-(1<<b);
		}
		bitcondition operator[](unsigned index) const
		{
			if (index == 0) {
				if (constant()) return bc_constant;
				if (plus()) return bc_plus;
				return bc_minus;
			} else if (index == 1) {
				if (constant())	{
					if (plus()) return bc_plus;
					return bc_minus;
				} else
					return bc_minus;
			} else
				return bc_minus;
		}
	};

	class booleanfunction {
	public:
		booleanfunction(const boost::function<uint32(uint32,uint32,uint32)>& F, const std::string& description = "");

		const bf_outcome& outcome(bitcondition input1, bitcondition input2, bitcondition input3)
		{ return outcome_table[(input1<<8) + (input2<<4) + input3]; }
		const bf_outcome& outcome(const bf_conditions& c)
		{ return outcome(c.first, c.second, c.third); }

		const bf_conditions& forwardconditions(bitcondition input1, bitcondition input2, bitcondition input3, bitcondition outcome)
		{ return forward_table[(outcome<<12)+(input1<<8)+(input2<<4)+input3]; }
		const bf_conditions& forwardconditions(const bf_conditions& c, bitcondition outcome)
		{ return forwardconditions(c.first, c.second, c.third, outcome); }

		const bf_conditions& backwardconditions(bitcondition input1, bitcondition input2, bitcondition input3, bitcondition outcome)
		{ return backward_table[(outcome<<12)+(input1<<8)+(input2<<4)+input3]; }
		const bf_conditions& backwardconditions(const bf_conditions& c, bitcondition outcome)
		{ return backwardconditions(c.first, c.second, c.third, outcome); }

		uint32 F(uint32 input1, uint32 input2, uint32 input3) const
		{ return f(input1, input2, input3); }

		const std::string& description() const
		{ return f_description; }

	private:
		boost::function<uint32(uint32,uint32,uint32)> f;
		std::string f_description;

		std::vector<bf_outcome> outcome_table;
		std::vector<bf_conditions> forward_table;
		std::vector<bf_conditions> backward_table;

		std::map<bf_conditions, std::set<uint32> > conds_to_values;

		void find_booleanfunction_outcomes(const bf_conditions& cond, bf_outcome& outcomes, std::set<uint32>& values);
		bf_conditions preferred_conditions(const bf_conditions& cond, std::vector<bf_conditions>& vec_conds);
	};

	extern booleanfunction MD5_F_data, MD5_G_data, MD5_H_data, MD5_I_data;

} // namespace hashclash

#endif //HASHCLASH_BOOLEANFUNCTION_HPP


#pragma once

#include "quadrature_base.hpp"

#include <boost/multi_array.hpp>
#include <vector>

namespace boltzmann {

template <typename Q1, typename Q2>
class TensorProductQuadrature : public Quadrature<2>
{
 public:
  typedef Quadrature<2> base_type;
  using base_type::dim;
  using base_type::coord_type;

 public:
  TensorProductQuadrature(const Q1& q1, const Q2& q2);

 private:
  using base_type::pts_;
  using base_type::wts_;
};

// ---------------------------------------------------------------------------
template <typename Q1, typename Q2>
TensorProductQuadrature<Q1, Q2>::TensorProductQuadrature(const Q1& q1, const Q2& q2)
    : base_type(q1.size() * q2.size())
{
  typedef boost::multi_array_ref<double, 2> wts_ref_t;
  typedef boost::multi_array_ref<coord_type, 2> pts_ref_t;

  wts_ref_t wts_ref(this->wts_.data(), boost::extents[q1.size()][q2.size()]);
  pts_ref_t pts_ref(this->pts_.data(), boost::extents[q1.size()][q2.size()]);

  for (unsigned int i = 0; i < q1.size(); ++i) {
    for (unsigned int j = 0; j < q2.size(); ++j) {
      wts_ref[i][j] = q1.wts(i) * q2.wts(j);
      pts_ref[i][j][0] = q1.pts(i);
      pts_ref[i][j][1] = q2.pts(j);
    }
  }
}

// ----------------------------------------------------------------------------
/**
 * @brief Build tensor product quadrature for R^2, from
 *        quad. rules in polar coordinates.
 *        Stores quadrature points as complex number
 *
 * @tparam Q1 Quad. rule in angular direction
 * @tparam Q2 Quad. rule in radial direction
 *
 */
template <typename Q1, typename Q2>
class TensorProductQuadratureC : public Quadrature<2>
{
 public:
  typedef Quadrature<2> base_type;
  using base_type::dim;
  using base_type::coord_type;

 public:
  TensorProductQuadratureC(const Q1& q1, const Q2& q2);

  /**
   * @brief returns quad. points as std::complex<double>
   *
   * @param i index
   */
  const std::complex<double>& ptsC(unsigned int i) const { return ptsC_[i]; }

  const std::vector<std::complex<double> >& ptsC() const { return ptsC_; }
  const std::array<unsigned int, 2>& dims() const { return dims_; }

 private:
  using base_type::pts_;
  using base_type::wts_;
  std::vector<std::complex<double> > ptsC_;
  std::array<unsigned int, 2> dims_;
};

// ---------------------------------------------------------------------------
template <typename Q1, typename Q2>
TensorProductQuadratureC<Q1, Q2>::TensorProductQuadratureC(const Q1& q1, const Q2& q2)
    : base_type(q1.size() * q2.size())
    , dims_({q1.size(), q2.size()})
{
  typedef boost::multi_array_ref<double, 2> wts_ref_t;
  typedef boost::multi_array_ref<coord_type, 2> pts_ref_t;

  wts_ref_t wts_ref(this->wts_.data(), boost::extents[q1.size()][q2.size()]);
  pts_ref_t pts_ref(this->pts_.data(), boost::extents[q1.size()][q2.size()]);

  for (unsigned int i = 0; i < q1.size(); ++i) {
    for (unsigned int j = 0; j < q2.size(); ++j) {
      wts_ref[i][j] = q1.wts(i) * q2.wts(j);
      pts_ref[i][j][0] = q1.pts(i);
      pts_ref[i][j][1] = q2.pts(j);
    }
  }

  std::complex<double> ii(0, 1);
  ptsC_.resize(pts_.size());
  for (unsigned int i = 0; i < pts_.size(); ++i) {
    ptsC_[i] = pts_[i][1] * std::exp(ii * pts_[i][0]);
  }
}

}  // end namespace boltzmann