/**
 * @date Fri Oct 14 18:07:56 2011 +0200
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 *
 * @brief Machines that implements the PLDA model
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <bob.core/assert.h>
#include <bob.core/check.h>
#include <bob.core/array_copy.h>
#include <bob.learn.em/PLDAMachine.h>
#include <bob.math/linear.h>
#include <bob.math/det.h>
#include <bob.math/inv.h>

#include <cmath>
#include <boost/lexical_cast.hpp>
#include <string>

bob::learn::em::PLDABase::PLDABase():
  m_variance_threshold(0.)
{
  resizeNoInit(0, 0, 0);
}

bob::learn::em::PLDABase::PLDABase(const size_t dim_d, const size_t dim_f,
    const size_t dim_g, const double variance_threshold):
  m_variance_threshold(variance_threshold)
{
  resize(dim_d, dim_f, dim_g);
}

bob::learn::em::PLDABase::PLDABase(const bob::learn::em::PLDABase& other):
  m_dim_d(other.m_dim_d),
  m_dim_f(other.m_dim_f),
  m_dim_g(other.m_dim_g),
  m_F(bob::core::array::ccopy(other.m_F)),
  m_G(bob::core::array::ccopy(other.m_G)),
  m_sigma(bob::core::array::ccopy(other.m_sigma)),
  m_mu(bob::core::array::ccopy(other.m_mu)),
  m_variance_threshold(other.m_variance_threshold),
  m_cache_isigma(bob::core::array::ccopy(other.m_cache_isigma)),
  m_cache_alpha(bob::core::array::ccopy(other.m_cache_alpha)),
  m_cache_beta(bob::core::array::ccopy(other.m_cache_beta)),
  m_cache_gamma(),
  m_cache_Ft_beta(bob::core::array::ccopy(other.m_cache_Ft_beta)),
  m_cache_Gt_isigma(bob::core::array::ccopy(other.m_cache_Gt_isigma)),
  m_cache_logdet_alpha(other.m_cache_logdet_alpha),
  m_cache_logdet_sigma(other.m_cache_logdet_sigma),
  m_cache_loglike_constterm(other.m_cache_loglike_constterm)
{
  bob::core::array::ccopy(other.m_cache_gamma, m_cache_gamma);
  resizeTmp();
}

bob::learn::em::PLDABase::PLDABase(bob::io::base::HDF5File& config) {
  load(config);
}

bob::learn::em::PLDABase::~PLDABase() {
}

bob::learn::em::PLDABase& bob::learn::em::PLDABase::operator=
    (const bob::learn::em::PLDABase& other)
{
  if (this != &other)
  {
    m_dim_d = other.m_dim_d;
    m_dim_f = other.m_dim_f;
    m_dim_g = other.m_dim_g;
    m_F.reference(bob::core::array::ccopy(other.m_F));
    m_G.reference(bob::core::array::ccopy(other.m_G));
    m_sigma.reference(bob::core::array::ccopy(other.m_sigma));
    m_mu.reference(bob::core::array::ccopy(other.m_mu));
    m_variance_threshold = other.m_variance_threshold;
    m_cache_isigma.reference(bob::core::array::ccopy(other.m_cache_isigma));
    m_cache_alpha.reference(bob::core::array::ccopy(other.m_cache_alpha));
    m_cache_beta.reference(bob::core::array::ccopy(other.m_cache_beta));
    bob::core::array::ccopy(other.m_cache_gamma, m_cache_gamma);
    m_cache_Ft_beta.reference(bob::core::array::ccopy(other.m_cache_Ft_beta));
    m_cache_Gt_isigma.reference(bob::core::array::ccopy(other.m_cache_Gt_isigma));
    m_cache_logdet_alpha = other.m_cache_logdet_alpha;
    m_cache_logdet_sigma = other.m_cache_logdet_sigma;
    m_cache_loglike_constterm = other.m_cache_loglike_constterm;
    resizeTmp();
  }
  return *this;
}

bool bob::learn::em::PLDABase::operator==
    (const bob::learn::em::PLDABase& b) const
{
  if (!(m_dim_d == b.m_dim_d && m_dim_f == b.m_dim_f &&
        m_dim_g == b.m_dim_g &&
        bob::core::array::isEqual(m_F, b.m_F) &&
        bob::core::array::isEqual(m_G, b.m_G) &&
        bob::core::array::isEqual(m_sigma, b.m_sigma) &&
        bob::core::array::isEqual(m_mu, b.m_mu) &&
        m_variance_threshold == b.m_variance_threshold &&
        bob::core::array::isEqual(m_cache_isigma, b.m_cache_isigma) &&
        bob::core::array::isEqual(m_cache_alpha, b.m_cache_alpha) &&
        bob::core::array::isEqual(m_cache_beta, b.m_cache_beta) &&
        bob::core::array::isEqual(m_cache_gamma, b.m_cache_gamma) &&
        bob::core::array::isEqual(m_cache_Ft_beta, b.m_cache_Ft_beta) &&
        bob::core::array::isEqual(m_cache_Gt_isigma, b.m_cache_Gt_isigma) &&
        m_cache_logdet_alpha == b.m_cache_logdet_alpha &&
        m_cache_logdet_sigma == b.m_cache_logdet_sigma))
    return false;

  // m_cache_loglike_constterm
  if (this->m_cache_loglike_constterm.size() != b.m_cache_loglike_constterm.size())
    return false;  // differing sizes, they are not the same
  std::map<size_t, double>::const_iterator i, j;
  for (i = this->m_cache_loglike_constterm.begin(), j = b.m_cache_loglike_constterm.begin();
    i != this->m_cache_loglike_constterm.end(); ++i, ++j)
  {
    if (i->first != j->first || i->second != j->second)
      return false;
  }

  return true;
}

bool bob::learn::em::PLDABase::operator!=
    (const bob::learn::em::PLDABase& b) const
{
  return !(this->operator==(b));
}

bool bob::learn::em::PLDABase::is_similar_to(const bob::learn::em::PLDABase& b,
  const double r_epsilon, const double a_epsilon) const
{
  return (m_dim_d == b.m_dim_d && m_dim_f == b.m_dim_f &&
          m_dim_g == b.m_dim_g &&
          bob::core::array::isClose(m_F, b.m_F, r_epsilon, a_epsilon) &&
          bob::core::array::isClose(m_G, b.m_G, r_epsilon, a_epsilon) &&
          bob::core::array::isClose(m_sigma, b.m_sigma, r_epsilon, a_epsilon) &&
          bob::core::array::isClose(m_mu, b.m_mu, r_epsilon, a_epsilon) &&
          bob::core::isClose(m_variance_threshold, b.m_variance_threshold, r_epsilon, a_epsilon) &&
          bob::core::array::isClose(m_cache_isigma, b.m_cache_isigma, r_epsilon, a_epsilon) &&
          bob::core::array::isClose(m_cache_alpha, b.m_cache_alpha, r_epsilon, a_epsilon) &&
          bob::core::array::isClose(m_cache_beta, b.m_cache_beta, r_epsilon, a_epsilon) &&
          bob::core::array::isClose(m_cache_gamma, b.m_cache_gamma, r_epsilon, a_epsilon) &&
          bob::core::array::isClose(m_cache_Ft_beta, b.m_cache_Ft_beta, r_epsilon, a_epsilon) &&
          bob::core::array::isClose(m_cache_Gt_isigma, b.m_cache_Gt_isigma, r_epsilon, a_epsilon) &&
          bob::core::isClose(m_cache_logdet_alpha, b.m_cache_logdet_alpha, r_epsilon, a_epsilon) &&
          bob::core::isClose(m_cache_logdet_sigma, b.m_cache_logdet_sigma, r_epsilon, a_epsilon) &&
          bob::core::isClose(m_cache_loglike_constterm, b.m_cache_loglike_constterm));
}

void bob::learn::em::PLDABase::load(bob::io::base::HDF5File& config)
{
  if (!config.contains("dim_d"))
  {
    // Then the model was saved using bob < 1.2.0
    //reads all data directly into the member variables
    m_F.reference(config.readArray<double,2>("F"));
    m_G.reference(config.readArray<double,2>("G"));
    m_dim_d = m_F.extent(0);
    m_dim_f = m_F.extent(1);
    m_dim_g = m_G.extent(1);
    m_sigma.reference(config.readArray<double,1>("sigma"));
    m_mu.reference(config.readArray<double,1>("mu"));
    m_cache_isigma.resize(m_dim_d);
    precomputeISigma();
    m_variance_threshold = 0.;
    m_cache_alpha.reference(config.readArray<double,2>("alpha"));
    m_cache_beta.reference(config.readArray<double,2>("beta"));
    // gamma and log like constant term (a-dependent terms)
    if (config.contains("a_indices"))
    {
      blitz::Array<uint32_t, 1> a_indices;
      a_indices.reference(config.readArray<uint32_t,1>("a_indices"));
      for (int i=0; i<a_indices.extent(0); ++i)
      {
        std::string str1 = "gamma_" + boost::lexical_cast<std::string>(a_indices(i));
        m_cache_gamma[a_indices(i)].reference(config.readArray<double,2>(str1));
        std::string str2 = "loglikeconstterm_" + boost::lexical_cast<std::string>(a_indices(i));
        m_cache_loglike_constterm[a_indices(i)] = config.read<double>(str2);
      }
    }
    m_cache_Ft_beta.reference(config.readArray<double,2>("Ft_beta"));
    m_cache_Gt_isigma.reference(config.readArray<double,2>("Gt_isigma"));
    m_cache_logdet_alpha = config.read<double>("logdet_alpha");
    m_cache_logdet_sigma = config.read<double>("logdet_sigma");
  }
  else
  {
    // Then the model was saved using bob >= 1.2.0
    //reads all data directly into the member variables
    m_F.reference(config.readArray<double,2>("F"));
    m_G.reference(config.readArray<double,2>("G"));
    // Conditional because previous versions had not these variables
    m_dim_d = config.read<uint64_t>("dim_d");
    m_dim_f = config.read<uint64_t>("dim_f");
    m_dim_g = config.read<uint64_t>("dim_g");
    m_sigma.reference(config.readArray<double,1>("sigma"));
    m_mu.reference(config.readArray<double,1>("mu"));
    m_cache_isigma.resize(m_dim_d);
    precomputeISigma();
    if (config.contains("variance_threshold"))
      m_variance_threshold = config.read<double>("variance_threshold");
    else if (config.contains("variance_thresholds")) // In case 1.2.0 alpha/beta version has been used
    {
      blitz::Array<double,1> tmp;
      tmp.reference(config.readArray<double,1>("variance_thresholds"));
      m_variance_threshold = tmp(0);
    }
    m_cache_alpha.reference(config.readArray<double,2>("alpha"));
    m_cache_beta.reference(config.readArray<double,2>("beta"));
    // gamma's (a-dependent terms)
    if(config.contains("a_indices_gamma"))
    {
      blitz::Array<uint32_t, 1> a_indices;
      a_indices.reference(config.readArray<uint32_t,1>("a_indices_gamma"));
      for(int i=0; i<a_indices.extent(0); ++i)
      {
        std::string str = "gamma_" + boost::lexical_cast<std::string>(a_indices(i));
        m_cache_gamma[a_indices(i)].reference(config.readArray<double,2>(str));
      }
    }
    // log likelihood constant term's (a-dependent terms)
    if(config.contains("a_indices_loglikeconstterm"))
    {
      blitz::Array<uint32_t, 1> a_indices;
      a_indices.reference(config.readArray<uint32_t,1>("a_indices_loglikeconstterm"));
      for(int i=0; i<a_indices.extent(0); ++i)
      {
        std::string str = "loglikeconstterm_" + boost::lexical_cast<std::string>(a_indices(i));
        m_cache_loglike_constterm[a_indices(i)] = config.read<double>(str);
      }
    }
    m_cache_Ft_beta.reference(config.readArray<double,2>("Ft_beta"));
    m_cache_Gt_isigma.reference(config.readArray<double,2>("Gt_isigma"));
    m_cache_logdet_alpha = config.read<double>("logdet_alpha");
    m_cache_logdet_sigma = config.read<double>("logdet_sigma");
  }
  resizeTmp();
}

void bob::learn::em::PLDABase::save(bob::io::base::HDF5File& config) const
{
  config.set("dim_d", (uint64_t)m_dim_d);
  config.set("dim_f", (uint64_t)m_dim_f);
  config.set("dim_g", (uint64_t)m_dim_g);
  config.setArray("F", m_F);
  config.setArray("G", m_G);
  config.setArray("sigma", m_sigma);
  config.setArray("mu", m_mu);
  config.set("variance_threshold", m_variance_threshold);
  config.setArray("alpha", m_cache_alpha);
  config.setArray("beta", m_cache_beta);
  // gamma's
  if(m_cache_gamma.size() > 0)
  {
    blitz::Array<uint32_t, 1> a_indices(m_cache_gamma.size());
    int i = 0;
    for(std::map<size_t,blitz::Array<double,2> >::const_iterator
        it=m_cache_gamma.begin(); it!=m_cache_gamma.end(); ++it)
    {
      a_indices(i) = it->first;
      std::string str = "gamma_" + boost::lexical_cast<std::string>(it->first);
      config.setArray(str, it->second);
      ++i;
    }
    config.setArray("a_indices_gamma", a_indices);
  }
  // log likelihood constant terms
  if(m_cache_loglike_constterm.size() > 0)
  {
    blitz::Array<uint32_t, 1> a_indices(m_cache_loglike_constterm.size());
    int i = 0;
    for(std::map<size_t,double>::const_iterator
        it=m_cache_loglike_constterm.begin(); it!=m_cache_loglike_constterm.end(); ++it)
    {
      a_indices(i) = it->first;
      std::string str = "loglikeconstterm_" + boost::lexical_cast<std::string>(it->first);
      config.set(str, it->second);
      ++i;
    }
    config.setArray("a_indices_loglikeconstterm", a_indices);
  }

  config.setArray("Ft_beta", m_cache_Ft_beta);
  config.setArray("Gt_isigma", m_cache_Gt_isigma);
  config.set("logdet_alpha", m_cache_logdet_alpha);
  config.set("logdet_sigma", m_cache_logdet_sigma);
}

void bob::learn::em::PLDABase::resizeNoInit(const size_t dim_d, const size_t dim_f,
    const size_t dim_g)
{
  m_dim_d = dim_d;
  m_dim_f = dim_f;
  m_dim_g = dim_g;
  m_F.resize(dim_d, dim_f);
  m_G.resize(dim_d, dim_g);
  m_sigma.resize(dim_d);
  m_mu.resize(dim_d);
  m_cache_alpha.resize(dim_g, dim_g);
  m_cache_beta.resize(dim_d, dim_d);
  m_cache_Ft_beta.resize(dim_f, dim_d);
  m_cache_Gt_isigma.resize(dim_g, dim_d);
  m_cache_gamma.clear();
  m_cache_isigma.resize(dim_d);
  m_cache_loglike_constterm.clear();
  resizeTmp();
}

void bob::learn::em::PLDABase::resizeTmp()
{
  m_tmp_d_1.resize(m_dim_d);
  m_tmp_d_2.resize(m_dim_d);
  m_tmp_d_ng_1.resize(m_dim_d, m_dim_g);
  m_tmp_nf_nf_1.resize(m_dim_f, m_dim_f);
  m_tmp_ng_ng_1.resize(m_dim_g, m_dim_g);
}

void bob::learn::em::PLDABase::resize(const size_t dim_d, const size_t dim_f,
    const size_t dim_g)
{
  resizeNoInit(dim_d, dim_f, dim_g);
  initMuFGSigma();
}

void bob::learn::em::PLDABase::setF(const blitz::Array<double,2>& F)
{
  bob::core::array::assertSameShape(F, m_F);
  m_F.reference(bob::core::array::ccopy(F));
  // Precomputes useful matrices
  precompute();
}

void bob::learn::em::PLDABase::setG(const blitz::Array<double,2>& G)
{
  bob::core::array::assertSameShape(G, m_G);
  m_G.reference(bob::core::array::ccopy(G));
  // Precomputes useful matrices and values
  precompute();
  precomputeLogDetAlpha();
}

void bob::learn::em::PLDABase::setSigma(const blitz::Array<double,1>& sigma)
{
  bob::core::array::assertSameShape(sigma, m_sigma);
  m_sigma.reference(bob::core::array::ccopy(sigma));
  // Apply variance flooring threshold: This will also
  // call the precompute() and precomputeLogLike() methods!
  applyVarianceThreshold();
}

void bob::learn::em::PLDABase::setMu(const blitz::Array<double,1>& mu)
{
  bob::core::array::assertSameShape(mu, m_mu);
  m_mu.reference(bob::core::array::ccopy(mu));
}

void bob::learn::em::PLDABase::setVarianceThreshold(const double value)
{
  // Variance flooring
  m_variance_threshold = value;
  // Apply variance flooring thresholds: This will also
  // call the precompute() and precomputeLogLike() methods!
  applyVarianceThreshold();
}

void bob::learn::em::PLDABase::applyVarianceThreshold()
{
   // Apply variance flooring threshold
  m_sigma = blitz::where( m_sigma < m_variance_threshold, m_variance_threshold, m_sigma);
  // Re-compute constants, because m_sigma has changed
  precompute();
  precomputeLogLike();
}

const blitz::Array<double,2>& bob::learn::em::PLDABase::getGamma(const size_t a) const
{
  if(!hasGamma(a))
    throw std::runtime_error("Gamma for this number of samples is not currently in cache. You could use the getAddGamma() method instead");
  return (m_cache_gamma.find(a))->second;
}

const blitz::Array<double,2>& bob::learn::em::PLDABase::getAddGamma(const size_t a)
{
  if(!hasGamma(a)) precomputeGamma(a);
  return m_cache_gamma[a];
}

void bob::learn::em::PLDABase::initMuFGSigma()
{
  // To avoid problems related to precomputation
  m_mu = 0.;
  bob::math::eye(m_F);
  bob::math::eye(m_G);
  m_sigma = 1.;
  // Precompute variables
  precompute();
  precomputeLogLike();
}

void bob::learn::em::PLDABase::precompute()
{
  precomputeISigma();
  precomputeGtISigma();
  precomputeAlpha();
  precomputeBeta();
  m_cache_gamma.clear();
  precomputeFtBeta();
  m_cache_loglike_constterm.clear();
}

void bob::learn::em::PLDABase::precomputeLogLike()
{
  precomputeLogDetAlpha();
  precomputeLogDetSigma();
}

void bob::learn::em::PLDABase::precomputeISigma()
{
  // Updates inverse of sigma
  m_cache_isigma = 1. / m_sigma;
}

void bob::learn::em::PLDABase::precomputeGtISigma()
{
  // m_cache_Gt_isigma = G^T \Sigma^{-1}
  blitz::firstIndex i;
  blitz::secondIndex j;
  blitz::Array<double,2> Gt = m_G.transpose(1,0);
  m_cache_Gt_isigma = Gt(i,j) * m_cache_isigma(j);
}

void bob::learn::em::PLDABase::precomputeAlpha()
{
  // alpha = (Id + G^T.sigma^-1.G)^-1

  // m_tmp_ng_ng_1 = G^T.sigma^-1.G
  bob::math::prod(m_cache_Gt_isigma, m_G, m_tmp_ng_ng_1);
  // m_tmp_ng_ng_1 = Id + G^T.sigma^-1.G
  for(int i=0; i<m_tmp_ng_ng_1.extent(0); ++i) m_tmp_ng_ng_1(i,i) += 1;
  // m_cache_alpha = (Id + G^T.sigma^-1.G)^-1
  bob::math::inv(m_tmp_ng_ng_1, m_cache_alpha);
}

void bob::learn::em::PLDABase::precomputeBeta()
{
  // beta = (sigma + G.G^T)^-1
  // BUT, there is a more efficient computation (Woodbury identity):
  // beta = sigma^-1 - sigma^-1.G.(Id + G^T.sigma^-1.G)^-1.G^T.sigma^-1
  // beta =  sigma^-1 - sigma^-1.G.alpha.G^T.sigma^-1

  blitz::Array<double,2> GtISigmaT = m_cache_Gt_isigma.transpose(1,0);
  // m_tmp_d_ng_1 = sigma^-1.G.alpha
  bob::math::prod(GtISigmaT, m_cache_alpha, m_tmp_d_ng_1);
  // m_cache_beta = -sigma^-1.G.alpha.G^T.sigma^-1
  bob::math::prod(m_tmp_d_ng_1, m_cache_Gt_isigma, m_cache_beta);
  m_cache_beta = -m_cache_beta;
  // m_cache_beta = sigma^-1 - sigma^-1.G.alpha.G^T.sigma^-1
  for(int i=0; i<m_cache_beta.extent(0); ++i) m_cache_beta(i,i) += m_cache_isigma(i);
}

void bob::learn::em::PLDABase::precomputeGamma(const size_t a)
{

  blitz::Array<double,2> gamma_a(getDimF(),getDimF());
  m_cache_gamma[a].reference(gamma_a);
  computeGamma(a, gamma_a);
}

void bob::learn::em::PLDABase::precomputeFtBeta()
{
  // m_cache_Ft_beta = F^T.beta = F^T.(sigma + G.G^T)^-1
  blitz::Array<double,2> Ft = m_F.transpose(1,0);
  bob::math::prod(Ft, m_cache_beta, m_cache_Ft_beta);
}

void bob::learn::em::PLDABase::computeGamma(const size_t a,
  blitz::Array<double,2> res) const
{
  // gamma = (Id + a.F^T.beta.F)^-1

  // Checks destination size
  bob::core::array::assertSameShape(res, m_tmp_nf_nf_1);
  // m_tmp_nf_nf_1 = F^T.beta.F
  bob::math::prod(m_cache_Ft_beta, m_F, m_tmp_nf_nf_1);
   // m_tmp_nf_nf_1 = a.F^T.beta.F
  m_tmp_nf_nf_1 *= static_cast<double>(a);
  // m_tmp_nf_nf_1 = Id + a.F^T.beta.F
  for(int i=0; i<m_tmp_nf_nf_1.extent(0); ++i) m_tmp_nf_nf_1(i,i) += 1;

  // res = (Id + a.F^T.beta.F)^-1
  bob::math::inv(m_tmp_nf_nf_1, res);
}

void bob::learn::em::PLDABase::precomputeLogDetAlpha()
{
  int sign;
  m_cache_logdet_alpha = bob::math::slogdet(m_cache_alpha, sign);
}

void bob::learn::em::PLDABase::precomputeLogDetSigma()
{
  m_cache_logdet_sigma = blitz::sum(blitz::log(m_sigma));
}

double bob::learn::em::PLDABase::computeLogLikeConstTerm(const size_t a,
  const blitz::Array<double,2>& gamma_a) const
{
  // loglike_constterm[a] = a/2 *
  //  ( -D*log(2*pi) -log|sigma| +log|alpha| +log|gamma_a|)
  int sign;
  double logdet_gamma_a = bob::math::slogdet(gamma_a, sign);
  double ah = static_cast<double>(a)/2.;
  double res = ( -ah*((double)m_dim_d)*log(2*M_PI) -
      ah*m_cache_logdet_sigma + ah*m_cache_logdet_alpha + logdet_gamma_a/2.);
  return res;
}

double bob::learn::em::PLDABase::computeLogLikeConstTerm(const size_t a)
{
  const blitz::Array<double,2>& gamma_a = getAddGamma(a);
  return computeLogLikeConstTerm(a, gamma_a);
}

void bob::learn::em::PLDABase::precomputeLogLikeConstTerm(const size_t a)
{
  double val = computeLogLikeConstTerm(a);
  m_cache_loglike_constterm[a] = val;
}

double bob::learn::em::PLDABase::getLogLikeConstTerm(const size_t a) const
{
  if(!hasLogLikeConstTerm(a))
    throw std::runtime_error("The LogLikelihood constant term for this number of samples is not currently in cache. You could use the getAddLogLikeConstTerm() method instead");
  return (m_cache_loglike_constterm.find(a))->second;
}

double bob::learn::em::PLDABase::getAddLogLikeConstTerm(const size_t a)
{
  if(!hasLogLikeConstTerm(a)) precomputeLogLikeConstTerm(a);
  return m_cache_loglike_constterm[a];
}

void bob::learn::em::PLDABase::clearMaps()
{
  m_cache_gamma.clear();
  m_cache_loglike_constterm.clear();
}

double bob::learn::em::PLDABase::computeLogLikelihoodPointEstimate(
  const blitz::Array<double,1>& xij, const blitz::Array<double,1>& hi,
  const blitz::Array<double,1>& wij) const
{
  // Check inputs
  bob::core::array::assertSameDimensionLength(xij.extent(0), getDimD());
  bob::core::array::assertSameDimensionLength(hi.extent(0), getDimF());
  bob::core::array::assertSameDimensionLength(wij.extent(0), getDimG());
  // Computes: -D/2 log(2pi) -1/2 log(det(\Sigma))
  //   -1/2 {(x_{ij}-(\mu+Fh_{i}+Gw_{ij}))^{T}\Sigma^{-1}(x_{ij}-(\mu+Fh_{i}+Gw_{ij}))}
  double res = -0.5*((double)m_dim_d)*log(2*M_PI) - 0.5*m_cache_logdet_sigma;
  // m_tmp_d_1 = (x_{ij} - (\mu+Fh_{i}+Gw_{ij}))
  m_tmp_d_1 = xij - m_mu;
  bob::math::prod(m_F, hi, m_tmp_d_2);
  m_tmp_d_1 -= m_tmp_d_2;
  bob::math::prod(m_G, wij, m_tmp_d_2);
  m_tmp_d_1 -= m_tmp_d_2;
  // add third term to res
  res += -0.5*blitz::sum(blitz::pow2(m_tmp_d_1) * m_cache_isigma);
  return res;
}

namespace bob { namespace learn { namespace em {
  /**
   * @brief Prints a PLDABase in the output stream. This will print
   * the values of the parameters \f$\mu\f$, \f$F\f$, \f$G\f$ and
   * \f$\Sigma\f$ of the PLDA model.
   */
  std::ostream& operator<<(std::ostream& os, const PLDABase& m) {
    os << "mu = " << m.m_mu << std::endl;
    os << "sigma = " << m.m_sigma << std::endl;
    os << "F = " << m.m_F << std::endl;
    os << "G = " << m.m_G << std::endl;
    return os;
  }
} } }

bob::learn::em::PLDAMachine::PLDAMachine():
  m_plda_base(),
  m_n_samples(0), m_nh_sum_xit_beta_xi(0), m_weighted_sum(0),
  m_loglikelihood(0), m_cache_gamma(), m_cache_loglike_constterm(),
  m_tmp_d_1(0), m_tmp_d_2(0), m_tmp_nf_1(0), m_tmp_nf_2(0), m_tmp_nf_nf_1(0,0)
{
}

bob::learn::em::PLDAMachine::PLDAMachine(const boost::shared_ptr<bob::learn::em::PLDABase> plda_base):
  m_plda_base(plda_base),
  m_n_samples(0), m_nh_sum_xit_beta_xi(0), m_weighted_sum(plda_base->getDimF()),
  m_loglikelihood(0), m_cache_gamma(), m_cache_loglike_constterm()
{
  resizeTmp();
}

bob::learn::em::PLDAMachine::PLDAMachine(const bob::learn::em::PLDAMachine& other):
  m_plda_base(other.m_plda_base),
  m_n_samples(other.m_n_samples),
  m_nh_sum_xit_beta_xi(other.m_nh_sum_xit_beta_xi),
  m_weighted_sum(bob::core::array::ccopy(other.m_weighted_sum)),
  m_loglikelihood(other.m_loglikelihood), m_cache_gamma(),
  m_cache_loglike_constterm(other.m_cache_loglike_constterm)
{
  bob::core::array::ccopy(other.m_cache_gamma, m_cache_gamma);
  resizeTmp();
}

bob::learn::em::PLDAMachine::PLDAMachine(bob::io::base::HDF5File& config,
    const boost::shared_ptr<bob::learn::em::PLDABase> plda_base):
  m_plda_base(plda_base)
{
  load(config);
}

bob::learn::em::PLDAMachine::~PLDAMachine() {
}

bob::learn::em::PLDAMachine& bob::learn::em::PLDAMachine::operator=
(const bob::learn::em::PLDAMachine& other)
{
  if(this!=&other)
  {
    m_plda_base = other.m_plda_base;
    m_n_samples = other.m_n_samples;
    m_nh_sum_xit_beta_xi = other.m_nh_sum_xit_beta_xi;
    m_weighted_sum.reference(bob::core::array::ccopy(other.m_weighted_sum));
    m_loglikelihood = other.m_loglikelihood;
    bob::core::array::ccopy(other.m_cache_gamma, m_cache_gamma);
    m_cache_loglike_constterm = other.m_cache_loglike_constterm;
    resizeTmp();
  }
  return *this;
}

bool bob::learn::em::PLDAMachine::operator==
    (const bob::learn::em::PLDAMachine& b) const
{
  if (!(( (!m_plda_base && !b.m_plda_base) ||
          ((m_plda_base && b.m_plda_base) && *(m_plda_base) == *(b.m_plda_base))) &&
        m_n_samples == b.m_n_samples &&
        m_nh_sum_xit_beta_xi ==b.m_nh_sum_xit_beta_xi &&
        bob::core::array::isEqual(m_weighted_sum, b.m_weighted_sum) &&
        m_loglikelihood == b.m_loglikelihood &&
        bob::core::array::isEqual(m_cache_gamma, b.m_cache_gamma)))
    return false;

  // m_cache_loglike_constterm
  if (this->m_cache_loglike_constterm.size() != b.m_cache_loglike_constterm.size())
    return false;  // differing sizes, they are not the same
  std::map<size_t, double>::const_iterator i, j;
  for (i = this->m_cache_loglike_constterm.begin(), j = b.m_cache_loglike_constterm.begin();
    i != this->m_cache_loglike_constterm.end(); ++i, ++j)
  {
    if (i->first != j->first || i->second != j->second)
      return false;
  }

  return true;
}

bool bob::learn::em::PLDAMachine::operator!=
    (const bob::learn::em::PLDAMachine& b) const
{
  return !(this->operator==(b));
}

bool bob::learn::em::PLDAMachine::is_similar_to(
  const bob::learn::em::PLDAMachine& b, const double r_epsilon,
  const double a_epsilon) const
{
  return (( (!m_plda_base && !b.m_plda_base) ||
            ((m_plda_base && b.m_plda_base) &&
             m_plda_base->is_similar_to(*(b.m_plda_base), r_epsilon, a_epsilon))) &&
          m_n_samples == b.m_n_samples &&
          bob::core::isClose(m_nh_sum_xit_beta_xi, b.m_nh_sum_xit_beta_xi, r_epsilon, a_epsilon) &&
          bob::core::array::isClose(m_weighted_sum, b.m_weighted_sum, r_epsilon, a_epsilon) &&
          bob::core::isClose(m_loglikelihood, b.m_loglikelihood, r_epsilon, a_epsilon) &&
          bob::core::array::isClose(m_cache_gamma, b.m_cache_gamma, r_epsilon, a_epsilon) &&
          bob::core::isClose(m_cache_loglike_constterm, b.m_cache_loglike_constterm, r_epsilon, a_epsilon));
}

void bob::learn::em::PLDAMachine::load(bob::io::base::HDF5File& config)
{
  //reads all data directly into the member variables
  m_n_samples = config.read<uint64_t>("n_samples");
  m_nh_sum_xit_beta_xi = config.read<double>("nh_sum_xit_beta_xi");
  m_weighted_sum.reference(config.readArray<double,1>("weighted_sum"));
  m_loglikelihood = config.read<double>("loglikelihood");
  // gamma and log like constant term (a-dependent terms)
  clearMaps();
  if(config.contains("a_indices"))
  {
    blitz::Array<uint32_t, 1> a_indices;
    a_indices.reference(config.readArray<uint32_t,1>("a_indices"));
    for(int i=0; i<a_indices.extent(0); ++i)
    {
      std::string str1 = "gamma_" + boost::lexical_cast<std::string>(a_indices(i));
      m_cache_gamma[a_indices(i)].reference(config.readArray<double,2>(str1));
      std::string str2 = "loglikeconstterm_" + boost::lexical_cast<std::string>(a_indices(i));
      m_cache_loglike_constterm[a_indices(i)] = config.read<double>(str2);
    }
  }
  resizeTmp();
}

void bob::learn::em::PLDAMachine::save(bob::io::base::HDF5File& config) const
{
  config.set("n_samples", m_n_samples);
  config.set("nh_sum_xit_beta_xi", m_nh_sum_xit_beta_xi);
  config.setArray("weighted_sum", m_weighted_sum);
  config.set("loglikelihood", m_loglikelihood);
  // Gamma
  if(m_cache_gamma.size() > 0)
  {
    blitz::Array<uint32_t, 1> a_indices(m_cache_gamma.size());
    int i = 0;
    for(std::map<size_t,blitz::Array<double,2> >::const_iterator
        it=m_cache_gamma.begin(); it!=m_cache_gamma.end(); ++it)
    {
      a_indices(i) = it->first;
      std::string str1 = "gamma_" + boost::lexical_cast<std::string>(it->first);
      config.setArray(str1, it->second);
      std::string str2 = "loglikeconstterm_" + boost::lexical_cast<std::string>(it->first);
      double v = m_cache_loglike_constterm.find(it->first)->second;
      config.set(str2, v);
      ++i;
    }
    config.setArray("a_indices", a_indices);
  }
}

void bob::learn::em::PLDAMachine::setPLDABase(const boost::shared_ptr<bob::learn::em::PLDABase> plda_base)
{
  m_plda_base = plda_base;
  m_weighted_sum.resizeAndPreserve(getDimF());
  clearMaps();
  resizeTmp();
}

void bob::learn::em::PLDAMachine::setWeightedSum(const blitz::Array<double,1>& ws)
{
  if(ws.extent(0) != m_weighted_sum.extent(0)) {
    boost::format m("size of parameter `ws' (%d) does not match the expected size (%d)");
    m % ws.extent(0) % m_weighted_sum.extent(0);
    throw std::runtime_error(m.str());
  }
  m_weighted_sum.reference(bob::core::array::ccopy(ws));
}

const blitz::Array<double,2>& bob::learn::em::PLDAMachine::getGamma(const size_t a) const
{
  // Checks in both base machine and this machine
  if (m_plda_base->hasGamma(a)) return m_plda_base->getGamma(a);
  else if (!hasGamma(a))
    throw std::runtime_error("Gamma for this number of samples is not currently in cache. You could use the getAddGamma() method instead");
  return (m_cache_gamma.find(a))->second;
}

const blitz::Array<double,2>& bob::learn::em::PLDAMachine::getAddGamma(const size_t a)
{
  if (m_plda_base->hasGamma(a)) return m_plda_base->getGamma(a);
  else if (hasGamma(a)) return m_cache_gamma[a];
  // else computes it and adds it to this machine
  blitz::Array<double,2> gamma_a(getDimF(),getDimF());
  m_cache_gamma[a].reference(gamma_a);
  m_plda_base->computeGamma(a, gamma_a);
  return m_cache_gamma[a];
}

double bob::learn::em::PLDAMachine::getLogLikeConstTerm(const size_t a) const
{
  // Checks in both base machine and this machine
  if (!m_plda_base) throw std::runtime_error("No PLDABase set to this machine");
  if (m_plda_base->hasLogLikeConstTerm(a)) return m_plda_base->getLogLikeConstTerm(a);
  else if (!hasLogLikeConstTerm(a))
    throw std::runtime_error("The LogLikelihood constant term for this number of samples is not currently in cache. You could use the getAddLogLikeConstTerm() method instead");
  return (m_cache_loglike_constterm.find(a))->second;
}

double bob::learn::em::PLDAMachine::getAddLogLikeConstTerm(const size_t a)
{
  if (!m_plda_base) throw std::runtime_error("No PLDABase set to this machine");
  if (m_plda_base->hasLogLikeConstTerm(a)) return m_plda_base->getLogLikeConstTerm(a);
  else if (hasLogLikeConstTerm(a)) return m_cache_loglike_constterm[a];
  // else computes it and adds it to this machine
  m_cache_loglike_constterm[a] =
        m_plda_base->computeLogLikeConstTerm(a, getAddGamma(a));
  return m_cache_loglike_constterm[a];
}

void bob::learn::em::PLDAMachine::clearMaps()
{
  m_cache_gamma.clear();
  m_cache_loglike_constterm.clear();
}

double bob::learn::em::PLDAMachine::forward(const blitz::Array<double,1>& sample)
{
  return forward_(sample);
}

double bob::learn::em::PLDAMachine::forward_(const blitz::Array<double,1>& sample)
{
  // Computes the log likelihood ratio
  return computeLogLikelihood(sample, true) - // match
          (computeLogLikelihood(sample, false) + m_loglikelihood); // no match
}

double bob::learn::em::PLDAMachine::forward(const blitz::Array<double,2>& samples)
{
  // Computes the log likelihood ratio
  return computeLogLikelihood(samples, true) - // match
          (computeLogLikelihood(samples, false) + m_loglikelihood); // no match
}

double bob::learn::em::PLDAMachine::computeLogLikelihood(const blitz::Array<double,1>& sample,
  bool enroll) const
{
  if (!m_plda_base) throw std::runtime_error("No PLDABase set to this machine");
  // Check dimensionality
  bob::core::array::assertSameDimensionLength(sample.extent(0), getDimD());

  int n_samples = 1 + (enroll?m_n_samples:0);

  // 3/ Third term of the likelihood: -1/2*X^T*(SIGMA+A.A^T)^-1*X
  //    Efficient way: -1/2*sum_i(xi^T.sigma^-1.xi - xi^T.sigma^-1*G*(I+G^T.sigma^-1.G)^-1*G^T*sigma^-1.xi
  //      -1/2*sumWeighted^T*(I+aF^T.(sigma^-1-sigma^-1*G*(I+G^T.sigma^-1.G)^-1*G^T*sigma^-1).F)^-1*sumWeighted
  //      where sumWeighted = sum_i(F^T*(sigma^-1-sigma^-1*G*(I+G^T.sigma^-1.G)^-1*G^T*sigma^-1)*xi)
  const blitz::Array<double,2>& beta = getPLDABase()->getBeta();
  const blitz::Array<double,2>& Ft_beta = getPLDABase()->getFtBeta();
  const blitz::Array<double,1>& mu = getPLDABase()->getMu();
  double terma = (enroll?m_nh_sum_xit_beta_xi:0.);
  // sumWeighted
  if (enroll && m_n_samples > 0) m_tmp_nf_1 = m_weighted_sum;
  else m_tmp_nf_1 = 0;

  // terma += -1 / 2. * (xi^t*beta*xi)
  m_tmp_d_1 = sample - mu;
  bob::math::prod(beta, m_tmp_d_1, m_tmp_d_2);
  terma += -1 / 2. * (blitz::sum(m_tmp_d_1*m_tmp_d_2));

  // sumWeighted
  bob::math::prod(Ft_beta, m_tmp_d_1, m_tmp_nf_2);
  m_tmp_nf_1 += m_tmp_nf_2;
  blitz::Array<double,2> gamma_a;
  if (hasGamma(n_samples) || m_plda_base->hasGamma(n_samples))
    gamma_a.reference(getGamma(n_samples));
  else
  {
    gamma_a.reference(m_tmp_nf_nf_1);
    m_plda_base->computeGamma(n_samples, gamma_a);
  }
  bob::math::prod(gamma_a, m_tmp_nf_1, m_tmp_nf_2);
  double termb = 1 / 2. * (blitz::sum(m_tmp_nf_1*m_tmp_nf_2));

  // 1/2/ Constant term of the log likelihood:
  //      1/ First term of the likelihood: -Nsamples*D/2*log(2*PI)
  //      2/ Second term of the likelihood: -1/2*log(det(SIGMA+A.A^T))
  //        Efficient way: -Nsamples/2*log(det(sigma))-Nsamples/2*log(det(I+G^T.sigma^-1.G))
  //       -1/2*log(det(I+aF^T.(sigma^-1-sigma^-1*G*(I+G^T.sigma^-1.G)*G^T*sigma^-1).F))
  double log_likelihood; // = getAddLogLikeConstTerm(static_cast<size_t>(n_samples));
  if (hasLogLikeConstTerm(n_samples) || m_plda_base->hasLogLikeConstTerm(n_samples))
    log_likelihood = getLogLikeConstTerm(n_samples);
  else
    log_likelihood = m_plda_base->computeLogLikeConstTerm(n_samples, gamma_a);

  log_likelihood += terma + termb;
  return log_likelihood;
}

double bob::learn::em::PLDAMachine::computeLogLikelihood(const blitz::Array<double,2>& samples,
  bool enroll) const
{
  if (!m_plda_base) throw std::runtime_error("No PLDABase set to this machine");
  // Check dimensionality
  bob::core::array::assertSameDimensionLength(samples.extent(1), getDimD());

  int n_samples = samples.extent(0) + (enroll?m_n_samples:0);
  // 3/ Third term of the likelihood: -1/2*X^T*(SIGMA+A.A^T)^-1*X
  //    Efficient way: -1/2*sum_i(xi^T.sigma^-1.xi - xi^T.sigma^-1*G*(I+G^T.sigma^-1.G)^-1*G^T*sigma^-1.xi
  //      -1/2*sumWeighted^T*(I+aF^T.(sigma^-1-sigma^-1*G*(I+G^T.sigma^-1.G)^-1*G^T*sigma^-1).F)^-1*sumWeighted
  //      where sumWeighted = sum_i(F^T*(sigma^-1-sigma^-1*G*(I+G^T.sigma^-1.G)^-1*G^T*sigma^-1)*xi)
  const blitz::Array<double,2>& beta = getPLDABase()->getBeta();
  const blitz::Array<double,2>& Ft_beta = getPLDABase()->getFtBeta();
  const blitz::Array<double,1>& mu = getPLDABase()->getMu();
  double terma = (enroll?m_nh_sum_xit_beta_xi:0.);
  // sumWeighted
  if (enroll && m_n_samples > 0) m_tmp_nf_1 = m_weighted_sum;
  else m_tmp_nf_1 = 0;
  for (int k=0; k<samples.extent(0); ++k)
  {
    blitz::Array<double,1> samp = samples(k,blitz::Range::all());
    m_tmp_d_1 = samp - mu;
    // terma += -1 / 2. * (xi^t*beta*xi)
    bob::math::prod(beta, m_tmp_d_1, m_tmp_d_2);
    terma += -1 / 2. * (blitz::sum(m_tmp_d_1*m_tmp_d_2));

    // sumWeighted
    bob::math::prod(Ft_beta, m_tmp_d_1, m_tmp_nf_2);
    m_tmp_nf_1 += m_tmp_nf_2;
  }

  blitz::Array<double,2> gamma_a;
  if (hasGamma(n_samples) || m_plda_base->hasGamma(n_samples))
    gamma_a.reference(getGamma(n_samples));
  else
  {
    gamma_a.reference(m_tmp_nf_nf_1);
    m_plda_base->computeGamma(n_samples, gamma_a);
  }
  bob::math::prod(gamma_a, m_tmp_nf_1, m_tmp_nf_2);
  double termb = 1 / 2. * (blitz::sum(m_tmp_nf_1*m_tmp_nf_2));

  // 1/2/ Constant term of the log likelihood:
  //      1/ First term of the likelihood: -Nsamples*D/2*log(2*PI)
  //      2/ Second term of the likelihood: -1/2*log(det(SIGMA+A.A^T))
  //        Efficient way: -Nsamples/2*log(det(sigma))-Nsamples/2*log(det(I+G^T.sigma^-1.G))
  //       -1/2*log(det(I+aF^T.(sigma^-1-sigma^-1*G*(I+G^T.sigma^-1.G)*G^T*sigma^-1).F))
  double log_likelihood; // = getAddLogLikeConstTerm(static_cast<size_t>(n_samples));
  if (hasLogLikeConstTerm(n_samples) || m_plda_base->hasLogLikeConstTerm(n_samples))
    log_likelihood = getLogLikeConstTerm(n_samples);
  else
    log_likelihood = m_plda_base->computeLogLikeConstTerm(n_samples, gamma_a);

  log_likelihood += terma + termb;
  return log_likelihood;
}

void bob::learn::em::PLDAMachine::resize(const size_t dim_d, const size_t dim_f,
  const size_t dim_g)
{
  m_weighted_sum.resizeAndPreserve(dim_f);
  clearMaps();
  resizeTmp();
}

void bob::learn::em::PLDAMachine::resizeTmp()
{
  if (m_plda_base)
  {
    m_tmp_d_1.resize(getDimD());
    m_tmp_d_2.resize(getDimD());
    m_tmp_nf_1.resize(getDimF());
    m_tmp_nf_2.resize(getDimF());
    m_tmp_nf_nf_1.resize(getDimF(), getDimF());
  }
}


// BSD 3-Clause License

// Copyright (c) 2021, Chenyu
// All rights reserved.

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

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

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

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

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

#include "rotation_averaging/lagrange_dual_rotation_estimator.h"

#include <ceres/rotation.h>
#include <glog/logging.h>
#include <omp.h>

#include <Eigen/Eigenvalues>
#include <Eigen/QR>
#include <algorithm>
#include <cmath>
#include <iomanip>
#include <limits>

#include "Spectra/MatOp/SparseSymMatProd.h"
#include "Spectra/SymEigsSolver.h"

#include "rotation_averaging/internal/rotation_estimator_util.h"
#include "solver/bcm_sdp_solver.h"
#include "solver/rbr_sdp_solver.h"
#include "solver/rank_restricted_sdp_solver.h"
#include "solver/riemannian_staircase.h"

namespace gopt {
LagrangeDualRotationEstimator::LagrangeDualRotationEstimator(const int N,
                                                             const int dim)
    : LagrangeDualRotationEstimator(N, dim, solver::SDPSolverOptions()) {}

LagrangeDualRotationEstimator::LagrangeDualRotationEstimator(
    const int N, const int dim, const solver::SDPSolverOptions& options)
    : options_(options), images_num_(N), dim_(dim) {
  R_ = Eigen::SparseMatrix<double>(dim_ * N, dim_ * N);
  alpha_max_ = 0.0;
}

void LagrangeDualRotationEstimator::SetViewIdToIndex(
    const std::unordered_map<image_t, int>& view_id_to_index) {
  view_id_to_index_ = view_id_to_index;
}

void LagrangeDualRotationEstimator::SetRAOption(
    const solver::SDPSolverOptions& options) {
  options_ = options;
}

const solver::Summary& LagrangeDualRotationEstimator::GetRASummary() const {
  return summary_;
}

double LagrangeDualRotationEstimator::GetErrorBound() const {
  return alpha_max_;
}

bool LagrangeDualRotationEstimator::EstimateRotations(
    const std::unordered_map<ImagePair, TwoViewGeometry>& view_pairs,
    std::unordered_map<image_t, Eigen::Vector3d>* global_rotations) {
  const int N = images_num_;

  CHECK_GT(view_pairs.size(), 0);
  CHECK_GT(N, 0);

  if (view_id_to_index_.empty()) {
    internal::ViewIdToAscentIndex(*global_rotations, &view_id_to_index_);
  }

  // Set for R_
  std::unordered_map<size_t, std::vector<size_t>> adj_edges;
  FillinRelativeGraph(view_pairs, R_, adj_edges);

  std::unique_ptr<solver::SDPSolver> solver = this->CreateSDPSolver(N, dim_);
  solver->SetCovariance(-R_);
  solver->SetAdjacentEdges(adj_edges);
  solver->Solve(summary_);
  Y_ = solver->GetSolution();

  RetrieveRotations(Y_, global_rotations);

  LOG(INFO) << "LagrangeDual converged in "
            << summary_.total_iterations_num << " iterations.";
  LOG(INFO) << "Total time [LagrangeDual]: " << summary_.TotalTime() << " ms.";

  return true;
}

void LagrangeDualRotationEstimator::ComputeErrorBound(
    const std::unordered_map<ImagePair, TwoViewGeometry>& view_pairs) {
  const int N = images_num_;

  std::vector<Eigen::Triplet<double>> a_triplets;
  // adjacency matrix
  Eigen::SparseMatrix<double> A(N, N);
  // degree matrix (a diagonal matrix)
  Eigen::SparseMatrix<double> D(N, N);
  std::vector<Eigen::Triplet<double>> d_triplets;
  std::vector<double> degrees(N, 0);

  for (auto& view_pair : view_pairs) {
    ImagePair pair = view_pair.first;
    const int i = view_id_to_index_[pair.first];
    const int j = view_id_to_index_[pair.second];
    degrees[i]++;
    degrees[j]++;
  }

  double max_degree = 0;
  for (auto& view_pair : view_pairs) {
    ImagePair pair = view_pair.first;
    const int i = view_id_to_index_[pair.first];
    const int j = view_id_to_index_[pair.second];

    a_triplets.push_back(Eigen::Triplet<double>(i, j, 1.0));
    a_triplets.push_back(Eigen::Triplet<double>(j, i, 1.0));

    d_triplets.push_back(Eigen::Triplet<double>(i, i, degrees[i]));
    d_triplets.push_back(Eigen::Triplet<double>(j, j, degrees[j]));
    max_degree = std::max(std::max(max_degree, degrees[i]), degrees[j]);
  }
  A.setFromTriplets(a_triplets.begin(), a_triplets.end());
  A.makeCompressed();
  D.setFromTriplets(d_triplets.begin(), d_triplets.end());
  D.makeCompressed();

  // laplacian matrix
  Eigen::SparseMatrix<double> L = D - A;

  // compute the bound of residual error
  Spectra::SparseSymMatProd<double> op(L);
  Spectra::SymEigsSolver<double, Spectra::SMALLEST_ALGE,
                         Spectra::SparseSymMatProd<double>>
      eigs(&op, 2, 5);
  eigs.init();
  eigs.compute();

  double lambda2 = 0.0;
  if (eigs.info() == Spectra::SUCCESSFUL) {
    lambda2 = eigs.eigenvalues()[0];
  } else {
    LOG(INFO) << "Computing Eigenvalue fails";
  }

  // get the second smallest eigen value
  alpha_max_ =
      2 * std::asin(std::sqrt(0.25 + lambda2 / (2.0 * max_degree)) - 0.5);
}

void LagrangeDualRotationEstimator::RetrieveRotations(
    const Eigen::MatrixXd& Y,
    std::unordered_map<image_t, Eigen::Vector3d>* global_rotations) {
  for (auto orientation : *global_rotations) {
    image_t view_id = orientation.first;
    const int i = view_id_to_index_[view_id];
    // After fixing Equ.(10)
    Eigen::Matrix3d R = Y.block(0, 3 * i, 3, 3).transpose();
    if (R.determinant() < 0) R = -R;

    // CHECK_GE(R.determinant(), 0);
    // CHECK_NEAR(R.determinant(), 1, 1e-8);

    Eigen::Vector3d angle_axis;
    ceres::RotationMatrixToAngleAxis(R.data(), angle_axis.data());
    // LOG(INFO) << angle_axis.norm();
    (*global_rotations)[view_id] = angle_axis;
  }
}

void LagrangeDualRotationEstimator::FillinRelativeGraph(
    const std::unordered_map<ImagePair, TwoViewGeometry>& view_pairs,
    Eigen::SparseMatrix<double>& R,
    std::unordered_map<size_t, std::vector<size_t>>& adj_edges) {
  std::vector<Eigen::Triplet<double>> triplets;
  for (auto it = view_pairs.begin(); it != view_pairs.end(); ++it) {
    // image_t i = it->first.first, j = it->first.second;
    const int i = view_id_to_index_[it->first.first];
    const int j = view_id_to_index_[it->first.second];
    // CHECK_LT(i, j);
    Eigen::Matrix3d R_ij;
    ceres::AngleAxisToRotationMatrix(it->second.rotation_2.data(), R_ij.data());

    // After fixing Eq.(9)
    // R.block(3 * i, 3 * j, 3, 3) = R_ij.transpose();
    // R.block(3 * j, 3 * i, 3, 3) = R_ij;
    for (int l = 0; l < 3; l++) {
      for (int r = 0; r < 3; r++) {
        triplets.push_back(
            Eigen::Triplet<double>(dim_ * i + l, dim_ * j + r, R_ij(r, l)));
        triplets.push_back(
            Eigen::Triplet<double>(dim_ * j + l, dim_ * i + r, R_ij(l, r)));
      }
    }

    adj_edges[i].push_back(j);
    adj_edges[j].push_back(i);
  }
  R.setFromTriplets(triplets.begin(), triplets.end());
  R.makeCompressed();
}

std::unique_ptr<solver::SDPSolver>
LagrangeDualRotationEstimator::CreateSDPSolver(
    const int n, const int dim) {
  switch (options_.solver_type) {
    case solver::RBR_BCM:
      return std::unique_ptr<solver::SDPSolver>(
          new solver::RBRSDPSolver(n, dim, options_));
      break;
    case solver::RANK_DEFICIENT_BCM:
      return std::unique_ptr<solver::SDPSolver>(
            new solver::RankRestrictedSDPSolver(n, dim, options_));
      break;
    case solver::RIEMANNIAN_STAIRCASE:
        return std::unique_ptr<solver::SDPSolver>(
            new solver::RiemannianStaircase(n, dim, options_));
        break;
    default:
      LOG(WARNING) << "Solve Type is not supported!";
      return nullptr;
      break;
  }
}

}  // namespace gopt


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/ext/std/ratio.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/orderable.hpp>

#include <ratio>
using namespace boost::hana;

int main() {
    auto ratios = make<Tuple>(
          std::ratio<0>{}
        , std::ratio<1, 3>{}
        , std::ratio<1, 2>{}
        , std::ratio<2, 6>{}
        , std::ratio<7, 8>{}
        , std::ratio<3, 5>{}
    );

    //////////////////////////////////////////////////////////////////////////
    // Orderable
    //////////////////////////////////////////////////////////////////////////
    {
        // less
        {
            BOOST_HANA_CONSTANT_CHECK(less(
                std::ratio<1>{},
                std::ratio<3>{}
            ));

            BOOST_HANA_CONSTANT_CHECK(less(
                std::ratio<4, 10>{},
                std::ratio<3, 5>{}
            ));

            BOOST_HANA_CONSTANT_CHECK(not_(less(
                std::ratio<3, 5>{},
                std::ratio<4, 10>{}
            )));
        }

        // laws
        test::TestOrderable<ext::std::Ratio>{ratios};
    }
}


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#ifndef __multicontact_api_geometry_second_order_cone_hpp__
#define __multicontact_api_geometry_second_order_cone_hpp__

#include <Eigen/Dense>
#include <iostream>

#include "multicontact-api/geometry/fwd.hpp"
#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/eigen-matrix.hpp"

namespace multicontact_api
{
  namespace geometry
  {

    template<typename _Scalar, int _dim, int _Options>
    struct SecondOrderCone : public serialization::Serializable< SecondOrderCone<_Scalar,_dim,_Options> >
    {
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      enum
      {
        dim = _dim,
        Options = _Options
      };
      typedef _Scalar Scalar;
      typedef Eigen::Matrix<Scalar,dim,dim,Options> MatrixD;
      typedef Eigen::Matrix<Scalar,dim,1,Options> VectorD;
      typedef Eigen::DenseIndex DenseIndex;

      SecondOrderCone()
      : m_Q(MatrixD::Identity())
      , m_QPo(_dim,_dim)
      , m_direction(VectorD::Zero())
      , m_Pd(_dim,_dim)
      , m_Po(_dim,_dim)
      {
        m_direction[_dim-1] = 1.;
        computeProjectors();
      }

      SecondOrderCone(const MatrixD & Q, const VectorD & direction)
      : m_Q(Q)
      , m_QPo(_dim,_dim)
      , m_direction(direction.normalized())
      , m_Pd(_dim,_dim)
      , m_Po(_dim,_dim)
      {
        assert(direction.norm() >= Eigen::NumTraits<Scalar>::dummy_precision());
        assert((Q - Q.transpose()).isMuchSmallerThan(Q));
        computeProjectors();
      }

      ///
      /// \brief Build a regular cone from a given friction coefficient and a direction.
      ///
      /// \param mu Friction coefficient.
      /// \param direction Direction of the cone.
      ///
      /// \returns A second order cone.
      ///
      static SecondOrderCone RegularCone(const Scalar mu, const VectorD & direction)
      {
        assert(mu > 0 && "The friction coefficient must be non-negative");
        MatrixD Q(MatrixD::Zero());
        Q.diagonal().fill(1./mu);

        return SecondOrderCone(Q,direction);
      }

      template<typename S2, int O2>
      bool operator==(const SecondOrderCone<S2,dim,O2> & other) const
      {
        return m_Q == other.m_Q
        && m_direction == other.m_direction;
      }

      template<typename S2, int O2>
      bool operator!=(const SecondOrderCone<S2,dim,O2> & other) const
      { return !(*this == other); }

      /// \returns the value of lhs of the conic inequality
      Scalar lhsValue(const VectorD & point) const
      {
//        const VectorD x_Po(m_Po * point);
        return (m_QPo*point).norm();
      }

      /// \returns the value of rhs of the conic inequality
      Scalar rhsValue(const VectorD & point) const
      {
        return m_direction.dot(point);
      }

      /// \returns true if the point is inside the cone
      bool check(const VectorD & point, const Scalar factor = 1.) const
      { return lhsValue(point) <= factor * rhsValue(point); }

      /// \returns the direction of the cone.
      const VectorD & direction() const { return m_direction; }
      void setDirection(const VectorD & direction)
      {
        assert(direction.norm() >= Eigen::NumTraits<Scalar>::dummy_precision());
        m_direction = direction.normalized();
        computeProjectors();
      }

      template<typename S2, int O2>
      bool isApprox(const SecondOrderCone<S2,dim,O2> & other,
                    const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
      {
        return m_direction.isApprox(other.m_direction,prec)
        && m_Q.isApprox(other.m_Q,prec);
      }

      /// \returns the quadratic term of the lhs norm.
      const MatrixD & Q() const { return m_Q; }
      void setQ(const MatrixD & Q)
      {
        assert((Q - Q.transpose()).isMuchSmallerThan(Q));
        m_Q = Q;
        computeProjectors();
      }

      void disp(std::ostream & os) const
      {
        os
        << "Q:\n" << m_Q << std::endl
        << "direction: " << m_direction.transpose() << std::endl;
      }

      friend std::ostream & operator << (std::ostream & os, const SecondOrderCone & C)
      { C.disp(os); return os; }

    protected:

      inline void computeProjectors()
      {
        m_Pd = m_direction * m_direction.transpose();
        m_Po = MatrixD::Identity() - m_Pd;
        m_QPo.noalias() = m_Q * m_Po;
      }

      /// \brief Cholesky decomposition matrix reprensenting the conic norm
      MatrixD m_Q;
      /// \brief Cholesky decomposition projected on the orthogonal of m_direction
      MatrixD m_QPo;

      /// \brief Direction of the cone
      VectorD m_direction;

      /// \brief Projector along the direction of d
      MatrixD m_Pd;
      /// \brief Projector orthogonal to d
      MatrixD m_Po;

    private:

      // Serialization of the class
      friend class boost::serialization::access;

      template<class Archive>
      void save(Archive & ar, const unsigned int /*version*/) const
      {
        ar & boost::serialization::make_nvp("quadratic_term",m_Q);
        ar & boost::serialization::make_nvp("direction",m_direction);
      }

      template<class Archive>
      void load(Archive & ar, const unsigned int /*version*/)
      {
        ar >> boost::serialization::make_nvp("quadratic_term",m_Q);
        ar >> boost::serialization::make_nvp("direction",m_direction);

        computeProjectors();
      }

      BOOST_SERIALIZATION_SPLIT_MEMBER()

    };

  }
}

#endif // ifndef __multicontact_api_geometry_second_order_cone_hpp__


//==============================================================================
//         Copyright 2015 J.T. Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_GENERAL_SIGNM_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_GENERAL_SIGNM_HPP_INCLUDED

#include <nt2/linalg/functions/signm.hpp>
#include <nt2/include/functions/ctranspose.hpp>
#include <nt2/include/functions/diag_of.hpp>
#include <nt2/include/functions/from_diag.hpp>
#include <nt2/include/functions/globalsum.hpp>
#include <nt2/include/functions/isdiagonal.hpp>
#include <nt2/include/functions/is_nez.hpp>
#include <nt2/include/functions/issquare.hpp>
#include <nt2/include/functions/mtimes.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/schur.hpp>
#include <nt2/include/functions/sign.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/include/constants/ten.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/core/container/dsl/as_terminal.hpp>
#include <nt2/core/container/colon/colon.hpp>
#include <nt2/sdk/complex/meta/is_complex.hpp>
#include <boost/assert.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/dispatch/attributes.hpp>

namespace nt2
{
  namespace ext
  {
    BOOST_DISPATCH_IMPLEMENT  ( signm_, tag::cpu_
                              , (A0)
                              , (scalar_< unspecified_<A0> >)
                              )
    {
      typedef A0 result_type;
      NT2_FUNCTOR_CALL(1)
      {
        return sign(real(a0));
      }
    };

    BOOST_DISPATCH_IMPLEMENT  ( signm_, tag::cpu_
                              , (A0)(A1)
                              , (scalar_< unspecified_<A0> >)
                                (scalar_< floating_<A1> >)
                              )
    {
      typedef A0 result_type;
      BOOST_FORCEINLINE result_type operator()(A0& a0, const A1&) const
      {
        return sign(real(a0));
      }
    };

  BOOST_DISPATCH_IMPLEMENT  ( signm_, tag::cpu_
                            , (A0)(N0)(A1)(N1)
                            , ((node_<A0, nt2::tag::signm_
                                    , N0, nt2::container::domain
                                      >
                              ))
                              ((node_<A1, nt2::tag::tie_
                                    , N1, nt2::container::domain
                                     >
                              ))
                            )
    {
      typedef void                                                    result_type;
      typedef typename boost::proto::result_of::child_c<A0&,0>::value_type child0;
      typedef typename child0::value_type                                  type_t;
      typedef typename meta::as_real<type_t>::type                        rtype_t;
      typedef typename meta::as_complex<rtype_t>::type                    ctype_t;
      typedef table<ctype_t>                                               ctab_t;
      typedef table<type_t>                                                 tab_t;
      typedef typename meta::is_complex<type_t>::type                    iscplx_t;
      typedef nt2::memory::container<tag::table_, type_t, nt2::_2D>    o_semantic;

      BOOST_FORCEINLINE result_type operator()(A0& a0, const A1& a1) const
      {
        NT2_AS_TERMINAL_IN(o_semantic, a, boost::proto::child_c<0>(a0));
        BOOST_ASSERT_MSG(issquare(a), "input is not square");
        ctab_t s, u, t;
        nt2::tie(u, t) = schur(a, nt2::cmplx_);
        rtype_t epsi;
        gettolerance(a0, epsi, N0());
        compute_s(s, u, t, epsi);
        finalize_s(a1, s, iscplx_t());
        compute_n(a1, a, boost::proto::child_c<0>(a1), N1());
      }

      template < class S>
      BOOST_FORCEINLINE static void finalize_s(A1& a1, S &s, const boost::mpl::false_&)
      {
        boost::proto::child_c<0>(a1) =  real(s);
      }
      template < class S>
      BOOST_FORCEINLINE static void finalize_s(A1& a1, S &s, const boost::mpl::true_&)
      {
        boost::proto::child_c<0>(a1) = s;
      }

      template < class A, class S>
      BOOST_FORCEINLINE static void compute_n(A1& a1, const A& a, const S& s
                                                , const boost::mpl::long_<2>&)
      {
        boost::proto::child_c<1>(a1) = mtimes(s, a);
      }

      template < class A, class S>
      BOOST_FORCEINLINE static void compute_n(A1&, const A&, const S&
                                                , const boost::mpl::long_<1>)
      {
        // do nothing
      }

      template < class S, class U, class T >
      BOOST_FORCEINLINE static void compute_s(S& s, const U& u, const T& t, rtype_t epsi)
      {
        // from "Functions of Matrices: Theory and Computation" by Nicholas J. Higham
        // Algorithm 5.5 p 112
        typedef typename T::value_type t_t;
        std::size_t n = height(t);
        s = from_diag( sign( diag_of(real(t)) ) );
        if (!isdiagonal(t, (n+1)*epsi))
        {
          for(std::size_t j = 2; j <= n; ++j)
          {
            for(std::size_t i = j-1; i >= 1; --i)
            {
              t_t d = s(i,i)+s(j,j);
              auto k = evaluate(_(i+1, j-1));
              s(i, j) = is_nez(d)
                ?  s(i,j) = -globalsum(s(i,k)(_)*s(k,j))/d
                : (t(i,j)*(s(j,j)-s(i,i)) + globalsum(t(i,k)(_)*s(k,j)) - globalsum(s(i,k)(_)*t(k,j)))/(t(j,j)-t(i,i));
            }
          }
        }
        s = mtimes(mtimes(u, s), ctrans(u));
      }

      BOOST_FORCEINLINE static void gettolerance(const A0&, rtype_t & epsi
                                                  , const boost::mpl::long_<1>&)
      {
        epsi = Ten<rtype_t>()*Eps<rtype_t>();
      }

      BOOST_FORCEINLINE static void gettolerance(const A0& a0, rtype_t & epsi
                                                  , const boost::mpl::long_<2>&)
      {
        epsi = boost::proto::value(boost::proto::child_c<1>(a0));
      }

    };
  }
}

#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2016 Alec Jacobson <alecjacobson@gmail.com>
//
// 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/.
#include "qslim_optimal_collapse_edge_callbacks.h"
#include "quadric_binary_plus_operator.h"
#include <Eigen/LU>

IGL_INLINE void igl::qslim_optimal_collapse_edge_callbacks(
  Eigen::MatrixXi & E,
  std::vector<std::tuple<Eigen::MatrixXd,Eigen::RowVectorXd,double> > &
    quadrics,
  int & v1,
  int & v2,
  decimate_cost_and_placement_callback & cost_and_placement,
  decimate_pre_collapse_callback       & pre_collapse,
  decimate_post_collapse_callback      & post_collapse)
{
  typedef std::tuple<Eigen::MatrixXd,Eigen::RowVectorXd,double> Quadric;
  cost_and_placement = [&quadrics,&v1,&v2](
    const int e,
    const Eigen::MatrixXd & V,
    const Eigen::MatrixXi & /*F*/,
    const Eigen::MatrixXi & E,
    const Eigen::VectorXi & /*EMAP*/,
    const Eigen::MatrixXi & /*EF*/,
    const Eigen::MatrixXi & /*EI*/,
    double & cost,
    Eigen::RowVectorXd & p)
  {
    // Combined quadric
    Quadric quadric_p;
    quadric_p = quadrics[E(e,0)] + quadrics[E(e,1)];
    // Quadric: p'Ap + 2b'p + c
    // optimal point: Ap = -b, or rather because we have row vectors: pA=-b
    const auto & A = std::get<0>(quadric_p);
    const auto & b = std::get<1>(quadric_p);
    const auto & c = std::get<2>(quadric_p);
    p = -b*A.inverse();
    cost = p.dot(p*A) + 2*p.dot(b) + c;
    // Force infs and nans to infinity
    if(std::isinf(cost) || cost!=cost)
    {
      cost = std::numeric_limits<double>::infinity();
      // Prevent NaNs. Actually NaNs might be useful for debugging.
      p.setConstant(0);
    }
  };
  // Remember endpoints
  pre_collapse = [&v1,&v2](
    const Eigen::MatrixXd &                             ,/*V*/
    const Eigen::MatrixXi &                             ,/*F*/
    const Eigen::MatrixXi & E                           ,
    const Eigen::VectorXi &                             ,/*EMAP*/
    const Eigen::MatrixXi &                             ,/*EF*/
    const Eigen::MatrixXi &                             ,/*EI*/
    const igl::min_heap< std::tuple<double,int,int> > & ,/*Q*/
    const Eigen::VectorXi &                             ,/*EQ*/
    const Eigen::MatrixXd &                             ,/*C*/
    const int e)->bool
  {
    v1 = E(e,0);
    v2 = E(e,1);
    return true;
  };
  // update quadric
  post_collapse = [&v1,&v2,&quadrics](
      const Eigen::MatrixXd &                             ,   /*V*/
      const Eigen::MatrixXi &                             ,   /*F*/
      const Eigen::MatrixXi &                             ,   /*E*/
      const Eigen::VectorXi &                             ,/*EMAP*/
      const Eigen::MatrixXi &                             ,  /*EF*/
      const Eigen::MatrixXi &                             ,  /*EI*/
      const igl::min_heap< std::tuple<double,int,int> > & ,/*Q*/
      const Eigen::VectorXi &                             ,/*EQ*/
      const Eigen::MatrixXd &                             ,   /*C*/
      const int                                           ,   /*e*/
      const int                                           ,  /*e1*/
      const int                                           ,  /*e2*/
      const int                                           ,  /*f1*/
      const int                                           ,  /*f2*/
      const bool                                          collapsed
      )->void
  {
    if(collapsed)
    {
      quadrics[v1<v2?v1:v2] = quadrics[v1] + quadrics[v2];
    }
  };
}


#include <k52/dsp/transform/fast_fourier_transform.h>
#include <stdexcept>

#ifdef BUILD_WITH_FFTW3

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

#endif

using ::std::vector;
using ::std::complex;
using ::std::invalid_argument;
using ::std::runtime_error;

namespace k52
{
namespace dsp
{

#ifdef BUILD_WITH_FFTW3

//Using pImpl approach to hide fftw3 for outside use
class FastFourierTransform::FastFourierTransformImpl
{
public:
    FastFourierTransformImpl(size_t sequence_size)
        : sequence_size_(sequence_size)
    {
        boost::mutex::scoped_lock scoped_lock(fftw_mutex_);

        if(sequence_size <= 0)
        {
            throw std::invalid_argument("sequence_size <= 0");
        }

        in_ = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * sequence_size_);
        out_ = (fftw_complex*) fftw_malloc(sizeof(fftw_complex) * sequence_size_);
        plan_ = fftw_plan_dft_1d(sequence_size_, in_, out_, FFTW_FORWARD, FFTW_PATIENT);
    }

    ~FastFourierTransformImpl()
    {
        boost::mutex::scoped_lock scoped_lock(fftw_mutex_);

        fftw_destroy_plan(plan_);
        fftw_free(in_);
        fftw_free(out_);
    }

    std::vector< std::complex< double > > Transform(
            const std::vector< std::complex< double > > &sequence)
    {
        if(sequence_size_ != sequence.size())
        {
            throw std::invalid_argument(
                    "FastFourierTransform can transform only data of the same size as was specified on construction.");
        }

        for (size_t n = 0; n < sequence_size_; ++n)
        {
            in_[n][0] = sequence[n].real();
            in_[n][1] = sequence[n].imag();
        }

        //Actual computations
        fftw_execute(plan_);

        vector< complex< double > > result(sequence_size_);

        for (size_t n = 0; n < sequence_size_; ++n)
        {
            result[n].real( out_[n][0] );
            result[n].imag( out_[n][1] );
        }

        return result;
    }

private:
    size_t sequence_size_;

    fftw_complex* in_;
    fftw_complex* out_;
    fftw_plan plan_;

    static boost::mutex fftw_mutex_;
};

boost::mutex FastFourierTransform::FastFourierTransformImpl::fftw_mutex_;

FastFourierTransform::FastFourierTransform(size_t sequence_size)
{
    fast_fourier_transform_impl_ = new FastFourierTransformImpl(sequence_size);
}

FastFourierTransform::~FastFourierTransform()
{
    delete fast_fourier_transform_impl_;
}

vector< complex< double > > FastFourierTransform::Transform(
        const vector< complex< double > > &sequence) const
{
    return fast_fourier_transform_impl_->Transform(sequence);
}

#else

class FastFourierTransform::FastFourierTransformImpl{};

FastFourierTransform::FastFourierTransform(size_t sequence_size){}

FastFourierTransform::~FastFourierTransform(){}

vector< complex< double > > FastFourierTransform::Transform(
        const vector< complex< double > > &sequence) const
{
    throw runtime_error("The k52 library must be compiled with fftw3 to use FastFourierTransform class");
}

#endif //BUILD_WITH_FFTW3

} // namespace dsp
} // namespace k52


#include <pcl/search/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <Eigen/Dense>
#include <cmath>

#ifndef DON_ESTIMATOR_HPP
#define DON_ESTIMATOR_HPP

#ifndef MY_PI
#define MY_PI 3.14159265
#endif

using namespace std;
using namespace pcl;
using namespace Eigen;

//class used to calculate the normals on a cloud of points
// DoN Filtering done by DoNFilter

template<typename PointType>
class DonEstimator
{
public:
  typedef PointCloud<PointType> Cloud;
  typedef typename PointCloud<PointType>::Ptr CloudPtr;
  typedef typename PointCloud<PointType>::iterator CloudItr;
  typedef PointNormal PNorm;
  typedef PointCloud<PNorm> NormalCloud;
  typedef typename PointCloud<PNorm>::iterator NCloudItr;
  typedef typename PointCloud<PNorm>::Ptr NormalCloudPtr;

  DonEstimator()
  : maxNeighbors_(100)
  , minDistThresh_(sin((28.6/64)*MY_PI/180.0)*3.5)
  , maxDistScale_(3.5)
  , dotProdThresh_(cos(45.0*MY_PI/180.0))
  , viewpoint_(0.0,0.0,0.0)
  , myMesh_(0)
  , createMesh(false){}

  inline void setInputCloud(CloudPtr cloud){inputCloud_ = cloud;}

  inline void setMinDistThread(double dist){minDistThresh_=dist;}

  inline void setMaxDistScale(double scale){maxDistScale_=scale;}

  inline void setMaxNeigbors(int max){maxNeighbors_=max;}

  inline void setDotProductThreshold(double value){dotProdThresh_=value;}

  inline void setViewpoint(float x, float y, float z)
  {
    viewpoint_(0) = x;
    viewpoint_(1) = y;
    viewpoint_(2) = z;
  }

  inline std::vector<pcl::Vertices> buildMesh(CloudPtr baseCloud)

  {
    NormalCloudPtr dummy(new NormalCloud);
    setInputCloud(baseCloud);
    calculateDoN(dummy);

    return myMesh_;
  }

  inline double getMaxRadiusForPoint(PointType pt)
  {
    Vector3f distToVp;
    double minDist;

    distToVp = pt.getVector3fMap()-viewpoint_;
    minDist = sqrt(distToVp.norm())*minDistThresh_;

    return minDist*maxDistScale_;
  }

  inline double getMinRadiusForPoint(PointType pt)
  {
    Vector3f distToVp;
    double minDist;

    distToVp = pt.getVector3fMap()-viewpoint_;
    minDist = sqrt(distToVp.norm())*minDistThresh_;

    return minDist;
  }

  inline void calculateDoN(NormalCloudPtr cloudOut)
  {
    CloudItr cItr;
    NCloudItr nItr;
    int i,res,rmn = (maxNeighbors_+1);
    PNorm smallNorm,dNorm;
    vector<int> ptIds(rmn);
    vector<float> nDists(rmn);
    Vector3f distToVp;
    Vector4f nBuff;
    double minDist,maxDist;
    int badCount = 0;

    cloudOut->clear();

    myTree_.setInputCloud(inputCloud_);

    for (cItr=inputCloud_->begin();cItr<inputCloud_->end();cItr++)
    {
      //reset diff and p2->p5
      dNorm.curvature = 0;
      dNorm.x = cItr->x;
      dNorm.y = cItr->y;
      dNorm.z = cItr->z;

      //calculate distance to viewpoint
      distToVp = cItr->getVector3fMap()-viewpoint_;
      minDist = sqrt(distToVp.norm())*minDistThresh_;
      maxDist = minDist*maxDistScale_;
      minDist *= minDist;

      //do nearest neighbor search
      res = myTree_.radiusSearch(*cItr,maxDist,ptIds,nDists,rmn);
      if (!res){cout << "Found 0 Points!!!!\n";}
      for ( i=1; i<res; i++ )
      {
	if (nDists[i] > minDist)
	{
	  break;
	}
      }

      vector<int> normSet(ptIds.begin(),ptIds.begin() + res);

      computePointNormal(*inputCloud_,normSet,nBuff,dNorm.curvature);
      flipNormalTowardsViewpoint(*cItr,viewpoint_[0],viewpoint_[1],viewpoint_[2],nBuff);
      dNorm.data_n[0] = nBuff[0];
      dNorm.data_n[1] = nBuff[1];
      dNorm.data_n[2] = nBuff[2];

      normSet.resize(i);

      computePointNormal(*inputCloud_,normSet,nBuff,smallNorm.curvature);
      flipNormalTowardsViewpoint(*cItr,viewpoint_[0],viewpoint_[1],viewpoint_[2],nBuff);
      smallNorm.data_n[0] = nBuff[0];
      smallNorm.data_n[1] = nBuff[1];
      smallNorm.data_n[2] = nBuff[2];

      dNorm.getNormalVector3fMap() = (smallNorm.getNormalVector3fMap() - dNorm.getNormalVector3fMap())/2.0;

      if( !pcl_isfinite (dNorm.normal_x) ||
	  !pcl_isfinite (dNorm.normal_y) ||
	  !pcl_isfinite (dNorm.normal_z) )
      {
	badCount++;
      }
      else
      {
	dNorm.curvature = dNorm.getNormalVector3fMap ().norm();
	cloudOut->push_back(dNorm);
      }
    }

    //cout << "got " << badCount << " bad points out of " << inputCloud_->size() << endl;
  }

  inline void setMeshFlag(bool flag){createMesh = flag;}

protected:

  KdTreeFLANN<PointType> myTree_;
  KdTreeFLANN<PNorm> nTree_;
  CloudPtr inputCloud_;
  int maxNeighbors_;
  double minDistThresh_;
  double maxDistScale_;
  double dotProdThresh_;
  Vector3f viewpoint_;
  vector<Vertices> myMesh_;
  bool createMesh;
};

#endif


#include <chrono>
#include <iostream>
#include <armadillo>

int main() {

	for (size_t i = 8; i < 15; i ++) {
		std::cout << "Generating matrices" << std::endl;
		size_t size = 1 << i;
		arma::fmat a = arma::randu<arma::fmat>(size, size);
		arma::fmat b = arma::randu<arma::fmat>(size, size);

		std::cout << "Starting armadillo" << std::endl;
		std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now();

		arma::fmat c = a * b;

		size_t millis = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start).count();
		size_t secs = millis / 1000;
		millis %= 1000;

		std::cout << size << ": " << secs << "." << millis << std::endl;
	}
}


/*
 * @Description: loop closure detection using scan context
 * @Author: Ge Yao
 * @Date: 2020-10-28 15:43:03
 */

#ifndef LIDAR_LOCALIZATION_MODELS_SCAN_CONTEXT_MANAGER_HPP_
#define LIDAR_LOCALIZATION_MODELS_SCAN_CONTEXT_MANAGER_HPP_

#include <yaml-cpp/yaml.h>

#include <vector>

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

#include "lidar_localization/sensor_data/cloud_data.hpp"
#include "lidar_localization/sensor_data/key_frame.hpp"

#include "lidar_localization/models/scan_context_manager/kdtree_vector_of_vectors_adaptor.hpp"

namespace lidar_localization {

class ScanContextManager {
public:
    typedef Eigen::MatrixXf ScanContext;
    typedef std::vector<float> RingKey;
    typedef std::vector<RingKey> RingKeys;
    typedef KDTreeVectorOfVectorsAdaptor<RingKeys, float> RingKeyIndex;

    static const int NONE = -1;

    ScanContextManager(const YAML::Node& node);

    void Update(
        const CloudData &scan,
        const KeyFrame &key_frame
    );

    /**
     * @brief  get loop closure proposal using the latest key scan
     * @param  none
     * @return loop closure proposal (key_frame_id, scan_context_distance) as std::pair<int, float>
     */
    std::pair<int, float> DetectLoopClosure(void);
    /**
     * @brief  get loop closure proposal using the given key scan
     * @param  scan, query key scan
     * @param  pose, matched pose
     * @return true for success match otherwise false
     */
    bool DetectLoopClosure(const CloudData &scan,Eigen::Matrix4f &pose);

    /**
     * @brief  save scan context index & data to persistent storage
     * @param  output_path, scan context output path
     * @return true for success otherwise false
     */
    bool Save(const std::string &output_path);
    /**
     * @brief  load scan context index & data from persistent storage
     * @param  input_path, scan context input path
     * @return true for success otherwise false
     */
    bool Load(const std::string &input_path);

private:
    /**
     * @brief  get scan context of given lidar scan
     * @param  scan, lidar scan of key frame
     * @return scan context as Eigen::MatrixXd
     */
    ScanContext GetScanContext(const CloudData &scan);
    /**
     * @brief  get ring key of given scan context
     * @param  scan_context, scan context of key scan
     * @return ring key as RingKey
     */
    RingKey GetRingKey(
        const ScanContext &scan_context
    );
    /**
     * @brief  generate random ring keys for indexing test
     * @return void
     */
    void GenerateRandomRingKey(
        RingKeys &samples,
        const int N, const int D, const float max_range
    );
    /**
     * @brief  get sector key of given scan context
     * @param  scan_context, scan context of key scan
     * @return sector key as RingKey
     */
    Eigen::MatrixXf GetSectorKey(
        const Eigen::MatrixXf &scan_context
    );

    /**
     * @brief  get orientation of point measurement
     * @param  x, x component of point measurement
     * @param  y, y component of point measurement
     * @return point measurement orientation, [0.0f, 360.0f)
     */
    float GetOrientation(const float &x, const float &y);
    /**
     * @brief  convert floating point value to integer index
     * @param  value, target floating point value
     * @param  MAX_VALUE, max. floating point value
     * @param  RESOLUTION, resolution
     * @return integer index, {0, ..., RESOLUTION - 1}
     */
    int GetIndex(const float &value, const float &MAX_VALUE, const int RESOLUTION);
    /**
     * @brief  get candidate scan context indices
     * @param  ring_key, query ring key
     * @param  N, num. of nearest neighbor candidates
     * @param  indices, candidate indices
     * @param  distances, candidate distances
     * @return void
     */
    void GetCandidateIndices(
        const RingKey &ring_key, const int N,
        std::vector<size_t> &indices,
        std::vector<float> &distances
    );
    /**
     * @brief  circular shift mat to right by shift
     * @param  mat, original matrix
     * @param  shift, right shift amount
     * @return shifted matrix
     */
    Eigen::MatrixXf CircularShift(const Eigen::MatrixXf &mat, int shift);
    /**
     * @brief  get optimal shift estimation using sector key
     * @param  target, target sector key
     * @param  source, source sector key
     * @return void
     */
    int GetOptimalShiftUsingSectorKey(
        const Eigen::MatrixXf &target,
        const Eigen::MatrixXf &source
    );
    /**
     * @brief  compute cosine distance between target and source scan context
     * @param  target_scan_context, target scan context
     * @param  source_scan_context, source scan context
     * @return scan context cosine distance
     */
    float GetCosineDistance(
        const ScanContext &target_scan_context,
        const ScanContext &source_scan_context
    );
    /**
     * @brief  get scan context match result between target and source scan context
     * @param  target_scan_context, target scan context
     * @param  source_scan_context, source scan context
     * @return scan context match result as std::pair<int, float>
     */
    std::pair<int, float> GetScanContextMatch(
        const ScanContext &target_scan_context,
        const ScanContext &source_scan_context
    );

    /**
     * @brief  update scan context index
     * @param  MIN_KEY_FRAME_SEQ_DISTANCE, min. seq distance from current key frame
     * @return true if success otherwise false
     */
    bool UpdateIndex(const int MIN_KEY_FRAME_SEQ_DISTANCE);

    /**
     * @brief  get loop closure match result for given scan context and ring key
     * @param  query_scan_context, query scan context
     * @param  query_ring_key, query ring key
     * @return loop closure match result as std::pair<int, float>
     */
    std::pair<int, float> GetLoopClosureMatch(
        const ScanContext &query_scan_context,
        const RingKey &query_ring_key
    );

    /**
     * @brief  save scan context index
     * @param  output_path, scan context index output path
     * @return true for success otherwise false
     */
    bool SaveIndex(const std::string &output_path);
    /**
     * @brief  save scan contexts
     * @param  output_path, scan contexts output path
     * @return true for success otherwise false
     */
    bool SaveScanContexts(const std::string &output_path);
    /**
     * @brief  save ring keys
     * @param  output_path, ring keys output path
     * @return true for success otherwise false
     */
    bool SaveRingKeys(const std::string &output_path);
    /**
     * @brief  save key frames
     * @param  output_path, key frames output path
     * @return true for success otherwise false
     */
    bool SaveKeyFrames(const std::string &output_path);

    /**
     * @brief  load scan contexts
     * @param  input_path, scan contexts input path
     * @return true for success otherwise false
     */
    bool LoadScanContexts(const std::string &input_path);
    /**
     * @brief  load ring keys
     * @param  input_path, ring keys input path
     * @return true for success otherwise false
     */
    bool LoadRingKeys(const std::string &input_path);
    /**
     * @brief  load key frames
     * @param  input_path, key frames input path
     * @return true for success otherwise false
     */
    bool LoadKeyFrames(const std::string &input_path);

    // states:
    struct {
        // a. scan context buffer:
        std::vector<ScanContext> scan_context_;
        // b. ring-key buffer:
        RingKeys ring_key_;
        // c. key frame buffer:
        std::vector<KeyFrame> key_frame_;
        // d. ring key indexing counter:
        struct {
            // 1. indexing interval counter:
            int counter_ = 0;
            // 2. kd-tree:
            std::shared_ptr<RingKeyIndex> kd_tree_;
            // 3. data:
            struct {
                RingKeys ring_key_;
                std::vector<KeyFrame> key_frame_;
            } data_;
        } index_;
    } state_;

    // hyper-params:
    // a. ROI definition:
    float MAX_RADIUS_;
    float MAX_THETA_;
    // b. resolution:
    int NUM_RINGS_;
    int NUM_SECTORS_;
    float DEG_PER_SECTOR_;
    // c. ring key indexing interval:
    int INDEXING_INTERVAL_;
    // d. min key frame sequence distance:
    int MIN_KEY_FRAME_SEQ_DISTANCE_;
    // e. num. of nearest-neighbor search candidates:
    int NUM_CANDIDATES_;
    // f. sector key fast alignment search ratio:
    float FAST_ALIGNMENT_SEARCH_RATIO_;
    // g. scan context distance threshold:
    float SCAN_CONTEXT_DISTANCE_THRESH_;
};

} // namespace lidar_localization

#endif


#include <string>
#include <iostream>

#include <Eigen/Dense>

#include "mex.h"
#include "matrix.h"

#include "SparseIndexSet.hpp"
#include "TDindexSet.hpp"
#include "HCindexSet.hpp"
#include "SparseQuadrature.hpp"
#include "Cell2univariateQuadrature.hpp"

// fancy compare functor that performs the callback to Matlab
struct MatlabCpFun {
  const mxArray *_prhs[2];

  mxArray *_MatAlpha;
  int _dim;
  MatlabCpFun(const mxArray *prhs, int dim) {
    _MatAlpha = mxCreateDoubleMatrix(dim, 1, mxREAL);
    _prhs[0] = prhs;
    _prhs[1] = _MatAlpha;
    _dim = dim;
  };
  double operator()(const Eigen::VectorXi &alpha) const {
    mxArray *plhs;
    Eigen::Map<Eigen::VectorXd>(mxGetPr(_MatAlpha), _dim) =
        alpha.cast<double>();
    mexCallMATLAB(1, &plhs, 2, const_cast<mxArray **>(_prhs),
                  (const char *)"feval");
    if (mxIsScalar(plhs))
      return *(mxGetPr(plhs));
    else
      mexErrMsgIdAndTxt(
          "MATLAB:MXsparseQuadraturecpp",
          "MXsparseQuadrature requires adequate function handle. Check help.");
    return 0.;
  };
};

/** nlhs Number of expected output mxArrays
*   plhs Array of pointers to the expected output mxArrays
*   nrhs Number of input mxArrays
*   prhs Array of pointers to the input mxArrays.
*        Do not modify any prhs values in your MEX file.
*        Changing the data in these read-only mxArrays can
*        produce undesired side effects.
*/
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  int q = 0;
  int dim = 0;
  int nPts = 0;
  int myCASE = 0;
  TDindexSet TDind;
  HCindexSet HCind;
  SparseIndexSet GENind;
  SparseQuadrature Q;
  Cell2univariateQuadrature CellUniQ;

  if (nrhs != 5) {
    mexErrMsgIdAndTxt(
        "MATLAB:MXsparseQuadraturecpp:nargin",
        "MXsparseQuadrature requires five input arguments. Check help.");
  } else if (nlhs < 2) {
    mexErrMsgIdAndTxt("MATLAB:MXsparseQuadraturecpp:nargin",
                      "MXsparseQuadrature requires at least two output "
                      "arguments. Check help.");
  } else if (!mxIsScalar(prhs[0]) || !mxIsScalar(prhs[1])) {
    mexErrMsgIdAndTxt("MATLAB:MXsparseQuadraturecpp",
                      "First two arguments have to be scalar. Check help.");
  } else if (!mxIsChar(prhs[2])) {
    mexErrMsgIdAndTxt("MATLAB:MXsparseQuadraturecpp",
                      "Third argument has to be a string. Check help.");
  } else if (!mxIsCell(prhs[3]))
    mexErrMsgIdAndTxt("MATLAB:MXsparseQuadraturecpp",
                      "Fourth argument has to be a Cell array. Check help.");

  q = std::round(*(mxGetPr(prhs[0])));
  dim = std::round(*(mxGetPr(prhs[1])));
  std::string type(mxArrayToString(prhs[2]));

  CellUniQ.initQuadrature(prhs[3]);

  // convert type to uppercase
  for (auto i = type.begin(); i != type.end(); ++i)
    *i = std::toupper(*i);

  if (type == "TD") {
    if (mxIsClass(prhs[4], "function_handle"))
      mexErrMsgIdAndTxt(
          "MATLAB:MXsparseQuadraturecpp",
          "Last argument has to be array if TD is used. Check help.");
    TDind.computeIndexSet(q, Eigen::Map<Eigen::VectorXd>(mxGetPr(prhs[4]), dim));
    Q = SparseQuadrature(TDind, CellUniQ);
    Q.purgeSparseQuadrature();
    plhs[2] = mxCreateDoubleMatrix(dim, 1, mxREAL);
    Eigen::Map<Eigen::VectorXd> sort(mxGetPr(plhs[2]), dim);
    const Eigen::VectorXi &mySort = TDind.get_sortW();
    sort = mySort.cast<double>().array() + 1;
  } else if (type == "HC") {
    if (mxIsClass(prhs[4], "function_handle"))
      mexErrMsgIdAndTxt(
          "MATLAB:MXsparseQuadraturecpp",
          "Last argument has to be array if HC is used. Check help.");

    HCind.computeIndexSet(q, Eigen::Map<Eigen::VectorXd>(mxGetPr(prhs[4]), dim));
    Q = SparseQuadrature(HCind, CellUniQ);
    Q.purgeSparseQuadrature();
  } else if (type == "GEN") {
    if (!mxIsClass(prhs[4], "function_handle"))
      mexErrMsgIdAndTxt("MATLAB:MXsparseQuadraturecpp",
                        "Last argument has to be function handle if Gen is "
                        "used. Check help.");

    GENind.computeIndexSet(q, dim, MatlabCpFun(prhs[4], dim));
    Q = SparseQuadrature(GENind, CellUniQ);
    Q.purgeSparseQuadrature();
  } else {
    mexErrMsgIdAndTxt("MATLAB:MXsparseQuadraturecpp",
                        "type has an invalid value. Check help.");
  }

  const Eigen::MatrixXd &myqPoints = Q.get_qPoints();
  const Eigen::VectorXd &myqWeights = Q.get_qWeights();

  nPts = myqWeights.size();

  plhs[0] = mxCreateDoubleMatrix(dim, nPts, mxREAL);
  plhs[1] = mxCreateDoubleMatrix(nPts, 1, mxREAL);

  if (nlhs == 3 && type != "TD") plhs[2] = mxCreateDoubleMatrix(0, 0, mxREAL);

  // set quadrature Points and Weights
  Eigen::Map<Eigen::MatrixXd> qPoints(mxGetPr(plhs[0]), dim, nPts);
  Eigen::Map<Eigen::VectorXd> qWeights(mxGetPr(plhs[1]), nPts);

  qPoints = myqPoints;
  qWeights = myqWeights;

  return;
}


//==================================================================================================
/*!
  @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_ARCH_PPC_VMX_SIMD_FUNCTION_RSQRT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_PPC_VMX_SIMD_FUNCTION_RSQRT_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/sqr.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;

   BOOST_DISPATCH_OVERLOAD( rsqrt_
                          , (typename A0)
                          , bs::vmx_
                          , bs::pack_< bd::single_<A0>, bs::vmx_>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(const A0& a0) const BOOST_NOEXCEPT
      {
        A0 o  = One<A0>();
        A0 estimate = fast_(rsqrt)(a0);
        A0 se = sqr(estimate);
        A0 he = estimate*Half<A0>();
        A0 st = vec_nmsub(a0.storage(),se.storage(),o.storage());
        return fma(st, he, estimate);
      }
   };

   BOOST_DISPATCH_OVERLOAD( rsqrt_
                          , (typename A0)
                          , bs::vmx_
                          , bs::fast_tag
                          , bs::pack_< bd::single_<A0>, bs::vmx_>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(bs::fast_tag const&, const A0& a0) const BOOST_NOEXCEPT
      {
        return vec_rsqrte( a0.storage() );
      }
   };
} } }

#endif


/**
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  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.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

/**
 *
 * \brief This file contains an example of graphlab used for stitching
 * multiple images into a panorama. The code is based on a example
 * stiching application in OpenCV.
 *
 *  \author Dhruv Batra
 */

#ifndef __STITCH_GRLAB_HPP__
#define __STITCH_GRLAB_HPP__

#include <vector>
#include <string>
#include <fstream>
#include <iostream>
#include <math.h>
#include <unistd.h>

#include <Eigen/Dense>

#include <graphlab.hpp>
#include <graphlab/util/fs_util.hpp>

#include "eigen_serialization.hpp"
#include "opencv_serialization.hpp"
#include "stitch_opts.hpp"
#include "utils.hpp"
//#include "seam_finders_gr.hpp"
#include "precomp.hpp"

#include <graphlab/macros_def.hpp>

#include "opencv2/opencv_modules.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/stitching/detail/autocalib.hpp"
#include "opencv2/stitching/detail/blenders.hpp"
#include "opencv2/stitching/detail/camera.hpp"
#include "opencv2/stitching/detail/exposure_compensate.hpp"
#include "opencv2/stitching/detail/matchers.hpp"
#include "opencv2/stitching/detail/motion_estimators.hpp"
#include "opencv2/stitching/detail/seam_finders.hpp"
#include "opencv2/stitching/detail/util.hpp"
#include "opencv2/stitching/detail/warpers.hpp"
#include "opencv2/stitching/warpers.hpp"

using namespace std;
using namespace cv;
using namespace cv::detail;

typedef Eigen::VectorXd vec;
typedef Eigen::MatrixXd mat;

/////////////////////////////////////////////////////////////////////////
// Edge and Vertex data and Graph Type
struct vertex_data
{
    bool empty; // used to quickly check if this is a dummy vertex.

    // path to image
    std::string img_path;

    cv::Mat full_img;       // Original image
    cv::Mat img;            // Used for feature computation
    cv::Mat img_warped;     // Used by gain compensator
    cv::Mat img_warped_f;   // Used by seam_finder

    cv::Size full_img_size;

    cv:: Size warp_size;

    cv::detail::ImageFeatures features;

    cv::detail::CameraParams camera;

    cv::Point2f corner;
    //cv::Mat mask;
    cv::Mat mask_warped;

    // constructor
    vertex_data() : empty(true)
    { }

    void save(graphlab::oarchive& arc) const
    {
        arc << empty << img_path
        << full_img << img << img_warped << img_warped_f
        << full_img_size << warp_size << features << camera
        << corner
        << mask_warped;
    }
    void load(graphlab::iarchive& arc)
    {
        arc >> empty >> img_path
        >> full_img >> img >> img_warped >> img_warped_f
        >> full_img_size >> warp_size >> features >> camera
        >> corner
        >> mask_warped;
    }
   //>> mask
    vertex_data operator+ (vertex_data& othervertex)
    {
        vertex_data sum;

        if (!empty && !othervertex.empty)
        {
            logstream(LOG_ERROR) << "Don't know about to merge two non-empty vertex-data structures" << std::endl;
            //return EXIT_FAILURE;
        }
        else if (!empty && othervertex.empty)
            sum = *this;
        else if (empty && !othervertex.empty)
            sum = othervertex;
        // Nothing to do if both empty.

        return sum;
    }

    vertex_data& operator+= (const vertex_data& othervertex)
    {
        if (!empty && !othervertex.empty)
        {
            logstream(LOG_ERROR) << "Don't know about to merge two non-empty vertex-data structures" << std::endl;
            //return EXIT_FAILURE;
        }
        else if (empty && !othervertex.empty)
            *this = othervertex;
        // Nothing to do if both empty or othervertex empty.

        return *this;
    }
}; // End of vertex data

//typedef graphlab::empty edge_data;
struct edge_data
{
    bool empty; // used to quickly check if this is a dummy edge.

    cv::detail::MatchesInfo matchinfo;

    // constructor
    edge_data() : empty(true)
    { }

    void save(graphlab::oarchive& arc) const
    {
        arc << empty << matchinfo;
    }
    void load(graphlab::iarchive& arc)
    {
        arc >> empty >> matchinfo;
    }

    edge_data operator+ (edge_data& otheredge)
    {
        edge_data sum;

        if (!empty && !otheredge.empty)
        {
            logstream(LOG_ERROR) << "Don't know about to merge two non-empty edge-data structures" << std::endl;
            //return EXIT_FAILURE;
        }
        else if (!empty && otheredge.empty)
            sum = *this;
        else if (empty && !otheredge.empty)
            sum = otheredge;
        // Nothing to do if both empty.

        return sum;
    }

    edge_data& operator+= (const edge_data& otheredge)
    {
        if (!empty && !otheredge.empty)
        {
            logstream(LOG_ERROR) << "Don't know about to merge two non-empty edge-data structures" << std::endl;
            //return EXIT_FAILURE;
        }
        else if (empty && !otheredge.empty)
            *this = otheredge;
        // Nothing to do if both empty or othervertex empty.

        return *this;
    }
}; // End of edge data

/**
 * The graph type
 */
typedef graphlab::distributed_graph<vertex_data, edge_data> graph_type;

/////////////////////////////////////////////////////////////////////////
// GraphLab Vertex Program (and Gather Type)
/**
 * The type passed around during the gather phase
 */
//struct gather_type
//{
//
//    gather_type& operator+=(const gather_type& other)
//    {
//    } // end of operator +=
//    void save(graphlab::oarchive& arc) const
//    {
//        arc << delf_i << delf_j;
//    }
//    void load(graphlab::iarchive& arc)
//    {
//        arc >> delf_i >> delf_j;
//    }
//}; // end of gather type
typedef graphlab::empty gather_type;

/**
 * The core stitching update function.
 */
class stitch_vertex_program :
public graphlab::ivertex_program<graph_type, gather_type,
graphlab::messages::sum_priority>,
public graphlab::IS_POD_TYPE
{
private:

public:

    stitch_vertex_program()  { }

    edge_dir_type gather_edges(icontext_type& context,
                               const vertex_type& vertex) const
    {
        return graphlab::ALL_EDGES;
    }; // end of gather_edges

    // Run the gather operation over all in edges
    gather_type gather(icontext_type& context, const vertex_type& target_vertex,
                       edge_type& edge) const
    {
        return gather_type();
    } // end of gather

    void apply(icontext_type& context, vertex_type& vertex,
               const gather_type& sum)
    {

        // Get vertex data
        vertex_data &vdata = vertex.data();

        logstream(LOG_EMPH) << "Features in image #" << vertex.id() << ": " << vdata.features.keypoints.size() << "\n";
    } // end of apply

    edge_dir_type scatter_edges(icontext_type& context,
                                const vertex_type& vertex) const
    {
        //return graphlab::ALL_EDGES;
        return graphlab::NO_EDGES;
    }; // end of gather_edges

};

/**
 * Define the engine type
 */
//typedef graphlab::synchronous_engine<mplp_vertex_program> engine_type;
//typedef graphlab::async_consistent_engine<stitch_vertex_program> engine_type;
typedef graphlab::omni_engine<stitch_vertex_program> engine_type;

#endif


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#include <boost/simd/function/scalar/is_flint.hpp>
#include <boost/simd/function/scalar/is_nltz.hpp>
#include <boost/simd/function/scalar/pow.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( realpow_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG( is_nltz(a0) || is_flint(a1),
                        "realpow cannot produce complex result." );
      return  pow(a0, a1);
    }
  };
} } }

#endif