//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EULER_FUNCTIONS_SIMD_COMMON_GAMMALN_HPP_INCLUDED
#define NT2_EULER_FUNCTIONS_SIMD_COMMON_GAMMALN_HPP_INCLUDED

#include <nt2/euler/functions/gammaln.hpp>
#include <nt2/euler/functions/details/gammaln_kernel.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/any.hpp>
#include <nt2/include/functions/simd/dec.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/floor.hpp>
#include <nt2/include/functions/simd/fma.hpp>
#include <nt2/include/functions/simd/if_allbits_else.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/inbtrue.hpp>
#include <nt2/include/functions/simd/is_equal.hpp>
#include <nt2/include/functions/simd/is_flint.hpp>
#include <nt2/include/functions/simd/is_greater.hpp>
#include <nt2/include/functions/simd/is_greater_equal.hpp>
#include <nt2/include/functions/simd/is_less.hpp>
#include <nt2/include/functions/simd/is_lez.hpp>
#include <nt2/include/functions/simd/is_ltz.hpp>
#include <nt2/include/functions/simd/log.hpp>
#include <nt2/include/functions/simd/logical_and.hpp>
#include <nt2/include/functions/simd/logical_andnot.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/plus.hpp>
#include <nt2/include/functions/simd/rec.hpp>
#include <nt2/include/functions/simd/seladd.hpp>
#include <nt2/include/functions/simd/seldec.hpp>
#include <nt2/include/functions/simd/selinc.hpp>
#include <nt2/include/functions/simd/selsub.hpp>
#include <nt2/include/functions/simd/sinpi.hpp>
#include <nt2/include/functions/simd/splat.hpp>
#include <nt2/include/functions/simd/sqr.hpp>
#include <nt2/include/functions/simd/unary_minus.hpp>
#include <nt2/include/constants/false.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/invpi.hpp>
#include <nt2/include/constants/logpi.hpp>
#include <nt2/include/constants/logsqrt2pi.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>

#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INFINITES
#include <nt2/include/functions/simd/logical_or.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( gammaln_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<double_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    typedef typename meta::as_logical<A0>::type bA0;
    NT2_FUNCTOR_CALL(1)
    {
      bA0 inf_result = logical_and(is_lez(a0), is_flint(a0));
      A0 x = if_nan_else(inf_result, a0);
      A0 q = nt2::abs(x);
      #ifndef BOOST_SIMD_NO_INFINITES
      inf_result = eq(q, Inf<A0>());
      #endif
      bA0 test = lt(a0, nt2::splat<A0>(-34.0));
      size_t nb = nt2::inbtrue(test);
      A0 r =  Nan<A0>();
      if(nb > 0)
      {
        //treat negative large with reflection
        r = large_negative(q);
        if (nb >= meta::cardinal_of<A0>::value)
          return nt2::if_else(inf_result, Nan<A0>(), r);
      }
      A0 r1 = other(a0);
      A0 r2 = if_else(test, r, r1);
      return nt2::if_else(eq(a0, Minf<A0>()),
                          Nan<A0>(),
                          nt2::if_else(inf_result, Inf<A0>(), r2)
                         );
    }
  private :
    static inline A0 large_negative(const A0& q)
    {
      A0 w = gammaln(q);
      A0 p = nt2::floor(q);
      A0 z = q - p;
      bA0 test2 = lt(z, nt2::Half<A0>() );
      z = nt2::selsub(test2, z, nt2::One<A0>());
      z = q*nt2::sinpi(z);
      z =  nt2::abs(z);
      return Logpi<A0>()-log(z)-w;
    }
    static inline A0 other(const A0& xx)
    {
      A0 x =  xx;
      bA0 test = lt(x, nt2::splat<A0>(13.0) );
      size_t nb = inbtrue(test);
      A0 r1 = Zero<A0>();
      if (nb > 0)
      {
        A0 z = One<A0>();
        A0 p = Zero<A0>();
        A0 u = x;
        bA0 test1 = ge(u,Three<A0>());
        while(nt2::any(test1))
        {
          p = seldec(test1, p);
          u = if_else(test1, x+p, u);
          z = if_else(test1, z*u, z);
          test1 = ge(u,Three<A0>());
        }
        //all u are less than 3
        bA0 test2 = lt(u,Two<A0>());

        while(nt2::any(test2))
        {
          z = if_else(test2, z/u, z);
          p = selinc(test2, p);
          u = if_else(test2, x+p, u);
          test2 = lt(u,Two<A0>());
        }
        z = nt2::abs(z);
        x +=  p-Two<A0>();
        r1 = x * details::gammaln_kernel<A0>::gammaln1(x)+nt2::log(z);
        if (nb >= meta::cardinal_of<A0>::value) return r1;
      }
      A0 r2 = fma(xx-Half<A0>(),nt2::log(xx), Logsqrt2pi<A0>()-xx);
      A0 p = rec(sqr(xx));
      r2 += details::gammaln_kernel<A0>::gammalnA(p)/xx;
      return if_else(test, r1, r2);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( gammaln_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<single_<A0>,X>))
                            )
  {

    typedef A0 result_type;
    typedef typename meta::as_logical<A0>::type bA0;
    typedef typename meta::scalar_of<A0>::type sA0;
    NT2_FUNCTOR_CALL(1)
    {
      bA0 inf_result = logical_and(is_lez(a0), is_flint(a0));
      A0 x = if_nan_else(inf_result, a0);
      A0 q = nt2::abs(x);
      #ifndef BOOST_SIMD_NO_INFINITES
      inf_result = logical_or(eq(x, Inf<A0>()), inf_result);
      #endif
      bA0 ltza0 = is_ltz(a0);
      size_t nb = nt2::inbtrue(ltza0);
      A0 r, r1 =  other(q);
      if(nb > 0)
      {
        //treat negative
        r = nt2::if_else(inf_result, Inf<A0>(), negative(q, r1));
        if (nb >= meta::cardinal_of<A0>::value) return r;
      }
      A0 r2 = if_else(ltza0, r, r1);
      return nt2::if_else(eq(a0, Minf<A0>()),
                          Nan<A0>(),
                          nt2::if_else(inf_result, Inf<A0>(), r2)
                         );
    }
  private :
    static inline A0 negative(const A0& q,  const A0& w)
    {
      A0 p = nt2::floor(q);
      A0 z = q - p;
      bA0 test2 = lt(z, nt2::Half<A0>() );
      z = nt2::selsub(test2, z, nt2::One<A0>());
      z = q*nt2::sinpi(z);
      z =  nt2::abs(z);
      return -log(Invpi<A0>()*nt2::abs(z))-w;
    }
    static inline A0 other(const A0& x)
    {
      bA0 xlt650 = lt(x, nt2::splat<A0>(6.50) );
      size_t nb = inbtrue(xlt650);
      A0 r0x = x;
      A0 r0z = x;
      A0 r0s = One<A0>();
      A0 r1 = Zero<A0>();
      A0 p =  Nan<A0>();
      if (nb > 0)
      {
        bA0 kernelC = False<bA0>();
        A0 z = One<A0>();
        A0 tx = x;
        A0 nx = Zero<A0>();

        const A0 _075 = nt2::splat<A0>(0.75);
        const A0 _150 = Two<A0>()*_075;
        const A0 _125 = nt2::splat<A0>(1.25);
        const A0 _250 = Two<A0>()*_125;
        bA0 xge150 = ge(x, _150);
        bA0 txgt250= gt(tx,_250);

        // x >= 1.5
        while (nt2::any(logical_and(xge150, txgt250)))
        {
          nx = seldec(txgt250, nx);
          tx = if_else(txgt250, x + nx, tx);
          z = if_else(txgt250, z*tx, z);
          txgt250= gt(tx,_250);
        }
        r0x = seladd(xge150, x, nx - Two<A0>());
        r0z = if_else(xge150, z, r0z);
        r0s = if_else(xge150,One<A0>(), r0s);

        // x >= 1.25 && x < 1.5
        bA0 xge125 = ge(x, _125);
        bA0 xge125t = l_andnot(xge125, xge150);
        if (nt2::any(xge125))
        {
          r0x =  if_else(xge125t, dec(x)    , r0x);
          r0z =  if_else(xge125t, z*x       , r0z);
          r0s =  if_else(xge125t, Mone<A0>(), r0s);
        }
        // x >= 0.75&& x < 1.5
        bA0 xge075  = ge(x, _075);
        bA0 xge075t = l_andnot(xge075, xge125);
        if (nt2::any(xge075t))
        {
          kernelC =  xge075t;
          r0x =  if_else(xge075t, dec(x)    , r0x);
          r0z =  if_else(xge075t, One<A0>() , r0z);
          r0s =  if_else(xge075t, Mone<A0>(), r0s);
          p = details::gammaln_kernel<A0>::gammalnC(r0x);
        }
        // tx < 1.5 && x < 0.75
        bA0 txlt150 = l_andnot(lt(tx,_150), xge075);
        if (nt2::any(txlt150))
        {
          bA0 orig = txlt150;
          while( nt2::any(txlt150) )
          {
            z  = if_else(txlt150, z*tx, z);
            nx = selinc(txlt150, nx);
            tx = if_else(txlt150, x + nx, tx);
            txlt150= l_andnot(lt(tx,_150), xge075);
          }
          r0x =  seladd(orig, r0x, nx - Two<A0>());
          r0z =  if_else(orig,z         , r0z);
          r0s =  if_else(orig,Mone<A0>(), r0s);
        }
        p =  if_else(kernelC, p, details::gammaln_kernel<A0>::gammalnB(r0x));
        if (nb >= meta::cardinal_of<A0>::value)
          return fma(r0x, p, r0s*nt2::log(nt2::abs(r0z)));
      }
      r0z = if_else(xlt650, nt2::abs(r0z), x);
      A0 m = nt2::log(r0z);
      r1 = fma(r0x, p, r0s*m);
      A0 r2 = fma(x-Half<A0>(),m,Logsqrt2pi<A0>()-x);
      r2 += details::gammaln_kernel<A0>::gammaln2(rec(sqr(x)))/x;
      return if_else(xlt650, r1, r2);
    }
  };
} }

#endif


/*
 * Copyright Nick Thompson, 2020
 * 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)
 */
#ifndef BOOST_MATH_QUADRATURE_WAVELET_TRANSFORMS_HPP
#define BOOST_MATH_QUADRATURE_WAVELET_TRANSFORMS_HPP
#include <boost/math/special_functions/daubechies_wavelet.hpp>
#include <boost/math/quadrature/trapezoidal.hpp>

namespace boost::math::quadrature {

template<class F, typename Real, int p>
class daubechies_wavelet_transform
{
public:
    daubechies_wavelet_transform(F f, int grid_refinements = -1, Real tol = 100*std::numeric_limits<Real>::epsilon(),
    int max_refinements = 12) : f_{f}, psi_(grid_refinements), tol_{tol}, max_refinements_{max_refinements}
    {}

    daubechies_wavelet_transform(F f, boost::math::daubechies_wavelet<Real, p> wavelet, Real tol = 100*std::numeric_limits<Real>::epsilon(),
    int max_refinements = 12) : f_{f}, psi_{wavelet}, tol_{tol}, max_refinements_{max_refinements}
    {}

    auto operator()(Real s, Real t)->decltype(std::declval<F>()(std::declval<Real>())) const
    {
        using std::sqrt;
        using std::abs;
        using boost::math::quadrature::trapezoidal;
        auto g = [&] (Real u) {
            return f_(s*u+t)*psi_(u);
        };
        auto [a,b] = psi_.support();
        return sqrt(abs(s))*trapezoidal(g, a, b, tol_, max_refinements_);
    }

private:
    F f_;
    boost::math::daubechies_wavelet<Real, p> psi_;
    Real tol_;
    int max_refinements_;
};

}
#endif


#ifndef VIENNACL_LINALG_DETAIL_AMG_AMG_INTERPOL_HPP
#define VIENNACL_LINALG_DETAIL_AMG_AMG_INTERPOL_HPP

/* =========================================================================
   Copyright (c) 2010-2012, 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 amg_interpol.hpp
    @brief Implementations of several variants of the AMG interpolation operators (setup phase). Experimental.
*/

#include <boost/numeric/ublas/vector.hpp>
#include <cmath>
#include "viennacl/linalg/amg.hpp"

#include <map>
#ifdef VIENNACL_WITH_OPENMP
#include <omp.h>
#endif

#include "amg_debug.hpp"

namespace viennacl
{
  namespace linalg
  {
    namespace detail
    {
      namespace amg
      {

    /** @brief Calls the right function to build interpolation matrix
     * @param level    Coarse level identifier
     * @param A      Operator matrix on all levels
     * @param P      Prolongation matrices. P[level] is constructed
     * @param Pointvector  Vector of points on all levels
     * @param tag    AMG preconditioner tag
    */
    template <typename InternalType1, typename InternalType2>
    void amg_interpol(unsigned int level, InternalType1 & A, InternalType1 & P, InternalType2 & Pointvector, amg_tag & tag)
    {
      switch (tag.get_interpol())
      {
        case VIENNACL_AMG_INTERPOL_DIRECT: amg_interpol_direct (level, A, P, Pointvector, tag); break;
        case VIENNACL_AMG_INTERPOL_CLASSIC: amg_interpol_classic (level, A, P, Pointvector, tag); break;
        case VIENNACL_AMG_INTERPOL_AG: amg_interpol_ag (level, A, P, Pointvector, tag); break;
        case VIENNACL_AMG_INTERPOL_SA: amg_interpol_sa (level, A, P, Pointvector, tag); break;
      }
    }
    /** @brief Direct interpolation. Multi-threaded! (VIENNACL_AMG_INTERPOL_DIRECT)
     * @param level    Coarse level identifier
     * @param A      Operator matrix on all levels
     * @param P      Prolongation matrices. P[level] is constructed
     * @param Pointvector  Vector of points on all levels
     * @param tag    AMG preconditioner tag
    */
    template <typename InternalType1, typename InternalType2>
    void amg_interpol_direct(unsigned int level, InternalType1 & A, InternalType1 & P, InternalType2 & Pointvector, amg_tag & tag)
    {
      typedef typename InternalType1::value_type SparseMatrixType;
      typedef typename InternalType2::value_type PointVectorType;
      typedef typename SparseMatrixType::value_type ScalarType;
      typedef typename SparseMatrixType::iterator1 InternalRowIterator;
      typedef typename SparseMatrixType::iterator2 InternalColIterator;

      ScalarType temp_res;
      ScalarType row_sum, c_sum, diag;
      //int diag_sign;
      unsigned int x, y;
      amg_point *pointx, *pointy;
      unsigned int c_points = Pointvector[level].get_cpoints();

      // Setup Prolongation/Interpolation matrix
      P[level] = SparseMatrixType(A[level].size1(),c_points);
      P[level].clear();

      // Assign indices to C points
      Pointvector[level].build_index();

      // Direct Interpolation (Yang, p.14)
#ifdef VIENNACL_WITH_OPENMP
      #pragma omp parallel for private (pointx,pointy,row_sum,c_sum,temp_res,y,x,diag) shared (P,A,Pointvector,tag)
#endif
      for (x=0; x < Pointvector[level].size(); ++x)
      {
        pointx = Pointvector[level][x];
        /*if (A[level](x,x) > 0)
          diag_sign = 1;
        else
          diag_sign = -1;*/

        // When the current line corresponds to a C point then the diagonal coefficient is 1 and the rest 0
        if (pointx->is_cpoint())
          P[level](x,pointx->get_coarse_index()) = 1;

        // When the current line corresponds to a F point then the diagonal is 0 and the rest has to be computed (Yang, p.14)
        if (pointx->is_fpoint())
        {
          // Jump to row x
          InternalRowIterator row_iter = A[level].begin1();
          row_iter += x;

          // Row sum of coefficients (without diagonal) and sum of influencing C point coefficients has to be computed
          row_sum = c_sum = diag = 0;
          for (InternalColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
          {
            y = col_iter.index2();
            if (x == y)// || *col_iter * diag_sign > 0)
            {
              diag += *col_iter;
              continue;
            }

            // Sum all other coefficients in line x
            row_sum += *col_iter;

            pointy = Pointvector[level][y];
            // Sum all coefficients that correspond to a strongly influencing C point
            if (pointy->is_cpoint())
              if (pointx->is_influencing(pointy))
                c_sum += *col_iter;
          }
          temp_res = -row_sum/(c_sum*diag);

          // Iterate over all strongly influencing points of point x
          for (amg_point::iterator iter = pointx->begin_influencing(); iter != pointx->end_influencing(); ++iter)
          {
            pointy = *iter;
            // The value is only non-zero for columns that correspond to a C point
            if (pointy->is_cpoint())
            {
              if (temp_res != 0)
                P[level](x, pointy->get_coarse_index()) = temp_res * A[level](x,pointy->get_index());
            }
          }

          //Truncate interpolation if chosen
          if (tag.get_interpolweight() != 0)
            amg_truncate_row(P[level], x, tag);
        }
      }

      // P test
      //test_interpolation(A[level], P[level], Pointvector[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Prolongation Matrix:" << std::endl;
      printmatrix (P[level]);
      #endif
    }

    /** @brief Classical interpolation. Don't use with onepass classical coarsening or RS0 (Yang, p.14)! Multi-threaded! (VIENNACL_AMG_INTERPOL_CLASSIC)
     * @param level    Coarse level identifier
     * @param A      Operator matrix on all levels
     * @param P      Prolongation matrices. P[level] is constructed
     * @param Pointvector  Vector of points on all levels
     * @param tag    AMG preconditioner tag
    */
    template <typename InternalType1, typename InternalType2>
    void amg_interpol_classic(unsigned int level, InternalType1 & A, InternalType1 & P, InternalType2 & Pointvector, amg_tag & tag)
    {
      typedef typename InternalType1::value_type SparseMatrixType;
      typedef typename InternalType2::value_type PointVectorType;
      typedef typename SparseMatrixType::value_type ScalarType;
      typedef typename SparseMatrixType::iterator1 InternalRowIterator;
      typedef typename SparseMatrixType::iterator2 InternalColIterator;

      ScalarType temp_res;
      ScalarType weak_sum, strong_sum;
      int diag_sign;
      amg_sparsevector<ScalarType> c_sum_row;
      amg_point *pointx, *pointy, *pointk, *pointm;
      unsigned int x, y, k, m;

      unsigned int c_points = Pointvector[level].get_cpoints();

      // Setup Prolongation/Interpolation matrix
      P[level] = SparseMatrixType(A[level].size1(), c_points);
      P[level].clear();

      // Assign indices to C points
      Pointvector[level].build_index();

      // Classical Interpolation (Yang, p.13-14)
#ifdef VIENNACL_WITH_OPENMP
      #pragma omp parallel for private (pointx,pointy,pointk,pointm,weak_sum,strong_sum,c_sum_row,temp_res,x,y,k,m,diag_sign) shared (A,P,Pointvector)
#endif
      for (x=0; x < Pointvector[level].size(); ++x)
      {
        pointx = Pointvector[level][x];
        if (A[level](x,x) > 0)
          diag_sign = 1;
        else
          diag_sign = -1;

        // When the current line corresponds to a C point then the diagonal coefficient is 1 and the rest 0
        if (pointx->is_cpoint())
          P[level](x,pointx->get_coarse_index()) = 1;

        // When the current line corresponds to a F point then the diagonal is 0 and the rest has to be computed (Yang, p.14)
        if (pointx->is_fpoint())
        {
          // Jump to row x
          InternalRowIterator row_iter = A[level].begin1();
          row_iter += x;

          weak_sum = 0;
          c_sum_row = amg_sparsevector<ScalarType>(A[level].size1());
          c_sum_row.clear();
          for (InternalColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
          {
            k = col_iter.index2();
            pointk = Pointvector[level][k];

            // Sum of weakly influencing neighbors + diagonal coefficient
            if (x == k || !pointx->is_influencing(pointk))// || *col_iter * diag_sign > 0)
            {
              weak_sum += *col_iter;
              continue;
            }

            // Sums of coefficients in row k (strongly influening F neighbors) of C point neighbors of x are calculated
            if (pointk->is_fpoint() && pointx->is_influencing(pointk))
            {
              for (amg_point::iterator iter = pointx->begin_influencing(); iter != pointx->end_influencing(); ++iter)
              {
                pointm = *iter;
                m = pointm->get_index();

                if (pointm->is_cpoint())
                  // Only use coefficients that have opposite sign of diagonal.
                  if (A[level](k,m) * diag_sign < 0)
                    c_sum_row[k] += A[level](k,m);
              }
              continue;
            }
          }

          // Iterate over all strongly influencing points of point x
          for (amg_point::iterator iter = pointx->begin_influencing(); iter != pointx->end_influencing(); ++iter)
          {
            pointy = *iter;
            y = pointy->get_index();

            // The value is only non-zero for columns that correspond to a C point
            if (pointy->is_cpoint())
            {
              strong_sum = 0;
              // Calculate term for strongly influencing F neighbors
              for (typename amg_sparsevector<ScalarType>::iterator iter2 = c_sum_row.begin(); iter2 != c_sum_row.end(); ++iter2)
              {
                k = iter2.index();
                // Only use coefficients that have opposite sign of diagonal.
                if (A[level](k,y) * diag_sign < 0)
                  strong_sum += (A[level](x,k) * A[level](k,y)) / (*iter2);
              }

              // Calculate coefficient
              temp_res = - (A[level](x,y) + strong_sum) / (weak_sum);
              if (temp_res != 0)
                P[level](x,pointy->get_coarse_index()) = temp_res;
            }
          }

          //Truncate iteration if chosen
          if (tag.get_interpolweight() != 0)
            amg_truncate_row(P[level], x, tag);
        }
      }

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Prolongation Matrix:" << std::endl;
      printmatrix (P[level]);
      #endif
    }

    /** @brief Interpolation truncation (for VIENNACL_AMG_INTERPOL_DIRECT and VIENNACL_AMG_INTERPOL_CLASSIC)
    *
    * @param P    Interpolation matrix
    * @param row  Row which has to be truncated
    * @param tag  AMG preconditioner tag
    */
    template <typename SparseMatrixType>
    void amg_truncate_row(SparseMatrixType & P, unsigned int row, amg_tag & tag)
    {
      typedef typename SparseMatrixType::value_type ScalarType;
      typedef typename SparseMatrixType::iterator1 InternalRowIterator;
      typedef typename SparseMatrixType::iterator2 InternalColIterator;

      ScalarType row_max, row_min, row_sum_pos, row_sum_neg, row_sum_pos_scale, row_sum_neg_scale;

      InternalRowIterator row_iter = P.begin1();
      row_iter += row;

      row_max = 0;
      row_min = 0;
      row_sum_pos = 0;
      row_sum_neg = 0;

      // Truncate interpolation by making values to zero that are a lot smaller than the biggest value in a row
      // Determine max entry and sum of row (seperately for negative and positive entries)
      for (InternalColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
      {
        if (*col_iter > row_max)
          row_max = *col_iter;
        if (*col_iter < row_min)
          row_min = *col_iter;
        if (*col_iter > 0)
          row_sum_pos += *col_iter;
        if (*col_iter < 0)
          row_sum_neg += *col_iter;
      }

      row_sum_pos_scale = row_sum_pos;
      row_sum_neg_scale = row_sum_neg;

      // Make certain values to zero (seperately for negative and positive entries)
      for (InternalColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
      {
        if (*col_iter > 0 && *col_iter < tag.get_interpolweight() * row_max)
        {
          row_sum_pos_scale -= *col_iter;
          *col_iter = 0;
        }
        if (*col_iter < 0 && *col_iter > tag.get_interpolweight() * row_min)
        {
          row_sum_pos_scale -= *col_iter;
          *col_iter = 0;
        }
      }

      // Scale remaining values such that row sum is unchanged
      for (InternalColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
      {
        if (*col_iter > 0)
          *col_iter = *col_iter *(row_sum_pos/row_sum_pos_scale);
        if (*col_iter < 0)
          *col_iter = *col_iter *(row_sum_neg/row_sum_neg_scale);
      }
    }

    /** @brief AG (aggregation based) interpolation. Multi-Threaded! (VIENNACL_INTERPOL_SA)
     * @param level    Coarse level identifier
     * @param A      Operator matrix on all levels
     * @param P      Prolongation matrices. P[level] is constructed
     * @param Pointvector  Vector of points on all levels
    */
    template <typename InternalType1, typename InternalType2>
    void amg_interpol_ag(unsigned int level, InternalType1 & A, InternalType1 & P, InternalType2 & Pointvector, amg_tag)
    {
      typedef typename InternalType1::value_type SparseMatrixType;
      typedef typename InternalType2::value_type PointVectorType;
      typedef typename SparseMatrixType::value_type ScalarType;
      typedef typename SparseMatrixType::iterator1 InternalRowIterator;
      typedef typename SparseMatrixType::iterator2 InternalColIterator;

      unsigned int x;
      amg_point *pointx, *pointy;
      unsigned int c_points = Pointvector[level].get_cpoints();

      P[level] = SparseMatrixType(A[level].size1(), c_points);
      P[level].clear();

      // Assign indices to C points
      Pointvector[level].build_index();

      // Set prolongation such that F point is interpolated (weight=1) by the aggregate it belongs to (Vanek et al p.6)
#ifdef VIENNACL_WITH_OPENMP
      #pragma omp parallel for private (x,pointx) shared (P)
#endif
      for (x=0; x<Pointvector[level].size(); ++x)
      {
        pointx = Pointvector[level][x];
        pointy = Pointvector[level][pointx->get_aggregate()];
        // Point x belongs to aggregate y.
        P[level](x,pointy->get_coarse_index()) = 1;
      }

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Aggregation based Prolongation:" << std::endl;
      printmatrix(P[level]);
      #endif
    }

    /** @brief SA (smoothed aggregate) interpolation. Multi-Threaded! (VIENNACL_INTERPOL_SA)
     * @param level    Coarse level identifier
     * @param A      Operator matrix on all levels
     * @param P      Prolongation matrices. P[level] is constructed
     * @param Pointvector  Vector of points on all levels
     * @param tag    AMG preconditioner tag
    */
    template <typename InternalType1, typename InternalType2>
    void amg_interpol_sa(unsigned int level, InternalType1 & A, InternalType1 & P, InternalType2 & Pointvector, amg_tag & tag)
    {
      typedef typename InternalType1::value_type SparseMatrixType;
      typedef typename InternalType2::value_type PointVectorType;
      typedef typename SparseMatrixType::value_type ScalarType;
      typedef typename SparseMatrixType::iterator1 InternalRowIterator;
      typedef typename SparseMatrixType::iterator2 InternalColIterator;

      unsigned int x,y;
      ScalarType diag = 0;
      unsigned int c_points = Pointvector[level].get_cpoints();

      InternalType1 P_tentative = InternalType1(P.size());
      SparseMatrixType Jacobi = SparseMatrixType(A[level].size1(), A[level].size2());
      Jacobi.clear();
      P[level] = SparseMatrixType(A[level].size1(), c_points);
      P[level].clear();

      // Build Jacobi Matrix via filtered A matrix (Vanek et al. p.6)
#ifdef VIENNACL_WITH_OPENMP
      #pragma omp parallel for private (x,y,diag) shared (A,Pointvector)
#endif
      for (x=0; x<A[level].size1(); ++x)
      {
        diag = 0;
        InternalRowIterator row_iter = A[level].begin1();
        row_iter += x;
        for (InternalColIterator col_iter = row_iter.begin(); col_iter != row_iter.end(); ++col_iter)
        {
          y = col_iter.index2();
          // Determine the structure of the Jacobi matrix by using a filtered matrix of A:
          // The diagonal consists of the diagonal coefficient minus all coefficients of points not in the neighborhood of x.
          // All other coefficients are the same as in A.
          // Already use Jacobi matrix to save filtered A matrix to speed up computation.
          if (x == y)
            diag += *col_iter;
          else if (!Pointvector[level][x]->is_influencing(Pointvector[level][y]))
            diag += -*col_iter;
          else
            Jacobi (x,y) = *col_iter;
        }
        InternalRowIterator row_iter2 = Jacobi.begin1();
        row_iter2 += x;
        // Traverse through filtered A matrix and compute the Jacobi filtering
        for (InternalColIterator col_iter2 = row_iter2.begin(); col_iter2 != row_iter2.end(); ++col_iter2)
        {
            *col_iter2 = - static_cast<ScalarType>(tag.get_interpolweight())/diag * *col_iter2;
        }
        // Diagonal can be computed seperately.
        Jacobi (x,x) = 1 - static_cast<ScalarType>(tag.get_interpolweight());
      }

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Jacobi Matrix:" << std::endl;
      printmatrix(Jacobi);
      #endif

      // Use AG interpolation as tentative prolongation
      amg_interpol_ag(level, A, P_tentative, Pointvector, tag);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Tentative Prolongation:" << std::endl;
      printmatrix(P_tentative[level]);
      #endif

      // Multiply Jacobi matrix with tentative prolongation to get actual prolongation
      amg_mat_prod(Jacobi,P_tentative[level],P[level]);

      #ifdef VIENNACL_AMG_DEBUG
      std::cout << "Prolongation Matrix:" << std::endl;
      printmatrix (P[level]);
      #endif
    }
      } //namespace amg
    }
  }
}

#endif


// Copyright 2019 Google LLC
//
// 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
//
//     https://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 <ros/ros.h>
#include <trajectory_following_controller/trajectory_following_controller.h>
#include <trajectory_math/float_comparison.h>
#include <trajectory_math/trajectory_algorithms.h>

#include <Eigen/Geometry>
#include <limits>

namespace bookbot {

constexpr double kPurePursuitLookaheadTime = 1;
constexpr double kPurePursuitMinLookaheadDistance = 0.5;
constexpr double kLongitudinalFeedbackGain = 0.1;
constexpr double kMaxCurvatureForNonzeroVelocity = 1.5;
constexpr double kMaxLateralAcceleration = 1.0;
constexpr double kMaxPositionError = 1.0;
constexpr double kMaxAngularVelocity = 1.0;

double ComputePurePursuitCurvature(Eigen::Vector2d robot_position,
                                   double robot_yaw, Eigen::Vector2d goal_pt) {
  if (robot_position == goal_pt) {
    return 0;
  }

  Eigen::Rotation2Dd R(-robot_yaw);

  Eigen::Vector2d relative_goal_pt =
      R * Eigen::Vector2d(goal_pt - robot_position);

  double desired_curvature =
      2 * relative_goal_pt[1] / relative_goal_pt.dot(relative_goal_pt);
  return desired_curvature;
}

ControlCommand ComputeControlCommand(const Eigen::Vector2d robot_position,
                                     double robot_yaw,
                                     const Trajectory& trajectory,
                                     double current_time,
                                     ControlIntrospection* introspection) {
  // Match to trajectory spatially to compute longitudinal error
  if (ApproxEqual(trajectory.front().distance_along_path,
                  trajectory.back().distance_along_path, 0.01)) {
    introspection->matched_point = trajectory.back();
    introspection->spatially_matched_point = trajectory.back();
    introspection->lookahead_point = trajectory.back().Position();
    introspection->desired_curvature = 0.;
    return {0., 0.};
  }

  const TrajectoryPoint spatially_matched_point = MatchToTrajectory(
      robot_position, std::begin(trajectory), std::end(trajectory));

  // Interpolate trajectory based on current time
  const double current_point_time =
      std::min(current_time, trajectory.back().time);

  // Exit early if temporally or spatially past end of trajectory
  if (ApproxEqual(spatially_matched_point.distance_along_path,
                  trajectory.back().distance_along_path) ||
      ApproxEqual(current_point_time, trajectory.back().time)) {
    introspection->matched_point = trajectory.back();
    introspection->spatially_matched_point = trajectory.back();
    introspection->lookahead_point = trajectory.back().Position();
    introspection->desired_curvature = 0.;
    return {0., 0.};
  }

  // Exit early if too far away from trajectory
  if (Eigen::Vector2d(robot_position - spatially_matched_point.Position())
          .squaredNorm() > kMaxPositionError * kMaxPositionError) {
    introspection->matched_point = spatially_matched_point;
    introspection->spatially_matched_point = spatially_matched_point;
    introspection->lookahead_point = spatially_matched_point.Position();
    introspection->desired_curvature = 0.;
    return {0., 0.};
  }

  TrajectoryPoint matched_point = InterpolateTrajectoryByTime(
      current_point_time, std::begin(trajectory), std::end(trajectory));

  const double longitudinal_error = matched_point.distance_along_path -
                                    spatially_matched_point.distance_along_path;

  const double commanded_forward_velocity =
      matched_point.velocity + kLongitudinalFeedbackGain * longitudinal_error;

  const double lookahead_relative_distance =
      std::max(matched_point.velocity * kPurePursuitLookaheadTime,
               kPurePursuitMinLookaheadDistance);
  double lookahead_distance =
      spatially_matched_point.distance_along_path + lookahead_relative_distance;

  if (ApproxEqual(lookahead_distance, trajectory.back().distance_along_path) ||
      lookahead_distance > trajectory.back().distance_along_path) {
    const double distance_past_end =
        lookahead_distance - trajectory.back().distance_along_path;
    // Trajectory is too short for pure pursuit, just go in a straight line
    introspection->matched_point = matched_point;
    introspection->spatially_matched_point = spatially_matched_point;
    introspection->lookahead_point = trajectory.back().Position();
    introspection->desired_curvature = 0.;
    return {commanded_forward_velocity, 0.};
  }

  TrajectoryPoint lookahead_trajectory_point =
      InterpolateTrajectoryByDistanceAlongPath(
          lookahead_distance, std::begin(trajectory), std::end(trajectory));
  Eigen::Vector2d lookahead_point = lookahead_trajectory_point.Position();

  const double desired_curvature =
      ComputePurePursuitCurvature(robot_position, robot_yaw, lookahead_point);

  // Limit commanded_forward_velocity based on curvature
  double max_velocity_based_on_curvature =
      std::numeric_limits<double>::infinity();
  if (!ApproxZero(desired_curvature)) {
    // Limit by maximum lateral acceleration
    max_velocity_based_on_curvature =
        std::sqrt(kMaxLateralAcceleration / std::abs(desired_curvature));
  }
  if (std::abs(desired_curvature) > kMaxCurvatureForNonzeroVelocity) {
    max_velocity_based_on_curvature = 0;
  }
  const double limited_commanded_forward_velocity =
      std::min(max_velocity_based_on_curvature, commanded_forward_velocity);

  const double commanded_angular_velocity =
      std::max(limited_commanded_forward_velocity, 1.0) * desired_curvature;

  // Limit commanded_angular_velocity
  const double limited_commanded_angular_velocity = std::copysign(
      std::min(std::abs(commanded_angular_velocity), kMaxAngularVelocity),
      commanded_angular_velocity);

  introspection->matched_point = matched_point;
  introspection->spatially_matched_point = spatially_matched_point;
  introspection->lookahead_point = lookahead_point;
  introspection->desired_curvature = desired_curvature;

  return {limited_commanded_forward_velocity,
          limited_commanded_angular_velocity};
}

}  // namespace bookbot


#include <data/Geometry.hpp>
#include <engine/Neighbours.hpp>
#include <engine/Vectormath.hpp>
#include <utility/Exception.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "Qhull.h"
#include "QhullFacetList.h"
#include "QhullVertexSet.h"

#include <array>

namespace Data
{
    Geometry::Geometry(std::vector<Vector3> bravais_vectors, intfield n_cells, std::vector<Vector3> cell_atoms,
        intfield cell_atom_types, scalar lattice_constant) :
        bravais_vectors(bravais_vectors), n_cells(n_cells),
        n_cell_atoms(cell_atoms.size()), cell_atoms(cell_atoms), lattice_constant(lattice_constant),
        nos(cell_atoms.size() * n_cells[0] * n_cells[1] * n_cells[2]), cell_atom_types(cell_atom_types),
        n_cells_total(n_cells[0] * n_cells[1] * n_cells[2])
    {
        for (int iatom = 0; iatom < n_cell_atoms; ++iatom)
        {
            // Get x,y,z of component of atom positions in unit of length (instead of in units of a,b,c)
            Vector3 build_array = bravais_vectors[0] * cell_atoms[iatom][0] + bravais_vectors[1] * cell_atoms[iatom][1] + bravais_vectors[2] * cell_atoms[iatom][2];
            cell_atoms[iatom] = lattice_constant * build_array;
        }

        // Generate positions and atom types
        this->positions = vectorfield(nos);
        this->atom_types = intfield(nos, 0);
        Engine::Vectormath::Build_Spins(positions, atom_types, cell_atoms, cell_atom_types, bravais_vectors, n_cells);

        // Calculate some info
        this->calculateBounds();
        this->calculateUnitCellBounds();
        this->calculateDimensionality();

        // Calculate center of the System
        this->center = 0.5 *  (this->bounds_min + this->bounds_max);

        // Calculate the type of geometry
        this->calculateGeometryType();

        // For updates of triangulation and tetrahedra
        this->last_update_n_cell_step = -1;
        this->last_update_n_cells = intfield(3, -1);
    }

    std::vector<tetrahedron_t> compute_delaunay_triangulation_3D(const std::vector<vector3_t> & points)
    {
        const int ndim = 3;
        std::vector<tetrahedron_t> tetrahedra;
        tetrahedron_t tmp_tetrahedron;
        int *current_index;

        orgQhull::Qhull qhull;
        qhull.runQhull("", ndim, points.size(), (coordT *) points.data(),  "d Qt Qbb Qz");
        orgQhull::QhullFacetList facet_list = qhull.facetList();
        for(const auto & facet : facet_list)
        {
            if(!facet.isUpperDelaunay())
            {
                current_index = &tmp_tetrahedron[0];
                for(const auto & vertex : facet.vertices())
                {
                    *current_index++ = vertex.point().id();
                }
                tetrahedra.push_back(tmp_tetrahedron);
            }
        }
        return tetrahedra;
    }

    std::vector<triangle_t> compute_delaunay_triangulation_2D(const std::vector<vector2_t> & points)
    {
        const int ndim = 2;
        std::vector<triangle_t> triangles;
        triangle_t tmp_triangle;
        int *current_index;

        orgQhull::Qhull qhull;
        qhull.runQhull("", ndim, points.size(), (coordT *) points.data(),  "d Qt Qbb Qz");
        for(const auto & facet : qhull.facetList())
        {
            if(!facet.isUpperDelaunay())
            {
                current_index = &tmp_triangle[0];
                for(const auto & vertex : facet.vertices())
                {
                    *current_index++ = vertex.point().id();
                }
                triangles.push_back(tmp_triangle);
            }
        }
        return triangles;
    }

    const std::vector<triangle_t>& Geometry::triangulation(int n_cell_step)
    {
        // Only every n_cell_step'th cell is used. So we check if there is still enough cells in all
        //      directions. Note: when visualising, 'n_cell_step' can be used to e.g. olny visualise
        //      every 2nd spin.
        if ( (n_cells[0]/n_cell_step < 2 && n_cells[0] > 1) ||
             (n_cells[1]/n_cell_step < 2 && n_cells[1] > 1) ||
             (n_cells[2]/n_cell_step < 2 && n_cells[2] > 1) )
        {
            _triangulation.clear();
            return _triangulation;
        }

        // 2D: triangulation
        if (dimensionality == 2)
        {
            // Check if the tetrahedra for this combination of n_cells and n_cell_step has already been calculated
            if (this->last_update_n_cell_step != n_cell_step ||
                this->last_update_n_cells[0]  != n_cells[0]  ||
                this->last_update_n_cells[1]  != n_cells[1]  ||
                this->last_update_n_cells[2]  != n_cells[2]  )
            {
                this->last_update_n_cell_step = n_cell_step;
                this->last_update_n_cells[0]  = n_cells[0];
                this->last_update_n_cells[1]  = n_cells[1];
                this->last_update_n_cells[2]  = n_cells[2];

                _triangulation.clear();

                std::vector<vector2_t> points;
                points.resize(positions.size());

                int icell = 0, idx;
                for (int cell_c=0; cell_c<n_cells[2]; cell_c+=n_cell_step)
                {
                    for (int cell_b=0; cell_b<n_cells[1]; cell_b+=n_cell_step)
                    {
                        for (int cell_a=0; cell_a<n_cells[0]; cell_a+=n_cell_step)
                        {
                            for (int ibasis=0; ibasis < n_cell_atoms; ++ibasis)
                            {
                                idx = ibasis + n_cell_atoms*cell_a + n_cell_atoms*n_cells[0]*cell_b + n_cell_atoms*n_cells[0]*n_cells[1]*cell_c;
                                points[icell].x = positions[idx][0];
                                points[icell].y = positions[idx][1];
                                ++icell;
                            }
                        }
                    }
                }
                _triangulation = compute_delaunay_triangulation_2D(points);
            }
        }// endif 2D
        // 0D, 1D and 3D give no triangulation
        else
        {
            _triangulation.clear();
        }
        return _triangulation;
    }

    const std::vector<tetrahedron_t>& Geometry::tetrahedra(int n_cell_step)
    {
        // Only every n_cell_step'th cell is used. So we check if there is still enough cells in all
        //      directions. Note: when visualising, 'n_cell_step' can be used to e.g. olny visualise
        //      every 2nd spin.
        if (n_cells[0]/n_cell_step < 2 || n_cells[1]/n_cell_step < 2 || n_cells[2]/n_cell_step < 2)
        {
            _tetrahedra.clear();
            return _tetrahedra;
        }

        // 3D: Tetrahedra
        if (dimensionality == 3)
        {
            // Check if the tetrahedra for this combination of n_cells and n_cell_step has already been calculated
            if (this->last_update_n_cell_step != n_cell_step ||
                this->last_update_n_cells[0]  != n_cells[0]  ||
                this->last_update_n_cells[1]  != n_cells[1]  ||
                this->last_update_n_cells[2]  != n_cells[2]  )
            {
                this->last_update_n_cell_step = n_cell_step;
                this->last_update_n_cells[0]  = n_cells[0];
                this->last_update_n_cells[1]  = n_cells[1];
                this->last_update_n_cells[2]  = n_cells[2];

                // If we have only one spin in the basis our lattice is a simple regular geometry
                bool is_simple_regular_geometry = n_cell_atoms == 1;

                // If we have a simple regular geometry everything can be calculated by hand
                if (is_simple_regular_geometry)
                {
                    _tetrahedra.clear();
                    int cell_indices[] = {
                        0, 1, 5, 3,
                        1, 3, 2, 5,
                        3, 2, 5, 6,
                        7, 6, 5, 3,
                        4, 7, 5, 3,
                        0, 4, 3, 5
                        };
                    int x_offset = 1;
                    int y_offset = n_cells[0]/n_cell_step;
                    int z_offset = n_cells[0]/n_cell_step*n_cells[1]/n_cell_step;
                    int offsets[] = {
                        0, x_offset, x_offset+y_offset, y_offset,
                        z_offset, x_offset+z_offset, x_offset+y_offset+z_offset, y_offset+z_offset
                        };

                    for (int ix = 0; ix < (n_cells[0]-1)/n_cell_step; ix++)
                    {
                        for (int iy = 0; iy < (n_cells[1]-1)/n_cell_step; iy++)
                        {
                            for (int iz = 0; iz < (n_cells[2]-1)/n_cell_step; iz++)
                            {
                                int base_index = ix*x_offset+iy*y_offset+iz*z_offset;
                                for (int j = 0; j < 6; j++)
                                {
                                    tetrahedron_t tetrahedron;
                                    for (int k = 0; k < 4; k++)
                                    {
                                        int index = base_index + offsets[cell_indices[j*4+k]];
                                        tetrahedron[k] = index;
                                    }
                                    _tetrahedra.push_back(tetrahedron);
                                }
                            }
                        }
                    }
                }
                // For general basis cells we calculate the Delaunay tetrahedra
                else
                {
                    std::vector<vector3_t> points;
                    points.resize(positions.size());

                    int icell = 0, idx;
                    for (int cell_c=0; cell_c<n_cells[2]; cell_c+=n_cell_step)
                    {
                        for (int cell_b=0; cell_b<n_cells[1]; cell_b+=n_cell_step)
                        {
                            for (int cell_a=0; cell_a<n_cells[0]; cell_a+=n_cell_step)
                            {
                                for (int ibasis=0; ibasis < n_cell_atoms; ++ibasis)
                                {
                                    idx = ibasis + n_cell_atoms*cell_a + n_cell_atoms*n_cells[0]*cell_b + n_cell_atoms*n_cells[0]*n_cells[1]*cell_c;
                                    points[icell].x = positions[idx][0];
                                    points[icell].y = positions[idx][1];
                                    points[icell].z = positions[idx][2];
                                    ++icell;
                                }
                            }
                        }
                    }
                    _tetrahedra = compute_delaunay_triangulation_3D(points);
                }
            }
        } // endif 3D
        // 0-2 D gives no tetrahedra
        else
        {
            _tetrahedra.clear();
        }
        return _tetrahedra;
    }

    std::vector<Vector3> Geometry::BravaisVectorsSC()
    {
        return { { 1,0,0 },
                 { 0,1,0 },
                 { 0,0,1 } };
    }

    std::vector<Vector3> Geometry::BravaisVectorsFCC()
    {
        return { { 0.5,0.0,0.5 },
                 { 0.5,0.5,0.0 },
                 { 0.0,0.5,0.5 } };
    }

    std::vector<Vector3> Geometry::BravaisVectorsBCC()
    {
        return { { 0.5, 0.5,-0.5 },
                 { -0.5, 0.5,-0.5 },
                 { 0.5,-0.5, 0.5 } };
    }

    std::vector<Vector3> Geometry::BravaisVectorsHex2D60()
    {
        return { { 1,   0,                0 },
                 { 0.5, 0.5*std::sqrt(3), 0 },
                 { 0,   0,                1 } };
    }

    std::vector<Vector3> Geometry::BravaisVectorsHex2D120()
    {
        return { {  1,   0,                0 },
                 { -0.5, 0.5*std::sqrt(3), 0 },
                 {  0,   0,                1 } };
    }

    void Geometry::calculateDimensionality()
    {
        int dims_basis = 0, dims_translations = 0;
        Vector3 test_vec_basis, test_vec_translations;

        // ----- Find dimensionality of the basis -----
        if      (n_cell_atoms == 1) dims_basis = 0;
        else if (n_cell_atoms == 2) dims_basis = 1;
        else if (n_cell_atoms == 3) dims_basis = 2;
        else
        {
            // Get basis atoms relative to the first atom
            Vector3 v0 = cell_atoms[0];
            std::vector<Vector3> b_vectors(n_cell_atoms-1);
            for (int i = 1; i < n_cell_atoms; ++i)
            {
                b_vectors[i-1] = cell_atoms[i] - v0;
            }
            // Calculate basis dimensionality
            // test vec is along line
            test_vec_basis = b_vectors[0];
            //		is it 1D?
            int n_parallel = 0;
            for (unsigned int i = 1; i < b_vectors.size(); ++i)
            {
                if (std::abs(b_vectors[i].dot(test_vec_basis) - 1.0) < 1e-9) ++n_parallel;
                // else n_parallel will give us the last parallel vector
                // also the if-statement for dims_basis=1 wont be met
                else break;
            }
            if (n_parallel == b_vectors.size() - 1)
            {
                dims_basis = 1;
            }
            else
            {
                // test vec is normal to plane
                test_vec_basis = b_vectors[0].cross(b_vectors[n_parallel+1]);
                //		is it 2D?
                int n_in_plane = 0;
                for (unsigned int i = 2; i < b_vectors.size(); ++i)
                {
                    if (std::abs(b_vectors[i].dot(test_vec_basis)) < 1e-9) ++n_in_plane;
                }
                if (n_in_plane == b_vectors.size() - 2)
                {
                    dims_basis = 2;
                }
                else
                {
                    this->dimensionality = 3;
                    return;
                }
            }
        }

        // ----- Find dimensionality of the translations -----
        //		The following are zero if the corresponding pair is parallel
        double t01, t02, t12;
        t01 = std::abs(bravais_vectors[0].dot(bravais_vectors[1]) - 1.0);
        t02 = std::abs(bravais_vectors[0].dot(bravais_vectors[2]) - 1.0);
        t12 = std::abs(bravais_vectors[1].dot(bravais_vectors[2]) - 1.0);
        //		Check if pairs are linearly independent
        int n_independent_pairs = 0;
        if (t01>1e-9 && n_cells[0] > 1 && n_cells[1] > 1) ++n_independent_pairs;
        if (t02>1e-9 && n_cells[0] > 1 && n_cells[2] > 1) ++n_independent_pairs;
        if (t12>1e-9 && n_cells[1] > 1 && n_cells[2] > 1) ++n_independent_pairs;
        //		Calculate translations dimensionality
        if (n_cells[0] == 1 && n_cells[1] == 1 && n_cells[2] == 1) dims_translations = 0;
        else if (n_independent_pairs == 0)
        {
            dims_translations = 1;
            // test vec is along the line
            for (int i=0; i<3; ++i) if (n_cells[i] > 1) test_vec_translations = bravais_vectors[i];
        }
        else if (n_independent_pairs < 3)
        {
            dims_translations = 2;
            // test vec is normal to plane
            int n = 0;
            std::vector<Vector3> plane(2);
            for (int i = 0; i < 3; ++i)
            {
                if (n_cells[i] > 1) plane[n] = bravais_vectors[i];
                ++n;
            }
            test_vec_translations = plane[0].cross(plane[1]);
        }
        else
        {
            this->dimensionality = 3;
            return;
        }

        // ----- Calculate dimensionality of system -----
        test_vec_basis.normalize();
        test_vec_translations.normalize();
        //		If one dimensionality is zero, only the other counts
        if (dims_basis == 0)
        {
            this->dimensionality = dims_translations;
            return;
        }
        else if (dims_translations == 0)
        {
            this->dimensionality = dims_basis;
            return;
        }
        //		If both are linear or both are planar, the test vectors should be parallel if the geometry is 1D or 2D
        else if (dims_basis == dims_translations)
        {
            if (std::abs(test_vec_basis.dot(test_vec_translations) - 1.0) < 1e-9)
            {
                this->dimensionality = dims_basis;
                return;
            }
            else if (dims_basis == 1)
            {
                this->dimensionality = 2;
                return;
            }
            else if (dims_basis == 2)
            {
                this->dimensionality = 3;
                return;
            }
        }
        //		If one is linear (1D), and the other planar (2D) then the test vectors should be orthogonal if the geometry is 2D
        else if ( (dims_basis == 1 && dims_translations == 2) || (dims_basis == 2 && dims_translations == 1) )
        {
            if (std::abs(test_vec_basis.dot(test_vec_translations)) < 1e-9)
            {
                this->dimensionality = 2;
                return;
            }
            else
            {
                this->dimensionality = 3;
                return;
            }
        }
    }

    void Geometry::calculateBounds()
    {
        this->bounds_max.setZero();
        this->bounds_min.setZero();
        for (int iatom = 0; iatom < nos; ++iatom)
        {
            for (int dim = 0; dim < 3; ++dim)
            {
                if (this->positions[iatom][dim] < this->bounds_min[dim]) this->bounds_min[dim] = positions[iatom][dim];
                if (this->positions[iatom][dim] > this->bounds_max[dim]) this->bounds_max[dim] = positions[iatom][dim];
            }
        }
    }

    void Geometry::calculateUnitCellBounds()
    {
        this->cell_bounds_max.setZero();
        this->cell_bounds_min.setZero();
        for (unsigned int ivec = 0; ivec < bravais_vectors.size(); ++ivec)
        {
            for (int iatom = 0; iatom < n_cell_atoms; ++iatom)
            {
                auto neighbour1 = cell_atoms[iatom] + bravais_vectors[ivec];
                auto neighbour2 = cell_atoms[iatom] - bravais_vectors[ivec];
                for (int dim = 0; dim < 3; ++dim)
                {
                    if (neighbour1[dim] < this->cell_bounds_min[dim]) this->cell_bounds_min[dim] = neighbour1[dim];
                    if (neighbour1[dim] > this->cell_bounds_max[dim]) this->cell_bounds_max[dim] = neighbour1[dim];
                    if (neighbour2[dim] < this->cell_bounds_min[dim]) this->cell_bounds_min[dim] = neighbour2[dim];
                    if (neighbour2[dim] > this->cell_bounds_max[dim]) this->cell_bounds_max[dim] = neighbour2[dim];
                }
            }
        }
        this->cell_bounds_min *= 0.5;
        this->cell_bounds_max *= 0.5;
    }

    void Geometry::calculateGeometryType()
    {
        // Automatically try to determine GeometryType
        // Single-atom unit cell
        if (cell_atoms.size() == 1)
        {
            // If the basis vectors are orthogonal, it is a rectilinear lattice
            if (std::abs(bravais_vectors[0].dot(bravais_vectors[1])) < 1e-6 &&
                std::abs(bravais_vectors[0].dot(bravais_vectors[2])) < 1e-6)
            {
                // If equidistant it is simple cubic
                if (bravais_vectors[0].norm() == bravais_vectors[1].norm() == bravais_vectors[2].norm())
                    this->classifier = BravaisLatticeType::SC;
                // Otherwise only rectilinear
                else
                    this->classifier = BravaisLatticeType::Rectilinear;
            }
        }
        // Regular unit cell with multiple atoms (e.g. bcc, fcc, hex)
        //else if (n_cell_atoms == 2)
        // Irregular unit cells arranged on a lattice (e.g. B20 or custom)
        /*else if (n_cells[0] > 1 || n_cells[1] > 1 || n_cells[2] > 1)
        {
            this->classifier = BravaisLatticeType::Lattice;
        }*/
        // A single irregular unit cell
        else
        {
            this->classifier = BravaisLatticeType::Irregular;
        }
    }
}


#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/list.hpp>
#include <scitbx/vec2.h>
#include <scitbx/vec3.h>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/shared.h>
#include <dxtbx/model/detector.h>
#include <dxtbx/model/panel.h>
#include <math.h>
#include <vector>

namespace kapton {

using dxtbx::model::Detector;
using scitbx::vec2;
using scitbx::vec3;

/**
 * Implementing a c++ version of the get_kapton_path function. Significant speedups
 * observed
 */

scitbx::af::shared<double> get_kapton_path_cpp(
  boost::python::list kapton_faces,
  scitbx::af::const_ref<vec3<double> > s1_flex) {
  scitbx::af::shared<double> kapton_path_mm;
  int nfaces = boost::python::len(kapton_faces);
  std::vector<Detector> all_kapton_faces;
  // Store all the detectors ahead of time to avoid making expensive extract calls every
  // time
  for (std::size_t i = 0; i < nfaces; ++i) {
    all_kapton_faces.push_back(boost::python::extract<Detector>(kapton_faces[i]));
  }
  for (scitbx::af::const_ref<vec3<double> >::const_iterator it = s1_flex.begin();
       it != s1_flex.end();
       ++it) {
    double max_d2 = -999.9;
    scitbx::af::shared<vec3<double> > intersection_xy_list;
    // Find out intersection points of s1 vector with all faces of kapton volume
    for (std::size_t j = 0; j < nfaces; ++j) {
      Detector detector = all_kapton_faces.at(j);
      try {
        vec2<double> px = detector[0].get_ray_intersection_px(*it);
        if (px[0] > 0.0 && px[1] > 0.0 && px[0] < detector[0].get_image_size()[0]
            && px[1] < detector[0].get_image_size()[1]) {
          intersection_xy_list.push_back(
            detector[0].get_lab_coord(detector[0].get_ray_intersection(*it)));
        }
      } catch (dxtbx::error) {
        // Do nothing
      }
    }  // kapton faces
    /*
     * Now find out the maximum path length through the kapton. That is the path
     * traversed by s1 vector if no intersection or intersects with only one face then
     * path length is set to 0.0
     */
    if (intersection_xy_list.size() == 0) {
      kapton_path_mm.push_back(0.0);
    } else if (intersection_xy_list.size() == 1) {
      kapton_path_mm.push_back(0.0);
    } else {
      double d2 = 0.0;
      for (std::size_t k1 = 0; k1 < intersection_xy_list.size() - 1; ++k1) {
        for (std::size_t k2 = k1 + 1; k2 < intersection_xy_list.size(); ++k2) {
          vec3<double> pt1 = intersection_xy_list[k1];
          vec3<double> pt2 = intersection_xy_list[k2];
          d2 = (pt1[0] - pt2[0]) * (pt1[0] - pt2[0])
               + (pt1[1] - pt2[1]) * (pt1[1] - pt2[1])
               + (pt1[2] - pt2[2]) * (pt1[2] - pt2[2]);
          if (d2 > max_d2) {
            max_d2 = d2;
          }
        }
      }
      double d = sqrt(max_d2);
      kapton_path_mm.push_back(d);
    }
  }  // s1
  return kapton_path_mm;
}
}  // namespace kapton

using namespace boost::python;
namespace kapton { namespace boost_python { namespace {

  void kapton_init_module() {
    using namespace boost::python;

    def("get_kapton_path_cpp", &kapton::get_kapton_path_cpp);
  }

}}}  // namespace kapton::boost_python::

BOOST_PYTHON_MODULE(dials_algorithms_integration_kapton_ext) {
  kapton::boost_python::kapton_init_module();
}


#include <iostream>
#include <iomanip>
#include <fstream>
#include <vector>
#include <numeric>
#include <cmath>
#include <stdexcept>
#ifdef _OPENMP
#include <omp.h>
#endif

#include "domain_partition.hpp"

#include "mba.hpp"

#include <boost/scope_exit.hpp>
#include <memory>
#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <boost/multi_array.hpp>

#if defined(SOLVER_BACKEND_VEXCL)
#  include <amgcl/backend/vexcl.hpp>
   typedef amgcl::backend::vexcl<double> Backend;
#elif defined(SOLVER_BACKEND_CUDA)
#  include <amgcl/backend/cuda.hpp>
#  include <amgcl/relaxation/cusparse_ilu0.hpp>
   typedef amgcl::backend::cuda<double> Backend;
#else
#  ifndef SOLVER_BACKEND_BUILTIN
#    define SOLVER_BACKEND_BUILTIN
#  endif
#  include <amgcl/backend/builtin.hpp>
   typedef amgcl::backend::builtin<double> Backend;
#endif

#include <amgcl/make_solver.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/solver/runtime.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/preconditioner/runtime.hpp>
#include <amgcl/mpi/direct_solver/runtime.hpp>
#include <amgcl/mpi/subdomain_deflation.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/adapter/zero_copy.hpp>
#include <amgcl/profiler.hpp>

namespace amgcl {
    profiler<> prof;
}

struct partitioned_deflation {
    unsigned nparts;
    std::vector<unsigned> domain;

    partitioned_deflation(
            boost::array<ptrdiff_t, 2> LO,
            boost::array<ptrdiff_t, 2> HI,
            unsigned nparts
            ) : nparts(nparts)
    {
        domain_partition<2> part(LO, HI, nparts);

        ptrdiff_t nx = HI[0] - LO[0] + 1;
        ptrdiff_t ny = HI[1] - LO[1] + 1;

        domain.resize(nx * ny);
        for(unsigned p = 0; p < nparts; ++p) {
            boost::array<ptrdiff_t, 2> lo = part.domain(p).min_corner();
            boost::array<ptrdiff_t, 2> hi = part.domain(p).max_corner();

            for(int j = lo[1]; j <= hi[1]; ++j) {
                for(int i = lo[0]; i <= hi[0]; ++i) {
                    domain[(j - LO[1]) * nx + (i - LO[0])] = p;
                }
            }
        }
    }

    size_t dim() const { return nparts; }

    double operator()(ptrdiff_t i, unsigned j) const {
        return domain[i] == j;
    }
};

struct linear_deflation {
    std::vector<double> x;
    std::vector<double> y;

    linear_deflation(
            ptrdiff_t chunk,
            boost::array<ptrdiff_t, 2> lo,
            boost::array<ptrdiff_t, 2> hi
            )
    {
        double hx = 1.0 / (hi[0] - lo[0]);
        double hy = 1.0 / (hi[1] - lo[1]);

        ptrdiff_t nx = hi[0] - lo[0] + 1;
        ptrdiff_t ny = hi[1] - lo[1] + 1;

        x.reserve(chunk);
        y.reserve(chunk);

        for (ptrdiff_t j = 0; j < ny; ++j) {
            for (ptrdiff_t i = 0; i < nx; ++i) {
                x.push_back(i * hx - 0.5);
                y.push_back(j * hy - 0.5);
            }
        }
    }

    size_t dim() const { return 3; }

    double operator()(ptrdiff_t i, unsigned j) const {
        switch(j) {
            default:
            case 0:
                return 1;
            case 1:
                return x[i];
            case 2:
                return y[i];
        }
    }
};

struct bilinear_deflation {
    size_t nv, chunk;
    std::vector<double> v;

    bilinear_deflation(
            ptrdiff_t n,
            ptrdiff_t chunk,
            boost::array<ptrdiff_t, 2> lo,
            boost::array<ptrdiff_t, 2> hi
            ) : nv(0), chunk(chunk)
    {
        // See which neighbors we have.
        int neib[2][2] = {
            {lo[0] > 0 || lo[1] > 0,     hi[0] + 1 < n || lo[1] > 0    },
            {lo[0] > 0 || hi[1] + 1 < n, hi[0] + 1 < n || hi[1] + 1 < n}
        };

        for(int j = 0; j < 2; ++j)
            for(int i = 0; i < 2; ++i)
                if (neib[j][i]) ++nv;

        if (nv == 0) {
            // Single MPI process?
            nv = 1;
            v.resize(chunk, 1);
            return;
        }

        v.resize(chunk * nv, 0);

        double *dv = v.data();

        ptrdiff_t nx = hi[0] - lo[0] + 1;
        ptrdiff_t ny = hi[1] - lo[1] + 1;

        double hx = 1.0 / (nx - 1);
        double hy = 1.0 / (ny - 1);

        for(int j = 0; j < 2; ++j) {
            for(int i = 0; i < 2; ++i) {
                if (!neib[j][i]) continue;

                boost::multi_array_ref<double, 2> V(dv, boost::extents[ny][nx]);

                for(ptrdiff_t jj = 0; jj < ny; ++jj) {
                    double y = jj * hy;
                    double b = std::abs((1 - j) - y);
                    for(ptrdiff_t ii = 0; ii < nx; ++ii) {
                        double x = ii * hx;

                        double a = std::abs((1 - i) - x);
                        V[jj][ii] = a * b;
                    }
                }

                dv += chunk;
            }
        }
    }

    size_t dim() const { return nv; }

    double operator()(ptrdiff_t i, unsigned j) const {
        return v[j * chunk + i];
    }
};

#ifndef SOLVER_BACKEND_CUDA
struct mba_deflation {
    size_t chunk, nv;
    std::vector<double> v;

    mba_deflation(
            ptrdiff_t n,
            ptrdiff_t chunk,
            boost::array<ptrdiff_t,2> lo,
            boost::array<ptrdiff_t,2> hi
            ) : chunk(chunk), nv(1)
    {
        // See which neighbors we have.
        int neib[2][2] = {
            {lo[0] > 0 || lo[1] > 0,     hi[0] + 1 < n || lo[1] > 0    },
            {lo[0] > 0 || hi[1] + 1 < n, hi[0] + 1 < n || hi[1] + 1 < n}
        };

        for(int j = 0; j < 2; ++j)
            for(int i = 0; i < 2; ++i)
                if (neib[j][i]) ++nv;

        v.resize(chunk * nv, 0);

        double *dv = v.data();
        std::fill(dv, dv + chunk, 1.0);
        dv += chunk;

        ptrdiff_t nx = hi[0] - lo[0] + 1;
        ptrdiff_t ny = hi[1] - lo[1] + 1;

        double hx = 1.0 / (nx - 1);
        double hy = 1.0 / (ny - 1);

        std::array<double, 2> cmin = {-0.01, -0.01};
        std::array<double, 2> cmax = { 1.01,  1.01};
        std::array<size_t, 2> grid = {3, 3};

        std::array< std::array<double, 2>, 4 > coo;
        std::array< double, 4 > val;

        for(int j = 0, idx = 0; j < 2; ++j) {
            for(int i = 0; i < 2; ++i, ++idx) {
                coo[idx][0] = i;
                coo[idx][1] = j;
            }
        }

        for(int j = 0, idx = 0; j < 2; ++j) {
            for(int i = 0; i < 2; ++i, ++idx) {
                if (!neib[j][i]) continue;

                std::fill(val.begin(), val.end(), 0.0);
                val[idx] = 1.0;

                mba::MBA<2> interp(cmin, cmax, grid, coo, val, 8, 1e-8, 0.5, zero);

                boost::multi_array_ref<double, 2> V(dv, boost::extents[ny][nx]);

                for(int jj = 0; jj < ny; ++jj)
                    for(int ii = 0; ii < nx; ++ii) {
                        std::array<double, 2> p = {ii * hx, jj * hy};
                        V[jj][ii] = interp(p);
                    }

                dv += chunk;
            }
        }
    }

    size_t dim() const { return nv; }

    double operator()(ptrdiff_t i, unsigned j) const {
        return v[j * chunk + i];
    }

    static double zero(const std::array<double, 2>&) {
        return 0;
    }
};
#endif

struct harmonic_deflation {
    size_t nv, chunk;
    std::vector<double> v;

    harmonic_deflation(
            ptrdiff_t n,
            ptrdiff_t chunk,
            boost::array<ptrdiff_t, 2> lo,
            boost::array<ptrdiff_t, 2> hi
            ) : nv(0), chunk(chunk)
    {
        // See which neighbors we have.
        int neib[2][2] = {
            {lo[0] > 0 || lo[1] > 0,     hi[0] + 1 < n || lo[1] > 0    },
            {lo[0] > 0 || hi[1] + 1 < n, hi[0] + 1 < n || hi[1] + 1 < n}
        };

        for(int j = 0; j < 2; ++j)
            for(int i = 0; i < 2; ++i)
                if (neib[j][i]) ++nv;

        if (nv == 0) {
            // Single MPI process?
            nv = 1;
            v.resize(chunk, 1);
            return;
        }

        v.resize(chunk * nv, 0);
        double *dv = v.data();

        ptrdiff_t nx = hi[0] - lo[0] + 1;
        ptrdiff_t ny = hi[1] - lo[1] + 1;

        std::vector<ptrdiff_t> ptr;
        std::vector<ptrdiff_t> col;
        std::vector<double>    val;
        std::vector<double>    rhs(chunk, 0.0);

        ptr.reserve(chunk + 1);
        col.reserve(chunk * 5);
        val.reserve(chunk * 5);

        ptr.push_back(0);

        for(int j = 0, k = 0; j < ny; ++j) {
            for(int i = 0; i < nx; ++i, ++k) {
                if (
                        (i == 0    && j == 0   ) ||
                        (i == 0    && j == ny-1) ||
                        (i == nx-1 && j == 0   ) ||
                        (i == nx-1 && j == ny-1)
                   )
                {
                    col.push_back(k);
                    val.push_back(1);
                } else {
                    col.push_back(k);
                    val.push_back(1.0);

                    if (j == 0) {
                        col.push_back(k + nx);
                        val.push_back(-0.5);
                    } else if (j == ny-1) {
                        col.push_back(k - nx);
                        val.push_back(-0.5);
                    } else {
                        col.push_back(k - nx);
                        val.push_back(-0.25);

                        col.push_back(k + nx);
                        val.push_back(-0.25);
                    }

                    if (i == 0) {
                        col.push_back(k + 1);
                        val.push_back(-0.5);
                    } else if (i == nx-1) {
                        col.push_back(k - 1);
                        val.push_back(-0.5);
                    } else {
                        col.push_back(k - 1);
                        val.push_back(-0.25);

                        col.push_back(k + 1);
                        val.push_back(-0.25);
                    }
                }

                ptr.push_back(col.size());
            }
        }

        amgcl::make_solver<
            amgcl::amg<
                amgcl::backend::builtin<double>,
                amgcl::coarsening::smoothed_aggregation,
                amgcl::relaxation::gauss_seidel
                >,
            amgcl::solver::gmres<
                amgcl::backend::builtin<double>
                >
            > solve( amgcl::adapter::zero_copy(chunk, ptr.data(), col.data(), val.data()) );

        for(int j = 0; j < 2; ++j) {
            for(int i = 0; i < 2; ++i) {
                if (!neib[j][i]) continue;

                ptrdiff_t idx = i * (nx - 1) + j * (ny - 1) * nx;
                rhs[idx] = 1.0;

                boost::iterator_range<double*> x(dv, dv + chunk);
                solve(rhs, x);

                rhs[idx] = 0.0;

                dv += chunk;
            }
        }
    }

    size_t dim() const { return nv; }

    double operator()(ptrdiff_t i, unsigned j) const {
        return v[j * chunk + i];
    }
};

struct renumbering {
    const domain_partition<2> ∂
    const std::vector<ptrdiff_t> &dom;

    renumbering(
            const domain_partition<2> &p,
            const std::vector<ptrdiff_t> &d
            ) : part(p), dom(d)
    {}

    ptrdiff_t operator()(ptrdiff_t i, ptrdiff_t j) const {
        boost::array<ptrdiff_t, 2> p = {{i, j}};
        std::pair<int,ptrdiff_t> v = part.index(p);
        return dom[v.first] + v.second;
    }
};

int main(int argc, char *argv[]) {
    int provided;
    MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
    BOOST_SCOPE_EXIT(void) {
        MPI_Finalize();
    } BOOST_SCOPE_EXIT_END

    amgcl::mpi::communicator world(MPI_COMM_WORLD);

    if (world.rank == 0)
        std::cout << "World size: " << world.size << std::endl;

    // Read configuration from command line
    ptrdiff_t n = 1024;
    std::string deflation_type = "bilinear";

    amgcl::runtime::coarsening::type  coarsening       = amgcl::runtime::coarsening::smoothed_aggregation;
    amgcl::runtime::relaxation::type  relaxation       = amgcl::runtime::relaxation::spai0;
    amgcl::runtime::solver::type      iterative_solver = amgcl::runtime::solver::bicgstabl;
    amgcl::runtime::mpi::direct::type direct_solver    = amgcl::runtime::mpi::direct::skyline_lu;

    bool just_relax = false;
    bool symm_dirichlet = true;
    std::string problem = "laplace2d";
    std::string parameter_file;
    std::string out_file;

    namespace po = boost::program_options;
    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "show help")
        (
         "problem",
         po::value<std::string>(&problem)->default_value(problem),
         "laplace2d, recirc2d"
        )
        (
         "symbc",
         po::value<bool>(&symm_dirichlet)->default_value(symm_dirichlet),
         "Use symmetric Dirichlet conditions in laplace2d"
        )
        (
         "size,n",
         po::value<ptrdiff_t>(&n)->default_value(n),
         "domain size"
        )
        (
         "coarsening,c",
         po::value<amgcl::runtime::coarsening::type>(&coarsening)->default_value(coarsening),
         "ruge_stuben, aggregation, smoothed_aggregation, smoothed_aggr_emin"
        )
        (
         "relaxation,r",
         po::value<amgcl::runtime::relaxation::type>(&relaxation)->default_value(relaxation),
         "gauss_seidel, ilu0, iluk, ilut, damped_jacobi, spai0, spai1, chebyshev"
        )
        (
         "iter_solver,i",
         po::value<amgcl::runtime::solver::type>(&iterative_solver)->default_value(iterative_solver),
         "cg, bicgstab, bicgstabl, gmres"
        )
        (
         "dir_solver,d",
         po::value<amgcl::runtime::mpi::direct::type>(&direct_solver)->default_value(direct_solver),
         "skyline_lu"
#ifdef AMGCL_HAVE_PASTIX
         ", pastix"
#endif
        )
        (
         "deflation,v",
         po::value<std::string>(&deflation_type)->default_value(deflation_type),
         "constant, partitioned, linear, bilinear, mba, harmonic"
        )
        (
         "subparts",
         po::value<int>()->default_value(16),
         "number of partitions for partitioned deflation"
        )
        (
         "params,P",
         po::value<std::string>(&parameter_file),
         "parameter file in json format"
        )
        (
         "prm,p",
         po::value< std::vector<std::string> >()->multitoken(),
         "Parameters specified as name=value pairs. "
         "May be provided multiple times. Examples:\n"
         "  -p solver.tol=1e-3\n"
         "  -p precond.coarse_enough=300"
        )
        (
         "just-relax,0",
         po::bool_switch(&just_relax),
         "Do not create AMG hierarchy, use relaxation as preconditioner"
        )
        (
         "out,o",
         po::value<std::string>(&out_file),
         "out file"
        )
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout << desc << std::endl;
        return 0;
    }

    boost::property_tree::ptree prm;
    if (vm.count("params")) read_json(parameter_file, prm);

    if (vm.count("prm")) {
        for(const std::string &v : vm["prm"].as< std::vector<std::string> >()) {
            amgcl::put(prm, v);
        }
    }

    prm.put("isolver.type", iterative_solver);
    prm.put("dsolver.type", direct_solver);

    const ptrdiff_t n2 = n * n;
    const double hinv  = (n - 1);
    const double h2i   = (n - 1) * (n - 1);
    const double h     = 1 / hinv;

    boost::array<ptrdiff_t, 2> lo = { {0, 0} };
    boost::array<ptrdiff_t, 2> hi = { {n - 1, n - 1} };

    using amgcl::prof;

    prof.tic("partition");
    domain_partition<2> part(lo, hi, world.size);
    ptrdiff_t chunk = part.size( world.rank );

    std::vector<ptrdiff_t> domain(world.size + 1);
    MPI_Allgather(
            &chunk, 1, amgcl::mpi::datatype<ptrdiff_t>(),
            &domain[1], 1, amgcl::mpi::datatype<ptrdiff_t>(), world);
    std::partial_sum(domain.begin(), domain.end(), domain.begin());

    lo = part.domain(world.rank).min_corner();
    hi = part.domain(world.rank).max_corner();
    prof.toc("partition");

    renumbering renum(part, domain);

    prof.tic("deflation");
    std::function<double(ptrdiff_t,unsigned)> dv;
    unsigned ndv = 1;

    if (deflation_type == "constant") {
        dv = amgcl::mpi::constant_deflation(1);
    } else if (deflation_type == "partitioned") {
        ndv = vm["subparts"].as<int>();
        dv  = partitioned_deflation(lo, hi, ndv);
    } else if (deflation_type == "linear") {
        ndv = 3;
        dv  = linear_deflation(chunk, lo, hi);
    } else if (deflation_type == "bilinear") {
        bilinear_deflation bld(n, chunk, lo, hi);
        ndv = bld.dim();
        dv  = bld;
#ifndef SOLVER_BACKEND_CUDA
    } else if (deflation_type == "mba") {
        mba_deflation mba(n, chunk, lo, hi);
        ndv = mba.dim();
        dv  = mba;
#endif
    } else if (deflation_type == "harmonic") {
        harmonic_deflation hd(n, chunk, lo, hi);
        ndv = hd.dim();
        dv  = hd;
    } else {
        throw std::runtime_error("Unsupported deflation type");
    }

    prm.put("num_def_vec", ndv);
    prm.put("def_vec", &dv);
    prof.toc("deflation");

    prof.tic("assemble");
    std::vector<ptrdiff_t> ptr;
    std::vector<ptrdiff_t> col;
    std::vector<double>    val;
    std::vector<double>    rhs;

    ptr.reserve(chunk + 1);
    col.reserve(chunk * 5);
    val.reserve(chunk * 5);
    rhs.reserve(chunk);

    ptr.push_back(0);

    if (problem == "recirc2d") {
        const double eps  = 1e-5;

        for(ptrdiff_t j = lo[1]; j <= hi[1]; ++j) {
            double y = h * j;
            for(ptrdiff_t i = lo[0]; i <= hi[0]; ++i) {
                double x = h * i;

                if (i == 0 || j == 0 || i + 1 == n || j + 1 == n) {
                    col.push_back(renum(i,j));
                    val.push_back(1);
                    rhs.push_back(
                            sin(M_PI * x) + sin(M_PI * y) +
                            sin(13 * M_PI * x) + sin(13 * M_PI * y)
                            );
                } else {
                    double a = -sin(M_PI * x) * cos(M_PI * y) * hinv;
                    double b =  sin(M_PI * y) * cos(M_PI * x) * hinv;

                    if (j > 0) {
                        col.push_back(renum(i,j-1));
                        val.push_back(-eps * h2i - std::max(b, 0.0));
                    }

                    if (i > 0) {
                        col.push_back(renum(i-1,j));
                        val.push_back(-eps * h2i - std::max(a, 0.0));
                    }

                    col.push_back(renum(i,j));
                    val.push_back(4 * eps * h2i + fabs(a) + fabs(b));

                    if (i + 1 < n) {
                        col.push_back(renum(i+1,j));
                        val.push_back(-eps * h2i + std::min(a, 0.0));
                    }

                    if (j + 1 < n) {
                        col.push_back(renum(i,j+1));
                        val.push_back(-eps * h2i + std::min(b, 0.0));
                    }

                    rhs.push_back(1.0);
                }
                ptr.push_back( col.size() );
            }
        }
    } else {
        for(ptrdiff_t j = lo[1]; j <= hi[1]; ++j) {
            for(ptrdiff_t i = lo[0]; i <= hi[0]; ++i) {
                if (!symm_dirichlet && (i == 0 || j == 0 || i + 1 == n || j + 1 == n)) {
                    col.push_back(renum(i,j));
                    val.push_back(1);
                    rhs.push_back(0);
                } else {
                    if (j > 0)  {
                        col.push_back(renum(i,j-1));
                        val.push_back(-h2i);
                    }

                    if (i > 0) {
                        col.push_back(renum(i-1,j));
                        val.push_back(-h2i);
                    }

                    col.push_back(renum(i,j));
                    val.push_back(4 * h2i);

                    if (i + 1 < n) {
                        col.push_back(renum(i+1,j));
                        val.push_back(-h2i);
                    }

                    if (j + 1 < n) {
                        col.push_back(renum(i,j+1));
                        val.push_back(-h2i);
                    }

                    rhs.push_back(1);
                }
                ptr.push_back( col.size() );
            }
        }
    }
    prof.toc("assemble");

    Backend::params bprm;

#if defined(SOLVER_BACKEND_VEXCL)
    vex::Context ctx(vex::Filter::Env);
    std::cout << ctx << std::endl;
    bprm.q = ctx;
#elif defined(SOLVER_BACKEND_CUDA)
    cusparseCreate(&bprm.cusparse_handle);
#endif

    auto f = Backend::copy_vector(rhs, bprm);
    auto x = Backend::create_vector(chunk, bprm);

    amgcl::backend::clear(*x);

    size_t iters;
    double resid, tm_setup, tm_solve;

    if (just_relax) {
        prm.put("local.class", "relaxation");
        prm.put("local.type", relaxation);
    } else {
        prm.put("local.coarsening.type", coarsening);
        prm.put("local.relax.type", relaxation);
    }

    prof.tic("setup");
    typedef
        amgcl::mpi::subdomain_deflation<
            amgcl::runtime::preconditioner< Backend >,
            amgcl::runtime::solver::wrapper,
            amgcl::runtime::mpi::direct::solver<double>
        > SDD;

    SDD solve(world, std::tie(chunk, ptr, col, val), prm, bprm);
    tm_setup = prof.toc("setup");

    prof.tic("solve");
    std::tie(iters, resid) = solve(*f, *x);
    tm_solve = prof.toc("solve");

    if (world.rank == 0) {
        std::cout
            << "Iterations: " << iters << std::endl
            << "Error:      " << resid << std::endl
            << prof << std::endl;

#ifdef _OPENMP
        int nt = omp_get_max_threads();
#else
        int nt = 1;
#endif
        std::ostringstream log_name;
        log_name << "log_" << n2 << "_" << nt << "_" << world.size << ".txt";
        std::ofstream log(log_name.str().c_str(), std::ios::app);
        log << n2 << "\t" << nt << "\t" << world.size
            << "\t" << tm_setup << "\t" << tm_solve
            << "\t" << iters << "\t" << std::endl;
    }

    if (!out_file.empty()) {
        std::vector<double> X(world.rank == 0 ? n2 : chunk);

#if defined(SOLVER_BACKEND_VEXCL)
        vex::copy(x->begin(), x->end(), X.begin());
#elif defined(SOLVER_BACKEND_CUDA)
        thrust::copy(x->begin(), x->end(), X.begin());
#else
        std::copy(x->data(), x->data() + chunk, X.begin());
#endif

        if (world.rank == 0) {
            for(int i = 1; i < world.size; ++i)
                MPI_Recv(&X[domain[i]], domain[i+1] - domain[i], MPI_DOUBLE, i, 42, MPI_COMM_WORLD, MPI_STATUS_IGNORE);

            std::ofstream f(out_file.c_str(), std::ios::binary);
            int m = n;
            f.write((char*)&m, sizeof(int));
            for(int j = 0; j < n; ++j) {
                for(int i = 0; i < n; ++i) {
                    double buf = X[renum(i,j)];
                    f.write((char*)&buf, sizeof(double));
                }
            }
        } else {
            MPI_Send(X.data(), chunk, MPI_DOUBLE, 0, 42, MPI_COMM_WORLD);
        }
    }
}


//=================================================================================================
// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Flight Systems and Automatic Control group,
//       TU Darmstadt, nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

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

#include <hector_pose_estimation/filter/ekf.h>
#include <hector_pose_estimation/system.h>

#include <boost/pointer_cast.hpp>

#ifdef USE_HECTOR_TIMING
  #include <hector_diagnostics/timing.h>
#endif

namespace hector_pose_estimation {
namespace filter {

EKF::EKF(State &state)
  : Filter(state)
{}

EKF::~EKF()
{}

bool EKF::init(PoseEstimation &estimator)
{
  x_diff = State::Vector(state_.getVectorDimension());
  A = State::SystemMatrix(state_.getCovarianceDimension(), state_.getCovarianceDimension());
  Q = State::Covariance(state_.getCovarianceDimension(), state_.getCovarianceDimension());
  return true;
}

bool EKF::preparePredict(double dt)
{
  x_diff.setZero();
  A.setIdentity();
  Q.setZero();
  return Filter::preparePredict(dt);
}

bool EKF::predict(const SystemPtr& system, double dt)
{
  if (!Filter::predict(system, dt)) return false;
  EKF::Predictor *predictor = boost::dynamic_pointer_cast<EKF::Predictor>(system->predictor());
  x_diff += predictor->x_diff;
  A += predictor->A;
  Q += predictor->Q;
  return true;
}

bool EKF::doPredict(double dt) {
  ROS_DEBUG_NAMED("ekf.prediction", "EKF prediction (dt = %f):", dt);

  ROS_DEBUG_STREAM_NAMED("ekf.prediction", "A      = [" << std::endl << A << "]");
  ROS_DEBUG_STREAM_NAMED("ekf.prediction", "Q      = [" << std::endl << Q << "]");

#ifdef USE_HECTOR_TIMING
  { hector_diagnostics::TimingSection section("predict.ekf.covariance");
#endif
  state().P() = A * state().P() * A.transpose() + Q;
  state().P().assertSymmetric();

#ifdef USE_HECTOR_TIMING
  }
  { hector_diagnostics::TimingSection section("predict.ekf.state");
#endif
  state().update(x_diff);

#ifdef USE_HECTOR_TIMING
  }
#endif

  ROS_DEBUG_STREAM_NAMED("ekf.prediction", "x_pred = [" << state().getVector().transpose() << "]");
  ROS_DEBUG_STREAM_NAMED("ekf.prediction", "P_pred = [" << std::endl << state().getCovariance() << "]");

  Filter::doPredict(dt);
  return true;
}

} // namespace filter
} // namespace hector_pose_estimation


#pragma once

#include <vector>
#include <algorithm>

#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_comparison.hpp>

#include "cartesian_tree.hpp"

namespace succinct {

    // XXX(ot): implement arbitrary comparator
    template <typename Vector>
    class topk_vector : boost::noncopyable {
    public:
        typedef Vector vector_type;
        typedef typename vector_type::value_type value_type;
        typedef boost::tuple<value_type, uint64_t> entry_type;
        typedef std::vector<entry_type> entry_vector_type;

        topk_vector()
        {}

        template <typename Range>
        topk_vector(Range const& v)
        {
            cartesian_tree(v, std::greater<typename boost::range_value<Range>::type>())
                .swap(m_cartesian_tree);
            vector_type(v).swap(m_v);
        }

        value_type const
        operator[](uint64_t idx) const
        {
            return m_v[idx];
        }

        uint64_t size() const
        {
            return m_v.size();
        }

        class enumerator
        {
        public:
            enumerator()
                : m_topkv(0)
            {}

            bool next()
            {
                using boost::tie;
                if (m_q.empty()) return false;

                value_type cur_mid_val;
                uint64_t cur_mid, cur_a, cur_b;

                std::pop_heap(m_q.begin(), m_q.end(), value_index_comparator());
                tie(cur_mid_val, cur_mid, cur_a, cur_b) = m_q.back();
                m_q.pop_back();

                m_cur = entry_type(cur_mid_val, cur_mid);

                if (cur_mid != cur_a) {
                    uint64_t m = m_topkv->m_cartesian_tree.rmq(cur_a, cur_mid - 1);
                    m_q.push_back(queue_element_type(m_topkv->m_v[m], m, cur_a, cur_mid - 1));
                    std::push_heap(m_q.begin(), m_q.end(), value_index_comparator());
                }

                if (cur_mid != cur_b) {
                    uint64_t m = m_topkv->m_cartesian_tree.rmq(cur_mid + 1, cur_b);
                    m_q.push_back(queue_element_type(m_topkv->m_v[m], m, cur_mid + 1, cur_b));
                    std::push_heap(m_q.begin(), m_q.end(), value_index_comparator());
                }

                return true;
            }

            entry_type const& value() const
            {
                return m_cur;
            }

            friend class topk_vector;

            void swap(enumerator& other)
            {
                using std::swap;
                swap(m_topkv, other.m_topkv);
                swap(m_q, other.m_q);
                swap(m_cur, other.m_cur);
            }

        private:

            void set(topk_vector const* topkv, uint64_t a, uint64_t b)
            {
                assert(a <= b);
                clear();
                m_topkv = topkv;

                uint64_t m = m_topkv->m_cartesian_tree.rmq(a, b);
                m_q.push_back(queue_element_type(m_topkv->m_v[m], m, a, b));
            }

            typedef boost::tuple<value_type, uint64_t, uint64_t, uint64_t> queue_element_type;

            struct value_index_comparator {
                template <typename Tuple>
                bool operator()(Tuple const& a, Tuple const& b) const
                {
                    using boost::get;
                    // lexicographic, increasing on value and decreasing
                    // on index
                    return (get<0>(a) < get<0>(b) ||
                            (get<0>(a) == get<0>(b) &&
                             get<1>(a) > get<1>(b)));
                }
            };

        public:
            void clear()
            {
                m_topkv = 0;
                m_q.clear();
            }

        private:
            topk_vector const* m_topkv;
            std::vector<queue_element_type> m_q;
            entry_type m_cur;
        };

        // NOTE this is b inclusive
        // XXX switch to [a, b) ?
        void get_topk_enumerator(uint64_t a, uint64_t b, enumerator& ret) const
        {
            ret.set(this, a, b);
        }

        enumerator get_topk_enumerator(uint64_t a, uint64_t b) const
        {
            enumerator ret;
            get_topk_enumerator(a, b, ret);
            return ret;
        }

        entry_vector_type
        topk(uint64_t a, uint64_t b, size_t k) const
        {
            entry_vector_type ret(std::min(size_t(b - a + 1), k));
            enumerator it = get_topk_enumerator(a, b);

            bool hasnext;
            for (size_t i = 0; i < ret.size(); ++i) {
                hasnext = it.next();
                assert(hasnext); (void)hasnext;
                ret[i] = it.value();
            }

            assert(ret.size() == k || !it.next());

            return ret;
        }

        template <typename Visitor>
        void map(Visitor& visit)
        {
            visit
                (m_v, "m_v")
                (m_cartesian_tree, "m_cartesian_tree");
        }

        void swap(topk_vector& other)
        {
            other.m_v.swap(m_v);
            other.m_cartesian_tree.swap(m_cartesian_tree);
        }

    protected:

        vector_type m_v;
        cartesian_tree m_cartesian_tree;
    };

}


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

/**
 * \file
 * \brief core fundamental matrix template implementations
 */

#include "fundamental_matrix.h"

#include <cmath>

#include <vital/exceptions/math.h>

#include <Eigen/SVD>

namespace kwiver {
namespace vital {

/// Construct from a provided matrix
template <typename T>
fundamental_matrix_<T>
::fundamental_matrix_( Eigen::Matrix<T,3,3> const &mat )
{
  Eigen::JacobiSVD<matrix_t> svd(mat, Eigen::ComputeFullU |
                                      Eigen::ComputeFullV);
  auto S = svd.singularValues();
  const matrix_t& U = svd.matrixU();
  const matrix_t& V = svd.matrixV();

  // clear the last singular value
  S[2] = T(0);
  S /= S.norm();
  mat_ = U*S.asDiagonal()*V.transpose();
}

/// Conversion Copy constructor -- float specialization
template <>
template <>
fundamental_matrix_<float>
::fundamental_matrix_( fundamental_matrix_<float> const &other )
  : mat_( other.mat_ )
{
}

/// Conversion Copy constructor -- double specialization
template <>
template <>
fundamental_matrix_<double>
::fundamental_matrix_( fundamental_matrix_<double> const &other )
  : mat_( other.mat_ )
{
}

/// Construct from a generic fundamental_matrix
template <typename T>
fundamental_matrix_<T>
::fundamental_matrix_( fundamental_matrix const &base )
  : mat_( base.matrix().template cast<T>() )
{
}

/// Construct from a generic fundamental_matrix -- double specialization
template <>
fundamental_matrix_<double>
::fundamental_matrix_( fundamental_matrix const &base )
  : mat_( base.matrix() )
{
}

/// Create a clone of outself as a shared pointer
template <typename T>
fundamental_matrix_sptr
fundamental_matrix_<T>
::clone() const
{
  return fundamental_matrix_sptr( new fundamental_matrix_<T>( *this ) );
}

/// Get a double-typed copy of the underlying matrix
template <typename T>
Eigen::Matrix<double,3,3>
fundamental_matrix_<T>
::matrix() const
{
  return this->mat_.template cast<double>();
}

/// Specialization for matrices with native double type
template <>
Eigen::Matrix<double,3,3>
fundamental_matrix_<double>
::matrix() const
{
  return this->mat_;
}

// ===========================================================================
// Other Functions
// ---------------------------------------------------------------------------

/// Output stream operator for \p fundamental_matrix instances
std::ostream&
operator<<( std::ostream &s, fundamental_matrix const &f )
{
  s << f.matrix();
  return s;
}

// ===========================================================================
// Template class instantiation
// ---------------------------------------------------------------------------
/// \cond DoxygenSuppress
#define INSTANTIATE_FUNDAMENTAL_MATRIX(T) \
  template class fundamental_matrix_<T>;

INSTANTIATE_FUNDAMENTAL_MATRIX(float);
INSTANTIATE_FUNDAMENTAL_MATRIX(double);
#undef INSTANTIATE_FUNDAMENTAL_MATRIX
/// \endcond

} } // end vital namespace