/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*          This file is part of the program and software framework          */
/*  CMAP-LAP --- Configurable Massively Parallel Solver for Lattice Problems */
/*                                                                           */
/*  Copyright Written by Nariaki Tateiwa <n-tateiwa@kyudai.jp>,              */
/*                       Yuji Shinano <shinano@zib.de>,                      */
/*            Copyright (C) 2021 by Zuse Institute Berlin,                   */
/*            licensed under LGPL version 3 or later.                        */
/*            Commercial licenses are available through <licenses@zib.de>    */
/*                                                                           */
/* This code is free software; you can redistribute it and/or                */
/* modify it under the terms of the GNU Lesser General Public License        */
/* as published by the Free Software Foundation; either version 3            */
/* of the License, or (at your option) any later version.                    */
/*                                                                           */
/* This program is distributed in the hope that it will be useful,           */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of            */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             */
/* GNU Lesser General Public License for more details.                       */
/*                                                                           */
/* You should have received a copy of the GNU Lesser General Public License  */
/* along with this program.  If not, see <http://www.gnu.org/licenses/>.     */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

/**@file    cmapLapParaLattice.cpp
 * @brief   Base class for Lattice.
 * @author  Nariaki Tateiwa, Yuji Shinano
 *
 *
 *
 */

/*---+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----8----+----9----+----0----+----1----+----2*/

#include "cmapLapParaLattice.h"
#include <NTL/tools.h>
#include <assert.h>
#include <float.h>
#include <utility>
#include <string>
#include <random>
#include <fstream>
#include "NTL/LLL.h"
#include "NTL/ZZ.h"
#include "NTL/matrix.h"
#include <eigen3/Eigen/Core>
#include "cmapLapParaDef.h"

namespace ParaCMapLAP
{

///
/// @brief constructor
/// @param[in] latticeBasis basis
///
template<typename BasisFloat, typename GSFloat>
void
Lattice<BasisFloat, GSFloat>::constructFromBasis(
      LatticeBasis<BasisFloat> &latticeBasis
      )
{
   n = latticeBasis.cols();
   m = latticeBasis.rows();
   basis = latticeBasis;
   mu.resize(m, m);
   B.resize(m);
   r__.resize(m);
   setGSO();
   GH = projectedGH();
   logVolume = logProjectedVolume();
   GSOType = 0;
   MLLLdelta = 0.99;
}

///
/// @brief constructor
/// @param[in] basisArray element of basis of RowMajor
/// @param[in] row   number of rows of basis
/// @param[in] col   number of columns of basis
///
template<typename BasisFloat, typename GSFloat>
void
Lattice<BasisFloat, GSFloat>::constructFromArray(
      int *basisArray,
      int row,
      int col
      )
{
   LatticeBasis<BasisFloat> inBasis;
   inBasis.resize(row, col);
   int index = 0;
   for ( int i = 0; i < row; ++i )
   {
      for ( int j = 0; j < col; ++j )
      {
         inBasis(i, j) = basisArray[index++];
      }
   }
   constructFromBasis(inBasis);
}

///
/// @brief read basis from file
/// @param[in] basisfile basis file path
///
template<typename BasisFloat, typename GSFloat>
void
Lattice<BasisFloat, GSFloat>::readFile(
      std::string basisfile
      )
{
   std::ifstream fin(basisfile);
   if( !fin.is_open() )
   {
      std::cout << "can not open input file: " << basisfile << std::endl;
      exit(0);
   }
   NTL::Mat<int> inputBasis;
   fin >> inputBasis;

   n = inputBasis.NumCols();
   m = inputBasis.NumRows();

   LatticeBasis<BasisFloat> eigenInputBasis;
   eigenInputBasis.resize(m, n);
   for( int row = 0; row < m; ++row )
   {
      for( int col = 0; col < n; ++col )
      {
         eigenInputBasis(row, col) = inputBasis[row][col];
      }
   }
   constructFromBasis(eigenInputBasis);
}

///
/// @brief Calculate GH(L') where L' is projected lattice {pi_k(b_k), ..., pi_k(b_l)}
/// @details GH(L') = (vol(L')/nu(d))**(1/d)
///          = exp( (log(vol(L')) - log(nu(d))) / d )
///          where d is dimension of projected lattice (l-k+1).
///          vol(L') = \prod_{i=k}^{l} norm(b*i)
/// @param[in] k index of basis
/// @param[in] l index of basis
///
template<typename BasisFloat, typename GSFloat>
double
Lattice<BasisFloat, GSFloat>::projectedGH(
      int k,
      int l
      )
{
   if( l < 0 ) l = m - 1;
   int d = l-k+1;
   long double __logProjectedVolume = logProjectedVolume(k, l);
   long double logUnitSphereVolume = d*std::log(M_PI)*0.5 - std::log(std::tgamma(d*0.5+1.0));
   return std::exp( (__logProjectedVolume-logUnitSphereVolume)/d );
}

///
/// @brief log volume of the projected lattice L' = L({pi_k(b_k), ..., pi_k(b_l)})
/// @return log( vol(L') ) = log( prod( norm(b*i) for k <= i <= l ) )
///                        = sum( log( norm(b*i) for k <= i <= l ) )
/// @param[in] k index of basis
/// @param[in] l index of basis
///
template<typename BasisFloat, typename GSFloat>
long double
Lattice<BasisFloat, GSFloat>::logProjectedVolume(
      int k,
      int l
      )
{
   if( l < 0 ) l = m - 1;
   long double logVolume__ = 0.0;
   for( int i = k; i <= l; ++i ){ logVolume__ += std::log( B(i) ); };
   return logVolume__ * 0.5;
}

///
/// @brief hash of basis
/// @retrn hash value
///
template<typename BasisFloat, typename GSFloat>
size_t
Lattice<BasisFloat, GSFloat>::hash(
   )
{
   size_t seed = 0;
   for( int i = 0; i < basis.size(); ++i )
   {
     BasisFloat elem = *(basis.data() + i);
     seed ^= std::hash<BasisFloat>()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
   }
   return seed;
}

///
/// @brief compute and set orthogonalised basis
/// @return
/// @details set mu to GSO coefficient matrix, and
///          set B to squared norms of GSO vectors
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::setGSO(
      int k,
      int l
      )
{
   if( l == -1 ){ l = m-1; }
   if( GSOType == 0 )
      return setGSO_CFA(k, l);
   else
      return setMGSO(k, l);
}

///
/// @brief Calculate coefficients and square norm of
///        GSO b*k,...,b*l by Cholesky decomposition
/// @param[in] k index of basis
/// @param[in] l index of basis
/// @return
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::setGSO_CFA(
      int k,
      int l
      )
{
   int i, j, jj;
   mu.middleRows(k, l-k+1).setZero();
   for( i = k; i <= l; ++i )
      mu(i, i) = 1.0;

   B.segment(k, l-k+1).setZero();
   for( i = k; i <= l; ++i )
   {
      for( j = 0; j < i; ++j )
      {
         r__.coeffRef(j) = basis.row(i).dot(basis.row(j));
         for( jj = 0; jj < j; ++jj )
         {
            r__.coeffRef(j) -= mu.coeff(j,jj) * r__.coeff(jj);
         }
         mu.coeffRef(i,j) = r__.coeff(j)/B.coeff(j);
      }
      B.coeffRef(i) = basis.row(i).squaredNorm();
      for(j=1;j<=i;++j)
      {
         B.coeffRef(i) -= mu.coeff(i,j-1) * r__.coeff(j-1);
      }
   }
   return true;
}

///
/// @brief Calculate coefficients and square norm of
///        GSO b*k,...,b*l by Modified Gram-Schmidt Orthogonalization
/// @param[in] k index of basis
/// @param[in] l index of basis
/// @note r(i, j) = <bj, b*i> / norm(b*i) ( i < j )
///       r(i, i) = norm(b*i)
///       q.col(i) = b*_i / norm(b*i)
///                = (bi - sum(<bi, b*j>b*j for 0 <= j <= i-1) ) / norm(b*i)
///                = (bi - sum(r(j, i)b*j for 0 <= j <= i-1)   ) / norm(b*i)
/// @return false if it detects error for floating else true
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::setMGSO(
      int k,
      int l
      )
{
   int i, j;
   q__.setZero(n, m);
   p__.setZero(m, m);
   mu.middleRows(k, l-k+1).setZero();
   B.segment(k, l-k+1).setZero();

   for( i = k; i <= l; ++i )
   {
      // q__.col(i) = basis.row(i).cast<GSFloat>();
      for( j = 0; j < n; ++j )
         q__.coeffRef(j, i) = basis.coeff(i, j);
      for( j = 0; j < i; ++j )
      {
         p__.coeffRef(j, i) = q__.col(j).dot(q__.col(i));
         q__.col(i) -= p__.coeff(j, i) * q__.col(j);
      }
      p__.coeffRef(i, i) = q__.col(i).norm();
      q__.col(i) /= p__.coeff(i, i);
   }

   for( i = k; i <= l; ++i )
   {
      mu.coeffRef(i, i) = 1.0;
      B.coeffRef(i) = p__.coeff(i, i) * p__.coeff(i, i);
      if( B.coeffRef(i) < LDBL_EPSILON )
      {
         std::cout << "Length error in MGSO" << std::endl;
         return false;
      }
      for( j = 0; j < i; ++j )
      {
         mu.coeffRef(i, j) = p__.coeff(j, i) / p__.coeff(j, j);
      }
   }
   return true;
}

///
/// @brief shortest norm
///
template<typename BasisFloat, typename GSFloat>
double
Lattice<BasisFloat, GSFloat>::shortestNorm(
      )
{
   return std::sqrt(B(0));
}

///
/// @brief Transform b_i so that it is close to being
///        orthogonal to {b_0, ..., b_{m-1}} with keeping GSO vector.
/// @param[in] eta 0.501 -- 0.51
/// @return
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::sizeReduce(
      double eta
      )
{
   return sizeReduce(m-1, eta);
}

///
/// @brief Transform b_i so that it is close to being
///        orthogonal to {b_0, ..., b_{i-1}} with keeping GSO vector.
/// @param[in] i 0 -- m-1
/// @param[in] eta 0.501 -- 0.51
/// @return
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::sizeReduce(
      int i,
      double eta
      )
{
   int j, q, k;
   for( j = i-1; j >= 0; j--)
   {
      if( std::abs(mu.coeff(i, j)) > eta )
      {
         q = std::round(mu.coeff(i,j));
         mu.row(i) -= q * mu.row(j);
         basis.row(i) -= q * basis.row(j);
      }
   }
   return true;
}

///
/// @brief get approximation factor
/// @param[in] norm
/// @param[in] k
/// @return norm / GH
///
template<typename BasisFloat, typename GSFloat>
double
Lattice<BasisFloat, GSFloat>::approxFactor(
      double norm,
      int k
      )
{
   assert( k >= 0 );
   if( k == 0 )
   {
      if( norm < 0 ) norm = shortestNorm();
      return norm / GH;
   }
   else
   {
      if( norm < 0 ) norm = std::sqrt(B[k]);
      return norm / projectedGH(k, m-1);
   }
}

///
/// @brief get hermite factor
/// @return norm / ( vol(L)^(1/m) ) = norm / exp( (1/m) log( vol(L) )
///                                 = exp( log(norm) - (1/m)log( vol(L) ) )
///
template<typename BasisFloat, typename GSFloat>
double
Lattice<BasisFloat, GSFloat>::hermiteFactor(
      double norm
      )
{
   if( norm < 0 ) norm = shortestNorm();
   return norm / std::exp( logVolume / m );
}

///
/// @brief get root hermite factor
/// @return ( hermiteFactor ) ^ (1/m)
///
template<typename BasisFloat, typename GSFloat>
double
Lattice<BasisFloat, GSFloat>::rootHermiteFactor(
      double norm
      )
{
   return std::pow( hermiteFactor(norm), (1.0/m) );
}

///
/// @brief get logarithm of orthogonality defect
/// @return prod( norm(bi) for 0 <= i <= m-1 ) / vol(L)
///         = exp( sum( log(norm(bi)) for 0 <= i <= m-1) - log(vol(L)) )
///
template<typename BasisFloat, typename GSFloat>
double
Lattice<BasisFloat, GSFloat>::logOrthogonalityDefect(
      )
{
   double logProdNorm = 0;
   for( int i = 0; i < m; ++i )
      logProdNorm += std::log( basis.row(i).squaredNorm() );
   return logProdNorm * 0.5 - logVolume;
}

///
/// @brief get enumeration cost of projceted lattice {pi_i(bi), ..., pi_i(bj)}, where i = begin, j = end
/// @details enumCost = 1/2 * sum( H_l  for 0 <= l <= m-1 ), where
///          H_l := nu(R, l+1) / vol(\pi_{m-1-l}(L)),
///          vol(\pi_{s}(L)) := prod( norm(b*i) for s <= i <= m-1 ) and
///          nu(R, s) is the volume of s-dimensional ball with R radius.
/// @note log(nu(R, s)) = s log(R) + log(nu(1, s)) ( because nu(R, s) = R^{s} * nu(1, s) ) and
///       log(vol(\pi_{s}(L))) = sum( log(norm(b*i)) for s <= i <= m-1 ), then we have
///       log(H_l) = (l+1) log(R) + log(nu(1, (l+1))) - sum( log(norm(b*i)) for m-1-l <= i <= m-1 )
/// @param[in] R radius; if it is negative, then R is set to shortest norm;
/// @param[in] begin
/// @param[in] end
///
template<typename BasisFloat, typename GSFloat>
long double
Lattice<BasisFloat, GSFloat>::enumCost(
      double R,
      int begin,
      int end
      )
{
   if( R < 0 ) R = std::sqrt(B(0));
   if( end < 0 ) end = m - 1;
   long double N = 0.0;
   long double __logProjectedVolume = 0.0;
   long double logSphereVolume = 0.0;
   long double logR = std::log(R);
   for( int i = m-1; i > m-1-begin ; --i ){ __logProjectedVolume += std::log(B(i)) * 0.5; }
   for( int l = begin; l <= end; ++l )
   {
      __logProjectedVolume += std::log(B(m-l-1)) * 0.5;
      logSphereVolume = (l+1) * logR + ( (l+1)*std::log(M_PI)*0.5 - std::log(std::tgamma((l+1)*0.5+1.0)) );
      N += std::exp( logSphereVolume - __logProjectedVolume );
   }
   return N * 0.5;
}

///
/// @brief slope of GSA({(i, log2(squared_norm(b*i))} for 0 <= i <= h })
/// @param[in] h index of basis; if it is negative, h is set to m
///
template<typename BasisFloat, typename GSFloat>
double
Lattice<BasisFloat, GSFloat>::slopeGSA(
      int h
      )
{
   if( h < 0 ) h = m;
   VectorB<GSFloat> x(h);
   double slope = 0.0;
   for( int i = 0; i < h; ++i ) x(i) = log2(B(i)) * 0.5;
   for( int i = 0; i < h; ++i ) slope += (2.0*i-h+1)*x(i);
   slope = (6.0*slope) / (h*(h-1)*(h+1));
   return slope;
}

///
/// @brief generate new lattice using basis[:h]
/// @param[in] h index of basis
///
template<typename BasisFloat, typename GSFloat>
Lattice<BasisFloat, GSFloat>
Lattice<BasisFloat, GSFloat>::copy(
      int h
      )
{
   if( h < 0 ) h = m - 1;
   LatticeBasis<BasisFloat> basis__ = basis.block(0, 0, h, n);
   Lattice <BasisFloat, GSFloat> copiedLattice{basis__};
   return copiedLattice;
}

///
/// @brief convert basis to NTL::mat_ZZ
/// @output NTL::mat_ZZ matrix
///
template<typename BasisFloat, typename GSFloat>
NTL::mat_ZZ
Lattice<BasisFloat, GSFloat>::toNLTMat(
      )
{
   NTL::mat_ZZ Mbasis;
   Mbasis.SetDims(m, n);
   for( int i = 0; i < m; i++ )
   {
      for( int j = 0; j < n; j++ )
      {
         Mbasis[i][j] = basis.coeff(i, j);
      }
   }
   return Mbasis;
}

///
/// @brief load basis from NTL::mat_ZZ
///
template<typename BasisFloat, typename GSFloat>
void
Lattice<BasisFloat, GSFloat>::fromNLTMat(
      NTL::mat_ZZ &Mbasis
      )
{
   // remove zero vectors
   // row from 0 to zeroIndex are zerovectors
   int zeroIndex = -1;
   bool zero = true;
   int ntlRows = Mbasis.NumRows();
   int ntlCols = Mbasis.NumCols();
   for( int i = 0; i < ntlRows; i++ )
   {
      for( int j = 0; j < ntlCols; j++ )
         if( NTL::to_int(Mbasis[i][j]) != 0 ){ zero = false; }
      if( zero == true )
         zeroIndex++;
      else
         break; // row i is not a zero vector
   }

   // resize
   ntlRows -= zeroIndex + 1;

   // check
   if( (ntlRows != m) || (ntlCols != n) )
   {
      std::cout
         << "invarid dimension, expected (" << m << ", " << n << "),"
         << "but got (" << ntlRows << ", " << ntlCols << ")"
         << std::endl;
      exit(0);
   }

   for( int i = 0; i < m; i++ )
   {
      for( int j = 0; j < n; j++ )
      {
         basis.coeffRef(i, j)
            = NTL::to_int(Mbasis[i+zeroIndex+1][j]);
      }
   }

   setGSO();
}

///
/// @brief resize of lattice basis
/// @param[in] rows number of resized basis
///
template<typename BasisFloat, typename GSFloat>
void
Lattice<BasisFloat, GSFloat>::resize(
      int rows
      )
{
   assert( rows <= m );
   if( rows == m ){ return; }
   m = rows;
   // error occures when directory resize by basis = basis.topRows(m)
   LatticeBasis<BasisFloat> __basis = basis;
   MatrixMu<GSFloat>       __mu     = mu;
   VectorB<GSFloat>        __B      = B;
   basis = __basis.topRows(m);
   mu    = __mu.topLeftCorner(m, m);
   B     = __B.head(m);
   // GH = projectedGH();
   // logVolume = logProjectedVolume();
}

///
/// @brief randomize basis[begin:end] and execute reduction
/// @param[in] seed        seed of unimodular randomization
/// @param[in] begin       index of basis
/// @param[in] end         index of basis
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::randomize(
      int seed,
      int begin,
      int end
      )
{
   assert( end == -1 || begin <= end );
   if( begin == end ){ return true; }
   if( end < 0 ) end = m - 1;
   int u = end - begin + 1;

   ///
   /// generate unimodular matrix U
   ///
   LatticeBasis<BasisFloat> U(u, u);
   U.setIdentity();
   std::uniform_int_distribution<int> uniform{0, u-1};
   std::mt19937 mt(seed);  ///< randomize generartor

   // 1. permute rows
   int i, a, b, nIter = 4 * u, scale = 3;
   for( i = 0; i < nIter; ++i )
   {
      a = uniform(mt);
      b = a;
      while( b == a ){ b = uniform(mt); }
      U.row(a).swap(U.row(b));
   }

   // 2. triangular transformation matrix with coefficnets in -1, 0, 1
   std::uniform_int_distribution<int> flag{0, 1};
   for( a = 0; a < u - 2; ++a )
   {
      std::uniform_int_distribution<int> uniform_b{a+1, u-1};
      for( i = 0; i < scale; ++i )
      {
         b = uniform_b(mt);
         if( flag(mt) ){ U.row(a) += U.row(b); }
         else          { U.row(a) -= U.row(b); }
      }
   }

   // unimodular conversion
   basis.block(end-u+1,0,u,n) = U * basis.block(end-u+1,0,u,n);
   setGSO();
   return true;
}

///
/// @brief MLLL for part of basis matrix {b_j,...,b_k-1,v,b_k,...,b_l}
/// @param[in] v inserted vector
/// @param[in] j index of basis
/// @param[in] k index of basis
/// @param[in] l index of basis
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::insertMlll(
      LatticeVector<BasisFloat> &v,
      int j,
      int k,
      int l
      )
{
   int i, ii;
   NTL::mat_ZZ Mbasis;
   Mbasis.SetDims(l-j+2, n);
   for( i = j; i < k; i++ )
      for( ii = 0; ii < n; ++ii ){ Mbasis[i-j][ii] = basis(i,ii); }
   for( ii = 0; ii < n; ++ii )   { Mbasis[k-j][ii] = v(ii); }
   for( i = k; i <= l; i++ )
      for( ii = 0; ii < n; ++ii ){ Mbasis[i+1-j][ii] = basis(i,ii); }

   NTL::LLL_FP(Mbasis,MLLLdelta,0,0,0);

   // Mbasis[0] is zero vector
   for( i = j; i <= l; i++ )
      for( ii = 0; ii < n; ++ii ){ basis(i,ii) = NTL::to_int(Mbasis[i+1-j][ii]); }

   setGSO();
   return true;
}

///
/// @brief merge other latice
/// @param[in] other merged lattice
/// @remark runningTime and mergeTime are updated in this function
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::merge(
      Lattice<BasisFloat, GSFloat> &other
      )
{
   assert( n == other.n );
   VectorB<GSFloat> preSqnorm = B;
   LatticeVector<BasisFloat> v;
   for( int i = 0; i < std::min(m, other.m); i++ )
   {
      if( basis.row(i) != other.basis.row(i) )
      {
         v = other.basis.row(i);
         insertMlll(v, 0, 0, m-1);
      }
   }
   if( B != preSqnorm )
      return true;
   else
      return false;
}

///
/// @brief compair two lattice-basis
/// @details two basis are compared by lexicographic order of (B(0), ..., B(m-1))
/// @param[in] other lattice
/// @return bool of ( self <= other )
/// @note If the number of rows of basis is different, the comparison is done according to the smaller number of rows.
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::isMoreReducedThan(
      Lattice &other
      )
{
   int h = std::min(m, other.m);
   for( int i = 0; i < h; ++i )
   {
      if( B(i) < other.B(i) - 1.0 )
      {
         return true;
      }
      if( B(i) > other.B(i) )
         return false;
   }
   return false;
}

///
/// @brief compair two lattice-basis
/// @details two basis are compared by lexicographic order of (B(0), ..., B(m-1))
/// @param[in] other lattice
/// @param[out] index minimum index {i; B(i) < C(i)} where C is other lattice basis
/// @return bool of ( self <= other )
/// @note If the number of rows of basis is different, the comparison is done according to the smaller number of rows.
///
template<typename BasisFloat, typename GSFloat>
bool
Lattice<BasisFloat, GSFloat>::isMoreReducedThan(
      Lattice &other,
      int &index
      )
{
   int h = std::min(m, other.m);
   for( int i = 0; i < h; ++i )
   {
      if( B(i) < other.B(i) - 1.0 )
      {
         index = i;
         return true;
      }
      if( B(i) > other.B(i) )
         return false;
   }
   return false;
}

///
/// @brief output basis
/// @param[in] os
///
template<typename BasisFloat, typename GSFloat>
void
Lattice<BasisFloat, GSFloat>::outputBasis(
      std::ostream *os
      )
{
   *os << toNLTMat();
}

///
/// @brief write basis into textfile
/// @param[in] filename write file path
///
template<typename BasisFloat, typename GSFloat>
void
Lattice<BasisFloat, GSFloat>::writeBasis(
      std::string filename
      )
{
   std::ofstream basefile(filename);
   outputBasis(&basefile);
}

///
/// @brief stringfy lattice
///
template<typename BasisFloat, typename GSFloat>
std::string
Lattice<BasisFloat, GSFloat>::toSimpleString(
      )
{
   std::ostringstream s;
   s << std::endl
     << "Shortest vector      : "
     << basis.row(0)          << std::endl
     << "Shortest norm        : "
     << shortestNorm()        << std::endl
     << "Approximate factor   : "
     << approxFactor()        << std::endl
     << "Hermite factor       : "
     << hermiteFactor()       << std::endl
     << "Root Hermite factor  : "
     << rootHermiteFactor()   << std::endl
     << "Gaussian Heuristics  : "
     << GH                    << std::endl;
   return s.str();
}

///
/// instantiation
///
template class Lattice<int, double>;
template class Lattice<int, long double>;
template class Lattice<long int, double>;
template class Lattice<long int, long double>;

}  // namespace ParaCMapLAP


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/hierarchy.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( divides_
                          , (typename T)
                          , bd::cpu_
                          , bd::scalar_<bd::unspecified_<T>>
                          , bd::scalar_<bd::unspecified_<T>>
                          )
  {
    BOOST_FORCEINLINE auto operator()(T const& a, T const& b) const BOOST_NOEXCEPT -> decltype(a/b)
    {
      return a/b;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( divides_
                          , (typename T)
                          ,  bd::cpu_
                          ,  bd::scalar_< bd::fundamental_<T>>
                          ,  bd::scalar_< bd::fundamental_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T a, T b) const BOOST_NOEXCEPT
    {
      return a/b;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( divides_
                          , (typename T)
                          , bd::cpu_
                          , boost::simd::fast_tag
                          , bd::scalar_<bd::unspecified_<T>>
                          , bd::scalar_<bd::unspecified_<T>>
                          )
  {
     BOOST_FORCEINLINE T operator()(const fast_tag &, T  a, T  b
                                  ) const BOOST_NOEXCEPT
    {
      return divides(a, b);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( divides_
                          , (typename T)
                          , bd::cpu_
                          , boost::simd::fast_tag
                          , bd::scalar_<bd::floating_<T>>
                          , bd::scalar_<bd::floating_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(const fast_tag &, T  a, T  b
                                  ) const BOOST_NOEXCEPT
    {
      return a*fast_(rec)(b);
    }
  };
   BOOST_DISPATCH_OVERLOAD ( divides_
                          , (typename T)
                          ,  bd::cpu_
                          ,  bs::saturated_tag
                          ,  bd::scalar_<bd::fundamental_<T>>
                          ,  bd::scalar_<bd::fundamental_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(const saturated_tag &, T a, T b) const BOOST_NOEXCEPT
    {
      return saturated_(divides)(a, b);
    }
  };
} } }

#include <boost/simd/arch/common/scalar/function/divides_s.hpp>

#endif


// Copyright (C) 2005-2006 Matthias Troyer

// Use, modification and distribution is 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)

// An example of a parallel Monte Carlo simulation using some nodes to produce
// data and others to aggregate the data
#include <iostream>

#include <boost/mpicxx.hpp>
#include <boost/random/parallel.hpp>
#include <boost/random.hpp>
#include <boost/foreach.hpp>
#include <iostream>
#include <cstdlib>

namespace mpi = boost::mpicxx;

enum {sample_tag, sample_skeleton_tag, sample_broadcast_tag, quit_tag};

void calculate_samples(int sample_length)
{
  int num_samples = 100;
  std::vector<double> sample(sample_length);

  // setup communicator by splitting

  mpi::communicator world;
  mpi::communicator calculate_communicator = world.split(0);

  unsigned int num_calculate_ranks = calculate_communicator.size();

  // the master of the accumulaion ranks is the first of them, hence
  // with a rank just one after the last calculation rank
  int master_accumulate_rank = num_calculate_ranks;

  // the master of the calculation ranks sends the skeleton of the sample
  // to the master of the accumulation ranks

  if (world.rank()==0)
    world.send(master_accumulate_rank,sample_skeleton_tag,mpi::skeleton(sample));

  // next we extract the content of the sample vector, to be used in sending
  // the content later on

  mpi::content sample_content = mpi::get_content(sample);

  // now intialize the parallel random number generator

  boost::lcg64 engine(
        boost::random::stream_number = calculate_communicator.rank(),
        boost::random::total_streams = calculate_communicator.size()
      );

  boost::variate_generator<boost::lcg64&,boost::uniform_real<> >
    rng(engine,boost::uniform_real<>());

  for (unsigned int i=0; i<num_samples/num_calculate_ranks+1;++i) {

    // calculate sample by filling the vector with random numbers
    // note that std::generate will not work since it takes the generator
    // by value, and boost::ref cannot be used as a generator.
    // boost::ref should be fixed so that it can be used as generator

    BOOST_FOREACH(double& x, sample)
      x = rng();

    // send sample to accumulation ranks
    // Ideally we want to do this as a broadcast with an inter-communicator
    // between the calculation and accumulation ranks. MPI2 should support
    // this, but here we present an MPI1 compatible solution.

    // send content of sample to first (master) accumulation process

    world.send(master_accumulate_rank,sample_tag,sample_content);

    // gather some results from all calculation ranks

    double local_result = sample[0];
    std::vector<double> gathered_results(calculate_communicator.size());
    mpi::all_gather(calculate_communicator,local_result,gathered_results);
  }

  // we are done: the master tells the accumulation ranks to quit
  if (world.rank()==0)
    world.send(master_accumulate_rank,quit_tag);
}

void accumulate_samples()
{
  std::vector<double> sample;

  // setup the communicator for all accumulation ranks by splitting

  mpi::communicator world;
  mpi::communicator accumulate_communicator = world.split(1);

  bool is_master_accumulate_rank = accumulate_communicator.rank()==0;

  // the master receives the sample skeleton

  if (is_master_accumulate_rank)
    world.recv(0,sample_skeleton_tag,mpi::skeleton(sample));

  // and broadcasts it to all accumulation ranks
  mpi::broadcast(accumulate_communicator,mpi::skeleton(sample),0);

  // next we extract the content of the sample vector, to be used in receiving
  // the content later on

  mpi::content sample_content = mpi::get_content(sample);

  // accumulate until quit is called
  double sum=0.;
  while (true) {

    // the accumulation master checks whether we should quit
    if (world.iprobe(0,quit_tag)) {
      world.recv(0,quit_tag);
      for (int i=1; i<accumulate_communicator.size();++i)
        accumulate_communicator.send(i,quit_tag);
      std::cout << sum << "\n";
      break; // We're done
    }

    // the otehr accumulation ranks check whether we should quit
    if (accumulate_communicator.iprobe(0,quit_tag)) {
      accumulate_communicator.recv(0,quit_tag);
      std::cout << sum << "\n";
      break; // We're done
    }

    // check whether the master accumulation rank has received a sample
    if (world.iprobe(mpi::any_source,sample_tag)) {
      BOOST_ASSERT(is_master_accumulate_rank);

      // receive the content
      world.recv(mpi::any_source,sample_tag,sample_content);

      // now we need to braodcast
      // the problam is we do not have a non-blocking broadcast that we could
      // abort if we receive a quit message from the master. We thus need to
      // first tell all accumulation ranks to start a broadcast. If the sample
      // is small, we could just send the sample in this message, but here we
      // optimize the code for large samples, so that the overhead of these
      // sends can be ignored, and we count on an optimized broadcast
      // implementation with O(log N) complexity

      for (int i=1; i<accumulate_communicator.size();++i)
        accumulate_communicator.send(i,sample_broadcast_tag);

      // now broadcast the contents of the sample to all accumulate ranks
      mpi::broadcast(accumulate_communicator,sample_content,0);

      // and handle the sample by summing the appropriate value
      sum += sample[0];
    }

    // the other accumulation ranks wait for a mesage to start the broadcast
    if (accumulate_communicator.iprobe(0,sample_broadcast_tag)) {
      BOOST_ASSERT(!is_master_accumulate_rank);

      accumulate_communicator.recv(0,sample_broadcast_tag);

      // receive broadcast of the sample contents
      mpi::broadcast(accumulate_communicator,sample_content,0);

      // and handle the sample

      // and handle the sample by summing the appropriate value
      sum += sample[accumulate_communicator.rank()];
    }
  }
}

int main(int argc, char** argv)
{
  mpi::environment env(argc, argv);
  mpi::communicator world;

  // half of the processes generate, the others accumulate
  // the sample size is just the number of accumulation ranks
  if (world.rank() < world.size()/2)
    calculate_samples(world.size()-world.size()/2);
  else
    accumulate_samples();

  return 0;
}


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

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/maybe.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_ASSERT(lift<List>('x') == list('x'));
    BOOST_HANA_CONSTEXPR_ASSERT(lift<Maybe>('x') == just('x'));
    //! [main]
}


//Note that according to the python documentation,
//the Python.h header file should be included first, before any other header file.
#include <Python.h>
#include <iostream>
#include <dlib/opencv.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <fstream>
#include <unistd.h>
#include <cmath>
#include <stdio.h>
#include <time.h>

//Intrisics can be calculated using opencv sample code under opencv/sources/samples/cpp/tutorial_code/calib3d
//Normally, you can also apprximate fx and fy by image width, cx by half image width, cy by half image height instead
double K[9] = {7.3530833553043510e+02, 0.0, 320.0, 0.0, 7.3530833553043510e+02, 240.0, 0.0, 0.0, 1.0};
double D[5] = {-2.3528667558034226e-02, 1.3301431879108856e+00, 0.0, 0.0,
    -6.0786673300480434e+00};
const int debounce_camera_time_delay = 1;
const double centimeter_to_inch_conversion = 1/2.54;

int main(int argc, char *argv[]){

  //The python documentation explains much of the python code (see "Extending and Embedding
  //Python")
    wchar_t *program = Py_DecodeLocale(argv[0], NULL);
    if (program == NULL) {
        fprintf(stderr, "Fatal error: cannot decode argv[0]\n");
        exit(1);
    }
    Py_SetProgramName(program);  /* optional but recommended */
    Py_Initialize();
    //Make the serial connection via the Python interpreter
    PyRun_SimpleString("import serial\n");
    PyRun_SimpleString("ser = serial.Serial('/dev/ttyACM0', 9600, timeout=5)");
    cv::VideoCapture cap(1);
    if (!cap.isOpened())
        {
        std::cout << "Unable to connect to camera" << std::endl;
        return EXIT_FAILURE;
        }
    //Load face detection and pose estimation models (dlib).
    dlib::frontal_face_detector detector;
    dlib::shape_predictor predictor;
    detector = dlib::get_frontal_face_detector();
    dlib::deserialize("../data/face_model_68_points.dat") >> predictor;
     //fill in cam intrinsics and distortion coefficients
    cv::Mat cam_matrix = cv::Mat(3, 3, CV_64FC1, K);
    cv::Mat dist_coeffs = cv::Mat(5, 1, CV_64FC1, D);
    //fill in 3D ref points(world coordinates), model referenced from http://aifi.isr.uc.pt/Downloads/OpenGL/glAnthropometric3DModel.cpp
    std::vector<cv::Point3d> object_pts;

    object_pts.push_back(cv::Point3d(6.825897, 6.760612, 4.402142));     //#33 left brow left corner
    object_pts.push_back(cv::Point3d(1.330353, 7.122144, 6.903745));     //#29 left brow right corner
    object_pts.push_back(cv::Point3d(-1.330353, 7.122144, 6.903745));    //#34 right brow left corner
    object_pts.push_back(cv::Point3d(-6.825897, 6.760612, 4.402142));    //#38 right brow right corner
    object_pts.push_back(cv::Point3d(5.311432, 5.485328, 3.987654));     //#13 left eye left corner
    object_pts.push_back(cv::Point3d(1.789930, 5.393625, 4.413414));     //#17 left eye right corner
    object_pts.push_back(cv::Point3d(-1.789930, 5.393625, 4.413414));    //#25 right eye left corner
    object_pts.push_back(cv::Point3d(-5.311432, 5.485328, 3.987654));    //#21 right eye right corner
    object_pts.push_back(cv::Point3d(2.005628, 1.409845, 6.165652));     //#55 nose left corner
    object_pts.push_back(cv::Point3d(-2.005628, 1.409845, 6.165652));    //#49 nose right corner
    object_pts.push_back(cv::Point3d(2.774015, -2.080775, 5.048531));    //#43 mouth left corner
    object_pts.push_back(cv::Point3d(-2.774015, -2.080775, 5.048531));   //#39 mouth right corner
    object_pts.push_back(cv::Point3d(0.000000, -3.116408, 6.097667));    //#45 mouth central bottom corner
    object_pts.push_back(cv::Point3d(0.000000, -7.415691, 4.070434));    //#6 chin corner

    //2D ref points(image coordinates), referenced from detected facial feature
    std::vector<cv::Point2d> image_pts;

    //result
    cv::Mat rotation_vec;                           //3 x 1
    cv::Mat rotation_mat;                           //3 x 3 R
    cv::Mat translation_vec;                        //3 x 1 T
    cv::Mat pose_mat = cv::Mat(3, 4, CV_64FC1);     //3 x 4 R | T
    cv::Mat euler_angle = cv::Mat(3, 1, CV_64FC1);

    //reproject 3D points world coordinate axis to verify result pose
    std::vector<cv::Point3d> reprojectsrc;
    reprojectsrc.push_back(cv::Point3d(10.0, 10.0, 10.0));
    reprojectsrc.push_back(cv::Point3d(10.0, 10.0, -10.0));
    reprojectsrc.push_back(cv::Point3d(10.0, -10.0, -10.0));
    reprojectsrc.push_back(cv::Point3d(10.0, -10.0, 10.0));
    reprojectsrc.push_back(cv::Point3d(-10.0, 10.0, 10.0));
    reprojectsrc.push_back(cv::Point3d(-10.0, 10.0, -10.0));
    reprojectsrc.push_back(cv::Point3d(-10.0, -10.0, -10.0));
    reprojectsrc.push_back(cv::Point3d(-10.0, -10.0, 10.0));

    //reprojected 2D points
    std::vector<cv::Point2d> reprojectdst;
    reprojectdst.resize(8);

    //temp buf for decomposeProjectionMatrix()
    cv::Mat out_intrinsics = cv::Mat(3, 3, CV_64FC1);
    cv::Mat out_rotation = cv::Mat(3, 3, CV_64FC1);
    cv::Mat out_translation = cv::Mat(3, 1, CV_64FC1);

    //text on screen
    std::ostringstream outtext;
    bool still_roll = false;
    bool still_pitch = false;
    bool still_yaw = false;
    bool still_y = false;
    bool still_x = false;
    time_t time_since_still_roll = 0;
    time_t time_since_still_pitch = 0;
    time_t time_since_still_yaw = 0;
    time_t time_since_still_y = 0;
    time_t time_since_still_x = 0;
    //Loop until the escape key is pressed.
    while (1)
    {
      //usleep(500000);
        // Grab a frame
        cv::Mat temp;
        cap >> temp;

        dlib::cv_image<dlib::bgr_pixel> cimg(temp);

        // Detect faces
        std::vector<dlib::rectangle> faces = detector(cimg);

        // Find the pose of each face
        if (faces.size() > 0)
            {
            //std::cout << "DETECTED " << faces.size() << std::endl << std::endl;
            //track features
            dlib::full_object_detection shape = predictor(cimg, faces[0]);

            //draw features
            for (unsigned int i = 0; i < 68; ++i)
                {
                cv::circle(temp, cv::Point(shape.part(i).x(), shape.part(i).y()), 2, cv::Scalar(0, 0, 255), -1);
                }

            //fill in 2D ref points, annotations follow https://ibug.doc.ic.ac.uk/resources/300-W/

            image_pts.push_back(cv::Point2d(shape.part(17).x(), shape.part(17).y())); //#17 left brow left corner
            image_pts.push_back(cv::Point2d(shape.part(21).x(), shape.part(21).y())); //#21 left brow right corner
            image_pts.push_back(cv::Point2d(shape.part(22).x(), shape.part(22).y())); //#22 right brow left corner
            image_pts.push_back(cv::Point2d(shape.part(26).x(), shape.part(26).y())); //#26 right brow right corner
            image_pts.push_back(cv::Point2d(shape.part(36).x(), shape.part(36).y())); //#36 left eye left corner
            image_pts.push_back(cv::Point2d(shape.part(39).x(), shape.part(39).y())); //#39 left eye right corner
            image_pts.push_back(cv::Point2d(shape.part(42).x(), shape.part(42).y())); //#42 right eye left corner
            image_pts.push_back(cv::Point2d(shape.part(45).x(), shape.part(45).y())); //#45 right eye right corner
            image_pts.push_back(cv::Point2d(shape.part(31).x(), shape.part(31).y())); //#31 nose left corner
            image_pts.push_back(cv::Point2d(shape.part(35).x(), shape.part(35).y())); //#35 nose right corner
            image_pts.push_back(cv::Point2d(shape.part(48).x(), shape.part(48).y())); //#48 mouth left corner
            image_pts.push_back(cv::Point2d(shape.part(54).x(), shape.part(54).y())); //#54 mouth right corner
            image_pts.push_back(cv::Point2d(shape.part(57).x(), shape.part(57).y())); //#57 mouth central bottom corner
            image_pts.push_back(cv::Point2d(shape.part(8).x(), shape.part(8).y()));   //#8 chin corner

            //calc pose
            cv::solvePnP(object_pts, image_pts, cam_matrix, dist_coeffs, rotation_vec, translation_vec);

            //reproject
            cv::projectPoints(reprojectsrc, rotation_vec, translation_vec, cam_matrix, dist_coeffs, reprojectdst);

            //draw axis
            cv::line(temp, reprojectdst[0], reprojectdst[1], cv::Scalar(0, 0, 255));
            cv::line(temp, reprojectdst[1], reprojectdst[2], cv::Scalar(0, 0, 255));
            cv::line(temp, reprojectdst[2], reprojectdst[3], cv::Scalar(0, 0, 255));
            cv::line(temp, reprojectdst[3], reprojectdst[0], cv::Scalar(0, 0, 255));
            cv::line(temp, reprojectdst[4], reprojectdst[5], cv::Scalar(0, 0, 255));
            cv::line(temp, reprojectdst[5], reprojectdst[6], cv::Scalar(0, 0, 255));
            cv::line(temp, reprojectdst[6], reprojectdst[7], cv::Scalar(0, 0, 255));
            cv::line(temp, reprojectdst[7], reprojectdst[4], cv::Scalar(0, 0, 255));
            cv::line(temp, reprojectdst[0], reprojectdst[4], cv::Scalar(0, 0, 255));
            cv::line(temp, reprojectdst[1], reprojectdst[5], cv::Scalar(0, 0, 255));
            cv::line(temp, reprojectdst[2], reprojectdst[6], cv::Scalar(0, 0, 255));
            cv::line(temp, reprojectdst[3], reprojectdst[7], cv::Scalar(0, 0, 255));

            //This line of code finds the point that is located at the center of the face that
            //is being detected.
            cv::Point2d my_point = ((reprojectdst[0] + reprojectdst[1] + reprojectdst[2] + reprojectdst[3] + reprojectdst[4] + reprojectdst[5] + reprojectdst[6] + reprojectdst[7])/8);
            //Get the x and y coordinates of the point at the center of the face that is being
            //detected.
            double my_x = my_point.x;
            double my_y = my_point.y;
            double x_for_pose = my_x/temp.cols;
            double y_for_pose = my_y/temp.rows;
            //std::cout << x_for_pose << std::endl;
            //std::cout << y_for_pose << std::endl;
            x_for_pose = x_for_pose -0.5;
            y_for_pose = y_for_pose -0.5;
            double x_distance = -translation_vec.at<double>(1,1)*atan(27.6*3.14159265/180)*x_for_pose/0.5;
            double y_distance = translation_vec.at<double>(1,1)*atan(20*3.14159265/180)*y_for_pose/0.5;

            //calc euler angle
            cv::Rodrigues(rotation_vec, rotation_mat);
            cv::hconcat(rotation_mat, translation_vec, pose_mat);
            cv::decomposeProjectionMatrix(pose_mat, out_intrinsics, out_rotation, out_translation, cv::noArray(), cv::noArray(), cv::noArray(), euler_angle);

                          //show angle result
            //outtext << "X: " << std::setprecision(3) << euler_angle.at<double>(0);
            double z_pos = -translation_vec.at<double>(1, 1)*centimeter_to_inch_conversion;
            outtext << "Distance from camera [in]: " << std::setprecision(3) << z_pos;
            cv::putText(temp, outtext.str(), cv::Point(50, 40), cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 0));
            outtext.str("");
            double x_pos = x_distance*centimeter_to_inch_conversion;
            outtext << "x position [in]: " << std::setprecision(3) << x_pos;
            //outtext << "Y: " << std::setprecision(3) << euler_angle.at<double>(1);
            cv::putText(temp, outtext.str(), cv::Point(50, 60), cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 0));
            outtext.str("");
            //outtext << "Z: " << std::setprecision(3) << euler_angle.at<double>(2);
            double y_pos = y_distance*centimeter_to_inch_conversion;
            outtext << "y position [in]: " << std::setprecision(3) << y_pos;
            cv::putText(temp, outtext.str(), cv::Point(50, 80), cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 0));
            outtext.str("");
            //outtext << "rot_vec 1: " << std::setprecision(3) << rotation_vec.at<double>(1, 1);
            double pitch = euler_angle.at<double>(0);
            outtext << "Pitch in degrees: " << std::setprecision(3) << pitch;
            cv::putText(temp, outtext.str(), cv::Point(50, 100), cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 0));
            outtext.str("");
            //outtext << "rot_vec 2: " << std::setprecision(3) << rotation_vec.at<double>(2, 1);
            double yaw = euler_angle.at<double>(1);
            outtext << "Yaw in degrees: " << std::setprecision(3) << yaw;
            cv::putText(temp, outtext.str(), cv::Point(50, 120), cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 0));
            outtext.str("");
            //outtext << "rot_vec 3: " << std::setprecision(3) << rotation_vec.at<double>(3, 1);
            double roll = euler_angle.at<double>(2);
            outtext << "Roll in degrees: " << std::setprecision(3) << roll;
            cv::putText(temp, outtext.str(), cv::Point(50, 140), cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 0));
            outtext.str("");
            char buffer[100];
            ////////////////////////// roll /////////////////////////////////// #
            if(still_roll)
            {
              time_since_still_roll = time(NULL);
            }
            if(abs(roll) < 5)
            {
              roll = 0.0;
              still_roll = true;
            }
            else
            {
              still_roll = false;
            }
            if(time(NULL) < time_since_still_roll + debounce_camera_time_delay)
            {
              roll = 0.0;
            }
            sprintf(buffer,"ser.write('#%+03d\\x00'.encode())", (int)(roll*-2));
            PyRun_SimpleString(buffer);
            PyRun_SimpleString("ser.flush()");

            ///////////////////////// pitch ////////////////////////////////// &
            if(still_pitch)
            {
              time_since_still_pitch = time(NULL);
            }
            if(abs(pitch) < 5)
            {
              pitch = 0.0;
              still_pitch = true;
            }
            else
            {
              still_pitch = false;
            }
            if(time(NULL) < time_since_still_pitch + debounce_camera_time_delay)
            {
              pitch = 0.0;
            }
            sprintf(buffer,"ser.write('&%+03d\\x00'.encode())", (int)(pitch*2));

            PyRun_SimpleString(buffer);
            PyRun_SimpleString("ser.flush()");

            outtext << buffer;
            cv::putText(temp, outtext.str(), cv::Point(100, 200), cv::FONT_HERSHEY_SIMPLEX, 1.5, cv::Scalar(255, 255, 255), 3);
            outtext.str("");

            ///////////////////////// yaw ///////////////////////////////////// *
            if(still_yaw)
            {
              time_since_still_yaw = time(NULL);
            }
            if(abs(yaw) < 5)
            {
              yaw = 0.0;
              still_yaw = true;
            }
            else
            {
              still_yaw = false;
            }
            if(time(NULL) < time_since_still_yaw + debounce_camera_time_delay)
            {
              yaw = 0.0;
            }
            sprintf(buffer,"ser.write('*%+03d\\x00'.encode())", (int)(yaw*-2));
            PyRun_SimpleString(buffer);
            PyRun_SimpleString("ser.flush()");

            ///////////////////////// y /////////////////////////////////////// $
            if(still_y)
            {
              time_since_still_y = time(NULL);
            }
            if(abs(y_pos) <= 3)
            {
              y_pos = 0;
              still_y = true;
            }
            else
            {
              still_y = false;
            }
            if(time(NULL) < time_since_still_y + debounce_camera_time_delay)
            {
              y_pos = 0.0;
            }

            sprintf(buffer,"ser.write('$%+03d\\x00'.encode())", (int)(y_pos*-2));
            PyRun_SimpleString(buffer);
            PyRun_SimpleString("ser.flush()");

            /////////////////////// x //////////////////////////////////////// ^
            if(still_x)
            {
              time_since_still_x = time(NULL);
            }
            if(abs(x_pos) <= 3)
            {
              x_pos = 0;
              still_x = true;
            }
            else
            {
              still_x = false;
            }
            if(time(NULL) < time_since_still_x + debounce_camera_time_delay)
            {
              x_pos = 0.0;
            }

            sprintf(buffer,"ser.write('^%+03d\\x00'.encode())", (int)(-x_pos));
            PyRun_SimpleString(buffer);
            PyRun_SimpleString("ser.flush()");

            ////////////////// clear image points ////////////////
            image_pts.clear();
            }
 //press esc to end
        cv::namedWindow( "demo", cv::WINDOW_NORMAL);
        cv::imshow("demo", temp);
        unsigned char key = cv::waitKey(1);
        if (key == 27)
            {
            break;
            }
    }
    PyMem_RawFree(program);
    return 0;
}


/* Copyright (C) 2010 Ion Torrent Systems, Inc. All Rights Reserved */

#include "BkgFitMatrixPacker.h"
#include "BkgFitMatDat.h"
#include <armadillo>
#include <MathUtil.h>
#include <DotProduct.h>

using namespace arma;

BkgFitMatrixPacker::~BkgFitMatrixPacker()
{
  for (int i=0;i<nInstr;i++)
  {
    delete [] instList[i].si;
  }

  delete [] instList;
  delete data;
}

BkgFitMatrixPacker:: BkgFitMatrixPacker (int imgLen,fit_instructions &fi,PartialDeriv_comp_list_item *PartialDeriv_list,int PartialDeriv_list_len, int flow_block_size)
    : my_fit_instructions (fi)
{
  nInstr = fi.input_len;
  instList = new mat_assembly_instruction[nInstr];
  this->PartialDeriv_list = PartialDeriv_list;
  nPartialDeriv = PartialDeriv_list_len;

  // the output_len is the total number of fitted parameters, and it is equal to the
  // dimensions of the jtj and rhs matricies
  // jtj  =  (Mat<double>*) new Mat<double>(fi.output_len, fi.output_len);
  // rhs = new Col<double>;
  // delta = new Col<double>
  data = new BkgFitMatDat();
  data->rhs->set_size (fi.output_len);
  data->delta->set_size (fi.output_len);
  numException = 0;
  (*data->jtj).zeros (fi.output_len, fi.output_len);

  PartialDeriv_mask = 0;

  for (int i=0;i<fi.input_len;i++)
  {
    instList[i].si = NULL;
    CreateInstrFromInputLine (instList+i,&fi.input[i],imgLen, flow_block_size);
    PartialDeriv_mask |= fi.input[i].comp1 | fi.input[i].comp2;
  }

  nOutputs = fi.output_len;
  outputList = fi.output;
  for (int i=0; i<nOutputs; ++i )
      compNames.push_back(outputList[i].name);
}

void BkgFitMatrixPacker::BuildMatrix (bool accum)
{
  mat_assembly_instruction *pinst = instList;

  // build JTJ and RHS matricies
  for (int i=0;i < nInstr;i++)
  {
    double sum=0.0;

    for (int j=0;j < pinst->cnt;j++)
        sum += DotProduct (pinst->si[j].len,pinst->si[j].src1,pinst->si[j].src2);

    if (accum)
      * (pinst->dst) += sum;
    else
      * (pinst->dst) = sum;

    pinst++;
  }
}

LinearSolverResult BkgFitMatrixPacker::GetOutput (BeadParams *bp, reg_params *rp, double lambda, double regularizer)
{
  bool delta_ok = true;
  Mat<double> jtj_lambda;

  jtj_lambda = trans (*data->jtj) + (*data->jtj) + (lambda-1.0) *diagmat (*data->jtj) + regularizer* eye(nOutputs,nOutputs);

  try
  {
 // not necessary - armadillo does fast-solvers for small matrix size
    {
      if (!solve (*data->delta,jtj_lambda,*data->rhs))
      {
        data->delta->set_size (nOutputs);
        data->delta->zeros (nOutputs);
        delta_ok = false;
        numException++;
      }
    }
  }
  catch (std::runtime_error& le)
  {
    data->delta->set_size (nOutputs);
    data->delta->zeros (nOutputs);
    delta_ok = false;
  }

  for (int i=0;i < nOutputs;i++)
  {
    if (std::isnan (data->delta->at (i)))
    {
      delta_ok = false;
      break;
    }
  }

  if (delta_ok)
  {
    // put outputs in the right place
    for (int i=0;i < nOutputs;i++){
      // What is that right place?
      float *dptr = outputList[i].bead_params_func ? (bp->*( outputList[i].bead_params_func ))()
                                                   : (rp->*( outputList[i].reg_params_func  ))();
      dptr += outputList[i].array_index;
      // safe extraction double->double
      double tmp_eval = data->delta->at (outputList[i].delta_ndx);
      // float added to double safe promotion
      tmp_eval += *dptr;
      if ((tmp_eval<(-FLT_MAX)) or (tmp_eval>FLT_MAX))
        tmp_eval = *dptr;
      // now that tmp_eval is safe, put back
      *dptr = tmp_eval ;
    }
  }
  else
  {
    data->delta->set_size (nOutputs);
    data->delta->zeros (nOutputs);
  }
  // if (numException >0)
  //   std::cout << "BkgFitMatrixPacker: numException = " << numException << std::endl;
  if (delta_ok)
    return LinearSolverSuccess;
  else
    return LinearSolverException;
}

double* BkgFitMatrixPacker::GetDestMatrixPtr (AssyMatID mat_id,int row,int col)
{
  double *ret = NULL;

  switch (mat_id)
  {
    case JTJ_MAT:
      ret = & (data->jtj->at (row,col));
      break;
    case RHS_MAT:
      ret = & (data->rhs->at (row));
      break;
  }

  return (ret);
}

float* BkgFitMatrixPacker::GetPartialDerivComponent (PartialDerivComponent comp)
{
  for (int i=0;i < nPartialDeriv;i++)
  {
    if (PartialDeriv_list[i].comp == comp)
      return (PartialDeriv_list[i].addr);
  }

  return NULL;
}

void BkgFitMatrixPacker::CreateInstrFromInputLine (mat_assembly_instruction *p,mat_assy_input_line *src,int imgLen, int flow_block_size)
{
  float *src1_base = GetPartialDerivComponent (src->comp1);
  float *src2_base = GetPartialDerivComponent (src->comp2);
  p->dst = GetDestMatrixPtr (src->matId,src->mat_row,src->mat_col);
  if (p->dst == NULL)
  {
    fprintf (stderr,"Unable to find PDE component\n");
  }

  int nstart =-1;
  int nlen   = 0;
  int cnt = 0;

  // we do this twice...once to figure out how many there are...
  for (int i=0;i<flow_block_size;i++)
  {
    if (src->GetAffectedFlow(i))
    {
      if (nlen == 0)
        nstart = i;
      nlen++;
    }
    else
    {

      if (nlen != 0)
      {
        // we just found a contiguous block of data points to dot-product together
        // convet it to a sub_instr
        cnt++;
        nlen = 0;
      }
    }
  }

  // special case...if all flows are affected
  if (nlen != 0)
    cnt++;
  // ...and a second time to actually fill in the data
  struct sub_instr *si = new struct sub_instr[cnt];
  nstart = -1;
  nlen = 0;
  cnt = 0;
  for (int i=0;i<flow_block_size;i++)
  {
    if (src->GetAffectedFlow(i))
    {
      if (nlen == 0)
        nstart = i;
      nlen++;
    }
    else
    {
      if (nlen != 0)
      {
        // we just found a contiguous block of data points to dot-product together
        // convet it to a sub_instr

        si[cnt].len = nlen*imgLen;
        si[cnt].src1 = src1_base + nstart*imgLen;
        si[cnt].src2 = src2_base + nstart*imgLen;

        cnt++;
        nlen = 0;
      }
    }
  }

  // special case...if all flows are affected
  if (nlen != 0)
  {
    si[cnt].len  = nlen*imgLen;
    si[cnt].src1 = src1_base + nstart*imgLen;
    si[cnt].src2 = src2_base + nstart*imgLen;
    cnt++;
  }
  if (p->si)
  {
    delete [] p->si;
  }
  p->si = si;
  p->cnt = cnt;
}

void BkgFitMatrixPacker::SetDataRhs (float value, int i)
{
  data->rhs->at (i) = value;
}

void BkgFitMatrixPacker::SetDataJtj (float value, int row, int col)
{
  data->jtj->at (row,col) = value;
}


#ifndef __CPU_SOLVER_HPP_IJ1ERDAZ
#define __CPU_SOLVER_HPP_IJ1ERDAZ

#include "puzzle/puzzle.cuh"
#include "puzzle/storage.hpp"
#include "puzzle/database.hpp"

#include <queue>
#include <boost/unordered_map.hpp>

namespace cpusolver {

template<int N>
struct node_t {
    PuzzleStorage<N> ps;
    node_t<N> *prev;
    int fValue;
    int gValue;
};

template<int N>
struct heap_t {
    int fValue;
    node_t<N> *node;
    bool operator<(const heap_t<N> &r) const {
        return fValue < r.fValue;
    }
    bool operator>(const heap_t<N> &r) const {
        return fValue > r.fValue;
    }
};

template <int N>
class CPUPuzzleSolver {
public:
    CPUPuzzleSolver(Puzzle *puzzle) : p(puzzle) { }
    ~CPUPuzzleSolver() {
        for (typename boost::unordered_map<
             PuzzleStorage<N>, node_t<N> * >::iterator
             it = closeList.begin(); it != closeList.end(); ++it)
            delete it->second;
    }
    void initialize() {
        if (N == 3) {
            dbCount = 1;
            tracked.resize(dbCount);
            database.resize(dbCount);
            for (int i = 1; i <= 8; ++i)
                tracked[0].push_back(i);
        } else if (N == 4) {
            dbCount = 2;
            tracked.resize(dbCount);
            database.resize(dbCount);

            tracked[0].push_back(1);
            tracked[0].push_back(2);
            tracked[0].push_back(3);
            tracked[0].push_back(4);
            tracked[0].push_back(5);
            tracked[0].push_back(6);
            tracked[0].push_back(9);
            tracked[0].push_back(13);

            tracked[1].push_back(7);
            tracked[1].push_back(8);
            tracked[1].push_back(10);
            tracked[1].push_back(11);
            tracked[1].push_back(12);
            tracked[1].push_back(14);
            tracked[1].push_back(15);
        } else if (N == 5) {
            dbCount = 4;
            tracked.resize(dbCount);
            database.resize(dbCount);

            tracked[0].push_back(3);
            tracked[0].push_back(4);
            tracked[0].push_back(5);
            tracked[0].push_back(10);
            tracked[0].push_back(15);
            tracked[0].push_back(20);

            tracked[1].push_back(2);
            tracked[1].push_back(1);
            tracked[1].push_back(6);
            tracked[1].push_back(11);
            tracked[1].push_back(16);
            tracked[1].push_back(21);

            tracked[2].push_back(7);
            tracked[2].push_back(8);
            tracked[2].push_back(9);
            tracked[2].push_back(12);
            tracked[2].push_back(17);
            tracked[2].push_back(22);

            tracked[3].push_back(13);
            tracked[3].push_back(14);
            tracked[3].push_back(18);
            tracked[3].push_back(19);
            tracked[3].push_back(23);
            tracked[3].push_back(24);
        } else
            assert(false);

        index.resize(dbCount);
        multiple.resize(dbCount);
        mapTracked.resize(N*N, make_pair(-1, -1));
        for (int i = 0; i < dbCount; ++i) {
            PatternDatabase pd(N, tracked[i]);
            database[i].resize(pd.size());
            pd.fetchDatabase(database[i].data());

            index[i].resize(tracked[i].size());
            multiple[i].reserve(tracked[i].size() + 1);

            for (int j = 0; j < (int)tracked[i].size(); ++j) {
                mapTracked[tracked[i][j]] = make_pair(i, j);
                multiple[i].push_back(N*N - j);
            }
            multiple[i].push_back(1);

            for (int j = tracked[i].size()-2; j >= 0; --j)
                multiple[i][j] *= multiple[i][j+1];
        }

        node_t<N> *node = new node_t<N>;
        vector<uint8_t> state;
        p->initialState(state);

        node->ps = PuzzleStorage<N>(
            *reinterpret_cast<uint8_t(*)[N][N]>(state.data()));
        node->prev = 0;
        node->fValue = computeHValue(
            *reinterpret_cast<uint8_t(*)[N][N]>(state.data()));
        node->gValue = 0;

        heap_t<N> heapItem;
        heapItem.fValue = node->fValue;
        heapItem.node = node;

        openList.push(heapItem);
        closeList[node->ps] = node;

        int count = 0;
        uint8_t _targetState[N][N];
        for (int i = 0; i < N; ++i)
            for (int j = 0; j < N; ++j)
                _targetState[i][j] = ++count;
        _targetState[N-1][N-1] = 0;
        targetState = PuzzleStorage<N>(_targetState);
    }

    bool solve() {
        uint8_t conf[N][N];

        int round = 0;
        int numDeduplicate = 0;
        while (!openList.empty()) {
            dprintf(" ======Round %d=======\n", round);
            heap_t<N> now;
            do {
                now = openList.top();
                openList.pop();
            } while (closeList[now.node->ps]->fValue != now.fValue);

            node_t<N> *node = now.node;
            if (node->ps == targetState) {
                printf("\t\tNumber of nodes deduplicated: %d\n", numDeduplicate);
                printf("\t\tNumber of nodes expanded: %d\n", (int)closeList.size());

                optimalNode = node;
                return true;
            }

            if (DEBUG_CONDITION) {
                dout << "\tfValue: " << node->fValue << endl;
                dout << "\tgValue: " << node->gValue << endl;
                printStorage(node->ps, "\t");
            }

            int x, y;
            node->ps.decompose(conf);
            getEmptyTile(conf, &x, &y);

            for (int i = 0; i < 4; ++i) {
                int nx = x + DX[i];
                int ny = y + DY[i];
                if (p->inrange(nx, ny)) {
                    std::swap(conf[x][y], conf[nx][ny]);

                    node_t<N> nnode;
                    nnode.ps = PuzzleStorage<N>(conf);
                    nnode.gValue = node->gValue + 1;
                    nnode.fValue = nnode.gValue + computeHValue(conf);
                    nnode.prev = node;

                    if (DEBUG_CONDITION) {
                        dout << "\t\tfValue: " << nnode.fValue << endl;
                        dout << "\t\tgValue: " << nnode.gValue << endl;
                        printStorage(nnode.ps, "\t\t");
                        dout << endl;
                    }

                    heap_t<N> heapItem;
                    heapItem.fValue = nnode.fValue;

                    if (closeList.count(nnode.ps) == 0) {
                        node_t<N> *inode = new node_t<N>(nnode);
                        closeList[nnode.ps] = inode;
                        heapItem.node = inode;

                        openList.push(heapItem);
                    } else {
                        node_t<N> *onode = closeList[nnode.ps];
                        if (onode->fValue > nnode.fValue) {
                            *onode = nnode;
                            heapItem.node = onode;

                            openList.push(heapItem);
                        } else
                            numDeduplicate++;
                    }
                    std::swap(conf[x][y], conf[nx][ny]);
                }
            }
            ++round;
        }

        return false;
    }

    void getSolution(int *optimal, vector<int> *pathList) {
        *optimal = optimalNode->fValue;

        node_t<N> *curr = optimalNode;
        node_t<N> *prev = curr->prev;

        uint8_t cconf[N][N];
        uint8_t pconf[N][N];

        pathList->clear();
        while (prev) {
            curr->ps.decompose(cconf);
            prev->ps.decompose(pconf);

            int cx, cy;
            int px, py;
            getEmptyTile(cconf, &cx, &cy);
            getEmptyTile(pconf, &px, &py);
            for (int i = 0; i < 4; ++i)
                if (px + DX[i] == cx && py + DY[i] == cy)
                    pathList->push_back(i);

            curr = prev;
            prev = curr->prev;
        }

        std::reverse(pathList->begin(), pathList->end());
    }

private:
    typedef typename std::priority_queue<
        heap_t<N>,
        vector< heap_t<N> >,
        std::greater< heap_t<N> > > openlist_t;

    typedef typename boost::unordered_map<
        PuzzleStorage<N>,
        node_t<N> * > closelist_t;

    Puzzle *p;

    openlist_t openList;
    closelist_t closeList;

    PuzzleStorage<N> targetState;
    node_t<N> *optimalNode;

    int dbCount;
    vector< vector<uint8_t> > database;
    vector< vector<int> > tracked;
    vector< pair<int, int> > mapTracked;
    vector< vector<int> > index;
    vector< vector<int> > multiple;

    int computeHValue(uint8_t conf[N][N]) {
        for (int i = 0; i < N; ++i)
            for (int j = 0; j < N; ++j)
                if (conf[i][j]) {
                    pair<int, int> p = mapTracked[conf[i][j]];
                    index[p.first][p.second] = i*N + j;
                }

        int retn = 0;
        for (int k = 0; k < dbCount; ++k) {
            int oindex[20];
            std::copy(index[k].begin(), index[k].end(), oindex);

            for (int i = 0; i < (int)index[k].size(); ++i)
                for (int j = i+1; j < (int)index[k].size(); ++j)
                    if (oindex[i] < oindex[j])
                        --index[k][j];
            int code = 0;
            for (int i = 0; i < (int)index[k].size(); ++i)
                 code += index[k][i] * multiple[k][i+1];

            // dout << "\t\t[" << code << "]" <<  (int)database[k][code] << " ";
            retn += database[k][code];
        }
        // dout << endl;

        return retn;
    }

    void getEmptyTile(uint8_t conf[N][N], int *x, int *y) {
        for (int i = 0; i < N; ++i)
            for (int j = 0; j < N; ++j)
                if (conf[i][j] == 0) {
                    *x = i;
                    *y = j;
                    return;
                }
    }
};

}

#endif /* end of include guard: __CPU_SOLVER_HPP_IJ1ERDAZ */


#ifndef DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP
#define DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP

#include <descartes_light/solvers/bgl/bgl_dijkstra_solver.h>

#include <descartes_light/descartes_macros.h>
DESCARTES_IGNORE_WARNINGS_PUSH
#include <boost/graph/dijkstra_shortest_paths.hpp>
DESCARTES_IGNORE_WARNINGS_POP

namespace descartes_light
{
template <typename FloatType>
SearchResult<FloatType> BGLDijkstraSVSESolver<FloatType>::search()
{
  // Convenience aliases
  auto& graph_ = BGLSolverBase<FloatType>::graph_;
  const auto& source_ = BGLSolverBase<FloatType>::source_;
  auto& predecessor_map_ = BGLSolverBase<FloatType>::predecessor_map_;
  const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;

  // Internal properties
  auto index_prop_map = boost::get(boost::vertex_index, graph_);
  auto weight_prop_map = boost::get(boost::edge_weight, graph_);
  auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph_);
  auto distance_prop_map = boost::get(&Vertex<FloatType>::distance, graph_);

  predecessor_map_.clear();
  boost::associative_property_map<std::map<VertexDesc<FloatType>, VertexDesc<FloatType>>> predecessor_prop_map(
      predecessor_map_);

  // Perform the search
  boost::dijkstra_shortest_paths(graph_,
                                 source_,
                                 predecessor_prop_map,
                                 distance_prop_map,
                                 weight_prop_map,
                                 index_prop_map,
                                 std::less<>(),
                                 std::plus<>(),
                                 std::numeric_limits<FloatType>::max(),
                                 static_cast<FloatType>(0.0),
                                 boost::default_dijkstra_visitor(),
                                 color_prop_map);

  // Find lowest cost node in last rung
  auto target = std::min_element(ladder_rungs_.back().begin(),
                                 ladder_rungs_.back().end(),
                                 [&](const VertexDesc<FloatType>& a, const VertexDesc<FloatType>& b) {
                                   return graph_[a].distance < graph_[b].distance;
                                 });

  SearchResult<FloatType> result;

  // Reconstruct the path from the predecesor map; remove the artificial start state
  const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, *target);
  result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
  result.trajectory.erase(result.trajectory.begin());

  result.cost = graph_[*target].distance;

  return result;
}

/**
 * @brief Visitor for Dijkstra search that terminates the search once a vertex in the last rung has been encountered
 */
template <typename FloatType>
class DijkstraTerminateEarlyVisitor : public boost::default_dijkstra_visitor
{
public:
  DijkstraTerminateEarlyVisitor(long last_rung_idx) : last_rung_idx_(last_rung_idx) {}

  /**
   * @brief Hook for introspecting the search when a new vertex is opened by the search.
   * @details Throws the vertex descriptor that is the termination of the path once the first vertex in the last rung of
   * the graph is encountered
   */
  void examine_vertex(VertexDesc<FloatType> u, const BGLGraph<FloatType>& g)
  {
    if (g[u].rung_idx == last_rung_idx_)
      throw u;
  }

private:
  const long last_rung_idx_;
};

template <typename FloatType>
SearchResult<FloatType> BGLEfficientDijkstraSVSESolver<FloatType>::search()
{
  // Convenience aliases
  auto& graph_ = BGLSolverBase<FloatType>::graph_;
  const auto& source_ = BGLSolverBase<FloatType>::source_;
  auto& predecessor_map_ = BGLSolverBase<FloatType>::predecessor_map_;
  const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;

  // Internal properties
  auto index_prop_map = boost::get(boost::vertex_index, graph_);
  auto weight_prop_map = boost::get(boost::edge_weight, graph_);
  auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph_);
  auto distance_prop_map = boost::get(&Vertex<FloatType>::distance, graph_);

  predecessor_map_.clear();
  boost::associative_property_map<std::map<VertexDesc<FloatType>, VertexDesc<FloatType>>> predecessor_prop_map(
      predecessor_map_);

  DijkstraTerminateEarlyVisitor<FloatType> visitor(static_cast<long>(ladder_rungs_.size() - 1));

  // Perform the search
  try
  {
    boost::dijkstra_shortest_paths(graph_,
                                   source_,
                                   predecessor_prop_map,
                                   distance_prop_map,
                                   weight_prop_map,
                                   index_prop_map,
                                   std::less<>(),
                                   std::plus<>(),
                                   std::numeric_limits<FloatType>::max(),
                                   static_cast<FloatType>(0.0),
                                   visitor,
                                   color_prop_map);
  }
  catch (const VertexDesc<FloatType>& target)
  {
    SearchResult<FloatType> result;

    // Reconstruct the path from the predecesor map; remove the artificial start state
    const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, target);
    result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
    result.trajectory.erase(result.trajectory.begin());

    result.cost = graph_[target].distance;

    return result;
  }

  // If the visitor never threw the vertex descriptor, there was an issue with the search
  throw std::runtime_error("Search failed to encounter vertex associated with the last waypoint in the trajectory");
}

}  // namespace descartes_light

#endif  // DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP