#include "mpfr/mpfr.hpp"

#include <boost/multiprecision/mpfr.hpp>
#include "nanobench.h"

template <int N> using scalar_t = mpfr::mp_float_t<mpfr::digits10{N}>;

auto main() -> int {

  auto bench = ankerl::nanobench::Bench();

  bench.minEpochTime(std::chrono::milliseconds{100UL});

  using T1 = scalar_t<512>;
  using T2 = scalar_t<1024>;

  T1 a = sqrt(T1{2.0});
  T2 b = sqrt(T2{2.0});
  auto a_ = a + 1;
  auto b_ = b + 1;

  T1 c;
  T2 d;

  ankerl::nanobench::doNotOptimizeAway(&a);
  ankerl::nanobench::doNotOptimizeAway(&b);
  ankerl::nanobench::doNotOptimizeAway(&c);
  ankerl::nanobench::doNotOptimizeAway(&d);

  bench.run("copying same size 512", [&] {
    c = a;
    ankerl::nanobench::clobberMemory();
  });
  bench.run("copying same size 1024", [&] {
    d = b;
    ankerl::nanobench::clobberMemory();
  });

  bench.run("copying 512 -> 1024", [&] {
    d = a;
    ankerl::nanobench::clobberMemory();
  });

  bench.run("copying 1024 -> 512", [&] {
    c = b;
    ankerl::nanobench::clobberMemory();
  });

  bench.run("add 512", [&] {
    c = a + a;
    ankerl::nanobench::clobberMemory();
  });

  a_ = 2;
  bench.run("add2 512", [&] {
    c = a_ + a_;
    ankerl::nanobench::clobberMemory();
  });
  a_ = a + 1;

  bench.run("sqr 512", [&] {
    c = a * a;
    ankerl::nanobench::clobberMemory();
  });

  bench.run("mul 512", [&] {
    c = a * a_;
    ankerl::nanobench::clobberMemory();
  });

  bench.run("div 512", [&] {
    c = a / a_;
    ankerl::nanobench::clobberMemory();
  });

  bench.run("mul2 512", [&] {
    c = a * 2;
    ankerl::nanobench::clobberMemory();
  });

  bench.run("add 1024", [&] {
    d = b + b;
    ankerl::nanobench::clobberMemory();
  });

  b_ = 2;
  bench.run("add2 1024", [&] {
    d = b_ + b_;
    ankerl::nanobench::clobberMemory();
  });
  b_ = b + 1;

  bench.run("sqr 1024", [&] {
    d = b * b;
    ankerl::nanobench::clobberMemory();
  });

  bench.run("mul 1024", [&] {
    d = b * b_;
    ankerl::nanobench::clobberMemory();
  });

  bench.run("div 1024", [&] {
    d = b / b_;
    ankerl::nanobench::clobberMemory();
  });

  bench.run("mul2 1024", [&] {
    d = b * 2;
    ankerl::nanobench::clobberMemory();
  });
}


//  (C) Copyright John Maddock 2022.
//  (C) Copyright James Folberth 2022.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// Issue 754
// Check that the class template argument deduction guides properly promote
// integral ctor args to a real floating point type.

#include <boost/math/distributions/arcsine.hpp>
#include <boost/math/distributions/bernoulli.hpp>
#include <boost/math/distributions/beta.hpp>
#include <boost/math/distributions/binomial.hpp>
#include <boost/math/distributions/cauchy.hpp>
#include <boost/math/distributions/chi_squared.hpp>
//#include <boost/math/distributions/empirical_cumulative_distribution_function.hpp>
#include <boost/math/distributions/exponential.hpp>
#include <boost/math/distributions/extreme_value.hpp>
#include <boost/math/distributions/fisher_f.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/distributions/geometric.hpp>
//#include <boost/math/distributions/hyperexponential.hpp>
//#include <boost/math/distributions/hypergeometric.hpp>
#include <boost/math/distributions/inverse_chi_squared.hpp>
#include <boost/math/distributions/inverse_gamma.hpp>
#include <boost/math/distributions/inverse_gaussian.hpp>
#include <boost/math/distributions/kolmogorov_smirnov.hpp>
#include <boost/math/distributions/laplace.hpp>
#include <boost/math/distributions/logistic.hpp>
#include <boost/math/distributions/lognormal.hpp>
#include <boost/math/distributions/negative_binomial.hpp>
#include <boost/math/distributions/non_central_beta.hpp>
#include <boost/math/distributions/non_central_chi_squared.hpp>
#include <boost/math/distributions/non_central_f.hpp>
#include <boost/math/distributions/non_central_t.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/pareto.hpp>
#include <boost/math/distributions/poisson.hpp>
#include <boost/math/distributions/rayleigh.hpp>
#include <boost/math/distributions/skew_normal.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/distributions/triangular.hpp>
#include <boost/math/distributions/uniform.hpp>
#include <boost/math/distributions/weibull.hpp>

// Instantiate a DistType object with the parameter pack given by Types.
// Then verify that the `RealType` template parameter of DistType (stored in
// in value_type) is promoted correctly according to the deduction guides.
template <template<class, class> class DistType, class PromType = double, class... Types>
void test_deduction_guide(Types... types)
{
   DistType d(types...);
   static_assert(std::is_same<typename decltype(d)::value_type, PromType>::value);
}

int main()
{
   using namespace boost::math;

   test_deduction_guide<arcsine_distribution>(0);
   test_deduction_guide<arcsine_distribution>(0, 1);

   test_deduction_guide<bernoulli_distribution>(0);

   test_deduction_guide<beta_distribution>(1);
   test_deduction_guide<beta_distribution>(1, 1);

   test_deduction_guide<binomial_distribution>(1);
   test_deduction_guide<binomial_distribution>(1, 0);

   test_deduction_guide<cauchy_distribution>(0);
   test_deduction_guide<cauchy_distribution>(0, 1);

   test_deduction_guide<chi_squared_distribution>(2);

   test_deduction_guide<exponential_distribution>(1);

   test_deduction_guide<extreme_value_distribution>(0);
   test_deduction_guide<extreme_value_distribution>(0, 1);

   test_deduction_guide<fisher_f_distribution>(1, 2);

   test_deduction_guide<gamma_distribution>(1);
   test_deduction_guide<gamma_distribution>(1, 1);

   test_deduction_guide<geometric_distribution>(1);

   test_deduction_guide<inverse_chi_squared_distribution>(1);
   test_deduction_guide<inverse_chi_squared_distribution>(1, 1);

   test_deduction_guide<inverse_gamma_distribution>(1);
   test_deduction_guide<inverse_gamma_distribution>(1, 1);

   test_deduction_guide<inverse_gaussian_distribution>(1);
   test_deduction_guide<inverse_gaussian_distribution>(1, 1);

   test_deduction_guide<kolmogorov_smirnov_distribution>(1);

   test_deduction_guide<laplace_distribution>(0);
   test_deduction_guide<laplace_distribution>(0, 1);

   test_deduction_guide<logistic_distribution>(0);
   test_deduction_guide<logistic_distribution>(0, 1);

   test_deduction_guide<lognormal_distribution>(0);
   test_deduction_guide<lognormal_distribution>(0, 1);

   test_deduction_guide<negative_binomial_distribution>(1, 1);

   test_deduction_guide<non_central_beta_distribution>(1, 1, 1);

   test_deduction_guide<non_central_chi_squared_distribution>(1, 1);

   test_deduction_guide<non_central_f_distribution>(1, 1, 1);

   test_deduction_guide<non_central_t_distribution>(1, 1);

   test_deduction_guide<normal_distribution>(2);
   test_deduction_guide<normal_distribution>(2, 3);

   test_deduction_guide<pareto_distribution>(2);
   test_deduction_guide<pareto_distribution>(2, 3);

   test_deduction_guide<poisson_distribution>(1);

   test_deduction_guide<rayleigh_distribution>(1);

   test_deduction_guide<skew_normal_distribution>(0);
   test_deduction_guide<skew_normal_distribution>(0, 1);
   test_deduction_guide<skew_normal_distribution>(0, 1, 0);

   test_deduction_guide<students_t_distribution>(2);

   test_deduction_guide<triangular_distribution>(-1);
   test_deduction_guide<triangular_distribution>(-1, 0);
   test_deduction_guide<triangular_distribution>(-1, 0, 1);

   test_deduction_guide<uniform_distribution>(0);
   test_deduction_guide<uniform_distribution>(0, 1);

   test_deduction_guide<weibull_distribution>(1);
   test_deduction_guide<weibull_distribution>(1, 1);
}


#ifndef VIENNACL_LINALG_LANCZOS_HPP_
#define VIENNACL_LINALG_LANCZOS_HPP_

/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** @file viennacl/linalg/lanczos.hpp
*   @brief Generic interface for the Lanczos algorithm.
*
*   Contributed by Guenther Mader and Astrid Rupp.
*/

#include <cmath>
#include <vector>
#include "viennacl/vector.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/inner_prod.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/io/matrix_market.hpp"
#include "viennacl/linalg/bisect.hpp"
#include <boost/random.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace viennacl
{
namespace linalg
{

/** @brief A tag for the lanczos algorithm.
*/
class lanczos_tag
{
public:

  enum
  {
    partial_reorthogonalization = 0,
    full_reorthogonalization,
    no_reorthogonalization
  };

  /** @brief The constructor
  *
  * @param factor                 Exponent of epsilon - tolerance for batches of Reorthogonalization
  * @param numeig                 Number of eigenvalues to be returned
  * @param met                    Method for Lanczos-Algorithm: 0 for partial Reorthogonalization, 1 for full Reorthogonalization and 2 for Lanczos without Reorthogonalization
  * @param krylov                 Maximum krylov-space size
  */

  lanczos_tag(double factor = 0.75,
              vcl_size_t numeig = 10,
              int met = 0,
              vcl_size_t krylov = 100) : factor_(factor), num_eigenvalues_(numeig), method_(met), krylov_size_(krylov) {}

  /** @brief Sets the number of eigenvalues */
  void num_eigenvalues(vcl_size_t numeig){ num_eigenvalues_ = numeig; }

    /** @brief Returns the number of eigenvalues */
  vcl_size_t num_eigenvalues() const { return num_eigenvalues_; }

    /** @brief Sets the exponent of epsilon */
  void factor(double fct) { factor_ = fct; }

  /** @brief Returns the exponent */
  double factor() const { return factor_; }

  /** @brief Sets the size of the kylov space */
  void krylov_size(vcl_size_t max) { krylov_size_ = max; }

  /** @brief Returns the size of the kylov space */
  vcl_size_t  krylov_size() const { return krylov_size_; }

  /** @brief Sets the reorthogonalization method */
  void method(int met){ method_ = met; }

  /** @brief Returns the reorthogonalization method */
  int method() const { return method_; }

private:
  double factor_;
  vcl_size_t num_eigenvalues_;
  int method_; // see enum defined above for possible values
  vcl_size_t krylov_size_;
};

namespace detail
{
  /**
  *   @brief Implementation of the Lanczos PRO algorithm
  *
  *   @param A            The system matrix
  *   @param r            Random start vector
  *   @param size         Size of krylov-space
  *   @param tag          Lanczos_tag with several options for the algorithm
  *   @return             Returns the eigenvalues (number of eigenvalues equals size of krylov-space)
  */

  template< typename MatrixT, typename VectorT >
  std::vector<
          typename viennacl::result_of::cpu_value_type<typename MatrixT::value_type>::type
          >
  lanczosPRO (MatrixT const& A, VectorT & r, vcl_size_t size, lanczos_tag const & tag)
  {
    typedef typename viennacl::result_of::value_type<MatrixT>::type        ScalarType;
    typedef typename viennacl::result_of::cpu_value_type<ScalarType>::type    CPU_ScalarType;

    // generation of some random numbers, used for lanczos PRO algorithm
    boost::mt11213b mt;
    boost::normal_distribution<CPU_ScalarType> N(0, 1);
    boost::bernoulli_distribution<CPU_ScalarType> B(0.5);
    boost::triangle_distribution<CPU_ScalarType> T(-1, 0, 1);

    boost::variate_generator<boost::mt11213b&, boost::normal_distribution<CPU_ScalarType> >     get_N(mt, N);
    boost::variate_generator<boost::mt11213b&, boost::bernoulli_distribution<CPU_ScalarType> >  get_B(mt, B);
    boost::variate_generator<boost::mt11213b&, boost::triangle_distribution<CPU_ScalarType> >   get_T(mt, T);

    long i, k, retry, reorths;
    std::vector<long> l_bound(size/2), u_bound(size/2);
    bool second_step;
    CPU_ScalarType squ_eps, eta, temp, eps, retry_th;
    vcl_size_t n = r.size();
    std::vector< std::vector<CPU_ScalarType> > w(2, std::vector<CPU_ScalarType>(size));
    CPU_ScalarType cpu_beta;

    boost::numeric::ublas::vector<CPU_ScalarType> s(n);

    VectorT t(n);
    CPU_ScalarType inner_rt;
    ScalarType vcl_beta;
    ScalarType vcl_alpha;
    std::vector<CPU_ScalarType> alphas, betas;
    boost::numeric::ublas::matrix<CPU_ScalarType> Q(n, size);

    second_step = false;
    eps = std::numeric_limits<CPU_ScalarType>::epsilon();
    squ_eps = std::sqrt(eps);
    retry_th = 1e-2;
    eta = std::exp(std::log(eps) * tag.factor());
    reorths = 0;
    retry = 0;

    vcl_beta = viennacl::linalg::norm_2(r);

    r /= vcl_beta;

    detail::copy_vec_to_vec(r,s);
    boost::numeric::ublas::column(Q, 0) = s;

    VectorT u = viennacl::linalg::prod(A, r);
    vcl_alpha = viennacl::linalg::inner_prod(u, r);
    alphas.push_back(vcl_alpha);
    w[0][0] = 1;
    betas.push_back(vcl_beta);

    long batches = 0;
    for (i = 1;i < static_cast<long>(size); i++)
    {
      r = u - vcl_alpha * r;
      vcl_beta = viennacl::linalg::norm_2(r);

      betas.push_back(vcl_beta);
      r = r / vcl_beta;

      vcl_size_t index = vcl_size_t(i % 2);
      w[index][vcl_size_t(i)] = 1;
      k = (i + 1) % 2;
      w[index][0] = (betas[1] * w[vcl_size_t(k)][1] + (alphas[0] - vcl_alpha) * w[vcl_size_t(k)][0] - betas[vcl_size_t(i) - 1] * w[index][0]) / vcl_beta + eps * 0.3 * get_N() * (betas[1] + vcl_beta);

      for (vcl_size_t j = 1; j < vcl_size_t(i - 1); j++)
      {
              w[index][j] = (betas[j + 1] * w[vcl_size_t(k)][j + 1] + (alphas[j] - vcl_alpha) * w[vcl_size_t(k)][j] + betas[j] * w[vcl_size_t(k)][j - 1] - betas[vcl_size_t(i) - 1] * w[index][j]) / vcl_beta + eps * 0.3 * get_N() * (betas[j + 1] + vcl_beta);
      }
      w[index][vcl_size_t(i) - 1] = 0.6 * eps * CPU_ScalarType(n) * get_N() * betas[1] / vcl_beta;

      if (second_step)
      {
        for (vcl_size_t j = 0; j < vcl_size_t(batches); j++)
        {
          l_bound[vcl_size_t(j)]++;
          u_bound[vcl_size_t(j)]--;

          for (k = l_bound[j];k < u_bound[j];k++)
          {
            detail::copy_vec_to_vec(boost::numeric::ublas::column(Q, vcl_size_t(k)), t);
            inner_rt = viennacl::linalg::inner_prod(r,t);
            r = r - inner_rt * t;
            w[index][vcl_size_t(k)] = 1.5 * eps * get_N();
            reorths++;
          }
        }
        temp = viennacl::linalg::norm_2(r);
        r = r / temp;
        vcl_beta = vcl_beta * temp;
        second_step = false;
      }
      batches = 0;

      for (vcl_size_t j = 0; j < vcl_size_t(i); j++)
      {
        if (std::fabs(w[index][j]) >= squ_eps)
        {
          detail::copy_vec_to_vec(boost::numeric::ublas::column(Q, j), t);
          inner_rt = viennacl::linalg::inner_prod(r,t);
          r = r - inner_rt * t;
          w[index][j] = 1.5 * eps * get_N();
          k = long(j) - 1;
          reorths++;
          while (k >= 0 && std::fabs(w[index][vcl_size_t(k)]) > eta)
          {
            detail::copy_vec_to_vec(boost::numeric::ublas::column(Q, vcl_size_t(k)), t);
            inner_rt = viennacl::linalg::inner_prod(r,t);
            r = r - inner_rt * t;
            w[index][vcl_size_t(k)] = 1.5 * eps * get_N();
            k--;
            reorths++;
          }
          l_bound[vcl_size_t(batches)] = k + 1;
          k = long(j) + 1;

          while (k < i && std::fabs(w[index][vcl_size_t(k)]) > eta)
          {
            detail::copy_vec_to_vec(boost::numeric::ublas::column(Q, vcl_size_t(k)), t);
            inner_rt = viennacl::linalg::inner_prod(r,t);
            r = r - inner_rt * t;
            w[index][vcl_size_t(k)] = 1.5 * eps * get_N();
            k++;
            reorths++;
          }
          u_bound[vcl_size_t(batches)] = k - 1;
          batches++;
          j = vcl_size_t(k);
        }
      }

      if (batches > 0)
      {
        temp = viennacl::linalg::norm_2(r);
        r = r / temp;
        vcl_beta = vcl_beta * temp;
        second_step = true;

        while (temp < retry_th)
        {
          for (vcl_size_t j = 0; j < vcl_size_t(i); j++)
          {
            detail::copy_vec_to_vec(boost::numeric::ublas::column(Q, vcl_size_t(k)), t);
            inner_rt = viennacl::linalg::inner_prod(r,t);
            r = r - inner_rt * t;
            reorths++;
          }
          retry++;
          temp = viennacl::linalg::norm_2(r);
          r = r / temp;
          vcl_beta = vcl_beta * temp;
        }
      }

      detail::copy_vec_to_vec(r,s);
      boost::numeric::ublas::column(Q, vcl_size_t(i)) = s;

      cpu_beta = vcl_beta;
      s = - cpu_beta * boost::numeric::ublas::column(Q, vcl_size_t(i - 1));
      detail::copy_vec_to_vec(s, u);
      u += viennacl::linalg::prod(A, r);
      vcl_alpha = viennacl::linalg::inner_prod(u, r);
      alphas.push_back(vcl_alpha);
    }

    return bisect(alphas, betas);
  }

  /**
  *   @brief Implementation of the lanczos algorithm without reorthogonalization
  *
  *   @param A            The system matrix
  *   @param r            Random start vector
  *   @param size         Size of krylov-space
  *   @return             Returns the eigenvalues (number of eigenvalues equals size of krylov-space)
  */
  template<typename MatrixT, typename VectorT>
  std::vector<
          typename viennacl::result_of::cpu_value_type<typename MatrixT::value_type>::type
          >
  lanczos (MatrixT const& A, VectorT & r, vcl_size_t size, lanczos_tag)
  {
    typedef typename viennacl::result_of::value_type<MatrixT>::type        ScalarType;
    typedef typename viennacl::result_of::cpu_value_type<ScalarType>::type    CPU_ScalarType;

    ScalarType vcl_beta;
    ScalarType vcl_alpha;
    std::vector<CPU_ScalarType> alphas, betas;
    CPU_ScalarType norm;
    vcl_size_t n = r.size();
    VectorT u(n), t(n);
    boost::numeric::ublas::vector<CPU_ScalarType> s(r.size()), u_zero(n), q(n);
    boost::numeric::ublas::matrix<CPU_ScalarType> Q(n, size);

    u_zero = boost::numeric::ublas::zero_vector<CPU_ScalarType>(n);
    detail::copy_vec_to_vec(u_zero, u);
    norm = norm_2(r);

    for (vcl_size_t i = 0;i < size; i++)
    {
      r /= norm;
      vcl_beta = norm;

      detail::copy_vec_to_vec(r,s);
      boost::numeric::ublas::column(Q, i) = s;

      u += prod(A, r);
      vcl_alpha = inner_prod(u, r);
      r = u - vcl_alpha * r;
      norm = norm_2(r);

      q = boost::numeric::ublas::column(Q, i);
      detail::copy_vec_to_vec(q, t);

      u = - norm * t;
      alphas.push_back(vcl_alpha);
      betas.push_back(vcl_beta);
      s.clear();
    }

    return bisect(alphas, betas);
  }

  /**
  *   @brief Implementation of the Lanczos FRO algorithm
  *
  *   @param A            The system matrix
  *   @param r            Random start vector
  *   @param size         Size of krylov-space
  *   @return             Returns the eigenvalues (number of eigenvalues equals size of krylov-space)
  */
  template< typename MatrixT, typename VectorT >
  std::vector<
          typename viennacl::result_of::cpu_value_type<typename MatrixT::value_type>::type
          >
  lanczosFRO (MatrixT const& A, VectorT & r, vcl_size_t size, lanczos_tag)
  {
    typedef typename viennacl::result_of::value_type<MatrixT>::type            NumericType;
    typedef typename viennacl::result_of::cpu_value_type<NumericType>::type    CPU_NumericType;

    CPU_NumericType temp;
    CPU_NumericType norm;
    NumericType vcl_beta;
    NumericType vcl_alpha;
    std::vector<CPU_NumericType> alphas, betas;
    vcl_size_t n = r.size();
    VectorT u(n), t(n);
    NumericType inner_rt;
    boost::numeric::ublas::vector<CPU_NumericType> u_zero(n), s(r.size()), q(n);
    boost::numeric::ublas::matrix<CPU_NumericType> Q(n, size);

    long reorths = 0;
    norm = norm_2(r);

    for (vcl_size_t i = 0; i < size; i++)
    {
      r /= norm;

      for (vcl_size_t j = 0; j < i; j++)
      {
        q = boost::numeric::ublas::column(Q, j);
        detail::copy_vec_to_vec(q, t);
        inner_rt = viennacl::linalg::inner_prod(r,t);
        r = r - inner_rt * t;
        reorths++;
      }
      temp = viennacl::linalg::norm_2(r);
      r = r / temp;
      vcl_beta = temp * norm;
      detail::copy_vec_to_vec(r,s);
      boost::numeric::ublas::column(Q, i) = s;

      u += viennacl::linalg::prod(A, r);
      vcl_alpha = viennacl::linalg::inner_prod(u, r);
      r = u - vcl_alpha * r;
      norm = viennacl::linalg::norm_2(r);
      q = boost::numeric::ublas::column(Q, i);
      detail::copy_vec_to_vec(q, t);
      u = - norm * t;
      alphas.push_back(vcl_alpha);
      betas.push_back(vcl_beta);
    }

    return bisect(alphas, betas);
  }

} // end namespace detail

/**
*   @brief Implementation of the calculation of eigenvalues using lanczos
*
*   @param matrix        The system matrix
*   @param tag           Tag with several options for the lanczos algorithm
*   @return              Returns the n largest eigenvalues (n defined in the lanczos_tag)
*/
template<typename MatrixT>
std::vector< typename viennacl::result_of::cpu_value_type<typename MatrixT::value_type>::type >
eig(MatrixT const & matrix, lanczos_tag const & tag)
{
  typedef typename viennacl::result_of::value_type<MatrixT>::type           NumericType;
  typedef typename viennacl::result_of::cpu_value_type<NumericType>::type   CPU_NumericType;
  typedef typename viennacl::result_of::vector_for_matrix<MatrixT>::type    VectorT;

  boost::mt11213b mt;
  boost::normal_distribution<CPU_NumericType>    N(0, 1);
  boost::bernoulli_distribution<CPU_NumericType> B(0.5);
  boost::triangle_distribution<CPU_NumericType>  T(-1, 0, 1);

  boost::variate_generator<boost::mt11213b&, boost::normal_distribution<CPU_NumericType> >     get_N(mt, N);
  boost::variate_generator<boost::mt11213b&, boost::bernoulli_distribution<CPU_NumericType> >  get_B(mt, B);
  boost::variate_generator<boost::mt11213b&, boost::triangle_distribution<CPU_NumericType> >   get_T(mt, T);

  std::vector<CPU_NumericType> eigenvalues;
  vcl_size_t matrix_size = matrix.size1();
  VectorT r(matrix_size);
  std::vector<CPU_NumericType> s(matrix_size);

  for (vcl_size_t i=0; i<s.size(); ++i)
    s[i] = 3.0 * get_B() + get_T() - 1.5;

  detail::copy_vec_to_vec(s,r);

  vcl_size_t size_krylov = (matrix_size < tag.krylov_size()) ? matrix_size
                                                              : tag.krylov_size();

  switch (tag.method())
  {
    case lanczos_tag::partial_reorthogonalization:
      eigenvalues = detail::lanczosPRO(matrix, r, size_krylov, tag);
      break;
    case lanczos_tag::full_reorthogonalization:
      eigenvalues = detail::lanczosFRO(matrix, r, size_krylov, tag);
      break;
    case lanczos_tag::no_reorthogonalization:
      eigenvalues = detail::lanczos(matrix, r, size_krylov, tag);
      break;
  }

  std::vector<CPU_NumericType> largest_eigenvalues;

  for (vcl_size_t i = 1; i<=tag.num_eigenvalues(); i++)
    largest_eigenvalues.push_back(eigenvalues[size_krylov-i]);

  return largest_eigenvalues;
}

} // end namespace linalg
} // end namespace viennacl
#endif


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * 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.
*/

#include "SiconosConfig.h"

#pragma GCC diagnostic ignored "-Wunused-local-typedefs"

#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

//#define BIND_FORTRAN_LOWERCASE_UNDERSCORE
#include <boost/numeric/bindings/ublas/vector_proxy.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/blas.hpp>
#include <boost/numeric/bindings/lapack.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/std/vector.hpp>

namespace lapack = boost::numeric::bindings::lapack;

#include "SiconosVector.hpp"
#include "cholesky.hpp"
#include "SimpleMatrix.hpp"
#include "BlockMatrixIterators.hpp"
#include "BlockMatrix.hpp"

#include "SiconosAlgebra.hpp"

using namespace Siconos;

void SimpleMatrix::PLUFactorizationInPlace()
{
  if (_isPLUFactorized)
  {
    std::cout << "SimpleMatrix::PLUFactorizationInPlace warning: this matrix is already PLUFactorized. " << std::endl;
    return;
  }
  if (_num == 1)
  {
    if (!_ipiv)
      _ipiv.reset(new VInt(size(0)));
    else
      _ipiv->resize(size(0));
    int info = lapack::getrf(*mat.Dense, *_ipiv);
    if (info != 0)
    {
      _isPLUFactorized = false;
      SiconosMatrixException::selfThrow("SimpleMatrix::PLUFactorizationInPlace failed: the matrix is singular.");
    }
    else _isPLUFactorized = true;
  }
  else
  {
    int info = cholesky_decompose(*sparse());
    // \warning: VA 24/11/2010: work only for symmetric matrices. Should be replaced by efficient implementatation (e.g. mumps )
    if (info != 0)
    {
      display();
      _isPLUFactorized = false;
      std::cout << "Problem in Cholesky Decomposition for the row number" << info   << std::endl;
      SiconosMatrixException::selfThrow("SimpleMatrix::PLUFactorizationInPlace failed. ");
    }
    else _isPLUFactorized = true;
  }

}

void SimpleMatrix::PLUInverseInPlace()
{
  if (!_isPLUFactorized)
    PLUFactorizationInPlace();
  if (_num != 1)
    SiconosMatrixException::selfThrow(" SimpleMatrix::PLUInverseInPlace: only implemented for dense matrices.");

#if defined(HAVE_ATLAS) && defined(OUTSIDE_FRAMEWORK_BLAS)
  int info = lapack::getri(*mat.Dense, *_ipiv);   // solve from factorization

  if (info != 0)
    SiconosMatrixException::selfThrow("SimpleMatrix::PLUInverseInPlace failed, the matrix is singular.");

  _isPLUInversed = true;
#else
  SiconosMatrixException::selfThrow("SimpleMatrix::PLUInverseInPlace not implemented with lapack.");
#endif
}

void SimpleMatrix::PLUForwardBackwardInPlace(SiconosMatrix &B)
{
  if (B.isBlock())
    SiconosMatrixException::selfThrow("SimpleMatrix PLUForwardBackwardInPlace(B) failed at solving Ax = B. Not yet implemented for a BlockMatrix B.");
  int info = 0;

  if (_num == 1)
  {
    if (!_isPLUFactorized) // call gesv => LU-factorize+solve
    {
      // solve system:
      if (!_ipiv)
        _ipiv.reset(new VInt(size(0)));
      else
        _ipiv->resize(size(0));
      info = lapack::gesv(*mat.Dense, *_ipiv, *(B.dense()));
      _isPLUFactorized = true;

      /*
        ublas::vector<double> S(std::max(size(0),size(1)));
        ublas::matrix<double, ublas::column_major> U(size(0),size(1));
        ublas::matrix<double, ublas::column_major> VT(size(0),size(1));

        int ierr = lapack::gesdd(*mat.Dense, S, U, VT);
        printf("info = %d, ierr = %d, emax = %f, emin = %f , cond = %f\n",info,ierr,S(0),S(2),S(0)/S(2));
      */
      // B now contains solution:
    }
    else // call getrs: only solve using previous lu-factorization
      if (B.num() == 1)
        info = lapack::getrs(*mat.Dense, *_ipiv, *(B.dense()));
      else
        SiconosMatrixException::selfThrow(" SimpleMatrix::PLUInverseInPlace: only implemented for dense matrices in RHS.");
  }
  else
  {
    if (!_isPLUFactorized) // call first PLUFactorizationInPlace
    {
      PLUFactorizationInPlace();
    }
    // and then solve
    if (B.num() == 1)
    {
      inplace_solve(*sparse(), *(B.dense()), ublas::lower_tag());
      inplace_solve(ublas::trans(*sparse()), *(B.dense()), ublas::upper_tag());
    }
    else if (B.num() == 4)
    {
      inplace_solve(*sparse(), *(B.sparse()), ublas::lower_tag());
      inplace_solve(ublas::trans(*sparse()), *(B.sparse()), ublas::upper_tag());
    }
    else
      SiconosMatrixException::selfThrow(" SimpleMatrix::PLUInverseInPlace: only implemented for dense ans sparse matrices in RHS.");
    info = 0 ;
  }
  //  SiconosMatrixException::selfThrow(" SimpleMatrix::PLUInverseInPlace: only implemented for dense matrices.");

  if (info != 0)
    SiconosMatrixException::selfThrow("SimpleMatrix::PLUForwardBackwardInPlace failed.");
}

void SimpleMatrix::PLUForwardBackwardInPlace(SiconosVector &B)
{
  if (B.isBlock())
    SiconosMatrixException::selfThrow("SimpleMatrix PLUForwardBackwardInPlace(V) failed. Not yet implemented for V being a BlockVector.");

  DenseMat tmpB(B.size(), 1);
  ublas::column(tmpB, 0) = *(B.dense()); // Conversion of vector to matrix. Temporary solution.
  int info;

  if (_num == 1)
  {
    if (!_isPLUFactorized) // call gesv => LU-factorize+solve
    {
      // solve system:
      if (!_ipiv)
        _ipiv.reset(new VInt(size(0)));
      else
        _ipiv->resize(size(0));

      info = lapack::gesv(*mat.Dense, *_ipiv, tmpB);
      _isPLUFactorized = true;

      /*
        ublas::matrix<double> COPY(*mat.Dense);
        ublas::vector<double> S(std::max(size(0),size(1)));
        ublas::matrix<double, ublas::column_major> U(size(0),size(1));
        ublas::matrix<double, ublas::column_major> VT(size(0),size(1));

        int ierr = lapack::gesdd(COPY, S, U, VT);
        printf("info = %d, ierr = %d, emax = %f, emin = %f , cond = %f\n",info,ierr,S(0),S(2),S(0)/S(2));
      */
      // B now contains solution:
    }
    else // call getrs: only solve using previous lu-factorization
      info = lapack::getrs(*mat.Dense, *_ipiv, tmpB);
  }
  else
  {
    if (!_isPLUFactorized) // call first PLUFactorizationInPlace
    {
      PLUFactorizationInPlace();
    }
    // and then solve
    inplace_solve(*sparse(), tmpB, ublas::lower_tag());
    inplace_solve(ublas::trans(*sparse()), tmpB, ublas::upper_tag());
    info = 0;
  }
  if (info != 0)
    SiconosMatrixException::selfThrow("SimpleMatrix::PLUForwardBackwardInPlace failed.");
  else
  {
    noalias(*(B.dense())) = ublas::column(tmpB, 0);
  }
}

void SimpleMatrix::resetLU()
{
  if (_ipiv) _ipiv->clear();
  _isPLUFactorized = false;
  _isPLUInversed = false;
}

void SimpleMatrix::resetQR()
{
  _isQRFactorized = false;

}
// const SimpleMatrix operator * (const SimpleMatrix & A, const SimpleMatrix& B )
// {
//   return (DenseMat)prod(*A.dense() , *B.dense());
//   //  return A;
// }

void SimpleMatrix::SolveByLeastSquares(SiconosMatrix &B)
{
  if (B.isBlock())
    SiconosMatrixException::selfThrow("SimpleMatrix::SolveByLeastSquares(Siconos Matrix &B) failed. Not yet implemented for M being a BlockMatrix.");
  int info = 0;
#ifdef USE_OPTIMAL_WORKSPACE
  info += lapack::gels(*mat.Dense, *(B.dense()), lapack::optimal_workspace());
#endif
#ifdef USE_MINIMAL_WORKSPACE
  info += lapack::gels(*mat.Dense, *(B.dense()), lapack::minimal_workspace());
#endif
  if (info != 0)
    SiconosMatrixException::selfThrow("SimpleMatrix::SolveByLeastSquares failed.");
}

void SimpleMatrix::SolveByLeastSquares(SiconosVector &B)
{
  if (B.isBlock())
    SiconosMatrixException::selfThrow("SimpleMatrix::SolveByLeastSquares(SiconosVector &B) failed. Not yet implemented for V being a BlockVector.");

  DenseMat tmpB(B.size(), 1);
  ublas::column(tmpB, 0) = *(B.dense()); // Conversion of vector to matrix. Temporary solution.
  int info = 0;

#ifdef USE_OPTIMAL_WORKSPACE
  info += lapack::gels(*mat.Dense, tmpB, lapack::optimal_workspace());
#endif
#ifdef USE_MINIMAL_WORKSPACE
  info += lapack::gels(*mat.Dense, tmpB, lapack::minimal_workspace());
#endif
  if (info != 0)
  {
    std::cout << "info = " << info << std::endl;
    SiconosMatrixException::selfThrow("SimpleMatrix::SolveByLeastSquares failed.");
  }
  else
  {
    noalias(*(B.dense())) = ublas::column(tmpB, 0);
  }

}

/*
void polePlacement(const SiconosMatrix& A, const SiconosVector& B, SiconosVector& P, bool transpose)
{
  unsigned int n = A.size(0);
  DenseMat AA(n, n);
  DenseMat Q(n, n);
  DenseVect tau(n);
  DenseVect BB(n);
  noalias(AA) = (*A.dense());
  lapack::gehrd(1, n, AA, tau);
  lapack::orghr(n, 1, n, Q, tau);
  noalias(BB) = prod(Q, *B.dense());
}
*/


#include "moja/datarepository/tileblockcellindexer.h"

#include <boost/format.hpp>
#include <boost/format/group.hpp>

#include <iomanip>

namespace moja {
namespace datarepository {

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

Rectangle::Rectangle(Point p, Size s) : _location(p), _size(s) {}
Rectangle::Rectangle(double x, double y, double width, double height) : _location({y, x}), _size({height, width}) {}
Rectangle::~Rectangle() {}

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

TileBlockCellIndexer::TileBlockCellIndexer(double tileLatSize, double tileLonSize, double blockLatSize,
                                           double blockLonSize, double cellLatSize, double cellLonSize) {
   // TODO: Check all sizes fit evenly, can't have part tiles/blocks/cells
   // throw an exception if we have an issue
   SanityChecksLatLon(tileLatSize, tileLonSize, blockLatSize, blockLonSize, cellLatSize, cellLonSize);

   SetGlobalData();

   tileDesc.latSize = tileLatSize;
   tileDesc.lonSize = tileLonSize;
   tileDesc.latSizeInv = 1.0 / tileDesc.latSize;
   tileDesc.lonSizeInv = 1.0 / tileDesc.lonSize;
   tileDesc.cols = convOtoI(globalDesc.lonSize, tileLonSize);
   tileDesc.rows = convOtoI(globalDesc.latSize, tileLatSize);
   tileDesc.indexLimit = tileDesc.cols * tileDesc.rows;

   blockDesc.latSize = blockLatSize;
   blockDesc.lonSize = blockLonSize;
   blockDesc.latSizeInv = 1.0 / blockDesc.latSize;
   blockDesc.lonSizeInv = 1.0 / blockDesc.lonSize;
   blockDesc.cols = convOtoI(tileDesc.lonSize, blockLonSize);
   blockDesc.rows = convOtoI(tileDesc.latSize, blockLatSize);
   blockDesc.indexLimit = blockDesc.cols * blockDesc.rows;

   cellDesc.latSize = cellLatSize;
   cellDesc.lonSize = cellLonSize;
   cellDesc.latSizeInv = 1.0 / cellDesc.latSize;
   cellDesc.lonSizeInv = 1.0 / cellDesc.lonSize;
   cellDesc.cols = convOtoI(blockDesc.lonSize, cellLonSize);
   cellDesc.rows = convOtoI(blockDesc.latSize, cellLatSize);
   cellDesc.indexLimit = cellDesc.cols * cellDesc.rows;

   cellResCompareValueLon = Int32((cellDesc.lonSize * 100000) + MathEx::k0Plus);  //  0.00025;
   cellResCompareValueLat = Int32((cellDesc.latSize * 100000) + MathEx::k0Plus);
}

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

TileBlockCellIndexer::TileBlockCellIndexer(UInt32 tileRows, UInt32 tileCols, UInt32 blockRows, UInt32 blockCols,
                                           UInt32 cellRows, UInt32 cellCols) {
   // TODO: Check all sizes fit evenly, can't have part tiles/blocks/cells
   // throw an exception if we have an issue
   SanityChecksIndex(tileRows, tileCols, blockRows, blockCols, cellRows, cellCols);
   SetGlobalData();

   tileDesc.cols = tileCols;
   tileDesc.rows = tileRows;
   tileDesc.latSize = globalDesc.latSize / tileRows;  // TODO: do I use MathEx::k0Plus here?
   tileDesc.lonSize = globalDesc.lonSize / tileCols;  // TODO: do I use MathEx::k0Plus here?
   tileDesc.latSizeInv = 1.0 / tileDesc.latSize;
   tileDesc.lonSizeInv = 1.0 / tileDesc.lonSize;
   tileDesc.indexLimit = tileDesc.cols * tileDesc.rows;

   blockDesc.cols = blockCols;
   blockDesc.rows = blockRows;
   blockDesc.latSize = tileDesc.latSize / blockRows;  // TODO: do I use MathEx::k0Plus here?
   blockDesc.lonSize = tileDesc.lonSize / blockCols;  // TODO: do I use MathEx::k0Plus here?
   blockDesc.latSizeInv = 1.0 / blockDesc.latSize;
   blockDesc.lonSizeInv = 1.0 / blockDesc.lonSize;
   blockDesc.indexLimit = blockDesc.cols * blockDesc.rows;

   cellDesc.cols = cellCols;
   cellDesc.rows = cellRows;
   cellDesc.latSize = blockDesc.latSize / cellRows;  // TODO: do I use MathEx::k0Plus here?
   cellDesc.lonSize = blockDesc.lonSize / cellCols;  // TODO: do I use MathEx::k0Plus here?
   cellDesc.latSizeInv = 1.0 / cellDesc.latSize;
   cellDesc.lonSizeInv = 1.0 / cellDesc.lonSize;
   cellDesc.indexLimit = cellDesc.cols * cellDesc.rows;

   cellResCompareValueLon = Int32((cellDesc.lonSize * 100000) + MathEx::k0Plus);  //  0.00025;
   cellResCompareValueLat = Int32((cellDesc.latSize * 100000) + MathEx::k0Plus);
}

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

void TileBlockCellIndexer::SetGlobalData() {
   globalDesc.name = "Global";
   tileDesc.name = "Tile";
   blockDesc.name = "Block";
   cellDesc.name = "Cell";

   globalDesc.latSize = 180.0;
   globalDesc.lonSize = 360.0;
   globalDesc.latSizeInv = 1.0 / 180.0;
   globalDesc.lonSizeInv = 1.0 / 360.0;
   globalDesc.cols = 1;
   globalDesc.rows = 1;
   globalDesc.indexLimit = 1;
}

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

void TileBlockCellIndexer::SanityChecksLatLon(double tileLatSize, double tileLonSize, double blockLatSize,
                                              double blockLonSize, double cellLatSize, double cellLonSize) {
   // Can't get these to work as the floating point MOD has issues
#if 0
	double result = fmod(180.0, tileLatSize);
	if (!FloatCmp::equalTo(result, 0.0)) {
		std::stringstream ss;
		ss << "fmod(" << tileLatSize << ", 180.0) = " << result;
		BOOST_THROW_EXCEPTION(TileBlockCellIndexerInvalidParameterException() << Component(ss.str()) << Constraint("180.0 % tileLatSize == 0"));
	}
	result = fmod(360.0, tileLonSize);
	if (!FloatCmp::equalTo(result, 0.0)) {
		std::stringstream ss;
		ss << "fmod(" << tileLonSize << ", 360.0) = " << result;
		BOOST_THROW_EXCEPTION(TileBlockCellIndexerInvalidParameterException() << Component(ss.str()) << Constraint("360.0 % tileLonSize == 0"));
	}

	result = fmod(tileLatSize, blockLatSize);
	if (!FloatCmp::equalTo(result, 0.0)) {
		std::stringstream ss;
		ss << "fmod(" << blockLatSize << ", tileLatSize) = " << result;
		BOOST_THROW_EXCEPTION(TileBlockCellIndexerInvalidParameterException() << Component(ss.str()) << Constraint("tileLatSize % blockLatSize == 0"));
	}

	result = fmod(tileLonSize, blockLonSize);
	if (!FloatCmp::equalTo(result, 0.0)) {
		std::stringstream ss;
		ss << "fmod(" << blockLonSize << ", tileLonSize) = " << result;
		BOOST_THROW_EXCEPTION(TileBlockCellIndexerInvalidParameterException() << Component(ss.str()) << Constraint("tileLonSize % blockLonSize == 0"));
	}

	result = fmod(blockLatSize, cellLatSize);
	if (!FloatCmp::equalTo(result, 0.0)) {
		std::stringstream ss;
		ss << "fmod(" << cellLatSize << ", blockLatSize) = " << result;
		BOOST_THROW_EXCEPTION(TileBlockCellIndexerInvalidParameterException() << Component(ss.str()) << Constraint("blockLatSize % cellLatSize == 0"));
	}

	result = fmod(blockLonSize, cellLonSize);
	if (!FloatCmp::equalTo(result, 0.0)) {
		std::stringstream ss;
		ss << "fmod(" << cellLonSize << ", blockLonSize) = " << result;
		BOOST_THROW_EXCEPTION(TileBlockCellIndexerInvalidParameterException() << Component(ss.str()) << Constraint("blockLonSize % cellLonSize == 0"));
	}
#endif
}

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

void TileBlockCellIndexer::SanityChecksIndex(UInt32 tileRows, UInt32 tileCols, UInt32 blockRows, UInt32 blockCols,
                                             UInt32 cellRows, UInt32 cellCols) {}

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

std::string TileBlockCellIndexer::toString() const {
   std::string text;
   const TileBlockCellLayerDesc* _globalDesc[4] = {&globalDesc, &tileDesc, &blockDesc, &cellDesc};
   for (auto i = 0; i < 4; i++) {
      text += (boost::format("%1%,%2%,%3%,%4%,%5%\n") % _globalDesc[i]->name %
               boost::io::group(std::setprecision(15), _globalDesc[i]->latSize) %
               boost::io::group(std::setprecision(15), _globalDesc[i]->lonSize) % _globalDesc[i]->rows %
               _globalDesc[i]->cols)
                  .str();
   }
   return text;
}

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

Point TileBlockCellIndexer::getLatLonFromIndex(const CellIdx& cellIndex) const {
   auto originLon = (cellIndex.tileX() * tileDesc.lonSize) + (cellIndex.blockX() * blockDesc.lonSize) +
                    (cellIndex.cellX() * cellDesc.lonSize);
   auto originLat = (cellIndex.tileY() * tileDesc.latSize) + (cellIndex.blockY() * blockDesc.latSize) +
                    (cellIndex.cellY() * cellDesc.latSize);
   return Point({convertFromLatOrigin(originLat), convertFromLonOrigin(originLon)});
}

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

Point TileBlockCellIndexer::getLatLonFromIndex(const BlockIdx& blockIndex) const {
   auto originLon = (blockIndex.tileX() * tileDesc.lonSize) + (blockIndex.blockX() * blockDesc.lonSize);
   auto originLat = (blockIndex.tileY() * tileDesc.latSize) + (blockIndex.blockY() * blockDesc.latSize);
   return Point({convertFromLatOrigin(originLat), convertFromLonOrigin(originLon)});
}

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

Point TileBlockCellIndexer::getLatLonFromIndex(const TileIdx& tileIndex) const {
   auto originLon = tileIndex.tileX() * tileDesc.lonSize;
   auto originLat = tileIndex.tileY() * tileDesc.latSize;
   return Point({convertFromLatOrigin(originLat), convertFromLonOrigin(originLon)});
}

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

double TileBlockCellIndexer::getLatFromIndex(const CellIdx& cellIndex) const {
   auto originLat = (cellIndex.tileY() * tileDesc.latSize) + (cellIndex.blockY() * blockDesc.latSize) +
                    (cellIndex.cellY() * cellDesc.latSize);
   return convertFromLatOrigin(originLat);
}

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

double TileBlockCellIndexer::getLatFromIndex(const BlockIdx& blockIndex) const {
   auto originLat = (blockIndex.tileY() * tileDesc.latSize) + (blockIndex.blockY() * blockDesc.latSize);
   return convertFromLatOrigin(originLat);
}

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

double TileBlockCellIndexer::getLatFromIndex(const TileIdx& tileIndex) const {
   auto originLat = tileIndex.tileY() * tileDesc.latSize;
   return convertFromLatOrigin(originLat);
}

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

Point TileBlockCellIndexer::getOriginFromIndex(const CellIdx& cellIndex) const {
   auto oLon = (cellIndex.tileX() * tileDesc.lonSize) + (cellIndex.blockX() * blockDesc.lonSize) +
               (cellIndex.cellX() * cellDesc.lonSize);
   auto oLat = (cellIndex.tileY() * tileDesc.latSize) + (cellIndex.blockY() * blockDesc.latSize) +
               (cellIndex.cellY() * cellDesc.latSize);
   return Point({oLat, oLon});
}

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

Point TileBlockCellIndexer::getOriginFromIndex(const BlockIdx& blockIndex) const {
   auto oLon = (blockIndex.tileX() * tileDesc.lonSize) + (blockIndex.blockX() * blockDesc.lonSize);
   auto oLat = (blockIndex.tileY() * tileDesc.latSize) + (blockIndex.blockY() * blockDesc.latSize);
   return Point({oLat, oLon});
}

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

Point TileBlockCellIndexer::getOriginFromIndex(const TileIdx& tileIndex) const {
   auto oLon = (tileIndex.tileX() * tileDesc.lonSize);
   auto oLat = (tileIndex.tileY() * tileDesc.latSize);
   return Point({oLat, oLon});
}

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

CellIdx TileBlockCellIndexer::getCellIndexFromLatLon(const Point& point) const {
   auto oLon = convertToLonOrigin(point.lon);
   auto oLat = convertToLatOrigin(point.lat);
   auto tileX = convOtoI(oLon, tileDesc.lonSize);
   auto tileY = convOtoI(oLat, tileDesc.latSize);
   auto blockX = convOtoI(oLon - (tileX * tileDesc.lonSize), blockDesc.lonSize);
   auto blockY = convOtoI(oLat - (tileY * tileDesc.latSize), blockDesc.latSize);
   auto cellX = convOtoI(oLon - (tileX * tileDesc.lonSize) - (blockX * blockDesc.lonSize), cellDesc.lonSize);
   auto cellY = convOtoI(oLat - (tileY * tileDesc.latSize) - (blockY * blockDesc.latSize), cellDesc.latSize);
   CellIdx cellIdx(XYIndex({tileX, tileY}), XYIndex({blockX, blockY}), XYIndex({cellX, cellY}), *this);
   return cellIdx;
}

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

BlockIdx TileBlockCellIndexer::getBlockIndexFromLatLon(const Point& point) const {
   auto oLon = convertToLonOrigin(point.lon);
   auto oLat = convertToLatOrigin(point.lat);
   auto tileX = convOtoI(oLon, tileDesc.lonSize);
   auto tileY = convOtoI(oLat, tileDesc.latSize);
   auto blockX = convOtoI(oLon - (tileX * tileDesc.lonSize), blockDesc.lonSize);
   auto blockY = convOtoI(oLat - (tileY * tileDesc.latSize), blockDesc.latSize);
   BlockIdx blockIdx(XYIndex({tileX, tileY}), XYIndex({blockX, blockY}), *this);
   return blockIdx;
}

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

TileIdx TileBlockCellIndexer::getTileIndexFromLatLon(const Point& point) const {
   auto oLon = convertToLonOrigin(point.lon);
   auto oLat = convertToLatOrigin(point.lat);
   auto tileX = convOtoI(oLon, tileDesc.lonSize);
   auto tileY = convOtoI(oLat, tileDesc.latSize);
   TileIdx tileIdx(XYIndex({tileX, tileY}), *this);
   return tileIdx;
}

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

CellIdx TileBlockCellIndexer::convertIndex(const CellIdx& cell) const {
   if (compare(*cell._indexer)) return cell;
   auto cellX = convOtoI(cell.cellX() * cell._indexer->cellDesc.lonSize, cellDesc.lonSize);
   auto cellY = convOtoI(cell.cellY() * cell._indexer->cellDesc.latSize, cellDesc.latSize);
   auto cellIdx = (cellY * cellDesc.cols + cellX);
   return CellIdx(cell.tileIdx, cell.blockIdx, cellIdx, *this);
}

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

std::string TileIdx::toString() const {
   std::string text;

#if 1
   text += (boost::format("(%1%)") % boost::io::group(std::setw(6), tileIdx)).str();
#else
   text += (boost::format("(%1%:[%2%,%3%])") % boost::io::group(std::setw(6), tileIdx) %
            boost::io::group(std::setw(3), tileX()) % boost::io::group(std::setw(3), tileY()))
               .str();
#endif
   return text;
}

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

std::string BlockIdx::toString() const {
   std::string text;

#if 1
   text +=
       (boost::format("(%1%,%2%)") % boost::io::group(std::setw(6), tileIdx) % boost::io::group(std::setw(3), blockIdx))
           .str();
#else
   text += (boost::format("(%1%,%2%:[%3%,%4%],[%5%,%6%])") % boost::io::group(std::setw(6), tileIdx) %
            boost::io::group(std::setw(3), blockIdx) % boost::io::group(std::setw(3), tileX()) %
            boost::io::group(std::setw(3), tileY()) % boost::io::group(std::setw(3), blockX()) %
            boost::io::group(std::setw(3), blockY()))
               .str();
#endif
   return text;
}

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

std::string CellIdx::toString() const {
   std::string text;

#if 1
   text += (boost::format("(%1%,%2%,%3%)") % boost::io::group(std::setw(6), tileIdx) %
            boost::io::group(std::setw(3), blockIdx) % boost::io::group(std::setw(6), cellIdx))
               .str();
#else
   text += (boost::format("(%1%,%2%,%3%:[%4%,%5%],[%6%,%7%],[%8%,%9%])") % boost::io::group(std::setw(6), tileIdx) %
            boost::io::group(std::setw(3), blockIdx) % cellIdx % boost::io::group(std::setw(3), tileX()) %
            boost::io::group(std::setw(3), tileY()) % boost::io::group(std::setw(3), blockX()) %
            boost::io::group(std::setw(3), blockY()) % boost::io::group(std::setw(4), cellX()) %
            boost::io::group(std::setw(4), cellY()))
               .str();
#endif
   return text;
}

}  // namespace datarepository
}  // namespace moja


#include <math.h>
#include "LITD_VirtualPoint.h"
#include <Eigen/Dense>

LITD_VirtualPoint::LITD_VirtualPoint()
: LITD_VirtualPoint(0.0, 0.0, 0.0, 0.0)
{
}

LITD_VirtualPoint::LITD_VirtualPoint(double x, double y, double k, double h)
: x(x),
  y(y),
  k(k),
  h(h)
{
}

Vector2d LITD_VirtualPoint::getVector2d() const
{
  return Vector2d(x,y);
}

std::string LITD_VirtualPoint::toString() const
{
  std::stringstream ss;
  ss << "[x ";
  ss << x;
  ss << " y ";
  ss << y;
  ss << " k ";
  ss << k;
  ss << " h ";
  ss << h;

  return ss.str();
}


/*-------------------------------------------------------------
Copyright 2019 Wenxin Liu, Kartik Mohta, Giuseppe Loianno

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

#include "sdd_vio/vo_stereo.h"

#include <iostream>
#include "sdd_vio/utils/math_utils.h"
#include <algorithm>
#include <math.h>
#include <ros/ros.h>
#include <cmath>
#include <assert.h>
#include <boost/thread.hpp>
#include "sdd_vio/utils/ros_params_helper.h"
#include "sdd_vio/utils/timer.hpp"
#include <ctime>
#include <opencv2/core/eigen.hpp>

namespace sdd_vio {

/*
 * Optimization using Forward Compositional Approach
 *
 * Input:
 *      img_curr: current frame image
 *      ilay: the index of tracking layer in the image pyramid. 0 is the largest layer for tracking.
 * Return:
 *      T_curr: current transformation matrix from keyframe to current frame
 *      T_wb: transformation between current robot frame to world frame
 *      T_: transformation between current camera frame to initial camera frame
 *      v_: velocity of current body frame
 * Use of private class variables:
 *      feat_3D_[ilay]: the vector of 3D points in keyframe
 *      intensities_[ilay]: the vector of pixel intensities in keyframe
 *      mask_[ilay]: indicator of whether points are in view
 * LMA params as private variables:
 *      lambda_
 *      up_factor_
 *      down_factor_
 *
 */
    void VoStereo::optimize_fca(const cv::Mat& img_curr, Eigen::Isometry3f& T_curr, Eigen::Isometry3f& T_wb, Eigen::Isometry3f& T, Eigen::Vector3f& v, int ilay) {

        /* obtain gradient image of the current frame */
        cv::Mat Gx_mat, Gy_mat;
        cv::Sobel(img_curr, Gx_mat, CV_32F, 1, 0, 3);
        cv::Sobel(img_curr, Gy_mat, CV_32F, 0, 1, 3);

        /* masked Jacobian matrix and error vector - remove points outside projection */
        Eigen::MatrixXf J_masked;  // every time J_masked is re-calculated for every pixel inside the view
        Eigen::VectorXf error_masked;
        Eigen::VectorXf W;  // diagonal weight matrix for Huber estimation

        /* Jacobian and error vector of IMU */
        Eigen::MatrixXf J_imu;
        Eigen::VectorXf r_imu;
        Eigen::MatrixXf Z_imu;  // inverse covariance of imu errors
        J_imu.resize(9,9);
        initJacobian_imu_fca(J_imu);
        r_imu.resize(9);
//        Eigen::VectorXf inv_cov_imu(9);
//        inv_cov_imu << weight_R_,weight_R_,weight_R_, weight_v_,weight_v_,weight_v_,weight_p_,weight_p_,weight_p_;
//        Z_imu = inv_cov_imu.asDiagonal();
        Z_imu = Cov_meas_.inverse();
//        std::cout<<"Z_imu:\n"<<Z_imu<<"\n";

        /* Levenberg Marquardt */
        float lambda = lambda_;  // initial lambda is equal to the initial one set
        float up = up_factor_;
        float down = down_factor_;
        bool if_break = false;

        int npts_in;  // number of points remained in view
        perc_ = 1.;  // percentage of points remained in view
        float err_init, err_last = 0;

        /* begins optimization iteration */
        for (int iter=0; iter<max_num_iter_; ++iter)
        {
            /* first iteration */
            if (iter == 0) {

                /* calculate error and Jacobian given initial transformation */
                getError_fca(img_curr, Gx_mat, Gy_mat, T_curr, T_wb, J_masked, error_masked, W, npts_in, ilay);
                getError_imu_fca(T_wb, v, r_imu, J_imu);

                perc_ = 100*npts_in/num_feat_pts_[ilay];
                float err_mean = get_error_mean(error_masked, r_imu);
                err_init = err_mean;
                err_last = err_mean;

            }

            using Mat9 = Eigen::Matrix<float, 9, 9>;
            using Vec9 = Eigen::Matrix<float, 9, 1>;

            if (use_lma_ == false)
            {
                /* obtain H and b */
                const Mat9 H = beta_*J_masked.transpose() * W.asDiagonal() * J_masked + alpha_ * (J_imu.transpose() * Z_imu * J_imu);  // H = J^t * W * J
                const Vec9 b = beta_*J_masked.transpose() * (W.asDiagonal() * error_masked) + alpha_ * (J_imu.transpose() * (Z_imu * r_imu));  // b = J^t * w * e

                /* get delta increment */
                const Vec9 delta = -H.ldlt().solve(b);
//                std::cout<<"delta: \n"<<delta<<"\n";

                update_optimization_step(delta, T_wb, v, T, T_curr);

                /* calculate error vector and Jacobian given transformation */
                getError_fca(img_curr, Gx_mat, Gy_mat, T_curr, T_wb, J_masked, error_masked, W, npts_in, ilay);
                getError_imu_fca(T_wb, v, r_imu, J_imu);
//                std::cout << "r_imu: \n"<<r_imu<<"\n";

                perc_ = 100*npts_in/num_feat_pts_[ilay];
                float err_mean = get_error_mean(error_masked, r_imu);
                float err_diff = (err_mean - err_last)/err_last;
                err_last = err_mean;

                /* terminal output */
                if (verbose_) {
                    std::cout << "iteration = "<<iter<<", err = "<< err_mean<<", derr = "<<err_diff<<std::endl;
                }

                /* Stop condition - if error is going up or decreased below target or reach max iterations */
                if (err_diff>0 || -err_diff < target_derr_ || iter == max_num_iter_-1)
                    if_break = true;

                /* end of optimization */
                if (if_break) {
                    ROS_INFO_STREAM("iteration "<<iter<<"--------");
                    ROS_INFO("err/initial err: %g/%g", err_mean, err_init);
                    if (err_mean > err_init)
                      ROS_WARN_STREAM("bad convergence - err/initial err: "<<err_mean<<"/"<<err_init<<", iteration"<<iter);
                    break;
                }

            }
            else  // use lma
            {

                /* temporary T_curr, is error goes up in subloop can switch back */
                Eigen::Isometry3f T_curr_temp;
                Eigen::Isometry3f T_wb_temp;
                Eigen::Isometry3f T_temp;
                Eigen::Vector3f v_temp;

                /* LMA */
                float mult = lambda;
                bool ill = true;  // if ill conditioned (error increase)
                int lmit = 0;  // number of iterations while ill=true

                /* obtain H and b */
                const Mat9 H = beta_* J_masked.transpose() * W.asDiagonal() * J_masked + alpha_ * (J_imu.transpose() * Z_imu * J_imu);  // H = J^t * W * J
                const Vec9 b = beta_* J_masked.transpose() * (W.asDiagonal() * error_masked) + alpha_ * (J_imu.transpose() * (Z_imu * r_imu));  // b = J^t * w * e

                float err_mean = 0, err_diff = 0;
                /* LMA wrapper of subloop */
                while ((ill==true) && (iter<max_num_iter_) && (lmit<5))
                {

                    /* lma update on H */
                    const Mat9 H_curr = H + Mat9(mult * H.diagonal().asDiagonal());

                    /* get delta increment */
                    const Vec9 delta = -H_curr.ldlt().solve(b);
//                    std::cout<<"delta: \n"<<delta<<"\n";

                    T_curr_temp = T_curr;
                    T_wb_temp = T_wb;
                    T_temp = T;
                    v_temp = v;
                    update_optimization_step(delta, T_wb_temp, v_temp, T_temp, T_curr_temp);

                    /* calculate error vector and Jacobian given transformation */
                    getError_fca(img_curr, Gx_mat, Gy_mat, T_curr_temp, T_wb_temp, J_masked, error_masked, W, npts_in, ilay);
                    getError_imu_fca(T_wb, v_temp, r_imu, J_imu);
//                    std::cout<<"J_imu: \n"<<J_imu<<"\n";
//                    std::cout << "r_imu: \n"<<r_imu<<"\n";

                    perc_ = 100*npts_in/num_feat_pts_[ilay];
                    err_mean = get_error_mean(error_masked, r_imu);
                    err_diff = (err_mean - err_last)/err_last;

                    ill = (err_diff > 0);  // if error larger than before, set ill = true, try increase lambda.

                    /* terminal output */
                    if (verbose_) {
                        std::cout << "iteration = "<<iter<<", lambda = "<<lambda<<", err = "<<
                            err_mean<<", derr = "<<err_diff<<std::endl;
                    }

                    /* if ill=true, higher lambda by up factor, count as one iteration */
                    if (ill) {
                          lambda *= up;
                        mult = lambda;
                        iter++;
                    }
                    lmit++;
                }

                err_last  = err_mean;

                /* update T with the one that makes error smaller or the last try */
                T_curr = T_curr_temp;
                T = T_temp;
                T_wb = T_wb_temp;
                v = v_temp;

                /* if error doesn't get higher for quite some time, this term will bring lambda down eventually */
                if (lambda > 1e-8)  // make sure doesn't overflow
                    lambda *= (1/down);

                /* if LM iterations didn't decrease the error, stop */
                if (ill) {
                    //ROS_WARN_STREAM("bad convergence!");
                    if_break = true;
                }

                /* if error is decreased below target or reach max iterations */
                if (-err_diff < target_derr_ || iter == max_num_iter_-1)
                    if_break = true;

                /* end of optimization */
                if (if_break) {
                    ROS_INFO_STREAM("iteration "<<iter<<"--------");
                    ROS_INFO("err/initial err: %g/%g", err_mean, err_init);
                    if (err_mean > err_init)
                      ROS_WARN_STREAM("bad convergence - err/initial err: "<<err_mean<<"/"<<err_init<<", iteration"<<iter);
                    break;
                }

            }

        } // for (int iter=0;

    }

/*
 * Update optimization targets given delta
 *
 */
    void VoStereo::update_optimization_step(const Eigen::Matrix<float, 9, 1>& delta, Eigen::Isometry3f &T_wb, Eigen::Vector3f &v, Eigen::Isometry3f &T, Eigen::Isometry3f &T_curr)
    {
        /* update current transformation */
        const Eigen::Vector3f delta_phi = delta.segment<3>(0);
        const Eigen::Matrix3f R_delta = exp_SO3(delta_phi);
        const Eigen::Vector3f t_delta = delta.segment<3>(3);
        const Eigen::Vector3f v_delta = delta.segment<3>(6);

        // update T_wb and v
        T_wb.linear() = T_wb.rotation() * R_delta;
        T_wb.translation() += t_delta;
        v += v_delta;

        // update T and T_curr
        T = T_bc_inv_ * T_wb * T_bc_;
        T_curr = T.inverse() * T_kf_;
    }

/*
 * Obtain err_mean for convergence evaluation
 *
 */
    float VoStereo::get_error_mean(const Eigen::VectorXf& error_masked, const Eigen::VectorXf& r_imu)
    {
        int npts_in = error_masked.size();
        float err_photo = error_masked.transpose()*error_masked; // sum of square errors
        // these are raw square errors - not weighed by huber weights
        // photometric part is the mean pixel intensity error, plus the IMU observation square error weighted by optimization weight alpha_
        float err_mean = beta_ * sqrt(err_photo/npts_in) / error_scale_factor_ + alpha_*(r_imu.transpose()*r_imu).value();
        return err_mean;
    }

 /*
  * initialize imu jacobian with the parts that do not change in iterations
  *
  */
    void VoStereo::initJacobian_imu_fca(Eigen::MatrixXf &J_imu)
    {
        /* these are the parts that do not change through iterations */
        J_imu.setZero();
        Eigen::Matrix3f R_i = T_wb_last_.rotation();
        J_imu.block<3,3>(6,3) = R_i.transpose();
        J_imu.block<3,3>(3,6) = R_i.transpose();
    }

/*
 * obtain imu error vector and update imu jacobian
 * Input:
 *      T_wb: current robot pose in world frame
 *      v: current robot velocity in world frame
 * Output:
 *      r_imu: 9x1 vector of imu errors
 *      J_imu: 9x9 jacobian of state (phi,p,v of current robot frame in world) with respect to imu errors
 *
 */
    void VoStereo::getError_imu_fca(const Eigen::Isometry3f &T_wb, const Eigen::Vector3f &v, Eigen::VectorXf &r_imu, Eigen::MatrixXf &J_imu)
    {

        Eigen::Matrix3f R_i = T_wb_last_.rotation();  // rotation w.r.t. world frame of last body frame
        Eigen::Matrix3f R_j = T_wb.rotation();  // rotation w.r.t. world frame of current body frame
        Eigen::Vector3f p_i = T_wb_last_.translation();  // position of last robot frame in world
        Eigen::Vector3f p_j = T_wb.translation();  // position of current robot frame in world

        Eigen::Matrix3f R_err = R_meas_.transpose() * R_i.transpose() * R_j;
        Eigen::Vector3f r_R_imu = log_SO3(R_err);
        Eigen::Vector3f r_v_imu = R_i.transpose() * (v - v_last_ - g_*t_meas_) - v_meas_;
        Eigen::Vector3f r_p_imu = R_i.transpose() * (p_j - p_i - v_last_*t_meas_ - 0.5*g_*t_meas_*t_meas_) - p_meas_;

        r_imu.block<3,1>(0,0) = r_R_imu;
        r_imu.block<3,1>(3,0) = r_v_imu;
        r_imu.block<3,1>(6,0) = r_p_imu;

        J_imu.block<3,3>(0,0) = log_Jacobian(r_R_imu);

    }

/*
 * Get error, Jacobian, and huber weights for Forward Compositional Apporach.
 *
 * Input:
 *      img_curr: current frame image, for getting pixel intensities
 *      Gx: current image gradient image on x direction
 *      Gy: current image gradient image on y direction
 *      ilay: the pyramid layer of current image
 *      T_curr: currently optimized transformation
 * Output:
 *      npts_in: the number of points falling in current camera view
 *      J_masked: Jacobian of the points in-view
 *      error_masked: error of the points in-view
 *      r_imu: 9x1 vector of IMU integrated observation error
 *      W: Huber weight for the points in-view
 * Use of class member:
 * Updates:
 *      mask_[ilay]: indicator of points in-view
 * Data-in:
 *      num_feat_pts_[ilay]
 *      feat_3D_[ilay]
 *      intensities_[ilay]
 *
 */
    void VoStereo::getError_fca(const cv::Mat& img_curr,
                                const cv::Mat& Gx_mat,
                                const cv::Mat& Gy_mat,
                                const Eigen::Isometry3f& T_curr,
                                const Eigen::Isometry3f& T_wb,
                                Eigen::MatrixXf& J_masked,
                                Eigen::VectorXf& error_masked,
                                Eigen::VectorXf& W,
                                int& npts_in,
                                const int ilay)
    {

        /* transformation of 3D point in keyframe to current frame and reproject */
        vector_aligned<Eigen::Vector3f> feat_3D_curr;  // 3D points in current camera's frame
        vector_aligned<Eigen::Vector2f> feat_pixels_curr;  // 2D pixel coordinates in current image
        transform3D(feat_3D_[ilay], feat_3D_curr, T_curr, num_feat_pts_[ilay]);
        cam_->get2DPixels(feat_3D_curr, feat_pixels_curr, num_feat_pts_[ilay], ilay);

        /* update mask and obtain gradients of reprojected points in current frame */
        npts_in = 0;
        std::vector<float> Gx, Gy;  // gradients of points in view
        std::vector<int> index;  // vector of size npts_in storing the corresponding index value
        std::vector<float> error_vec;  // error vector

        const float scale = error_scale_factor_; // Arbitrary scaling for better numerical conditioning
        for (int i=0; i<num_feat_pts_[ilay]; ++i) {
            if (inBound(feat_pixels_curr[i](0), feat_pixels_curr[i](1), 0, ilay)) {
                Gx.push_back(scale * interpolate_gradient(feat_pixels_curr[i](0), feat_pixels_curr[i](1), Gx_mat));
                Gy.push_back(scale * interpolate_gradient(feat_pixels_curr[i](0), feat_pixels_curr[i](1), Gy_mat));
                error_vec.push_back(scale * (intensities_[ilay][i]-interpolate(feat_pixels_curr[i](0),feat_pixels_curr[i](1),img_curr)));
                index.push_back(i);

//                mask_[ilay][i] = 1;
                npts_in += 1;
            }
//            else
//                mask_[ilay][i] = 0;
        }

        /* obtain photometric error for points in-view */
        error_masked = Eigen::Map<Eigen::VectorXf, Eigen::Unaligned>(error_vec.data(), error_vec.size());

        /* obtain jacobian matrix for photometric error with respect to state */
        getJacobian_fca(Gx, Gy, feat_3D_curr, index, T_curr, T_wb, J_masked, ilay, npts_in);

        if (use_huber_)
        {
            /* find standard deviation of the error vector and compute W */
            float r_mean = error_masked.mean();
            Eigen::VectorXf r_hat = error_masked.array() - r_mean;  // normalize by removing the mean (original order)
            float sigma = sqrt(((r_hat.array()).square()).sum()/npts_in);  // standard deviation
            // std::cout << "sigma: " << sigma << std::endl;

            W.resize(npts_in);
            for (int i=0; i<npts_in; ++i) {
                if (fabs(r_hat[i]) < 1.345*sigma)
                    W[i] = 1;
                else
                    W[i] = (1.345*sigma)/fabs(r_hat[i]);
                if(use_weights_)
                    W[i] = W[i] * c_/(c_+error_masked[i]*error_masked[i]);

            }
        }
        else {

            W.resize(npts_in);
            W = Eigen::VectorXf::Constant(npts_in,1);

            if(use_weights_)
                for (int i=0; i<npts_in; ++i) {
                    W[i]=c_/(c_+error_masked[i]*error_masked[i]);
                    //std::cout<<"W" << W[i];
                }
            //std::cout<<"W ="<<W;
        }

    }

/*
 * Compute the Jacobian matrix for Forward Compositional Approach (FCA)
 * Called every iteration
 *
 * Input:
 *      Gx: gradient vector in x direction of current image
 *      Gy: gradient vector in y direction of current image
 *      feat_3D_curr: 3D feature points represented in current frame, of size num_feat_pts[ilay]
 *      index: vector of index for each point in-view, of size npts_in, ranges within num_feat_pts[ilay]
 *      T_curr: current transformation from keyframe to current frame. p_curr = T_curr * p_kf
 *      T_wb: transformation from current body frame to world frame. p_w = T_wb * p_b
 *      ilay: number of layer we're optimizing on
 *      npts_in: number of points in-view
 * Output:
 *      J: jacobian of size npts_in x 6, photometric error with respect to delta rotation phi in so(3) and delta translation p.
 *
 *
 */
    void VoStereo::getJacobian_fca(const std::vector<float>& Gx, const std::vector<float>& Gy,
                                   const vector_aligned<Eigen::Vector3f>& feat_3D_curr, const std::vector<int> &index,
                                   const Eigen::Isometry3f& T_curr, const Eigen::Isometry3f& T_wb, Eigen::MatrixXf& J, const int ilay, const int npts_in)
    {
//        std::cout<<"+++++calculating jacobian \n";
//        int id_ind = 475;

        Eigen::Matrix<float,3,3> R_cb = T_bc_inv_.rotation();
        Eigen::Matrix<float,3,3> R_wb = T_wb.rotation();

        J.resize(npts_in,9);
        J.setZero();
        Eigen::Matrix<float,1,3> J_i_phi; // Jacobian of one point on rotation
        Eigen::Matrix<float,1,3> J_i_p;  // Jacobian of one point on position
        Eigen::Matrix<float,1,2> J_grad; // gradient jacobian
        Eigen::Matrix<float,2,3> J_proj; // projection jacobian
        Eigen::Matrix<float,3,3> J_SO3;  // exponential jacobian

        float fx = focal_pyr_[ilay];
        float fy = focal_pyr_[ilay];

//        std::cout<<"fx: "<<fx<<"\n";
//        bool jacobianhasnan = false;
//        bool jacobianhasinf = false;
//        bool jacobian_too_large = false;

        for (int i=0; i < npts_in; ++i)
        {
            /* calculate for each point the 1x6 Jacobian */
            Eigen::Vector3f p_curr;  // point in current frame
            p_curr(0) = feat_3D_curr[index[i]](0);
            p_curr(1) = feat_3D_curr[index[i]](1);
            p_curr(2) = feat_3D_curr[index[i]](2);

            Eigen::Vector3f p_kf;  // point in keyframe
            p_kf(0) = feat_3D_[ilay][index[i]](0);
            p_kf(1) = feat_3D_[ilay][index[i]](1);
            p_kf(2) = feat_3D_[ilay][index[i]](2);

            Eigen::Vector4f p_kf_homo = unproject3d(p_kf); // homogeneous coordinate in keyframe
            Eigen::Vector4f p_hat_homo = T_bc_ * T_curr * p_kf_homo;
            Eigen::Vector3f p_hat = project3d(p_hat_homo);  // point p_i prime in jacobian formulation

            J_grad(0,0) = Gx[i];
            J_grad(0,1) = Gy[i];

            J_proj(0,0) = fx/p_curr(2);
            J_proj(1,0) = 0;
            J_proj(0,1) = 0;
            J_proj(1,1) = fy/p_curr(2);
            J_proj(0,2) = -fx*p_curr(0)/(p_curr(2)*p_curr(2));
            J_proj(1,2) = -fy*p_curr(1)/(p_curr(2)*p_curr(2));

            J_SO3 << 0,-p_hat(2),p_hat(1),
                    p_hat(2),0,-p_hat(0),
                    -p_hat(1),p_hat(0),0;  // [p_hat]x

            J_i_phi = - J_grad * J_proj * R_cb * J_SO3;
            J_i_p = J_grad * J_proj * R_cb * R_wb.transpose();

//            if (i==id_ind)
//            {
//                std::cout<<"J_grad: \n"<<J_grad<<"\n";
//                std::cout<<"J_proj: \n"<<J_proj<<"\n";
//                std::cout<<"J_SO3: \n"<<J_SO3<<"\n";
//                std::cout<<"J_i_p: \n"<<J_i_p<<"\n";
//                std::cout<<"J_i_phi: \n"<<J_i_phi<<"\n";
//            }

            /* assign J_i to J */
            J.block<1,3>(i,0) = J_i_phi;
            J.block<1,3>(i,3) = J_i_p;

            /* for debug - will print out info if nan values found */

//            for (int k=0; k<6; ++k){
//                if (std::isnan(J(i,k)))
//                    jacobianhasnan = true;
//                if (std::isinf(J(i,k)))
//                    jacobianhasinf = true;
//                if(J(i,k) > error_scale_factor_ * 1e8)
//                {
//                    jacobian_too_large = true;
//                    ROS_WARN_STREAM("jacobian too large! i: " << i << ", Index[i]: " << index[i] << " / " << num_feat_pts_[ilay] << "\nJ.row: "<< J.row(i));
//                    std::cout<<"J_grad: \n"<<J_grad<<"\n";
//                    std::cout<<"J_proj: \n"<<J_proj<<"\n";
//                    std::cout<<"J_SO3: \n"<<J_SO3<<"\n";
//                    std::cout<<"J_i_p: \n"<<J_i_p<<"\n";
//                    std::cout<<"J_i_phi: \n"<<J_i_phi<<"\n";
//                }
//            }

            /* for debug */
        }
//        if (jacobianhasnan)
//            std::cout << "Jacobian has nan"<< std::endl;
//        if (jacobianhasinf)
//            std::cout <<"Jacobian has inf"<<std::endl;
    }

}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2003, 2004, 2005, 2006, 2007 StatPro Italia srl
 Copyright (C) 2006, 2008 Ferdinando Ametrano

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file vanillaswap.hpp
    \brief Simple fixed-rate vs Libor swap
*/

#ifndef quantlib_vanilla_swap_hpp
#define quantlib_vanilla_swap_hpp

#include <ql/instruments/swap.hpp>
#include <ql/time/daycounter.hpp>
#include <ql/time/schedule.hpp>
#include <boost/optional.hpp>

namespace QuantLib {

    class IborIndex;

    //! Plain-vanilla swap: fix vs floating leg
    /*! \ingroup instruments

        If no payment convention is passed, the convention of the
        floating-rate schedule is used.

        \warning if <tt>Settings::includeReferenceDateCashFlows()</tt>
                 is set to <tt>true</tt>, payments occurring at the
                 settlement date of the swap might be included in the
                 NPV and therefore affect the fair-rate and
                 fair-spread calculation. This might not be what you
                 want.

        \test
        - the correctness of the returned value is tested by checking
          that the price of a swap paying the fair fixed rate is null.
        - the correctness of the returned value is tested by checking
          that the price of a swap receiving the fair floating-rate
          spread is null.
        - the correctness of the returned value is tested by checking
          that the price of a swap decreases with the paid fixed rate.
        - the correctness of the returned value is tested by checking
          that the price of a swap increases with the received
          floating-rate spread.
        - the correctness of the returned value is tested by checking
          it against a known good value.
    */
    class VanillaSwap : public Swap {
      public:
        enum Type { Receiver = -1, Payer = 1 };
        class arguments;
        class results;
        class engine;
        VanillaSwap(
            Type type,
            Real nominal,
            const Schedule& fixedSchedule,
            Rate fixedRate,
            const DayCounter& fixedDayCount,
            const Schedule& floatSchedule,
            const boost::shared_ptr<IborIndex>& iborIndex,
            Spread spread,
            const DayCounter& floatingDayCount,
            boost::optional<BusinessDayConvention> paymentConvention =
                                                                 boost::none);
        //! \name Inspectors
        //@{
        Type type() const;
        Real nominal() const;

        const Schedule& fixedSchedule() const;
        Rate fixedRate() const;
        const DayCounter& fixedDayCount() const;

        const Schedule& floatingSchedule() const;
        const boost::shared_ptr<IborIndex>& iborIndex() const;
        Spread spread() const;
        const DayCounter& floatingDayCount() const;

        BusinessDayConvention paymentConvention() const;

        const Leg& fixedLeg() const;
        const Leg& floatingLeg() const;
        //@}

        //! \name Results
        //@{
        Real fixedLegBPS() const;
        Real fixedLegNPV() const;
        Rate fairRate() const;

        Real floatingLegBPS() const;
        Real floatingLegNPV() const;
        Spread fairSpread() const;
        //@}
        // other
        void setupArguments(PricingEngine::arguments* args) const;
        void fetchResults(const PricingEngine::results*) const;
      private:
        void setupExpired() const;
        Type type_;
        Real nominal_;
        Schedule fixedSchedule_;
        Rate fixedRate_;
        DayCounter fixedDayCount_;
        Schedule floatingSchedule_;
        boost::shared_ptr<IborIndex> iborIndex_;
        Spread spread_;
        DayCounter floatingDayCount_;
        BusinessDayConvention paymentConvention_;
        // results
        mutable Rate fairRate_;
        mutable Spread fairSpread_;
    };

    //! %Arguments for simple swap calculation
    class VanillaSwap::arguments : public Swap::arguments {
      public:
        arguments() : type(Receiver),
                      nominal(Null<Real>()) {}
        Type type;
        Real nominal;

        std::vector<Date> fixedResetDates;
        std::vector<Date> fixedPayDates;
        std::vector<Time> floatingAccrualTimes;
        std::vector<Date> floatingResetDates;
        std::vector<Date> floatingFixingDates;
        std::vector<Date> floatingPayDates;

        std::vector<Real> fixedCoupons;
        std::vector<Spread> floatingSpreads;
        std::vector<Real> floatingCoupons;
        void validate() const;
    };

    //! %Results from simple swap calculation
    class VanillaSwap::results : public Swap::results {
      public:
        Rate fairRate;
        Spread fairSpread;
        void reset();
    };

    class VanillaSwap::engine : public GenericEngine<VanillaSwap::arguments,
                                                     VanillaSwap::results> {};

    // inline definitions

    inline VanillaSwap::Type VanillaSwap::type() const {
        return type_;
    }

    inline Real VanillaSwap::nominal() const {
        return nominal_;
    }

    inline const Schedule& VanillaSwap::fixedSchedule() const {
        return fixedSchedule_;
    }

    inline Rate VanillaSwap::fixedRate() const {
        return fixedRate_;
    }

    inline const DayCounter& VanillaSwap::fixedDayCount() const {
        return fixedDayCount_;
    }

    inline const Schedule& VanillaSwap::floatingSchedule() const {
        return floatingSchedule_;
    }

    inline const boost::shared_ptr<IborIndex>& VanillaSwap::iborIndex() const {
        return iborIndex_;
    }

    inline Spread VanillaSwap::spread() const {
        return spread_;
    }

    inline const DayCounter& VanillaSwap::floatingDayCount() const {
        return floatingDayCount_;
    }

    inline BusinessDayConvention VanillaSwap::paymentConvention() const {
        return paymentConvention_;
    }

    inline const Leg& VanillaSwap::fixedLeg() const {
        return legs_[0];
    }

    inline const Leg& VanillaSwap::floatingLeg() const {
        return legs_[1];
    }

    std::ostream& operator<<(std::ostream& out,
                             VanillaSwap::Type t);

}

/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2003, 2004, 2005, 2006, 2007 StatPro Italia srl
 Copyright (C) 2007 Ferdinando Ametrano

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/cashflows/iborcoupon.hpp>
#include <ql/cashflows/cashflowvectors.hpp>
#include <ql/cashflows/cashflows.hpp>
#include <ql/cashflows/couponpricer.hpp>
#include <ql/indexes/iborindex.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>

namespace QuantLib {

    inline VanillaSwap::VanillaSwap(
                     Type type,
                     Real nominal,
                     const Schedule& fixedSchedule,
                     Rate fixedRate,
                     const DayCounter& fixedDayCount,
                     const Schedule& floatSchedule,
                     const boost::shared_ptr<IborIndex>& iborIndex,
                     Spread spread,
                     const DayCounter& floatingDayCount,
                     boost::optional<BusinessDayConvention> paymentConvention)
    : Swap(2), type_(type), nominal_(nominal),
      fixedSchedule_(fixedSchedule), fixedRate_(fixedRate),
      fixedDayCount_(fixedDayCount),
      floatingSchedule_(floatSchedule), iborIndex_(iborIndex), spread_(spread),
      floatingDayCount_(floatingDayCount) {

        if (paymentConvention)
            paymentConvention_ = *paymentConvention;
        else
            paymentConvention_ = floatingSchedule_.businessDayConvention();

        legs_[0] = FixedRateLeg(fixedSchedule_)
            .withNotionals(nominal_)
            .withCouponRates(fixedRate_, fixedDayCount_)
            .withPaymentAdjustment(paymentConvention_);

        legs_[1] = IborLeg(floatingSchedule_, iborIndex_)
            .withNotionals(nominal_)
            .withPaymentDayCounter(floatingDayCount_)
            .withPaymentAdjustment(paymentConvention_)
            .withSpreads(spread_);
        for (Leg::const_iterator i = legs_[1].begin(); i < legs_[1].end(); ++i)
            registerWith(*i);

        switch (type_) {
          case Payer:
            payer_[0] = -1.0;
            payer_[1] = +1.0;
            break;
          case Receiver:
            payer_[0] = +1.0;
            payer_[1] = -1.0;
            break;
          default:
            QL_FAIL("Unknown vanilla-swap type");
        }
    }

    inline void VanillaSwap::setupArguments(PricingEngine::arguments* args) const {

        Swap::setupArguments(args);

        VanillaSwap::arguments* arguments =
            dynamic_cast<VanillaSwap::arguments*>(args);

        if (!arguments)  // it's a swap engine...
            return;

        arguments->type = type_;
        arguments->nominal = nominal_;

        const Leg& fixedCoupons = fixedLeg();

        arguments->fixedResetDates = arguments->fixedPayDates =
            std::vector<Date>(fixedCoupons.size());
        arguments->fixedCoupons = std::vector<Real>(fixedCoupons.size());

        for (Size i=0; i<fixedCoupons.size(); ++i) {
            boost::shared_ptr<FixedRateCoupon> coupon =
                boost::dynamic_pointer_cast<FixedRateCoupon>(fixedCoupons[i]);

            arguments->fixedPayDates[i] = coupon->date();
            arguments->fixedResetDates[i] = coupon->accrualStartDate();
            arguments->fixedCoupons[i] = coupon->amount();
        }

        const Leg& floatingCoupons = floatingLeg();

        arguments->floatingResetDates = arguments->floatingPayDates =
            arguments->floatingFixingDates =
            std::vector<Date>(floatingCoupons.size());
        arguments->floatingAccrualTimes =
            std::vector<Time>(floatingCoupons.size());
        arguments->floatingSpreads =
            std::vector<Spread>(floatingCoupons.size());
        arguments->floatingCoupons = std::vector<Real>(floatingCoupons.size());
        for (Size i=0; i<floatingCoupons.size(); ++i) {
            boost::shared_ptr<IborCoupon> coupon =
                boost::dynamic_pointer_cast<IborCoupon>(floatingCoupons[i]);

            arguments->floatingResetDates[i] = coupon->accrualStartDate();
            arguments->floatingPayDates[i] = coupon->date();

            arguments->floatingFixingDates[i] = coupon->fixingDate();
            arguments->floatingAccrualTimes[i] = coupon->accrualPeriod();
            arguments->floatingSpreads[i] = coupon->spread();
            try {
                arguments->floatingCoupons[i] = coupon->amount();
            } catch (Error&) {
                arguments->floatingCoupons[i] = Null<Real>();
            }
        }
    }

    inline Rate VanillaSwap::fairRate() const {
        calculate();
        QL_REQUIRE(fairRate_ != Null<Rate>(), "result not available");
        return fairRate_;
    }

    inline Spread VanillaSwap::fairSpread() const {
        calculate();
        QL_REQUIRE(fairSpread_ != Null<Spread>(), "result not available");
        return fairSpread_;
    }

    inline Real VanillaSwap::fixedLegBPS() const {
        calculate();
        QL_REQUIRE(legBPS_[0] != Null<Real>(), "result not available");
        return legBPS_[0];
    }

    inline Real VanillaSwap::floatingLegBPS() const {
        calculate();
        QL_REQUIRE(legBPS_[1] != Null<Real>(), "result not available");
        return legBPS_[1];
    }

    inline Real VanillaSwap::fixedLegNPV() const {
        calculate();
        QL_REQUIRE(legNPV_[0] != Null<Real>(), "result not available");
        return legNPV_[0];
    }

    inline Real VanillaSwap::floatingLegNPV() const {
        calculate();
        QL_REQUIRE(legNPV_[1] != Null<Real>(), "result not available");
        return legNPV_[1];
    }

    inline void VanillaSwap::setupExpired() const {
        Swap::setupExpired();
        legBPS_[0] = legBPS_[1] = 0.0;
        fairRate_ = Null<Rate>();
        fairSpread_ = Null<Spread>();
    }

    inline void VanillaSwap::fetchResults(const PricingEngine::results* r) const {
        static const Spread basisPoint = 1.0e-4;

        Swap::fetchResults(r);

        const VanillaSwap::results* results =
            dynamic_cast<const VanillaSwap::results*>(r);
        if (results) { // might be a swap engine, so no error is thrown
            fairRate_ = results->fairRate;
            fairSpread_ = results->fairSpread;
        } else {
            fairRate_ = Null<Rate>();
            fairSpread_ = Null<Spread>();
        }

        if (fairRate_ == Null<Rate>()) {
            // calculate it from other results
            if (legBPS_[0] != Null<Real>())
                fairRate_ = fixedRate_ - NPV_/(legBPS_[0]/basisPoint);
        }
        if (fairSpread_ == Null<Spread>()) {
            // ditto
            if (legBPS_[1] != Null<Real>())
                fairSpread_ = spread_ - NPV_/(legBPS_[1]/basisPoint);
        }
    }

    inline void VanillaSwap::arguments::validate() const {
        Swap::arguments::validate();
        QL_REQUIRE(nominal != Null<Real>(), "nominal null or not set");
        QL_REQUIRE(fixedResetDates.size() == fixedPayDates.size(),
                   "number of fixed start dates different from "
                   "number of fixed payment dates");
        QL_REQUIRE(fixedPayDates.size() == fixedCoupons.size(),
                   "number of fixed payment dates different from "
                   "number of fixed coupon amounts");
        QL_REQUIRE(floatingResetDates.size() == floatingPayDates.size(),
                   "number of floating start dates different from "
                   "number of floating payment dates");
        QL_REQUIRE(floatingFixingDates.size() == floatingPayDates.size(),
                   "number of floating fixing dates different from "
                   "number of floating payment dates");
        QL_REQUIRE(floatingAccrualTimes.size() == floatingPayDates.size(),
                   "number of floating accrual Times different from "
                   "number of floating payment dates");
        QL_REQUIRE(floatingSpreads.size() == floatingPayDates.size(),
                   "number of floating spreads different from "
                   "number of floating payment dates");
        QL_REQUIRE(floatingPayDates.size() == floatingCoupons.size(),
                   "number of floating payment dates different from "
                   "number of floating coupon amounts");
    }

    inline void VanillaSwap::results::reset() {
        Swap::results::reset();
        fairRate = Null<Rate>();
        fairSpread = Null<Spread>();
    }

    inline std::ostream& operator<<(std::ostream& out,
                             VanillaSwap::Type t) {
        switch (t) {
          case VanillaSwap::Payer:
            return out << "Payer";
          case VanillaSwap::Receiver:
            return out << "Receiver";
          default:
            QL_FAIL("unknown VanillaSwap::Type(" << Integer(t) << ")");
        }
    }

}

#endif


/*
 * Neighbourhood.hpp
 *
 *  Created on: May 18, 2012
 *      Author: david
 */

#ifndef DASP_NEIGHBOURHOOD_HPP_
#define DASP_NEIGHBOURHOOD_HPP_

#include "Superpixels.hpp"
#include "Graph.hpp"
#include <Slimage/Slimage.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/copy.hpp>
#include <Eigen/Dense>
#include <vector>

namespace dasp
{

	/** Computes points which lie on the border between segments
	 * @param list of point indices
	 */
	std::vector<unsigned int> ComputeAllBorderPixels(const Superpixels& superpixels);

	struct NeighborGraphSettings
	{
		bool cut_by_spatial;
		float spatial_distance_mult_threshold;
		float pixel_distance_mult_threshold;
		float min_border_overlap;
		unsigned min_abs_border_overlap;

		static NeighborGraphSettings SpatialCut() {
			return NeighborGraphSettings{true, 5.0f, 4.0f, 0.0f, 1};
		}
		static NeighborGraphSettings NoCut() {
			return NeighborGraphSettings{false, 5.0f, 4.0f, 0.0f, 1};
		}
	};

	/** Computes the superpixel neighborhood graph
	 * Superpixels are neighbors if they share border pixels.
	 */
	NeighbourhoodGraph CreateNeighborhoodGraph(const Superpixels& superpixels,
		NeighborGraphSettings settings=NeighborGraphSettings::SpatialCut());

	/** Computes edge weights for a superpixel graph using the given metric
	 * Metric : Point x Point -> float (should be lightweight)
	 */
	template<typename Graph, typename Metric>
	UndirectedWeightedGraph ComputeEdgeWeights(const Superpixels& superpixels, const Graph& graph, const Metric& metric)
	{
		UndirectedWeightedGraph result;
		boost::copy_graph(graph, result,
			boost::edge_copy([&superpixels, &graph, &result, &metric](typename Graph::edge_descriptor src, UndirectedWeightedGraph::edge_descriptor dst) {
				const unsigned int ea = boost::source(src, graph);
				const unsigned int eb = boost::target(src, graph);
				const float w = metric(superpixels.cluster[ea].center, superpixels.cluster[eb].center);
				result[dst] = w;
			}));
		return result;
	}

	template<typename Metric>
	UndirectedWeightedGraph ComputeEdgeWeightsFromMetricWeighted(const Superpixels& superpixels, const NeighbourhoodGraph& graph, const Metric& metric)
	{
		std::vector<unsigned int> cluster_border_length_px(boost::num_vertices(graph));
		for(auto eid : as_range(boost::edges(graph))) {
			const unsigned int ea = boost::source(eid, graph);
			const unsigned int eb = boost::target(eid, graph);
			const unsigned int num = graph[eid].num_border_pixels;
			cluster_border_length_px[ea] += num;
			cluster_border_length_px[eb] += num;
		}
		UndirectedWeightedGraph result;
		boost::copy_graph(graph, result,
			boost::edge_copy([&superpixels, &graph, &result, &metric, &cluster_border_length_px](typename NeighbourhoodGraph::edge_descriptor src, UndirectedWeightedGraph::edge_descriptor dst) {
				const unsigned int ea = boost::source(src, graph);
				const unsigned int eb = boost::target(src, graph);
				const float w = static_cast<float>(graph[src].num_border_pixels)
					/ static_cast<float>(cluster_border_length_px[ea] + cluster_border_length_px[eb]);
				const float d = metric(ea, eb);
				result[dst] = 12.0f * w * d;
			}));
		return result;
	}

	/** Computes a superpixel graph */
	DaspGraph CreateDaspNeighbourhoodGraph(const Superpixels& superpixels);

	/** Computes a weighted superpixel graph with superpixel values as vertex properties
	 */
	DaspGraph CreateDaspGraph(const Superpixels& superpixels, const UndirectedWeightedGraph& weighted_graph);

	/** Copies graph and changes edge weights to exp(-w/sigma) */
	DaspGraph ConvertToSimilarityGraph(const DaspGraph& source, const float sigma);

	/** Computes a border image
	 * All border pixels for each edge are set to the edge weight in the image.
	 * Border pixels are given in index form x + y*w where w must be the same given as parameter.
	 * @param graph a boost graph
	 * @param weights an weight property map of type float
	 * @param border_pixels an edge property map of type std::vector<unsigned int>
	 * @param return image with painted border pixels
	 */
	template<typename Graph, typename WeightMap, typename BorderPixelMap>
	void PlotBorderPixels(Eigen::MatrixXf& mat, const Graph& graph, WeightMap weights, BorderPixelMap border_pixels)
	{
		float* p = mat.data();
		for(auto eid : as_range(boost::edges(graph))) {
			float v = weights[eid];
			for(unsigned int pid : boost::get(border_pixels, eid)) {
				p[pid] = v;
			}
		}
	}

}

#endif


#include <actions/io/json.h>
#include <actions/traits.h>
#include <possumwood_sdk/datatypes/filename.h>
#include <possumwood_sdk/node_implementation.h>
#include <tbb/parallel_for.h>

#include <boost/filesystem.hpp>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <sstream>

#include "frame.h"
#include "lightfields.h"
#include "lightfields/block.h"
#include "lightfields/metadata.h"
#include "lightfields/pattern.h"
#include "lightfields/raw.h"

namespace {

dependency_graph::InAttr<possumwood::Filename> a_filename;
dependency_graph::OutAttr<possumwood::opencv::Frame> a_frame;
dependency_graph::OutAttr<lightfields::Metadata> a_metadata;

cv::Mat decodeData(const unsigned char* data, std::size_t width, std::size_t height, int black[4], int white[4]) {
	cv::Mat result(width, height, CV_32F);

	tbb::parallel_for(std::size_t(0), width * height, [&](std::size_t i) {
		const unsigned char c1 = data[i / 2 * 3];
		const unsigned char c2 = data[i / 2 * 3 + 1];
		const unsigned char c3 = data[i / 2 * 3 + 2];

		unsigned short val;
		if(i % 2 == 0)
			val = ((unsigned short)c1 << 4) + (unsigned short)c2;
		else
			val = (((unsigned short)c2 & 0x0f) << 8) + (unsigned short)c3;

		const unsigned patternId = (i % width) % 2 + ((i / width) % 2) * 2;

		const float fval = ((float)val - (float)black[patternId]) / ((float)(white[patternId] - black[patternId]));

		result.at<float>(i / width, i % width) = fval;
	});

	return result;
}

template <typename T>
std::string str(const T& val) {
	return std::to_string(val);
}

template <>
std::string str<std::string>(const std::string& val) {
	return val;
}

template <typename T>
void checkThrow(const T& value, const T& expected, const std::string& error) {
	if(value != expected)
		throw std::runtime_error("Expected " + error + " " + str(expected) + ", got " + str(value) + "!");
}

dependency_graph::State compute(dependency_graph::Values& data) {
	const possumwood::Filename filename = data.get(a_filename);

	cv::Mat result;
	lightfields::Pattern pattern;
	lightfields::Metadata meta;

	if(!filename.filename().empty() && boost::filesystem::exists(filename.filename())) {
		int width = 0, height = 0;
		std::string metadataRef, imageRef;
		int black[4] = {0, 0, 0, 0}, white[4] = {255, 255, 255, 255};

		std::ifstream file(filename.filename().string(), std::ios::binary);
		lightfields::Raw raw;
		file >> raw;

		meta = raw.metadata();

		width = meta.metadata()["image"]["width"].asInt();
		height = meta.metadata()["image"]["height"].asInt();

		black[0] = meta.metadata()["image"]["rawDetails"]["pixelFormat"]["black"]["b"].asInt();
		black[1] = meta.metadata()["image"]["rawDetails"]["pixelFormat"]["black"]["gb"].asInt();
		black[2] = meta.metadata()["image"]["rawDetails"]["pixelFormat"]["black"]["gr"].asInt();
		black[3] = meta.metadata()["image"]["rawDetails"]["pixelFormat"]["black"]["r"].asInt();

		white[0] = meta.metadata()["image"]["rawDetails"]["pixelFormat"]["white"]["b"].asInt();
		white[1] = meta.metadata()["image"]["rawDetails"]["pixelFormat"]["white"]["gb"].asInt();
		white[2] = meta.metadata()["image"]["rawDetails"]["pixelFormat"]["white"]["gr"].asInt();
		white[3] = meta.metadata()["image"]["rawDetails"]["pixelFormat"]["white"]["r"].asInt();

		assert(raw.image() != nullptr);
		result = decodeData(raw.image(), width, height, black, white);
	}

	data.set(a_frame, possumwood::opencv::Frame(result));
	data.set(a_metadata, meta);

	return dependency_graph::State();
}

void init(possumwood::Metadata& meta) {
	meta.addAttribute(a_filename, "filename",
	                  possumwood::Filename({
	                      "Lytro files (*.lfr *.RAW)",
	                  }));
	meta.addAttribute(a_frame, "frame", possumwood::opencv::Frame(), possumwood::AttrFlags::kVertical);
	meta.addAttribute(a_metadata, "metadata", lightfields::Metadata(), possumwood::AttrFlags::kVertical);

	meta.addInfluence(a_filename, a_frame);
	meta.addInfluence(a_filename, a_metadata);

	meta.setCompute(compute);
}

possumwood::NodeImplementation s_impl("opencv/capture/lytro_lightfield", init);

}  // namespace


//
// Copyright 2020 Nedjima Belgacem <gb_nedjima@esi.dz>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#ifndef BOOST_GIL_EXTENSION_HISTOGRAM_HISTOGRAM_HPP
#define BOOST_GIL_EXTENSION_HISTOGRAM_HISTOGRAM_HPP

#include <boost/gil.hpp>

namespace boost {
namespace gil {

template<typename ColorSpace, typename PixelType = gray8_pixel_t>
class histogram
{
    int _histogram[1 << (8 * sizeof(PixelType))];
public:
    histogram() { std::fill(begin(), end(), 0); }
    int& operator[](const ColorSpace& color) {
        PixelType dst;
        color_convert(color, dst);
        return _histogram[dst];
    }
    int operator[](const ColorSpace& color) const {
        PixelType dst;
        color_convert(color, dst);
        return _histogram[dst];
    }
    constexpr size_t size() { return 1 << (8 * sizeof(PixelType)); };
    int* begin() { return _histogram; }
    int* end() { return _histogram + size(); }
};

}
}

#endif