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

#pragma once

/// @file
/// Classes and functions used for sampling from several distributions
/// used in the MC scheme.

#include <vector>
#include <utility>
#include <tuple>
#include <map>
#include <random>
#include <Eigen/Dense>
#include <pcg_random.hpp>
#include <structures.hpp>
#include <qpoint_grid.hpp>
#include <dos.hpp>
#include <processes.hpp>
#include <deviational_particle.hpp>
#include <mutex>

namespace alma {
/// Generate a random time delta from an exponential distribution
/// @param[in] w - a scattering rate
/// @param[in] rng - a random number generator
/// @return an exponential deviate
inline double random_dt(double w, pcg64& rng) {
    if (w == 0.)
        throw value_error("invalid scattering rate");
    ;
    double u = std::uniform_real_distribution(0., 1.)(rng);
    return -std::log(u) / w;
}

/// Base class for discrete distributions over a q-point grid.
class Grid_distribution {
protected:
    /// Distribution function.
    std::vector<double> cumulative;
    /// Random number generator.
    pcg64 rng;
    /// Vector with particle sign
    std::vector<alma::particle_sign> signs;
    /// The sum of cumulative
    double cumulsum = -1.0;
public:
    /// Number of q points.
    std::size_t nqpoints;
    /// Number of phonon modes at each q point.
    std::size_t nmodes;
    /// Empty constructor
    Grid_distribution() = default;

    /// Constructor.
    /// @param[in] grid - phonon spectrum on a regular grid
    /// @param[in] _rng - a random number generator
    Grid_distribution(const Gamma_grid& grid, pcg64& _rng)
        : rng(_rng), nqpoints(grid.nqpoints),
          nmodes(grid.get_spectrum_at_q(0).omega.size()) {
    }

    /// Trivial virtual destructor.
    virtual ~Grid_distribution() {
    }

    /// Fill the 'cumulative' vector.
    ///
    /// @param[in] p - vector with the unnormalized probabilities
    /// of each q point
    void fill_cumulative(const std::vector<double>& p);

    /// Draw a sample from the distribution.
    ///
    /// @return an array containing the mode number and the
    /// q-point number.
    std::array<std::size_t, 2> sample() {
        double u = std::uniform_real_distribution(0., 1.)(this->rng);
        auto pos = std::lower_bound(
                       this->cumulative.begin(), this->cumulative.end(), u) -
                   this->cumulative.begin();

        std::array<std::size_t, 2> nruter;
        nruter[0] = pos % this->nmodes;
        nruter[1] = pos / this->nmodes;
        return nruter;
    }

    /// @return a tuple containing the mode number, the
    /// q-point number and particle sign
    std::tuple<std::size_t, std::size_t, alma::particle_sign>
    sample_with_sign() {
        double u = std::uniform_real_distribution(0., 1.)(this->rng);
        auto pos = std::lower_bound(
                       this->cumulative.begin(), this->cumulative.end(), u) -
                   this->cumulative.begin();
        return std::make_tuple(
            pos % this->nmodes, pos / this->nmodes, this->signs[pos]);
    }
};

/// Objects of this class allow us to sample from a discrete
/// distribution over a q-point grid with a PMF proportional
/// to cv / tau, where cv is the contribution to the specific heat
/// and tau is the relaxation time.
class BE_derivative_distribution : public Grid_distribution {
public:
    /// Empty constructor

    BE_derivative_distribution() = default;

    /// Constructor.
    ///
    /// @param[in] grid - phonon spectrum on a regular grid
    /// @param[in] w - scattering rates
    /// @param[in] T - the temperature in K
    /// @param[in, out] _rng - a random number generator
    BE_derivative_distribution(const Gamma_grid& grid,
                               const Eigen::Ref<const Eigen::ArrayXXd>& w,
                               double T,
                               pcg64& _rng);
};

/// Objects of this class allow us to sample from a discrete
/// distribution over a q-point grid with a PMF proportional to one
/// component of the group velocity and to each mode's contribution
/// to the specific heat.
class Nabla_T_distribution : public Grid_distribution {
public:
    /// Empty constructor
    Nabla_T_distribution() = default;
    /// Constructor.
    ///
    /// @param[in] grid - phonon spectrum on a regular grid
    /// @param[in] nablaT - any vector parallel to the temperature
    /// gradient.
    /// @param[in] T - the temperature in K
    /// @param[in, out] _rng - a random number generator
    /// @param[in] theta - rotation of material with respect
    /// to x-axis
    Nabla_T_distribution(const Gamma_grid& grid,
                         const Eigen::Ref<const Eigen::Vector3d>& nablaT,
                         double T,
                         pcg64& _rng,
                         double theta = 0.);

    /// Get number of generated particles for given conditions
    ///
    ///@param[in] time - in ps
    ///@param[in] vol  - box_volume/unitcell_volume
    ///@param[in] Eff  - deviational particle energy
    std::size_t Ntogenerate(const double time,
                            const double vol,
                            const double Eff);

    /// Returns energy introduced by the generator per unit of time
    ///@param[in] vol  - box_volume/unitcell_volume
    double get_energy(const double vol);
};

/// Objects of this class allow us to sample from a discrete
/// distribution over a q-point grid given from a vector
class ref_distribution : public Grid_distribution {
private:
    std::mutex shield;
public:
    ///Empty constructor
    ref_distribution() = default;

    ref_distribution(const ref_distribution& that) : Grid_distribution(that) {
    }

    ref_distribution& operator=(const ref_distribution& that) {
        this->cumulative = that.cumulative;
        this->rng        = that.rng;
        this->signs      = that.signs;
        this->cumulsum   = that.cumulsum;
        this->nqpoints   = that.nqpoints;
        this->nmodes     = that.nmodes;
        return *this;
    }

    /// Constructor.
    ///
    /// @param[in] grid - phonon spectrum on a regular grid
    /// @param[in] fd   - the distribution
    /// @param[in, out] _rng - a random number generator
    ref_distribution(const Gamma_grid& grid,
                     const Eigen::VectorXd&  fd,
                     pcg64& _rng);

    /// Get number of generated particles for given conditions
    ///@param [in] vol - vol_box / volume_unit_cell
    ///@param [in] Eff - packet energy
    std::size_t Ntogenerate(const double vol,
                            const double Eff);

    std::tuple<std::size_t,std::size_t,
        alma::particle_sign> sample_dist() {

        shield.lock();
        auto res = this->sample_with_sign();
        shield.unlock();
        return res;
    }

};

/// Objects of this class allow us to sample from a discrete
/// distribution over a q-point grid with a PMF corresponding to an
/// isothermal wall of given orientation and equilibrium
/// temperature.  They are also useful for simple periodic systems
/// if Teq is set accordingly.
class Isothermal_wall_distribution : public Grid_distribution {
public:
    /// Empty constructor
    Isothermal_wall_distribution() = default;
    /// Constructor.
    ///
    /// @param[in] grid - phonon spectrum on a regular grid
    /// @param[in] Twall - the temperature of the wall in K
    /// @param[in] Teq - the simulation temperature in K
    /// @param[in] normal - a normal vector pointing out
    /// of the wall
    /// @param[in, out] _rng - a random number generator
    /// @param[in] theta - rotation of material with respect
    /// to x-axis
    Isothermal_wall_distribution(
        const Gamma_grid& grid,
        double Twall,
        double Teq,
        const Eigen::Ref<const Eigen::Vector3d>& normal,
        pcg64& _rng,
        double theta = 0.);

    /// Get number of generated particles for given conditions
    ///
    ///@param[in] time - in ps
    ///@param[in] spf  - area_boundary/unitcell_volume
    ///@param[in] Eff  - deviational particle energy
    std::size_t Ntogenerate(const double time,
                            const double spf,
                            const double Eff);

    /// Get the flux:
    ///@param[in] vuc - unitcell_volume
    double get_flux(const double vuc);
};

/// Objects of this class allow us to sample from a discrete
/// distribution over a q-point grid with a temperature
/// different from that of reference
class outTref_distribution : public Grid_distribution {
public:
    /// Empty constructor
    outTref_distribution() = default;
    /// Constructor.
    ///
    /// @param[in] grid - phonon spectrum on a regular grid
    /// @param[in] Tint - the temperature in K
    /// @param[in] Tref - reference temperature
    /// @param[in, out] _rng - a random number generator
    outTref_distribution(const Gamma_grid& grid,
                         double Tinit,
                         double Tref,
                         pcg64& _rng);

    /// Get number of generated particles for given conditions
    ///
    ///@param[in] vol  - box_volume/unitcell_volume
    ///@param[in] Eff  - deviational particle energy
    std::size_t Ntogenerate(const double vol, const double Eff);

    /// Returns energy introduced by the generator
    ///@param[in] vol  - box_volume/unitcell_volume
    double get_energy(const double vol);
};

/// planar_source_distribution
/// Emission probability for outgoing modes is proportional to
/// heat capacity * normal velocity.
class planar_source_distribution : public Grid_distribution {
public:
    /// Constructor.
    ///
    /// @param[in] grid - phonon spectrum on a regular grid
    /// @param[in] Tref - temperature in K at which to compute heat capacities
    /// @param[in] normal - a normal vector pointing out of the source
    /// @param[in, out] _rng - a random number generator
    planar_source_distribution(const Gamma_grid& grid,
                               double Tref,
                               const Eigen::Ref<const Eigen::Vector3d>& normal,
                               pcg64& _rng);
};

/// For layered systems, this class
/// defines the layers of each state
/// and use them to calculate the coupling
/// of two states in two stacks by using localization
/// distribution and Jensen-Shanon divergence

class layer_coupling {
private:
    /// Jensen shanon diveregence plus -1
    /// the function is defined for both STD maps
    /// and STD vectors
    inline double JSDm1(std::map<std::string, double>& P,
                        std::map<std::string, double>& Q) const {
        double PM = 0., QM = 0.;

        for (auto& [layer_id, p] : P) {
            double q = Q.at(layer_id);

            double M = 0.5 * (p + q);

            if (alma::almost_equal(M, 0.)) {
                continue;
            }

            if (!alma::almost_equal(q, 0.)) {
                QM += q * std::log2(q / M);
            }

            if (!alma::almost_equal(p, 0.)) {
                PM += p * std::log2(p / M);
            }
        }

        return std::max({1.0 - (0.5 * QM + 0.5 * PM), 0.});
    }

    inline double JSDm1(std::vector<double>& P, std::vector<double>& Q) const {
        double PM = 0., QM = 0.;

        for (std::size_t ii = 0; ii < P.size(); ii++) {
            double p = P[ii];
            double q = Q[ii];

            double M = 0.5 * (p + q);

            if (alma::almost_equal(M, 0.)) {
                continue;
            }

            if (!alma::almost_equal(q, 0.)) {
                QM += q * std::log2(q / M);
            }

            if (!alma::almost_equal(p, 0.)) {
                PM += p * std::log2(p / M);
            }
        }

        return std::max({1.0 - (0.5 * QM + 0.5 * PM), 0.});
    }

public:
    /// To store for each material the layers
    std::map<std::string, std::map<std::string, std::vector<int>>> layers;

    std::map<std::string, std::string> connection;
    bool stack_injection = false;

    /// Default constructor
    layer_coupling() = default;

    /// Calculates the coupling rate between two states
    double get_injection_rate(Eigen::VectorXcd& wfin_,
                              Eigen::VectorXcd& wfout_,
                              std::string material_in,
                              std::string material_out) const;
};

/// Diffuse mismatch distribution

class Diffuse_mismatch_distribution {
private:
    /// Number of branches, indexed via 'A' and 'B'.
    std::map<char, std::size_t> Nbranches;
    /// Number of phonon modes, indexed via 'A' and 'B'.
    std::map<char, std::size_t> Ntot;
    /// Unit cell volumes, indexed via 'A' and 'B'.
    std::map<char, double> volume;

    /// Material names:
    std::string material_A, material_B;

    /// Tuple describing the origin of each available mode in A or B and its
    /// contribution to the DOS.
    /// The first element is the material side
    /// The second element is a branch index
    /// The third element is a q-point index
    /// The fourth element is the projection of its velocity on the normal to
    /// the surface. The fifth element is the heat capacity The sixth element is
    /// a contribution to the DOS
    using dos_tuple = std::
        tuple<char, std::size_t, std::size_t, double, double, Gaussian_for_DOS>;
    /// Short notation for tuple specifying the material side, mode index, and
    /// cumulative probability
    typedef std::tuple<char, std::size_t, double> lookup_entry;
    /// Lookup table for incident A modes
    std::vector<std::vector<lookup_entry>> lookup_incidentA;
    /// Lookup table for incident B modes
    std::vector<std::vector<lookup_entry>> lookup_incidentB;
    /// Random number generator.
    pcg64 rng;
    /// Reference temperature (for computing heat capacities)
    double Tref;
    /// Gather information about all available phonon modes.
    ///
    /// @param[in] gridA - phonon spectrum of material A
    /// @param[in] gridB - phonon spectrum of material B
    /// @param[in] normal - a normal vector pointing from A to B.
    /// @param[in] scalebroad - factor modulating all the broadenings
    /// @return a vector of dos_tuples describing all modes
    std::vector<dos_tuple> get_modes(
        const Gamma_grid& gridA,
        const Gamma_grid& gridB,
        const Eigen::Ref<const Eigen::Vector3d>& normal,
        double scalebroad);

public:
    /// Empty Constructor.
    ///
    ///
    Diffuse_mismatch_distribution() = default;
    ///
    ///
    /// Constructor.
    ///
    /// @param[in] gridA - phonon spectrum of material A
    /// @param[in] poscarA - crystal structure of material A
    /// @param[in] gridB - phonon spectrum of material B
    /// @param[in] poscarB - crystal structure of material B
    /// @param[in] normal - a normal vector pointing from A to B.
    /// @param[in] scalebroad - factor modulating all the broadenings
    /// @param[in, out] _rng - a random number generator
    /// @param[in] Tref - reference temperature [K]
    /// @param[in] thicknessA - material A thickness for 2D simulations [nm]
    /// @param[in] thicknessB - material B thickness for 2D simulations [nm]
    /// @param[in] coupling - coupling between layers
    Diffuse_mismatch_distribution(
        const Gamma_grid& gridA,
        const Crystal_structure& poscarA,
        const Gamma_grid& gridB,
        const Crystal_structure& poscarB,
        const Eigen::Ref<const Eigen::Vector3d>& normal,
        double scalebroad,
        pcg64& _rng,
        double Tref = 300.0,
        double thicknessA = -1.,
        double thicknessB = -1.,
        layer_coupling coupling = layer_coupling(),
        std::string materialA = "None",
        std::string materialB = "None");

    /// Draw a final state for a particle incident from A or B.
    ///
    /// @param[in] incidence - 'A' or 'B'
    /// @param[in, out] particle - information about the particle, before and
    /// after the interaction.
    /// @param[in] account_for_velocity: if true, emission probability is
    /// proportional to abs(projected velocity).
    /// @return 'A' or 'B', depending on the direction of emission
    char reemit(char incidence, D_particle& particle);
};

/// Objects of this class allow us to simulate completely diffusive
/// interfaces between two media. An incident phonon undergoes
/// elastic diffusion and exits the interface in a mode chosen at
/// random.
///
/// In contrast with Diffuse_mismatch_distribution, in this case
/// the normal vector is not fixed at construction time.
class Elastic_interface_distribution {
private:
    /// Number of q points in grid A.
    std::size_t nqA;
    /// Number of q points in grid B.
    std::size_t nqB;
    /// Number of branches in grid A.
    std::size_t nmodesA;
    /// Number of branches in grid B.
    std::size_t nmodesB;
    /// Volume of the unit cell in A.
    double VA;
    /// Volume of the unit cell in B.
    double VB;
    /// Tuple describing the origin of each available mode
    /// and its contribution to the DOS.
    /// The first element is either 'A' or 'B'
    /// The second element is a branch index
    /// The third element is a q-point index
    /// The fourth element is a contribution to the DOS
    using dos_tuple =
        std::tuple<char, std::size_t, std::size_t, Gaussian_for_DOS>;
    /// Mode-to-mode cumulative transition probabilities.
    /// Only allowed transitions are considered.
    std::vector<std::vector<double>> cumulative;
    /// Modes to which the elements of cumulative refer.
    std::vector<std::vector<std::size_t>> allowed;
    /// Unit vectors parallel to each of the group velocities
    /// from the original grid. Each column is a 3-vector.
    Eigen::MatrixXd directions;
    /// Random number generator.
    pcg64& rng;
    /// Gather information about all available modes.
    ///
    /// @param[in] gridA - phonon spectrum of material A
    /// @param[in] gridB - phonon spectrum of material B
    /// @param[in] scalebroad - factor modulating all
    /// the broadenings
    /// @return a vector of tuples describing all modes
    std::vector<dos_tuple> get_all_modes(const Gamma_grid& gridA,
                                         const Gamma_grid& gridB,
                                         double scalebroad) const;

public:
    /// Constructor.
    ///
    /// @param[in] gridA - phonon spectrum of material A
    /// @param[in] gridB - phonon spectrum of material B
    /// @param[in] scalebroad - factor modulating all
    /// the broadenings
    /// @param[in, out] _rng - a random number generator
    Elastic_interface_distribution(const Gamma_grid& gridA,
                                   const Gamma_grid& gridB,
                                   double scalebroad,
                                   pcg64& _rng);
    /// Draw a final state for a particle incident from A or B or
    /// viceversa.
    ///
    /// @param[in] incidence - 'A' or 'B'
    /// @param[in] normal - normal to the interface, pointing
    /// from A to B.
    /// @param[in, out] particle - information about the particle,
    /// before and after the interaction.
    /// @return 'A' or 'B', depending on the direction of emission
    char reemit(char incidence,
                const Eigen::Ref<const Eigen::Vector3d>& normal,
                D_particle& particle);
};

// Simple and general class that allows us to simulate completely
// random elastic scattering.
class Elastic_distribution {
private:
    /// Number of q points in the grid.
    std::size_t nq;
    /// Number of branches in the grid..
    std::size_t nmodes;
    /// Tuple describing the origin of each available mode
    /// and its contribution to the DOS.
    /// The first element is a branch index
    /// The second element is a q-point index
    /// The third element is a contribution to the DOS
    using dos_tuple = std::tuple<std::size_t, std::size_t, Gaussian_for_DOS>;
    /// Mode-to-mode cumulative transition probabilities.
    /// Only allowed transitions are considered.
    std::vector<std::vector<double>> cumulative;
    /// Modes to which the elements of cumulative refer.
    std::vector<std::vector<std::size_t>> allowed;
    /// Unit vectors parallel to each of the group velocities
    /// from the original grid. Each column is a 3-vector.
    Eigen::MatrixXd directions;
    /// Random number generator.
    pcg64& rng;
    /// Gather information about all available modes.
    ///
    /// @param[in] grid - description of the phonon spectrum
    /// @param[in] scalebroad - factor modulating all
    /// the broadenings
    /// @return a vector of tuples describing all modes
    std::vector<dos_tuple> get_all_modes(const Gamma_grid& grid,
                                         double scalebroad) const;

public:
    /// Constructor.
    ///
    /// @param[in] grid - phonon spectrum of the material
    /// @param[in] scalebroad - factor modulating all
    /// the broadenings
    /// @param[in, out] _rng - a random number generator
    Elastic_distribution(const Gamma_grid& grid,
                         double scalebroad,
                         pcg64& _rng);
    /// Draw a final state for a particle.
    ///
    /// @param[in, out] particle - information about the particle,
    /// before and after the interaction.
    void scatter(D_particle& particle);

    /// Draw a final state for a particule with the constraint that
    /// one component of its group velocity has a predefined sign.
    ///
    /// @param[in] normal - direction of the projection
    /// @param[in] refsign - an integer with the desired sign
    /// @param[in, out] particle - information about the particle,
    /// before and after the interaction.
    void scatter(const Eigen::Ref<const Eigen::Vector3d>& normal,
                 const int refsign,
                 D_particle& particle);
};
} // namespace alma


#pragma once

#include <Eigen/Dense>

namespace kde1d {

namespace tools {

//! applies a function to each non-NaN value, otherwise returns NaN
//! @param x function argument.
//! @param func function to be applied.
template<typename T>
Eigen::MatrixXd
unaryExpr_or_nan(const Eigen::MatrixXd& x, const T& func)
{
  return x.unaryExpr([&func](double y) {
    if (std::isnan(y)) {
      return std::numeric_limits<double>::quiet_NaN();
    } else {
      return func(y);
    }
  });
}

//! applies a function to each non-NaN value, otherwise returns NaN
//! @param x function argument.
//! @param func function to be applied.
template<typename T>
Eigen::MatrixXd
unaryExpr_or_nan_int(const Eigen::MatrixXi& x, const T& func)
{
  return x.unaryExpr([&func](int y) {
    if (std::isnan(static_cast<double>(y))) {
      return std::numeric_limits<double>::quiet_NaN();
    } else {
      return func(y);
    }
  });
}

//! computes the inverse \f$ f^{-1} \f$ of a function \f$ f \f$ by the
//! bisection method.
//!
//! @param x evaluation points.
//! @param f the function to invert.
//! @param lb lower bound.
//! @param ub upper bound.
//! @param n_iter the number of iterations for the bisection.
//!
//! @return \f$ f^{-1}(x) \f$.
inline Eigen::VectorXd
invert_f(const Eigen::VectorXd& x,
         std::function<Eigen::VectorXd(const Eigen::VectorXd&)> f,
         const double lb,
         const double ub,
         int n_iter)
{
  Eigen::VectorXd xl = Eigen::VectorXd::Constant(x.size(), lb);
  Eigen::VectorXd xh = Eigen::VectorXd::Constant(x.size(), ub);
  Eigen::VectorXd x_tmp = x;
  for (int iter = 0; iter < n_iter; ++iter) {
    x_tmp = (xh + xl) / 2.0;
    Eigen::VectorXd fm = f(x_tmp) - x;
    xl = (fm.array() < 0).select(x_tmp, xl);
    xh = (fm.array() < 0).select(xh, x_tmp);
  }

  return x_tmp;
}

//! remove rows of a matrix which contain nan values or have zero weight
//! @param x the matrix.
//! @param a vector of weights that is either empty or whose size is equal to
//!   the number of columns of x.
inline void
remove_nans(Eigen::VectorXd& x, Eigen::VectorXd& weights)
{
  if ((weights.size() > 0) & (weights.size() != x.rows()))
    throw std::runtime_error("sizes of x and weights don't match.");

  // if an entry is nan or weight is zero, move it to the end
  size_t last = x.size() - 1;
  for (size_t i = 0; i < last + 1; i++) {
    bool is_nan = std::isnan(x(i));
    if (weights.size() > 0) {
      is_nan = is_nan | std::isnan(weights(i));
      is_nan = is_nan | (weights(i) == 0.0);
    }
    if (is_nan) {
      if (weights.size() > 0)
        std::swap(weights(i), weights(last));
      std::swap(x(i--), x(last--));
    }
  }

  // remove nan rows
  x.conservativeResize(last + 1);
  if (weights.size() > 0)
    weights.conservativeResize(last + 1);
}

inline Eigen::Matrix<size_t, Eigen::Dynamic, 1>
get_order(const Eigen::VectorXd& x)
{
  Eigen::Matrix<size_t, Eigen::Dynamic, 1> order(x.size());
  for (long i = 0; i < x.size(); ++i)
    order(i) = i;
  std::stable_sort(
    order.data(),
    order.data() + order.size(),
    [&](const size_t& a, const size_t& b) { return (x[a] < x[b]); });
  return order;
}

//! Computes bin counts for univariate data via the linear binning strategy.
//! @param x vector of observations
//! @param weights vector of weights for each observation.
inline Eigen::VectorXd
linbin(const Eigen::VectorXd& x,
       double lower,
       double upper,
       size_t num_bins,
       const Eigen::VectorXd& weights)
{
  Eigen::VectorXd gcnts = Eigen::VectorXd::Zero(num_bins + 1);
  double delta = (upper - lower) / num_bins;
  double rem, lxi;
  size_t li;
  for (long i = 0; i < x.size(); ++i) {
    lxi = (x(i) - lower) / delta;
    li = static_cast<size_t>(lxi);
    rem = lxi - li;
    if (li < num_bins) {
      gcnts(li) += (1 - rem) * weights(i);
      gcnts(li + 1) += rem * weights(i);
    }
  }

  return gcnts;
}

} // end kde1d tools

} // end kde1d


/*

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

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

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

*/

#ifndef PIC_COMPUTER_VISION_ESSENTIAL_MATRIX_HPP
#define PIC_COMPUTER_VISION_ESSENTIAL_MATRIX_HPP

#include <vector>
#include <random>
#include <stdlib.h>

#include "../base.hpp"

#include "../util/math.hpp"
#include "../util/eigen_util.hpp"

#include "../computer_vision/triangulation.hpp"
#include "../computer_vision/camera_matrix.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Dense"
    #include "../externals/Eigen/SVD"
    #include "../externals/Eigen/Geometry"
#else
    #include <Eigen/Dense>
    #include <Eigen/SVD>
    #include <Eigen/Geometry>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief computeEssentialMatrix computes the essential matrix, E, from the fundamental
 * matrix, F12, and the two intrics matrices K1 and K2
 * @param F is the fundamental matrix which maps points from camera 1 into camera 2
 * @param K1 is the camera 1 intrics matrix
 * @param K2 is the camera 2 intrics matrix
 * @return
 */
PIC_INLINE Eigen::Matrix3d computeEssentialMatrix(Eigen::Matrix3d &F, Eigen::Matrix3d &K1, Eigen::Matrix3d &K2)
{
    Eigen::Matrix3d K2t = Eigen::Transpose<Eigen::Matrix3d>(K2);
    return K2t * F * K1;
}

/**
 * @brief computeEssentialMatrix computes the essential matrix, E, from the fundamental
 * matrix, F, and a single instrics camera, K.
 * @param F
 * @param K
 * @return
 */
PIC_INLINE Eigen::Matrix3d computeEssentialMatrix(Eigen::Matrix3d &F, Eigen::Matrix3d &K)
{
    return computeEssentialMatrix(F, K, K);
}

/**
 * @brief decomposeEssentialMatrix decomposes an essential matrix E.
 * @param E is the essential matrix. Input.
 * Note1: E = S * R
 * Note2: S = [t]_x
 * Note3: there are four possible cases:
 * 1:     [R1 |  t]
 * 2:     [R1 | -t]
 * 3:     [R2 |  t]
 * 4:     [R2 | -t]
 * @param R1 is one possible rotation matrix. Output.
 * @param R2 is one possible rotation matrix. Output.
 * @param t is the translation vector which is not normalized. Output.
 */
PIC_INLINE void decomposeEssentialMatrix(Eigen::Matrix3d &E, Eigen::Matrix3d &R1, Eigen::Matrix3d &R2, Eigen::Vector3d &t)
{
    //Solving the linear system
    Eigen::JacobiSVD< Eigen::MatrixXd > svd(E, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    //Z matrix
    Eigen::Matrix3d Z;
    Z.setZero();
    Z(0, 1) =  1.0;
    Z(1, 0) = -1.0;

    //W matrix
    Eigen::Matrix3d W;
    W.setZero();
    W(0, 1) = -1.0;
    W(1, 0) =  1.0;
    W(2, 2) =  1.0;

    //Rotation matrices R1 and R2
    Eigen::Matrix3d Vt = Eigen::Transpose<Eigen::Matrix3d>(V);
    Eigen::Matrix3d Wt = Eigen::Transpose<Eigen::Matrix3d>(W);

    Eigen::Matrix3d  UVt = U * Vt;
    double detUVt = UVt.determinant();

    R1 = detUVt * U * W  * Vt;
    R2 = detUVt * U * Wt * Vt;

    R1 = RotationMatrixRefinement(R1);
    R2 = RotationMatrixRefinement(R2);

    //Translation vector
    Eigen::Matrix3d Ut = Eigen::Transpose<Eigen::Matrix3d>(U);
    Eigen::Matrix3d S = U * Z * Ut;

    t[0] = S(2, 1);
    t[1] = S(0, 2);
    t[2] = S(1, 0);

    t.normalize();
}

/**
 * @brief decomposeEssentialMatrixWithConfiguration decomposes an essential matrix E.
 * @param E is the essential matrix.
 * @param K0
 * @param K1
 * @param points0
 * @param points1
 * @param R
 * @param t
 * @return
 */
PIC_INLINE bool decomposeEssentialMatrixWithConfiguration(Eigen::Matrix3d &E, Eigen::Matrix3d &K0, Eigen::Matrix3d &K1,
                                               std::vector< Eigen::Vector2f > &points0, std::vector< Eigen::Vector2f > &points1,
                                               Eigen::Matrix3d &R, Eigen::Vector3d &t)
{
    if(points0.size() != points1.size()) {
        return false;
    }

    Eigen::Matrix3d R0, R1;
    Eigen::Vector3d T;
    decomposeEssentialMatrix(E, R0, R1, T);

    //for each configuration (R0, -T), (R0, T), (R1, -T), (R1, T)
    //the sign of reconstructed points is checked
    int type = -1;
    int counter = -1;

    for(unsigned int j = 0; j < 4; j++) {

        Eigen::Matrix3d tmp_R = (j < 2) ? R0 : R1;
        Eigen::Vector3d tmp_T;

        if((j % 2) == 0) {
            tmp_T = T;
        } else {
            tmp_T = -T;
        }

        Eigen::Matrix34d M0 = getCameraMatrixIdentity(K0);
        Eigen::Matrix34d M1 = getCameraMatrix(K1, tmp_R, tmp_T);

        int tmp_counter = 0;
        for(unsigned int i = 0; i < points0.size(); i++) {
            //homogeneous coordinates
            Eigen::Vector3d point_0 = Eigen::Vector3d(points0[i][0], points0[i][1], 1.0);
            Eigen::Vector3d point_1 = Eigen::Vector3d(points1[i][0], points1[i][1], 1.0);

            Eigen::Vector4d p0 = triangulationHartleySturm(point_0, point_1, M0, M1);
            Eigen::Vector3d p0_euc = Eigen::Vector3d(p0[0], p0[1], p0[2]);
            Eigen::Vector3d p1 = rigidTransform(p0_euc, tmp_R, tmp_T);

            if((p0[2] >= 0.0) && (p1[2] >= 0.0)) {
                tmp_counter++;
            }

        }

        #ifdef PIC_DEBUG
            printf("Front: %d %d\n",tmp_counter,j);
        #endif

        if(tmp_counter > counter) {
            type = j;
            counter = tmp_counter;
        }
    }

    if(type > -1) {

        R = (type < 2) ? R0 : R1;

        if((type % 2) == 0) {
            t = T;
        } else {
            t = -T;
        }

        return true;

    } else {
        R.setZero();
        t.setZero();

        return false;
    }
}

#endif // PIC_DISABLE_EIGEN

} // end namespace pic

#endif // PIC_COMPUTER_VISION_ESSENTIAL_MATRIX_HPP


/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: t; c-basic-offset: 4 -*- vim:set ts=4 sw=4 sts=4 noet: */
#include "method.h"

#include <cassert>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <iterator>
#include <sstream>
#include <string>
#include <vector>

#include <boost/uuid/uuid.hpp>
#include <boost/variant/static_visitor.hpp>

#include "db/ast-inserter.h"
#include "db/query.h"
#include "method/equation-grammar.h"
#include "method/equation-lexer.h"
#include "method/helper.h"
#include "method/parser.h"
#include "method/printer.h"

namespace flint {
namespace method {

namespace {

class VariantPrinter : public boost::static_visitor<> {
public:
	VariantPrinter(int k, int n, std::ostream *os)
		: k_(k)
		, n_(n)
		, os_(os)
	{}

	void operator()(const Compound &c) const {
		os_->put('(');
		*os_ << c.keyword;
		for (const auto &child : c.children) {
			os_->put(' ');
			boost::apply_visitor(*this, child);
		}
		os_->put(')');
	}

	void operator()(const std::string &s) const {
		if (s[0] != '%') {
			*os_ << s;
			return;
		}
		if (s == "%time") {
			if (n_ == 1) {
				*os_ << "(plus %time @dt)";
			} else {
				*os_ << "(plus %time (divide @dt "
					 << n_
					 << "))";
			}
			return;
		}
		*os_ << s << '#' << k_;
	}

	void operator()(int i) const {
		*os_ << i;
	}

	void operator()(const flint::lexer::Rational &r) const {
		*os_ << r.lexeme;
	}

	void operator()(const flint::lexer::Real &r) const {
		*os_ << r.lexeme;
	}

private:
	int k_;
	int n_;
	std::ostream *os_;
};

class Inserter : db::AstInserter {
public:
	explicit Inserter(sqlite3 *db)
		: db::AstInserter(db)
	{
	}

	bool PrintAndInsert(const boost::uuids::uuid &uuid,
						const Expr &lhs,
						const Expr &rhs)
	{
		std::ostringstream oss;
		if (lhs.which() == kExprIsString) {
			const std::string &id(boost::get<std::string>(lhs));
			if (!Even(uuid, id, rhs, 2, 2)) return false;
			if (!Even(uuid, id, rhs, 4, 2)) return false;
			if (!Even(uuid, id, rhs, 6, 1)) return false;
			if (!Even(uuid, id, rhs, 0, 1)) return false;
			return true;
		} else {
			assert(lhs.which() == kExprIsCompound);
			const Compound &c(boost::get<Compound>(lhs));
			if (c.children.size() != 2) {
				std::cerr << "unexpected expression with keyword: " << c.keyword << std::endl;
				return false;
			}
			const Expr &e(c.children.at(1));
			if (e.which() != kExprIsString) {
				std::cerr << "got an ill-formed derivative: " << c.keyword << std::endl;
				return false;
			}
			const std::string &id(boost::get<std::string>(e));
			std::string name;
			std::string math;

			// #1: k1 = dt * f(t_n, y_n)
			oss.str("");
			oss << id << "#1";
			name = oss.str();
			oss.str("");
			oss << "(times @dt ";
			boost::apply_visitor(Printer(&oss), rhs);
			oss.put(')');
			math = oss.str();
			if (!Insert(uuid, name.c_str(), math.c_str()))
				return false;

			// #2: y1 = y_n + k1/2
			oss.str("");
			oss << id << "#2";
			name = oss.str();
			oss.str("");
			oss << "(plus " << id << " (divide " << id << "#1 2))";
			math = oss.str();
			if (!Insert(uuid, name.c_str(), math.c_str()))
				return false;

			// #3: k2 = dt * f(t_n + dt/2, y1)
			oss.str("");
			oss << id << "#3";
			name = oss.str();
			oss.str("");
			oss << "(times @dt ";
			boost::apply_visitor(VariantPrinter(2, 2, &oss), rhs);
			oss.put(')');
			math = oss.str();
			if (!Insert(uuid, name.c_str(), math.c_str()))
				return false;

			// #4: y2 = y_n + k2/2
			oss.str("");
			oss << id << "#4";
			name = oss.str();
			oss.str("");
			oss << "(plus " << id << " (divide " << id << "#3 2))";
			math = oss.str();
			if (!Insert(uuid, name.c_str(), math.c_str()))
				return false;

			// #5: k3 = dt * f(t_n + dt/2, y2)
			oss.str("");
			oss << id << "#5";
			name = oss.str();
			oss.str("");
			oss << "(times @dt ";
			boost::apply_visitor(VariantPrinter(4, 2, &oss), rhs);
			oss.put(')');
			math = oss.str();
			if (!Insert(uuid, name.c_str(), math.c_str()))
				return false;

			// #6: y3 = y_n + k3
			oss.str("");
			oss << id << "#6";
			name = oss.str();
			oss.str("");
			oss << "(plus " << id << ' ' << id << "#5)";
			math = oss.str();
			if (!Insert(uuid, name.c_str(), math.c_str()))
				return false;

			// #7: k4 = dt * f(t_n + dt, y3)
			oss.str("");
			oss << id << "#7";
			name = oss.str();
			oss.str("");
			oss << "(times @dt ";
			boost::apply_visitor(VariantPrinter(6, 1, &oss), rhs);
			oss.put(')');
			math = oss.str();
			if (!Insert(uuid, name.c_str(), math.c_str()))
				return false;

			// #0: y_(n+1) = y_n + (k1 + 2*k2 + 2*k3 + k4)/6
			oss.str("");
			oss << id << "#0";
			name = oss.str();
			oss.str("");
			oss << "(plus " << id << " (divide (plus " << id << "#1 (plus (times 2 " << id << "#3) (plus (times 2 " << id << "#5) " << id << "#7))) 6))";
			math = oss.str();
			if (!Insert(uuid, name.c_str(), math.c_str()))
				return false;

			return true;
		}
	}

	bool PrintAndInsertWiener(const boost::uuids::uuid &,
							  const Expr &)
	{
		std::cerr << "The Euler method cannot solve SDEs, please use the Euler-Maruyama method instead." << std::endl;
		return false;
	}

private:
	bool Even(const boost::uuids::uuid &uuid, const std::string &id, const Expr &rhs,
			  int k, int n) {
		std::ostringstream oss;
		oss << id << '#' << k;
		std::string name = oss.str();
		oss.str("");
		boost::apply_visitor(VariantPrinter(k, n, &oss), rhs);
		std::string math = oss.str();
		return Insert(uuid, name.c_str(), math.c_str());
	}
};

}

bool Rk4(sqlite3 *db, const char *input, sqlite3 *output)
{
	return Parse<8, EquationLexer, EquationGrammar, Inserter>(db, input, output);
}

}
}


#include <Functions/FunctionFactory.h>
#include <Functions/geometryConverters.h>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include <base/logger_useful.h>

#include <Columns/ColumnArray.h>
#include <Columns/ColumnTuple.h>
#include <Columns/ColumnConst.h>
#include <DataTypes/DataTypeArray.h>
#include <DataTypes/DataTypeTuple.h>
#include <DataTypes/DataTypeCustomGeo.h>

#include <memory>
#include <utility>

namespace DB
{

namespace ErrorCodes
{
    extern const int ILLEGAL_TYPE_OF_ARGUMENT;
}

template <typename Point>
class FunctionPolygonsSymDifference : public IFunction
{
public:
    static const char * name;

    explicit FunctionPolygonsSymDifference() = default;

    static FunctionPtr create(ContextPtr)
    {
        return std::make_shared<FunctionPolygonsSymDifference>();
    }

    String getName() const override
    {
        return name;
    }

    bool isVariadic() const override
    {
        return false;
    }

    size_t getNumberOfArguments() const override
    {
        return 2;
    }

    DataTypePtr getReturnTypeImpl(const DataTypes &) const override
    {
        return DataTypeFactory::instance().get("MultiPolygon");
    }

    bool isSuitableForShortCircuitArgumentsExecution(const DataTypesWithConstInfo & /*arguments*/) const override { return true; }

    ColumnPtr executeImpl(const ColumnsWithTypeAndName & arguments, const DataTypePtr & /*result_type*/, size_t input_rows_count) const override
    {
        MultiPolygonSerializer<Point> serializer;

        callOnTwoGeometryDataTypes<Point>(arguments[0].type, arguments[1].type, [&](const auto & left_type, const auto & right_type)
        {
            using LeftConverterType = std::decay_t<decltype(left_type)>;
            using RightConverterType = std::decay_t<decltype(right_type)>;

            using LeftConverter = typename LeftConverterType::Type;
            using RightConverter = typename RightConverterType::Type;

            if constexpr (std::is_same_v<ColumnToPointsConverter<Point>, LeftConverter> || std::is_same_v<ColumnToPointsConverter<Point>, RightConverter>)
                throw Exception(fmt::format("Any argument of function {} must not be Point", getName()), ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT);
            else
            {
                auto first = LeftConverter::convert(arguments[0].column->convertToFullColumnIfConst());
                auto second = RightConverter::convert(arguments[1].column->convertToFullColumnIfConst());

                /// NOLINTNEXTLINE(clang-analyzer-core.uninitialized.Assign)
                for (size_t i = 0; i < input_rows_count; ++i)
                {
                    boost::geometry::correct(first[i]);
                    boost::geometry::correct(second[i]);

                    MultiPolygon<Point> sym_difference{};
                    boost::geometry::sym_difference(first[i], second[i], sym_difference);

                    serializer.add(sym_difference);
                }
            }
        });

        return serializer.finalize();
    }

    bool useDefaultImplementationForConstants() const override
    {
        return true;
    }
};

template <>
const char * FunctionPolygonsSymDifference<CartesianPoint>::name = "polygonsSymDifferenceCartesian";

template <>
const char * FunctionPolygonsSymDifference<SphericalPoint>::name = "polygonsSymDifferenceSpherical";

void registerFunctionPolygonsSymDifference(FunctionFactory & factory)
{
    factory.registerFunction<FunctionPolygonsSymDifference<CartesianPoint>>();
    factory.registerFunction<FunctionPolygonsSymDifference<SphericalPoint>>();
}

}


/*===========================================================================*\
 *                                                                           *
 *                               CoMISo                                      *
 *      Copyright (C) 2008-2009 by Computer Graphics Group, RWTH Aachen      *
 *                           www.rwth-graphics.de                            *
 *                                                                           *
 *---------------------------------------------------------------------------*
 *  This file is part of CoMISo.                                             *
 *                                                                           *
 *  CoMISo 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.                                      *
 *                                                                           *
 *  CoMISo 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 CoMISo.  If not, see <http://www.gnu.org/licenses/>.          *
 *                                                                           *
\*===========================================================================*/

#include <CoMISo/Config/config.hh>
#include <vector>
#include <cstdlib>
#include <iostream>

//------------------------------------------------------------------------------------------------------
#if COMISO_SUITESPARSE_SPQR_AVAILABLE // additional spqr library required
//------------------------------------------------------------------------------------------------------

#include <CoMISo/Utils/StopWatch.hh>
#include <Eigen/Sparse>
#include <CoMISo/Solver/SparseQRSolver.hh>
#include <CoMISo/Solver/Eigen_Tools.hh>

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

// Example main
int main(void)
{
  std::cout << "---------- 0) Using Sparse QR for solving underdetermined equations and computing Null spaces " << std::endl;

  typedef Eigen::SparseMatrix< double > SpMatrix;
  typedef Eigen::MatrixXd DenMatrix;
  typedef Eigen::Triplet< double > Triplet;

  using COMISO_GMM::operator<<;

  int dimr(4+1);
  int dimc(4+2);

  std::cout << "---------- 1) Creating matrix " << std::endl;
  std::vector< Triplet > triplets;
  for( int i = 0; i < dimc*dimr/2; ++i)
  {
    int x( rand()%(dimr-1));
    int y( rand()%dimc);
    double val( rand()%10);
    //std::cerr << " setting (" << x << ", " << y << ") to " << val << std::endl;
    triplets.push_back( Triplet( x, y, val));
  }
  SpMatrix A(dimr,dimc);
  A.setFromTriplets(triplets.begin(), triplets.end());

  std::cerr << DenMatrix(A) << std::endl;
  int m = dimr;
  int n = dimc;

  if( m < n )
  {
    std::swap( m,n);
    std::cerr << " ... m < n -> form transposed ..." << std::endl;
    A = SpMatrix(A.transpose());
    // test make also row -rank-deficinet
    A.middleCols(n-1,1) = A.middleCols(0,1);
    A.middleCols(0,1) = A.middleCols(n-2,1);
    std::cerr << DenMatrix(A) << std::endl;
  }

  std::cerr << " ... m = " << m << "; n = " << n << std::endl;
  std::cerr << std::endl;

  std::cout << "---------- 2) Sparse QR " << std::endl;
  COMISO::SparseQRSolver spqr;
  SpMatrix Q,R;
  std::vector< size_t > P;
  int rank = spqr.factorize_system_eigen( A, Q, R, P);
  int nullity(dimc-rank);
  // setup permutation matrix
  SpMatrix Pm( n, n);
  if( !P.empty())
  {
    for( size_t i = 0; i < P.size(); ++i)
    {
      Pm.coeffRef( (int)i, (int)P[i]) = 1;
    }
  }

  std::cout << "---------- 3) Result " << std::endl;
  std::cerr << " Q         " << std::endl << DenMatrix(Q) << std::endl;
  std::cerr << " R         " << std::endl << DenMatrix(R) << std::endl;
  std::cerr << " P         " << std::endl << P << std::endl;
  std::cerr << " P matrix  " << std::endl << DenMatrix(Pm) << std::endl;
  std::cerr << " Rank      " << rank << std::endl;
  std::cerr << " Nullity   " << nullity << std::endl;
  // extract nullspace
  SpMatrix NullSpace( Q.middleCols( std::max( 0, m-nullity), nullity));
  std::cerr << " Nullspace " << std::endl << DenMatrix(NullSpace) << std::endl;
  // non nullspace part of R
  //// assuming superflous column in R is the last (if A is also row deficient)
  //SpMatrix Rtmp(R.middleCols(0,std::min(n,n-(n-rank))).transpose());
  //SpMatrix R1( R.transpose().middleCols(0, m-nullity));
  SpMatrix Rtmp(R.transpose());
  SpMatrix R1t( Rtmp.middleCols(0,m-nullity));
  SpMatrix R1( R1t.transpose());
  std::cerr << " Non-Nullspace R " << std::endl << DenMatrix(R1) << std::endl;

  std::cout << "---------- 4) Verification " << std::endl;
  SpMatrix reconstructedA(Q*R*Pm.transpose());
  std::cerr << " Q orthogonal? \t " << ((fabs((Q.transpose()*Q).squaredNorm()-m) < 1e-8)?"yes":"no") << std::endl;
  std::cerr << " A = QR?       \t " << (((reconstructedA-A).squaredNorm() < 1e-8)? "yes":"no") << std::endl;

  std::cerr << std::endl << std::endl;
  std::cout << "---------- 5) Solving Ax=b (with x without nullspace component)" << std::endl;
  // NOTE: A was transposed above to be m>n
  SpMatrix b(n,1);
  SpMatrix x(m,1);
  for( int i = 0; i < n; ++i)
    b.coeffRef(i,0) = rand()%10;
  std::cerr << " ... System Ax = b .. \n";
  std::cerr << " A " << std::endl << DenMatrix(A.transpose()) << " x " << std::endl << DenMatrix(x) << " b " << std::endl << DenMatrix(b) << std::endl;

  std::cout << "---------- 5.1) test: solve using sparse QR solving .." << std::endl;
  SpMatrix At(A.transpose());
  spqr.solve_system_eigen( At, b, x);

  std::cerr << " ... solution x .. " << std::endl;
  std::cerr << DenMatrix(x) << std::endl;

  std::cerr << " ... test: is a solution ? " << (((A.transpose()*x-b).squaredNorm()<1e-8)?"yes":"no") << std::endl;
  std::cerr << " ... test: has nullspace component ? " << ((x.transpose()*NullSpace).squaredNorm()<1e-8?"yes":"no") << std::endl;
  std::cerr << " ... Nullspace projections : " << (x.transpose()*NullSpace) << std::endl;

  std::cout << "---------- 5.2) test: solve without nullspace .." << std::endl;
  SpMatrix Atnull(At);
  SpMatrix bnull(b);
  SpMatrix xnull(m,1);
  spqr.solve_system_eigen_min2norm( Atnull, bnull, xnull);
  std::cerr << " ... solution x .. " << std::endl;
  std::cerr << DenMatrix(xnull) << std::endl;

  std::cerr << " ... test: is a solution ? " << (((A.transpose()*xnull-bnull).squaredNorm()<1e-8)?"yes":"no") << std::endl;
  std::cerr << " ... test: has nullspace component ? " << ((xnull.transpose()*NullSpace).squaredNorm()<1e-8?"yes":"no") << std::endl;
  std::cerr << " ... Nullspace projections : " << (xnull.transpose()*NullSpace) << std::endl;

  return 0;
}

#else // COMISO_SUITESPARSE_SPQR_AVAILABLE

int main(void)
{
  std::cerr << " SUITESPARSE_SPQR not available, please re-configure!\n";
  return 0;
}

#endif  // COMISO_SUITESPARSE_SPQR_AVAILABLE


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the secant of the angle in radian: \f$1/\cos(x)\f$.

    @see cos, secd, secpi

    @par Header <boost/simd/function/sec.hpp>

    @par Example:

      @snippet sec.cpp sec

    @par Possible output:

      @snippet sec.txt sec

  **/
  IEEEValue sec(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/sec.hpp>
#include <boost/simd/function/simd/sec.hpp>

#endif


#ifndef DQMC_CHECKERBOARD_HPP
#define DQMC_CHECKERBOARD_HPP

#include "la.hpp"
#include "parameters.hpp"

#include <boost/multi_array.hpp>

#include <exception>
#include <cmath>

namespace dqmc {
    namespace cx_checkerboard {

	inline void graph_to_checkerboard(dqmc::parameters& p,
					  dqmc::cx_workspace& ws) {
	    for (int i = 0; i < p.num_bonds; ++i) {
		ws.bond_used[i] = -1;
	    }

	    int bonds_used = 0;
	    int coupling_used = 0;
	    int sites_used = 0;
	    int first_unused_bond = 0;

	    alps::graph_helper<>::bond_iterator itr1, itr1_end;
	    int b, s1, s2, bt;
	    double t;
	    while (bonds_used < p.num_bonds) {
		coupling_used = 0;
		while (coupling_used < p.ts.size()) {
		    cx_sp_mat hopping(p.N, p.N);
		    cx_sp_mat hopping_inv(p.N, p.N);

		    int elements = 0;

		    for (int i = 0; i < p.N; ++i) {
			ws.site_used[i] = -1;
		    }

		    for (boost::tie(itr1, itr1_end) = p.graph.bonds(); itr1 != itr1_end; ++itr1) {
			if (p.graph.bond_type(*itr1) != coupling_used) continue;

			b = p.graph.index(*itr1);
			s1 = p.graph.source(*itr1);
			s2 = p.graph.target(*itr1);
			bt = p.graph.bond_type(*itr1);

			if (ws.bond_used[b] == 1) continue;
			if (ws.site_used[s1] == 1 || ws.site_used[s2] == 1) continue;

			ws.chkr_bonds[bt].push_back(bond(s1, s2));

			ws.bond_used[b] = 1;
			ws.site_used[s1] = 1;
			ws.site_used[s2] = 1;
			t = p.ts[bt];
			hopping.insert(s1, s1) = cx_double(cosh(t * p.delta_tau/2.), 0);
			hopping.insert(s2, s2) = cx_double(cosh(t * p.delta_tau/2.), 0);
			hopping.insert(s1, s2) = cx_double(sinh(t * p.delta_tau/2.), 0);
			hopping.insert(s2, s1) = cx_double(sinh(t * p.delta_tau/2.), 0);

			hopping_inv.insert(s1, s1) = cx_double(cosh(t * p.delta_tau/2.), 0);
			hopping_inv.insert(s2, s2) = cx_double(cosh(t * p.delta_tau/2.), 0);
			hopping_inv.insert(s1, s2) = cx_double(-sinh(t * p.delta_tau/2.), 0);
			hopping_inv.insert(s2, s1) = cx_double(-sinh(t * p.delta_tau/2.), 0);

			// std::cout << "Using " << s1 << " and " << s2 << std::endl;
			++bonds_used;
			++elements;
		    }

		    if (elements != 0) {
			for (int i = 0; i < p.N; ++i) {
			    if(ws.site_used[i] == -1) {
				hopping.insert(i, i) = 1.;
				hopping_inv.insert(i, i) = 1.;
			    }
			}
			// arma::mat(hopping).print("A hopping matrix");
			ws.sparse_hoppings.push_back(hopping);
			ws.sparse_hoppings_inv.push_back(hopping_inv);
		    }
		    ++coupling_used;
		}
	    }
	}

	inline void graph_to_checkerboard_renyi(dqmc::parameters& p, dqmc::cx_workspace& ws) {
	    for (int i = 0; i < p.num_bonds; ++i) {
		ws.bond_used[i] = -1;
	    }

	    int bonds_used = 0;
	    int coupling_used = 0;
	    int sites_used = 0;
	    int first_unused_bond = 0;

	    alps::graph_helper<>::bond_iterator itr1, itr1_end;
	    int b, s1, s2, s1a, s2a;
	    double t;
	    while (bonds_used < p.num_bonds) {
		coupling_used = 0;
		while (coupling_used < p.ts.size()) {
		    cx_sp_mat hopping_0(ws.vol, ws.vol), hopping_0_inv(ws.vol, ws.vol),
			hopping_1(ws.vol, ws.vol), hopping_1_inv(ws.vol, ws.vol),
			hopping_2(ws.vol, ws.vol), hopping_2_inv(ws.vol, ws.vol),
			hopping_3(ws.vol, ws.vol), hopping_3_inv(ws.vol, ws.vol);

		    int elements = 0;

		    for (int i = 0; i < p.N; ++i) ws.site_used[i] = -1;
		    for (int i = 0; i < p.N + p.n_B; ++i) {
			hopping_0.insert(i, i) = cx_double(1., 0);
			hopping_0_inv.insert(i, i) = cx_double(1., 0);
			hopping_1.insert(i, i) =  cx_double(1., 0);
			hopping_1_inv.insert(i, i) =  cx_double(1., 0);
			hopping_2.insert(i, i) =  cx_double(1., 0);
			hopping_2_inv.insert(i, i) =  cx_double(1., 0);
			hopping_3.insert(i, i) =  cx_double(1., 0);
			hopping_3_inv.insert(i, i) =  cx_double(1., 0);
		    }

		    for (boost::tie(itr1, itr1_end) = p.graph.bonds(); itr1 != itr1_end; ++itr1) {
			if (p.graph.bond_type(*itr1) != coupling_used) continue;

			b = p.graph.index(*itr1);
			s1 = p.graph.source(*itr1);
			s2 = p.graph.target(*itr1);
			s1a = s1;
			s2a = s2;

			if (ws.bond_used[b] == 1) continue;
			if (ws.site_used[s1] == 1 || ws.site_used[s2] == 1) continue;

			ws.bond_used[b] = 1;
			ws.site_used[s1] = 1;
			ws.site_used[s2] = 1;

			t = p.ts[p.graph.bond_type(*itr1)];

			if (s1 >= p.n_A) s1a = s1 + p.n_B;
			if (s2 >= p.n_A) s2a = s2 + p.n_B;

			hopping_0.coeffRef(s1, s1) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_0.coeffRef(s2, s2) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_0.coeffRef(s1, s2) = cx_double(sinh(t * p.delta_tau/2.));
			hopping_0.coeffRef(s2, s1) = cx_double(sinh(t * p.delta_tau/2.));

			hopping_0_inv.coeffRef(s1, s1) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_0_inv.coeffRef(s2, s2) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_0_inv.coeffRef(s1, s2) = cx_double(-sinh(t * p.delta_tau/2.));
			hopping_0_inv.coeffRef(s2, s1) = cx_double(-sinh(t * p.delta_tau/2.));

			hopping_1.coeffRef(s1, s1) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_1.coeffRef(s2, s2) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_1.coeffRef(s1, s2) = cx_double(sinh(t * p.delta_tau/2.));
			hopping_1.coeffRef(s2, s1) = cx_double(sinh(t * p.delta_tau/2.));

			hopping_1_inv.coeffRef(s1, s1) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_1_inv.coeffRef(s2, s2) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_1_inv.coeffRef(s1, s2) = cx_double(-sinh(t * p.delta_tau/2.));
			hopping_1_inv.coeffRef(s2, s1) = cx_double(-sinh(t * p.delta_tau/2.));

			hopping_2.coeffRef(s1a, s1a) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_2.coeffRef(s2a, s2a) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_2.coeffRef(s1a, s2a) = cx_double(sinh(t * p.delta_tau/2.));
			hopping_2.coeffRef(s2a, s1a) = cx_double(sinh(t * p.delta_tau/2.));

			hopping_2_inv.coeffRef(s1a, s1a) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_2_inv.coeffRef(s2a, s2a) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_2_inv.coeffRef(s1a, s2a) = cx_double(-sinh(t * p.delta_tau/2.));
			hopping_2_inv.coeffRef(s2a, s1a) = cx_double(-sinh(t * p.delta_tau/2.));

			hopping_3.coeffRef(s1a, s1a) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_3.coeffRef(s2a, s2a) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_3.coeffRef(s1a, s2a) = cx_double(sinh(t * p.delta_tau/2.));
			hopping_3.coeffRef(s2a, s1a) = cx_double(sinh(t * p.delta_tau/2.));

			hopping_3_inv.coeffRef(s1a, s1a) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_3_inv.coeffRef(s2a, s2a) = cx_double(cosh(t * p.delta_tau/2.));
			hopping_3_inv.coeffRef(s1a, s2a) = cx_double(-sinh(t * p.delta_tau/2.));
			hopping_3_inv.coeffRef(s2a, s1a) = cx_double(-sinh(t * p.delta_tau/2.));

			// std::cout << "Using " << s1 << " and " << s2 << std::endl;
			++bonds_used;
			++elements;
		    }

		    if (elements != 0) {
			// arma::mat(hopping_0).print("Pushing back");
			using namespace std;
			ws.sparse_hoppings_0.push_back(hopping_0);
			ws.sparse_hoppings_0_inv.push_back(hopping_0_inv);

			ws.sparse_hoppings_1.push_back(hopping_1);
			ws.sparse_hoppings_1_inv.push_back(hopping_1_inv);

			ws.sparse_hoppings_2.push_back(hopping_2);
			ws.sparse_hoppings_2_inv.push_back(hopping_2_inv);

			ws.sparse_hoppings_3.push_back(hopping_3);
			ws.sparse_hoppings_3_inv.push_back(hopping_3_inv);
		    }
		    ++coupling_used;
		}
	    }
	}

	inline void hop_left(dqmc::cx_workspace * ws, cx_mat& M, double pref) {
	    using namespace std;
	    int par = 0;

	    if (pref > 0) {
		for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
		    if (par == 0)
			ws->hop_temp = ws->sparse_hoppings[i] * M;
		    else
			M = ws->sparse_hoppings[i] * ws->hop_temp;
		    par = (par == 0) ? 1 : 0;
		}

		for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
		    if (par == 0)
			ws->hop_temp = ws->sparse_hoppings[i] * M;
		    else
			M = ws->sparse_hoppings[i] * ws->hop_temp;
		    par = (par == 0) ? 1 : 0;
		}
	    }
	    else {
		for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
		    if (par == 0)
			ws->hop_temp = ws->sparse_hoppings_inv[i] * M;
		    else
			M = ws->sparse_hoppings_inv[i] * ws->hop_temp;
		    par = (par == 0) ? 1 : 0;
		}

		for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
		    if (par == 0)
			ws->hop_temp = ws->sparse_hoppings_inv[i] * M;
		    else
			M = ws->sparse_hoppings_inv[i] * ws->hop_temp;
		    par = (par == 0) ? 1 : 0;
		}
	    }
	    if (par == 1)
		M = ws->hop_temp;
	}

	inline void hop_right(dqmc::cx_workspace * ws, cx_mat& M, double pref) {
	    int par = 0;

	    if (pref > 0) {
		for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
		    if (par == 0)
			ws->hop_temp = M * ws->sparse_hoppings[i];
		    else
			M = ws->hop_temp * ws->sparse_hoppings[i];
		    par = (par == 0) ? 1 : 0;
		}

		for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
		    if (par == 0)
			ws->hop_temp = M * ws->sparse_hoppings[i];
		    else
			M = ws->hop_temp * ws->sparse_hoppings[i];
		    par = (par == 0) ? 1 : 0;
		}
	    }
	    else {
		for (int i = 0; i < ws->sparse_hoppings.size(); ++i) {
		    if (par == 0)
			ws->hop_temp = M * ws->sparse_hoppings_inv[i];
		    else
			M = ws->hop_temp * ws->sparse_hoppings_inv[i];
		    par = (par == 0) ? 1 : 0;
		}

		for (int i = ws->sparse_hoppings.size() - 1; i >=0; --i) {
		    if (par == 0)
			ws->hop_temp = M * ws->sparse_hoppings_inv[i];
		    else
			M = ws->hop_temp * ws->sparse_hoppings_inv[i];
		    par = (par == 0) ? 1 : 0;
		}
	    }
	    if (par == 1)
		M = ws->hop_temp;
	}

	// inline void hop_left_renyi(dqmc::cx_parameters * p,
	// 			   dqmc::cx_workspace * ws,
	// 			   cx_mat& M, double spin,
	// 			   double pref, int section,
	// 			   int slice) {
	//     using namespace std;
	//     int par = 0;
	//     // cout << "section " << section << endl;
	//     if (pref > 0) {
	// 	for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
	// 	    if (par == 0) {
	// 		if (section == 0) ws->hop_temp = ws->sparse_hoppings_0[i] * M;
	// 		if (section == 1) ws->hop_temp = ws->sparse_hoppings_1[i] * M;
	// 		if (section == 2) ws->hop_temp = ws->sparse_hoppings_2[i] * M;
	// 		if (section == 3) ws->hop_temp = ws->sparse_hoppings_3[i] * M;
	// 	    } else {
	// 		if (section == 0) M = ws->sparse_hoppings_0[i] * ws->hop_temp ;
	// 		if (section == 1) M = ws->sparse_hoppings_1[i] * ws->hop_temp ;
	// 		if (section == 2) M = ws->sparse_hoppings_2[i] * ws->hop_temp ;
	// 		if (section == 3) M = ws->sparse_hoppings_3[i] * ws->hop_temp ;
	// 	    }
	// 	    par = (par == 0) ? 1 : 0;
	// 	}

	// 	// dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
	// 	for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
	// 	    if (par == 0) {
	// 		if (section == 0) ws->hop_temp = ws->sparse_hoppings_0[i] * M;
	// 		if (section == 1) ws->hop_temp = ws->sparse_hoppings_1[i] * M;
	// 		if (section == 2) ws->hop_temp = ws->sparse_hoppings_2[i] * M;
	// 		if (section == 3) ws->hop_temp = ws->sparse_hoppings_3[i] * M;
	// 	    } else {
	// 		if (section == 0) M = ws->sparse_hoppings_0[i] * ws->hop_temp ;
	// 		if (section == 1) M = ws->sparse_hoppings_1[i] * ws->hop_temp ;
	// 		if (section == 2) M = ws->sparse_hoppings_2[i] * ws->hop_temp ;
	// 		if (section == 3) M = ws->sparse_hoppings_3[i] * ws->hop_temp ;
	// 	    }
	// 	    par = (par == 0) ? 1 : 0;
	// 	}
	//     }
	//     else {
	// 	for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
	// 	    if (par == 0) {
	// 		if (section == 0) ws->hop_temp = ws->sparse_hoppings_0_inv[i] * M;
	// 		if (section == 1) ws->hop_temp = ws->sparse_hoppings_1_inv[i] * M;
	// 		if (section == 2) ws->hop_temp = ws->sparse_hoppings_2_inv[i] * M;
	// 		if (section == 3) ws->hop_temp = ws->sparse_hoppings_3_inv[i] * M;
	// 	    } else {
	// 		if (section == 0) M = ws->sparse_hoppings_0_inv[i] * ws->hop_temp ;
	// 		if (section == 1) M = ws->sparse_hoppings_1_inv[i] * ws->hop_temp ;
	// 		if (section == 2) M = ws->sparse_hoppings_2_inv[i] * ws->hop_temp ;
	// 		if (section == 3) M = ws->sparse_hoppings_3_inv[i] * ws->hop_temp ;
	// 	    }
	// 	    par = (par == 0) ? 1 : 0;
	// 	}

	// 	// dqmc::interaction::onsite_left_fake(p, ws, M, spin, pref, section, slice);
	// 	for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
	// 	    if (par == 0) {
	// 		if (section == 0) ws->hop_temp = ws->sparse_hoppings_0_inv[i] * M;
	// 		if (section == 1) ws->hop_temp = ws->sparse_hoppings_1_inv[i] * M;
	// 		if (section == 2) ws->hop_temp = ws->sparse_hoppings_2_inv[i] * M;
	// 		if (section == 3) ws->hop_temp = ws->sparse_hoppings_3_inv[i] * M;
	// 	    } else {
	// 		if (section == 0) M = ws->sparse_hoppings_0_inv[i] * ws->hop_temp ;
	// 		if (section == 1) M = ws->sparse_hoppings_1_inv[i] * ws->hop_temp ;
	// 		if (section == 2) M = ws->sparse_hoppings_2_inv[i] * ws->hop_temp ;
	// 		if (section == 3) M = ws->sparse_hoppings_3_inv[i] * ws->hop_temp ;
	// 	    }
	// 	    par = (par == 0) ? 1 : 0;
	// 	}
	//     }
	//     if (par == 1)
	// 	M = ws->hop_temp;
	// }

	// inline void hop_right_renyi(dqmc::cx_parameters * p,
	// 			    dqmc::cx_workspace * ws,
	// 			    cx_mat&__restrict__ M, double spin,
	// 			    double pref, int section,
	// 			    int slice) {
	//     int par = 0;

	//     if (pref > 0) {
	// 	for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
	// 	    if (par == 0) {
	// 		if (section == 0) ws->hop_temp = M * ws->sparse_hoppings_0[i];
	// 		if (section == 1) ws->hop_temp = M * ws->sparse_hoppings_1[i];
	// 		if (section == 2) ws->hop_temp = M * ws->sparse_hoppings_2[i];
	// 		if (section == 3) ws->hop_temp = M * ws->sparse_hoppings_3[i];
	// 	    } else {
	// 		if (section == 0) M = ws->hop_temp * ws->sparse_hoppings_0[i];
	// 		if (section == 1) M = ws->hop_temp * ws->sparse_hoppings_1[i];
	// 		if (section == 2) M = ws->hop_temp * ws->sparse_hoppings_2[i];
	// 		if (section == 3) M = ws->hop_temp * ws->sparse_hoppings_3[i];
	// 	    }
	// 	    par = (par == 0) ? 1 : 0;
	// 	}

	// 	for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
	// 	    if (par == 0) {
	// 		if (section == 0) ws->hop_temp = M * ws->sparse_hoppings_0[i];
	// 		if (section == 1) ws->hop_temp = M * ws->sparse_hoppings_1[i];
	// 		if (section == 2) ws->hop_temp = M * ws->sparse_hoppings_2[i];
	// 		if (section == 3) ws->hop_temp = M * ws->sparse_hoppings_3[i];
	// 	    } else {
	// 		if (section == 0) M = ws->hop_temp * ws->sparse_hoppings_0[i];
	// 		if (section == 1) M = ws->hop_temp * ws->sparse_hoppings_1[i];
	// 		if (section == 2) M = ws->hop_temp * ws->sparse_hoppings_2[i];
	// 		if (section == 3) M = ws->hop_temp * ws->sparse_hoppings_3[i];
	// 	    }
	// 	    par = (par == 0) ? 1 : 0;
	// 	}
	//     }
	//     else {
	// 	for (int i = 0; i < ws->sparse_hoppings_0.size(); ++i) {
	// 	    if (par == 0) {
	// 		if (section == 0) ws->hop_temp = M * ws->sparse_hoppings_0_inv[i];
	// 		if (section == 1) ws->hop_temp = M * ws->sparse_hoppings_1_inv[i];
	// 		if (section == 2) ws->hop_temp = M * ws->sparse_hoppings_2_inv[i];
	// 		if (section == 3) ws->hop_temp = M * ws->sparse_hoppings_3_inv[i];
	// 	    } else {
	// 		if (section == 0) M = ws->hop_temp * ws->sparse_hoppings_0_inv[i];
	// 		if (section == 1) M = ws->hop_temp * ws->sparse_hoppings_1_inv[i];
	// 		if (section == 2) M = ws->hop_temp * ws->sparse_hoppings_2_inv[i];
	// 		if (section == 3) M = ws->hop_temp * ws->sparse_hoppings_3_inv[i];
	// 	    }
	// 	    par = (par == 0) ? 1 : 0;
	// 	}

	// 	for (int i = ws->sparse_hoppings_0.size() - 1; i >=0; --i) {
	// 	    if (par == 0) {
	// 		if (section == 0) ws->hop_temp = M * ws->sparse_hoppings_0_inv[i];
	// 		if (section == 1) ws->hop_temp = M * ws->sparse_hoppings_1_inv[i];
	// 		if (section == 2) ws->hop_temp = M * ws->sparse_hoppings_2_inv[i];
	// 		if (section == 3) ws->hop_temp = M * ws->sparse_hoppings_3_inv[i];
	// 	    } else {
	// 		if (section == 0) M = ws->hop_temp * ws->sparse_hoppings_0_inv[i];
	// 		if (section == 1) M = ws->hop_temp * ws->sparse_hoppings_1_inv[i];
	// 		if (section == 2) M = ws->hop_temp * ws->sparse_hoppings_2_inv[i];
	// 		if (section == 3) M = ws->hop_temp * ws->sparse_hoppings_3_inv[i];
	// 	    }
	// 	    par = (par == 0) ? 1 : 0;
	// 	}
	//     }
	//     if (par == 1)
	// 	M = ws->hop_temp;
	// }
    }
}
#endif


/* -*-C++-*- */
/*
   (c) Copyright 1993-2005, Hewlett-Packard Development Company, LP

   See the file named COPYING for license details
*/

/** @file
    \brief Header file for a histogram class.
*/

///////////////////////////////////////////////////////////////////////////////
//
// This package provides histogram gathering classes on variables.
// StatsHistograms can be used to approximate the mode and percentiles.
//
// The histograms collect samples into bins, where the number and size of
// bins is determined when the histogram object is created.  Two
// variations are provided: one version rescales bins when values falling
// outside any bin are sampled, while the other lumps extreme values into
// the end bins.  The former are called `scaling' histograms while the
// latter are `fixed'.
//
// When specifying the domain of a histogram, you specify the low and
// high values.  The domain is then all values low <= x < high.  For an
// integer histogram, where you are (say) interested in collecting values
// 0 through 11, specify 0 as the low and _12_ as the high point.
//
// Note also that the histograms will round the number of bins up to an
// even number.  This may have unfortunate side-effects when working with
// integer histograms.  To be safe, you should always specify an even
// number of bins.  You can usually put an extra bin on the high or low
// end.
//
// Note also that the `width' of bins is always a double, even in the
// integer histograms.  This is because width is calculated from the high
// and low points and the number of bins.  If you specify a funny
// combination of these, you might get (say) bins alternating with two
// and three bins.
//
// There are those who believe that integer histograms probably aren't
// terribly useful.  They are probably right.
//
//////////////////////////////////////////////////////////////////////////////

#ifndef LINTEL_STATSHISTOGRAM_HPP
#define LINTEL_STATSHISTOGRAM_HPP

#include <vector>
#include <math.h>
#include <boost/config.hpp>
#include <iostream>
#include <fstream>
#include <Lintel/AssertBoost.hpp>
#include <Lintel/Stats.hpp>

/// \brief Histogram statistics class, interface
class StatsHistogram : public Stats {

public:
    enum HistMode { Uniform, Log, UniformAccum, LogAccum };
    StatsHistogram();
    virtual ~StatsHistogram();
    virtual void reset();	// Put everything back to initial values

    //----Record a new value
    virtual void add(const double value) = 0;
    virtual void add(const Stats &stat);
    virtual void add(const double index_value, const double data_value) = 0;

    //----Histogram-wide access functions
    virtual double binWidth(unsigned index) const = 0;
    virtual double binWidth() const { return binWidth(0); }
    virtual double low() const = 0;
    virtual double binlow(unsigned index) const = 0;
    virtual double bincenter(unsigned index) const = 0;
    virtual double high() const = 0;
    virtual double binhigh(unsigned index) const = 0;
    virtual unsigned numBins() const = 0;
    virtual unsigned numRescales() const = 0;
    virtual unsigned numGrows() const = 0;
    virtual bool isScalable() const = 0;
    virtual bool isGrowable() const = 0;
    virtual double mode() const = 0;
    virtual const std::string getType() const = 0;
    virtual void printTabular(int depth, std::ostream &out) const;
    virtual void printHistogramRandomInput(std::ostream& out) const;
    virtual double percentile(double p) const = 0;   // 0.0 <=p<= 1.0
    virtual unsigned long operator[](unsigned index) const = 0;
				// returns the count in a particular bin

    //----Value-access functions
    virtual std::string debugString() const = 0;

    //----Sometimes we want another one of these, just like this one, but fresh.
    virtual Stats *another_new() const = 0;
};

/// \brief Histogram statistics class, uniformly-sized bins
class StatsHistogramUniform : public StatsHistogram {
protected:
    unsigned       num_bins;	// number of bins
    double        bin_width;	// width of each bin
    double        bin_low;	// lower bound of lowest bin
    double        bin_high;	// upper bound of highest bin
    unsigned long *bins;	// array of bin-counters
    const bool    is_scalable;	// true if rescale on outlier, false if fixed
    const bool    is_growable;	// true if grow on outlier, false if fixed
    unsigned       num_rescales;// number of times rescaled
    unsigned       num_grows;	// number of times rescaled

    unsigned sampleBin(const double x) const;

public:
    StatsHistogramUniform(const unsigned bins_in,
		   const double   low_in,
		   const double   high_in,
		   const bool     scalable = false,
		   const bool     growable = false);
    virtual ~StatsHistogramUniform();
    virtual void reset();	// Put everything back to initial values

    //----Record a new value
    virtual void add(const double value);
    virtual void add(const double index_value, const double data_value);
    using StatsHistogram::add;

    //----Histogram-wide access functions
    double binWidth() const { return bin_width; }
    double binWidth(unsigned /*index*/) const { return bin_width; }
    double   low()          const { return bin_low; }
    double binlow(unsigned index) const { return bin_low + index*bin_width; };
    double bincenter(unsigned index) const { return bin_low + (index+0.5)*bin_width; };
    double   high()         const { return bin_high; }
    double binhigh(unsigned index) const { return bin_low + (index+1)*bin_width; };
    unsigned  numBins() const { return num_bins; }
    unsigned  numRescales()  const { return num_rescales; }
    unsigned  numGrows()  const { return num_grows; }
    bool     isScalable()   const { return is_scalable; }
    bool     isGrowable()   const { return is_growable; }
    double   mode() const;
    double   percentile(double p) const;   // 0.0 <= p <= 1.0
    unsigned long  operator[](unsigned index) const;
				// Count in a particular bin

    //----Value-access functions
    virtual std::string debugString() const;

    virtual const std::string getType() const { return std::string("uniform");}
    virtual void printRome(int depth, std::ostream &out) const;

    //----Sometimes we want another one of these, just like this one, but fresh.
    virtual Stats *another_new() const;
};

/// \brief Histogram statistics class, exponentially-sized bins
class StatsHistogramLog : public StatsHistogram {
protected:
    unsigned	  num_bins;	// number of bins
    double        smallest_bin;	// width of bin zero
    double        bin_high;	// highest bound of highest bin
    double        bin_scaling;	// scales log(smallest-bin count) to bin#
    const bool    is_scalable;	// true if rescale on outlier, false if fixed
    unsigned	  num_rescales; // number of times rescaled
    unsigned long *bins;	// array of bin-counters

    unsigned sampleBin(double x) const;
    double binOffset(double index) const;

public:
    StatsHistogramLog(const unsigned bins_in,
		   const double   low_in,
		   const double   high_in,
                   const bool     scalable_in = false);
    virtual ~StatsHistogramLog();
    virtual void reset();	// Put everything back to initial values

    //----Record a new value
    virtual void add(const double value);
    virtual void add(const double index_value, const double data_value);
    using StatsHistogram::add;

    //----Histogram-wide access functions
    double binWidth() const { return binOffset(1.0); }
    double binWidth(unsigned index) const { return binOffset(index+1)-binOffset(index); }
    double low() const { return smallest_bin; }
    double binlow(unsigned index) const { return binOffset(index);};
    double bincenter(unsigned index) const {return binOffset(index+0.5);};
    double high() const { return bin_high; };
    double binhigh(unsigned index) const { return binOffset(index+1); };
    unsigned  numBins() const { return num_bins; }
    unsigned  numRescales()  const { return 0; }
    bool     isScalable()   const { return false; }
    unsigned  numGrows()  const { return 0; }
    bool     isGrowable()   const { return false; }
    double   mode() const;
    double   percentile(double p) const;   // 0.0 <= p <= 1.0
    unsigned long  operator[](unsigned index) const;  // Count in a particular bin

    //----Value-access functions
    virtual std::string debugString() const;

    virtual const std::string getType() const { return std::string("log");}
    virtual void printRome(int depth, std::ostream &out) const;
    //----Sometimes we want another one of these, just like this one, but fresh.
    virtual Stats *another_new() const;
};

/// \brief Histogram statistics class, exponentially-sized bins, sum as well as count
///
/// a kind of histogram that has exponentially-sized bins, and that
/// accumulates a value in the bin as well as the hit count.
///
/// The histogram can accumulate either the index value, using
/// the add(index_value) method, or some other data value associated
/// with the index value, using the add(index_value, data_value) method.
/// For example, the latter form makes it possible to accumulate response
/// times (data values) as a function of jump distance (index values).

class StatsHistogramLogAccum : public StatsHistogramLog {
private:
    double       *val_bins;

public:
    StatsHistogramLogAccum(const unsigned bins_in,
		   const double   low_in,
		   const double   high_in);
    virtual ~StatsHistogramLogAccum();
    virtual void reset();	// Put everything back to initial values

    //----Record a new value
    virtual void add(const double value);
    virtual void add(const double index_value, const double data_value);
    using StatsHistogram::add;

    //----Histogram-wide access functions
    double value(unsigned index) const;	// value in a particular bin

    //----Value-access functions
    virtual void printRome(int depth, std::ostream &out) const;

    //----Sometimes we want another one of these, just like this one, but fresh.
    virtual Stats *another_new() const;
};

/// \brief Histogram statistics class, exponentially-sized bins, sum as well as count
///
/// a kind of histogram that has uniformly-sized bins, and that
/// accumulates a value in the bin as well as the hit count.
///
/// The histogram can accumulate either the index value, using
/// the add(index_value) method, or some other data value associated
/// with the index value, using the add(index_value, data_value) method.
/// For example, the latter form makes it possible to accumulate response
/// times (data values) as a function of jump distance (index values).
class StatsHistogramUniformAccum : public StatsHistogramUniform {
private:
    double       *val_bins;

public:
    StatsHistogramUniformAccum(const unsigned bins_in,
		   const double   low_in,
		   const double   high_in,
   	           const bool     scalable_in = false,
   	           const bool     growable_in = false);
    virtual ~StatsHistogramUniformAccum();
    virtual void reset();	// Put everything back to initial values

    //----Record a new value
    virtual void add(const double value);
    virtual void add(const double index_value, const double data_value);
    using StatsHistogramUniform::add;

    //----Histogram-wide access functions
    double value(unsigned index) const;	// value in a particular bin

    //----Value-access functions
    virtual void printRome(int depth, std::ostream &out) const;

    //----Sometimes we want another one of these, just like this one, but fresh.
    virtual Stats *another_new() const;
};

/// \brief Histogram with specified subranges and different histogram types for each range.
///
/// StatsHistogramGroup: a collection of histograms, allowing multiple
/// granularities of histograms over different ranges plus a low, high,
/// and overall stats.
///
/// The histograms may be all the same type, or of different types,
/// depending on the constructor method chosen. For example, a uniform histogram
/// around 0 and exponential above 100.
class StatsHistogramGroup : public Stats {
public:
    // ranges=[0,1,10,100] => low, hist[0..1[, hist[1..10[, hist[10,100[, high
    StatsHistogramGroup(const StatsHistogram::HistMode mode,
                        const int sub_hist_buckets,
                        const std::vector<double> &ranges);
    StatsHistogramGroup(const std::vector<StatsHistogram::HistMode> &_modes,
                        const std::vector<int> &sub_hist_buckets,
                        const std::vector<double> &_ranges);
    virtual ~StatsHistogramGroup();
    virtual void reset();	// Put everything back to initial values

    //----Record a new value
    virtual void add(const double value);
    virtual void add(const double index_value, const double data_value);
    virtual void add(const Stats &stats);

    const StatsHistogram &getHistogram(unsigned int n) const {
        INVARIANT(n < histograms.size(),
                  boost::format("Requested out of bounds histogram %d valid range [%d .. %d]")
                  % n % 0 % histograms.size());
        return *histograms[n];
    }

    const Stats &getLow() const { return low; }
    const Stats &getHigh() const { return high; }
    virtual void printRome(int depth, std::ostream &out) const;

private:
    std::vector<StatsHistogram *> histograms;
    const std::vector<StatsHistogram::HistMode> modes;
    const std::vector<double> ranges;
    Stats low,high;
};

#endif /* _LINTEL_HISTOGRAM_H_INCLUDED */


#include <algorithm>
#include <queue>
#include <vector>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>
#include <CGAL/boost/graph/iterator.h>
#include <CGAL/boost/graph/properties.h>

namespace Euclid
{

namespace _impl
{

template<typename Mesh>
std::vector<vertex_t<Mesh>> mark_shortest_path(
    const Mesh& mesh,
    vertex_t<Mesh> src,
    std::vector<bool>& on_shortest_path_tree)
{
    auto vimap = get(boost::vertex_index, mesh);
    auto eimap = get(boost::edge_index, mesh);
    std::vector<vertex_t<Mesh>> predecessors(num_vertices(mesh));
    boost::dijkstra_shortest_paths(
        mesh,
        src,
        boost::predecessor_map(
            boost::make_iterator_property_map(predecessors.begin(), vimap)));

    for (auto v : vertices(mesh)) {
        if (v != src) {
            auto [h, exist] = halfedge(v, predecessors[get(vimap, v)], mesh);
            assert(exist == true);
            auto e = edge(h, mesh);
            on_shortest_path_tree[get(eimap, e)] = true;
        }
    }

    return predecessors;
}

template<typename Mesh>
void spanning_tree_on_dual_graph_not_cross_shortest_path_tree(
    const Mesh& mesh,
    const std::vector<bool>& on_shortest_path_tree,
    std::vector<bool>& cross_spanning_tree)
{
    using FD = face_t<Mesh>;
    auto fimap = get(CGAL::face_index, mesh);
    auto eimap = get(CGAL::edge_index, mesh);

    auto frange = faces(mesh);
    auto froot = *frange.first;
    std::queue<FD> q;
    q.push(froot);
    std::vector<bool> touched(num_faces(mesh), false);
    touched[get(fimap, froot)] = true;

    while (!q.empty()) {
        auto f = q.front();
        q.pop();
        for (auto h : CGAL::halfedges_around_face(halfedge(f, mesh), mesh)) {
            auto e = edge(h, mesh);
            auto eidx = get(eimap, e);
            if (!on_shortest_path_tree[eidx]) {
                auto ho = opposite(h, mesh);
                auto fo = face(ho, mesh);
                auto fidx = get(fimap, fo);
                if (!touched[fidx]) {
                    touched[fidx] = true;
                    q.push(fo);
                    cross_spanning_tree[eidx] = true;
                }
            }
        }
    }
}

template<typename Mesh>
VertexChain<Mesh> find_generator(
    const Mesh& mesh,
    edge_t<Mesh> e,
    vertex_t<Mesh> src,
    const std::vector<vertex_t<Mesh>>& predecessors)
{
    auto vimap = get(boost::vertex_index, mesh);
    auto h = halfedge(e, mesh);
    auto v1 = source(h, mesh);
    auto v2 = target(h, mesh);

    VertexChain<Mesh> generator;
    while (v1 != src) {
        generator.push_back(v1);
        v1 = predecessors[get(vimap, v1)];
    }
    generator.push_back(src);
    std::reverse(generator.begin(), generator.end());
    while (v2 != src) {
        generator.push_back(v2);
        v2 = predecessors[get(vimap, v2)];
    }
    return generator;
}

template<typename Mesh>
VertexChains<Mesh> find_generators(
    const Mesh& mesh,
    vertex_t<Mesh> src,
    const std::vector<bool>& on_shortest_path_tree,
    const std ::vector<bool>& cross_spanning_tree,
    const std ::vector<vertex_t<Mesh>>& predecessors)
{
    auto eimap = get(boost::edge_index, mesh);

    // find edges not on shortest path tree and crossing spanning tree
    std::vector<edge_t<Mesh>> left_edges;
    for (auto e : edges(mesh)) {
        auto eidx = get(eimap, e);
        if (!on_shortest_path_tree[eidx] && !cross_spanning_tree[eidx]) {
            left_edges.push_back(e);
        }
    }

    // find generators from left edges, should be 2G generators
    VertexChains<Mesh> generators;
    for (auto e : left_edges) {
        generators.push_back(find_generator(mesh, e, src, predecessors));
    }

    return generators;
}

} // namespace _impl

template<typename Mesh>
VertexChains<Mesh> greedy_homotopy_generators(const Mesh& mesh,
                                              vertex_t<Mesh> v)
{
    std::vector<bool> on_shortest_path_tree(num_edges(mesh), false);
    auto predecessors =
        _impl::mark_shortest_path(mesh, v, on_shortest_path_tree);

    std::vector<bool> cross_spanning_tree(num_edges(mesh), false);
    _impl::spanning_tree_on_dual_graph_not_cross_shortest_path_tree(
        mesh, on_shortest_path_tree, cross_spanning_tree);

    auto generators = _impl::find_generators(
        mesh, v, on_shortest_path_tree, cross_spanning_tree, predecessors);

    return generators;
}

} // namespace Euclid


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include<nav_msgs/Odometry.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <stdio.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#define ture 1
#define false 0
// #include "extra.h" // use this if in OpenCV2
using namespace std;
using namespace cv;
Mat frame0,frame1;
/****************************************************
 * 本程序演示了如何使用2D-2D的特征匹配估计相机运动
 * **************************************************/

void find_feature_matches (
    const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches );

void pose_estimation_2d2d (
    std::vector<KeyPoint> keypoints_1,
    std::vector<KeyPoint> keypoints_2,
    std::vector< DMatch > matches,
    Mat& R, Mat& t );

// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );

void imageCallback0(const sensor_msgs::ImageConstPtr& msg)
{

  cv_bridge::CvImagePtr cv_ptr;
  try
  {
     cv_ptr = cv_bridge::toCvCopy(msg, "mono8");
  }
  catch (cv_bridge::Exception& e)
  {
     ROS_ERROR("cv_bridge exception: %s", e.what());
     return;
  }

  // cv::Mat caml;
  frame0 = cv_ptr->image;
//imshow("1",caml);
  // fail if don't have waitKey(3).
 // cv::waitKey(3);
}
void imageCallback1(const sensor_msgs::ImageConstPtr& msg)
{

  cv_bridge::CvImagePtr cv_ptr;
  try
  {
     cv_ptr = cv_bridge::toCvCopy(msg, "mono8");
  }
  catch (cv_bridge::Exception& e)
  {
     ROS_ERROR("cv_bridge exception: %s", e.what());
     return;
  }

  // cv::Mat caml;
  frame1 = cv_ptr->image;
//imshow("1",caml);
  // fail if don't have waitKey(3).
 // cv::waitKey(3);
}

int main ( int argc, char** argv )
{
    // if ( argc != 3 )
    // {
    //     cout<<"usage: pose_estimation_2d2d img1 img2"<<endl;
    //     return 1;
    // }
    //-- 读取图像
    ros::init(argc, argv, "pose");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
   image_transport::Subscriber sub0 = it.subscribe("camera/fisheye0", 1, imageCallback0);
   image_transport::Subscriber sub1 = it.subscribe("camera/fisheye1", 1, imageCallback1);
    ros::Publisher path_pub = nh.advertise<nav_msgs::Path>("trajectory",1, true);
    ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 50);
    ros::Time current_time, last_time;
    current_time = ros::Time::now();
    last_time = ros::Time::now();

    nav_msgs::Path path;
    //nav_msgs::Path path;
    path.header.stamp=current_time;
    path.header.frame_id="odom";
 ros::Rate loop_rate(10);

 while (nh.ok())
 {

    // Mat img_1 = imread ( "/home/fsdh/桌面/slambook-master/ch7/1.png", CV_LOAD_IMAGE_COLOR );
    // Mat img_2 = imread ( "/home/fsdh/桌面/slambook-master/ch7/2.png", CV_LOAD_IMAGE_COLOR );
   if(!frame0.empty()&!frame1.empty()){
    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( frame0, frame1, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;
     current_time = ros::Time::now();
    //-- 估计两张图像间运动
    Mat R,t;
    pose_estimation_2d2d ( keypoints_1, keypoints_2, matches, R, t );

     Eigen::Matrix3d ER;
        ER<<R.at<double> ( 0,0 ),R.at<double> ( 0,1 ),R.at<double> ( 0,2 ),R.at<double> ( 1,0 ),R.at<double> ( 1,1 ),R.at<double> ( 1,2 ),R.at<double> ( 2,0 ),R.at<double> ( 2,1 ),R.at<double> ( 2,2 );
         Eigen::Quaterniond q = Eigen::Quaterniond(ER);
    // geometry_msgs::PoseStamped this_pose_stamped;
    //     this_pose_stamped.pose.position.x =t.at<double> ( 0,0 );
    //     this_pose_stamped.pose.position.y = 0;
    // this_pose_stamped.pose.position.z =0;

    //     this_pose_stamped.pose.orientation.x=q.x();
    //     this_pose_stamped.pose.orientation.y=q.y();
    //     this_pose_stamped.pose.orientation.z=q.z();
    //     this_pose_stamped.pose.orientation.w=q.w();

    //     this_pose_stamped.header.stamp=current_time;
    //     this_pose_stamped.header.frame_id="odom";
    //     path.poses.push_back(this_pose_stamped);

   nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

    //set the position
    odom.pose.pose.position.x = 0;
    odom.pose.pose.position.y = 0;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation.x =q.x();
    odom.pose.pose.orientation.y =q.y();
    odom.pose.pose.orientation.z =q.z();
    odom.pose.pose.orientation.w =q.w();
    //set the velocity
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = 0;
    odom.twist.twist.linear.y = 0;
    odom.twist.twist.angular.z = 0;
        //path_pub.publish(path);

    odom_pub.publish(odom);
     //-- 验证E=t^R*scale
    // Mat t_x = ( Mat_<double> ( 3,3 ) <<
    //             0,                      -t.at<double> ( 2,0 ),     t.at<double> ( 1,0 ),
    //             t.at<double> ( 2,0 ),      0,                      -t.at<double> ( 0,0 ),
    //             -t.at<double> ( 1,0 ),     t.at<double> ( 0,0 ),      0 );

    // cout<<"t^R="<<endl<<t_x*R<<endl;

    // //-- 验证对极约束
    // Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    // for ( DMatch m: matches )
    // {
    //     Point2d pt1 = pixel2cam ( keypoints_1[ m.queryIdx ].pt, K );
    //     Mat y1 = ( Mat_<double> ( 3,1 ) << pt1.x, pt1.y, 1 );
    //     Point2d pt2 = pixel2cam ( keypoints_2[ m.trainIdx ].pt, K );
    //     Mat y2 = ( Mat_<double> ( 3,1 ) << pt2.x, pt2.y, 1 );
    //     Mat d = y2.t() * t_x * R * y1;
    //     cout << "epipolar constraint = " << d << endl;
    // }
   }
   ros::spinOnce();
   last_time = current_time;
     loop_rate.sleep();   /* code for loop body */
 }

    return 0;
}

void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
    //BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, match );

    //-- 第四步:匹配点对筛选
    double min_dist=10000, max_dist=0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = match[i].distance;
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_dist ) max_dist = dist;
    }

    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( match[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            matches.push_back ( match[i] );
        }
    }
     Mat img_goodmatch;

    drawMatches ( img_1, keypoints_1, img_2, keypoints_2, matches, img_goodmatch );

    imshow ( "优化后匹配点对", img_goodmatch );
    cv::waitKey(3);
}

Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
           (
               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
}

void pose_estimation_2d2d ( std::vector<KeyPoint> keypoints_1,
                            std::vector<KeyPoint> keypoints_2,
                            std::vector< DMatch > matches,
                            Mat& R, Mat& t )
{
    // 相机内参,TUM Freiburg2
    Mat K = ( Mat_<double> ( 3,3 ) << 259.8074, 0, 328.0466, 0, 261.0161, 236.2571, 0, 0, 1 );

    //-- 把匹配点转换为vector<Point2f>的形式
    vector<Point2f> points1;
    vector<Point2f> points2;

    for ( int i = 0; i < ( int ) matches.size(); i++ )
    {
        points1.push_back ( keypoints_1[matches[i].queryIdx].pt );
        points2.push_back ( keypoints_2[matches[i].trainIdx].pt );
    }

    //-- 计算基础矩阵
    Mat fundamental_matrix;
    fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT );
   // cout<<"fundamental_matrix is "<<endl<< fundamental_matrix<<endl;

    //-- 计算本质矩阵
    Point2d principal_point ( 325.1, 249.7 );	//相机光心, TUM dataset标定值
    double focal_length = 521;			//相机焦距, TUM dataset标定值
    Mat essential_matrix;
    essential_matrix = findEssentialMat ( points1, points2, focal_length, principal_point );
   // cout<<"essential_matrix is "<<endl<< essential_matrix<<endl;

    //-- 计算单应矩阵
    Mat homography_matrix;
    homography_matrix = findHomography ( points1, points2, RANSAC, 3 );
  //  cout<<"homography_matrix is "<<endl<<homography_matrix<<endl;

    //-- 从本质矩阵中恢复旋转和平移信息.
    recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );
    // Eigen::Matrix3d ER;
    // ER<<R.at<double> ( 0,0 ),R.at<double> ( 0,1 ),R.at<double> ( 0,2 ),R.at<double> ( 1,0 ),R.at<double> ( 1,1 ),R.at<double> ( 1,2 ),R.at<double> ( 2,0 ),R.at<double> ( 2,1 ),R.at<double> ( 2,2 );
    //  Eigen::Quaterniond q = Eigen::Quaterniond(ER);
    cout<<"R is "<<endl<<R<<endl;
    cout<<"t is "<<endl<<t<<endl;
    // cout<<"ER is "<<endl<<q.coeffs()<<endl;

}


/*
 * main_optimize.cpp
 * Date: 2013-01-28
 * Author: Karsten Ahnert (karsten.ahnert@gmx.de)
 */

#define FUSION_MAX_VECTOR_SIZE 20

#include <gpcxx/tree.hpp>
#include <gpcxx/generate.hpp>
#include <gpcxx/operator.hpp>
#include <gpcxx/eval.hpp>
#include <gpcxx/evolve.hpp>
#include <gpcxx/io.hpp>
#include <gpcxx/stat.hpp>
#include <gpcxx/app.hpp>

#include <boost/fusion/include/make_vector.hpp>

#include <iostream>
#include <random>
#include <vector>
#include <functional>

const std::string tab = "\t";

namespace pl = std::placeholders;
namespace fusion = boost::fusion;

int main( int argc , char *argv[] )
{
    typedef std::mt19937 rng_type ;
    typedef double value_type;
    typedef char symbol_type;
    typedef gpcxx::regression_training_data< value_type , 3 > training_data_type;
    typedef gpcxx::regression_context< value_type , 3 > context_type;
    typedef std::vector< value_type > fitness_type;

    auto eval = gpcxx::make_static_eval_erc< value_type , symbol_type , context_type >(
        fusion::make_vector( 1.0 , std::normal_distribution<>( 0.0 , 1.0 ) ) ,
        fusion::make_vector(
            fusion::make_vector( 'x' , []( context_type const& t ) { return t[0]; } )
          , fusion::make_vector( 'y' , []( context_type const& t ) { return t[1]; } )
          , fusion::make_vector( 'z' , []( context_type const& t ) { return t[2]; } )
          ) ,
        fusion::make_vector(
            fusion::make_vector( 's' , []( double v ) -> double { return std::sin( v ); } )
          , fusion::make_vector( 'c' , []( double v ) -> double { return std::cos( v ); } )
          ) ,
        fusion::make_vector(
            fusion::make_vector( '+' , std::plus< double >() )
          , fusion::make_vector( '-' , std::minus< double >() )
          , fusion::make_vector( '*' , std::multiplies< double >() )
          , fusion::make_vector( '/' , std::divides< double >() )
          ) );
    typedef decltype( eval ) eval_type;
    typedef eval_type::node_attribute_type node_attribute_type;

    typedef gpcxx::basic_tree< node_attribute_type > tree_type;
    typedef std::vector< tree_type > population_type;
    typedef gpcxx::static_pipeline< population_type , fitness_type , rng_type > evolver_type;

    rng_type rng;

    training_data_type c;
    gpcxx::generate_regression_test_data( c , 1024 , rng , []( double x1 , double x2 , double x3 )
                    { return  x1 * x1 * x1 + 1.0 / 10.0 * x2 * x2 - 3.0 / 4.0 * ( x3 - 4.0 ) + 1.0 ; } );

    size_t population_size = 512;
    size_t number_elite = 1;
    double mutation_rate = 0.2;
    double crossover_rate = 0.6;
    double reproduction_rate = 0.3;
    size_t min_tree_height = 8 , max_tree_height = 8;

    auto terminal_gen = eval.get_terminal_symbol_distribution();
    auto unary_gen = eval.get_unary_symbol_distribution();
    auto binary_gen = eval.get_binary_symbol_distribution();
    std::array< double , 3 > weights = {{ 2.0 * double( terminal_gen.num_symbols() ) ,
                                       double( unary_gen.num_symbols() ) ,
                                       double( binary_gen.num_symbols() ) }};
    auto tree_generator = gpcxx::make_ramp( rng , terminal_gen , unary_gen , binary_gen , min_tree_height , max_tree_height , 0.5 , weights );

    evolver_type evolver( number_elite , mutation_rate , crossover_rate , reproduction_rate , rng );
    fitness_type fitness( population_size , 0.0 );
    population_type population( population_size );

    auto fitness_f = gpcxx::regression_fitness< eval_type >( eval );
    evolver.mutation_function() = gpcxx::make_mutation(
        gpcxx::make_simple_mutation_strategy( rng , terminal_gen , unary_gen , binary_gen ) ,
        gpcxx::make_random_selector( rng ) );
    evolver.crossover_function() = gpcxx::make_crossover(
        gpcxx::make_one_point_crossover_strategy( rng , 10 ) ,
        gpcxx::make_random_selector( rng ) );
    evolver.reproduction_function() = gpcxx::make_reproduce( gpcxx::make_random_selector( rng ) );

    // initialize population with random trees and evaluate fitness
    for( size_t i=0 ; i<population.size() ; ++i )
    {
        tree_generator( population[i] );
        fitness[i] = fitness_f( population[i] , c );
    }
    std::cout << "Best individuals" << std::endl << gpcxx::best_individuals( population , fitness ) << std::endl;
    std::cout << "Statistics : " << gpcxx::calc_population_statistics( population ) << std::endl;
    std::cout << std::endl << std::endl;

    for( size_t i=0 ; i<100 ; ++i )
    {
        evolver.next_generation( population , fitness );
        for( size_t i=0 ; i<population.size() ; ++i )
            fitness[i] = fitness_f( population[i] , c );

        std::cout << "Iteration " << i << std::endl;
        std::cout << "Best individuals" << std::endl << gpcxx::best_individuals( population , fitness , 1 ) << std::endl;
        std::cout << "Statistics : " << gpcxx::calc_population_statistics( population ) << std::endl << std::endl;
    }

    return 0;
}