/**
 * @file MathTools.hpp
 * @author John D. Jakeman
 * @date 31 October 2011
 * @brief Miscelaneous math functions.
 */

#ifndef MATH_TOOLS_HPP
#define MATH_TOOLS_HPP

#include "LinearAlgebra.hpp"
#include <algorithm>
#include <functional>
#include <ctime>
#include <boost/functional/hash.hpp>
#include <boost/unordered_set.hpp>

namespace Pecos {

/**
 * \brief Map a scalar index of a flat 1D array to the equivalent
 *        d-dimensional index
 *
 * Example:
 \f[
 \begin{bmatrix}
 1 & 4 & 7\\
 2 & 5 & 8\\
 3 & 6 & 9
 \end{bmatrix}
 \rightarrow
 \begin{bmatrix}
 1,1 & 1,2 & 1,3\\
 2,1 & 2,2 & 2,3\\
 3,1 & 3,2 & 3,3
 \end{bmatrix}
 \f]
 *
 * @param sizes the number of elems in each dimension. For a 2D index
 * sizes = [numRows, numCols]
 * @param index the scalar index
 * @param num_elems the total number of elements in the d-dimensional matrix
 * @return result the d-dimensional index
 */
void ind2sub( const IntVector &sizes, int index, int num_elems,
	      IntVector &result );

/**
 * \brief Map a d-dimensional index to the scalar index of the equivalent flat
 * 1D array
 *
 * Example:
 \f[
 \begin{bmatrix}
 1,1 & 1,2 & 1,3\\
 2,1 & 2,2 & 2,3\\
 3,1 & 3,2 & 3,3
 \end{bmatrix}
 \rightarrow
 \begin{bmatrix}
 1 & 4 & 7\\
 2 & 5 & 8\\
 3 & 6 & 9
 \end{bmatrix}
 \f]
 *
 * @param sizes the number of elems in each dimension. For a 2D index
 * sizes = [numRows, numCols]
 * @param multi_index the d-dimensional index
 * @return scalar_index the scalar index of the flat array
 */
int sub2ind( const IntVector &sizes, const IntVector &multi_index );

/**
 * \brief Compute the cartesian product of an arbitray number of sets.
 *
 * These sets can consist of numbers of be themselves sets of vectors
 * @param inputSets the sets to be used in the cartesian product
 * @param elem_size the size of the vectors within each set.
 * @return the cartesian product
 */
template<typename O, typename T>
void cartesian_product( const std::vector< Teuchos::SerialDenseVector<O,T> > &input_sets, Teuchos::SerialDenseMatrix<O,T> &result, int elem_size )
{
#ifdef DEBUG
  if ( input_sets.size() == 0 )
    {
      throw(std::invalid_argument("CartesianProduct() there must be at least one input set"));
    }
#endif
  int num_elems = 1;
  int num_sets = input_sets.size();
  IntVector sizes;
  sizes.resize( num_sets );
  IntVector multi_index;
  for ( int i = 0; i < num_sets; i++ )
    {
      sizes[i] = input_sets[i].length() / elem_size;
      num_elems *= sizes[i];
    }
  result.reshape( num_sets*elem_size, num_elems );
  for ( int i = 0; i < num_elems; i++ )
    {
      ind2sub( sizes, i, num_elems, multi_index );
      for ( int j = 0; j < num_sets; j++ )
	{
	  for ( int k = 0; k < elem_size; k++ )
	    {
	      result( j*elem_size+k, i ) =
		input_sets[j][multi_index[j]*elem_size+k];
	    }
	}
    }
};

/**
 * \brief Construct the outer product of an arbitray number of sets.
 *
 * Example:
 \f[ \{1,2\}\times\{3,4\}=\{1\times3, 2\times3, 1\times4, 2\times4\} =
 \{3, 6, 4, 8\} \f]
 * @param input_sets "Vector.hpp" of sets to be used in the outer product
 * @return result the outer product
 */
template<typename O, typename T>
void outer_product( const std::vector< Teuchos::SerialDenseVector<O,T> > &input_sets, Teuchos::SerialDenseVector<O,T> &result )
{
  int num_elems = 1;
  int num_sets = input_sets.size();
  IntVector sizes( num_sets, false );
  for ( int i = 0; i < num_sets; i++ )
    {
      sizes[i] = input_sets[i].numRows();
      num_elems *= sizes[i];
    }
  IntVector multi_index( num_sets, false );
  result.sizeUninitialized( num_elems );
  for ( int i = 0; i < num_elems; i++ )
    {
      result[i] = 1.0;
      ind2sub( sizes, i, num_elems, multi_index );
      for ( int j = 0; j < num_sets; j++ )
	result[i] *= input_sets[j][multi_index[j]];
    }
}

/**
 * \brief Build a vector containing all integers in [m,n) seperated by incr
 *
 * E.g Range( 1, 10, 2 ) -> [1,3,5,7,9]
 */
template<typename O, typename T>
void range( Teuchos::SerialDenseVector<O,T> &result, T m, T n, T incr = 1 )
{
  T i = m;
  O j = 0;
  O len = (O)( n - m ) / (O)incr;
  if ( (O)( n - m ) % (O)incr != 0 ) len++;
  result.sizeUninitialized( len );
  while( i < n )
    {
      result[j] = i;
      i += incr;
      j++;
    }
};

/**
 * \brief Generate a vector whose n elements are equidistantly spaced in the
 * interval [a,b].
 *
 * @param [a,b] the range
 * @param n the number of times the interval [a,b] is divided
 * @return vec n numbers equidistant is [a,b]
 */
void linspace( RealVector &result, Real a, Real b, int n );

/**
 * \brief Round a Real to the nearest integer.
 */
Real round( Real r );

/*
 * \brief Function to allow for searching for maximum absolute
 * value (Real) in a containter
 *
 * @param a first value
 * @param b second value
 * @return true if a < b, false otherwise
 */
bool abs_compare( Real a, Real b );

/**
 * \brief Return the nearest integer to x
 * @param x the value
 * return int the neartest integer to x
 */
int nearest_integer( Real x );

/**
 * \brief Calulate n!
 *
 * @param n the factorial of interest
 * @return n!
 */
Real factorial( int n );

/**
 * \brief Return the number of possible combinations of k elements that can be
 * chosen from n elements.
 *
 *\f[ { n \choose k} = \frac{n!}{k!(n-k)!}\f]
 * @param n number of elements
 * @param k the number of elements to be chosen. k<=n.
 * @return the total number of combinations.
 */
int nchoosek( int n, int k );

/**
 * \brief Transform a point x in the interval [a,b] onto the interval [0,1] and
 * vice versa.
 *
 * @param x the value to be transformed
 * @param [a,b] the new/old interval
 * @param dir specifies whether to map to [a,b] or from [a,b]. That is
 * dir=0  returns [0,1] -> [a,b]
 * dir!=0 returns [a,b]  -> [0,1]
 * @return the rescaled value
 */
Real rescale( Real x, Real a, Real b, int dir );

/**
 * \brief Transform a point x in the interval
 * \f$[a_1,b_1] \times \cdots \times [a_d,b_d] \f$ onto
 * the hypercube interval \f$[0,1]^d\f$ and vice versa.
 *
 * @param x the value to be transformed
 * @param [a,b] the new/old interval
 * @param dir specifies whether to map to [a,b] or from [a,b]. That is
 * dir=0  returns [0,1] -> [a,b]
 * dir!=0 returns [a,b]  -> [0,1]
 * @return the rescaled value
 */
void rescale( RealMatrix &x, const RealVector &domain, int dir );

/**
 *\brief transform points in domain_in to points in domain_out
 */
void hypercube_map( RealMatrix &x, const RealVector &domain_in,
		    const RealVector &domain_out,
		    RealMatrix &result );

/**
 * \brief Construct a d-dimensional mesh on a hyper-rectangle. The meshes are
 * equidistant with respect to each coordinate direction.
 *
 * @param num_pts_1d array specifying the number of meshpoints for each dimension
 * @param domain the min and max value of each dimension.
 *        That is \f$ [a_1,b_1,...,a_d,b_d]\f$
 * @return the multi-dimensional mesh coordinates. ( num_dims x num_pts ) array.
 * num_pts = num_pts_1d[0]*...*num_pts_1d[d-1]
 */
void mesh_grid( const IntVector &num_pts_1d,
		const RealVector &domain,
		RealMatrix &result );

/**
 * Get all the multi-dimensional indices of a multi-dimensional polynomial
 * basis with a specified degree sum. A cleaner interface is provided by
 * GetMultiDimensionalPolynomial_indices()
 * @return B the multi-dimensional indices
 */
void get_multi_dimensional_polynomial_subspace_indices( IntMatrix &B,
							int* elems,
							int len_elems,
							int* pos,
							int len_pos,
							int choices_made,
							int first_pos,
							int order, int &row );

/**
 * \brief Get all the multi-dimensional indices of a multi-dimensional polynomial
 * basis with a specified degree sum.
 *
 * @param num_dims the dimensionailty
 * @param degree the degree sum of the polynomial indices wanted
 * @return B the multi-dimensional polynomial_indices
 */
void get_multi_dimensional_polynomial_indices( int num_dims, int degree,
					       IntMatrix &result );

/**
 * \brief Get the smallest degree of the total degree polynomial such that the
 * number of terms in the basis is larger than or equal to num_samples
*/
int get_total_degree_from_num_samples( int num_dims, int num_samples );

/**
 * \brief Get the multi-dimensional hypercube \f$[a,b]^d\f$
 *
 * @param num_dims the dimension of the computational domain.
 * @param \f$[a,b]\f$ the one-dimensional bounds of the hypercube.
 * @return result the multi-dimensional bounds of the hypercube.
 */
void set_hypercube_domain( RealVector &result, int num_dims, Real a, Real b );

/// Perturb the columns of a matrix
void get_permutations( IntMatrix &permutations,
		       int M , int N, unsigned int seed );

void compute_hyperbolic_level_subdim_indices( int num_dims, int level,
					      int num_active_dims,
					      Real p,
					      IntMatrix &result );

void compute_hyperbolic_level_indices( int num_dims, int level, Real p,
				       IntMatrix &result );

void compute_hyperbolic_indices( int num_dims, int level, Real p,
				 IntMatrix &result );

// weights ( num_dims x 1 ) vector with values in [0,1]
void compute_anisotropic_hyperbolic_indices( int num_dims, int level, Real p,
					     RealVector &weights,
					     IntMatrix &result );

// prepend "mt_" for MathTools to avoid name clash
enum lp_norm
  {
    mt_l1_norm,
    mt_l2_norm,
    mt_linf_norm,
  };

/**
 * \brief Return the index of the element of x with the minimum value.
 *
 *\f[
 \arg \! \min_i x_i\quad i=1,\ldots,n
 \f]
*/
 template<typename T>
int argmin( int n, T* x )
{
  int amin( 0 );
  T min = x[0];
  for ( int i = 1; i < n; i++ )
    {
      if ( x[i] < min )
	{
	  min = x[i];
	  amin = i;
	}
    }
  return amin;
};

/**
 * \brief Return the index of the element of x with the maximum value.
 *
 *\f[
 \arg \! \max_i x_i \quad i=1,\ldots,n
 \f]
*/
template<typename T>
int argmax( int n, T* x )
{
  int amax( 0 );
  T max = x[0];
  for ( int i = 1; i < n; i++ )
    {
      if ( x[i] > max )
	{
	  max = x[i];
	  amax = i;
	}
    }
  return amax;
};

template<typename T>
int magnitude_argmax( int n, T* x )
{
  int amax( 0 );
  T max = std::abs( x[0] );
  for ( int i = 1; i < n; i++ )
    {
      Real abs_x = std::abs( x[i] );
      if ( abs_x > max )
	{
	  max = abs_x;
	  amax = i;
	}
    }
  return amax;
};

/**
 * \brief Return the sum of the elements in the array x.
 *
 *\f[
 \sum_{i=1}^N x_i
 \f]
*/
template<typename T>
T sum( int n, T* x )
{
  T sum = x[0];
  for ( int i = 1; i < n; i++ )
    sum += x[i];
  return sum;
};

/**
 * \brief Return the sum of the elements in the vector x.
 *
 *\f[
 \sum_{i=1}^N x_i
 \f]
 *
 * This will not return the same result as sum( int n, T* x ) if
 * the vector is a subvector of another vector. That is stride does not equal
 * the number of rows.
 */
template < typename O, typename T >
T sum( Teuchos::SerialDenseVector<O,T> &v )
{
  T tmp = Teuchos::ScalarTraits<T>::zero();
  for ( O i = 0; i < v.length(); i++ )
    tmp += v[i];
  return tmp;
};

/**
 * \brief Return the mean of the array x.
 *
 * \f[
 \bar{x} = \frac{1}{N}\sum_{i=1}^N x_i
 \f]
*/
Real mean( int n, Real *x );

/**
 * \brief Return the sample variance of the array x.
 *
 * \f[
 \sigma^2 = \frac{1}{N-\text{ddof}}\sum_{i=1}^N ( x_i-\bar{x} )^2
 \f]
 *
 * \param ddof Delta Degrees of Freedom (ddof).
 * The divisor used in calculations is \f$N - \text{ddof}\f$,
 * where \$fN\$f represents the number of elements.
 * By default ddof is one.
 */
Real variance( int n, Real *x, int ddof = 1 );

/**
 * \brief Calculate one of several different types of \f$\ell_p\f$ error norms
 * of a set of vectors.
 *
 * Each column of the reference_values matrix is considered
 * independently with the correpsonding column in the approximation_values.
 * The error between the each set of two columns tested is returned.
 * \param reference_values a matrix containing the reference values
 * \param approximate_values a matrix containig the approximate values
 * \param active_columns specifies which set of columns to consider. If empty
 *        all columns are considered
 * \pram normalise if true the error norms are normalised. The l_inf norm
 *                 is normalised by the half the range of the data in the
 *                 reference_values column. The L_2 norm is normalised by the
 *                 standard deviation of the data in the reference values column.
 */
void lp_error( RealMatrix &reference_values,
	       RealMatrix &approximate_values,
	       std::vector<lp_norm> error_norms, RealMatrix &error,
	       IntVector &active_columns, bool normalise = false );

/**
 *\brief Construct a Latin Hyperube Design (LHD)
 */
void latin_hypercube_design( int num_pts, int num_dims, RealMatrix &result,
			     int seed );

void compute_next_combination( int num_dims, int level, IntVector &index,
			       bool &extend, int &h, int &t );

void compute_combinations( int num_dims, int level, IntMatrix &result );

/**
 * \breif Return  the sign of x.
 *
 * \return 1 if the corresponding element of x is greater than zero;
 * 0 if the corresponding element of X equals zero;
 *-1 if the corresponding element of X is less than zero
 */
template<typename T>
int sgn( T x )
{
  return ( T(0) < x ) - ( x < T(0) );
}

template<typename T>
int num_non_zeros( T *data, int n )
{
  int num_non_zero_count( 0 );
  for ( int i = 0; i < n; i++ )
    {
      if ( std::abs( data[i] ) > 0 )
	num_non_zero_count++;
    }
  return num_non_zero_count;
}

/**
 *\brief return the median of a std::vector
 */
template<typename T>
double median( std::vector<T> &v )
{
  size_t n = v.size();
  typename std::vector<T>::iterator target = v.begin() + n / 2;
  std::nth_element( v.begin(), target, v.end() );
  if( n % 2 != 0 )
    {
      return (double)*target;
    }
  else
    {
      std::vector<Real>::iterator target_neighbour =
	std::max_element( v.begin(), target );
      return (double)( *target + *target_neighbour ) / 2.0;
    }
};

/**
 *\brief return the median of a RealVector.
 */
template<typename O, typename T>
double median( Teuchos::SerialDenseVector<O,T> &v )
{
  std::vector<T> tmp( v.length() );
  for ( int i = 0; i < v.length(); i++ )
    tmp[i] = v[i];
  return median( tmp );
}

template<typename O, typename T>
double pnorm( Teuchos::SerialDenseVector<O,T> &v, double p )
{
  double sum = 0.;
  for ( O i = 0; i < v.length(); i++ )
    {
      sum += std::pow( std::abs( (double)v[i] ), p );
    }
  return std::pow( sum, 1./p );
}

template<typename O, typename T>
O num_nonzeros( Teuchos::SerialDenseVector<O,T> &v )
{
  O num_nonzeros = Teuchos::ScalarTraits<O>::zero();
  for ( int d = 0; d < v.length(); d++ )
    {
      if ( v[d] != Teuchos::ScalarTraits<T>::zero() ) num_nonzeros++;
    }
  return num_nonzeros;
}

template<typename O, typename T>
void nonzero( Teuchos::SerialDenseVector<O,T> &v,
	      IntVector &result )
{
  int num_nonzeros = 0;
  result.sizeUninitialized( v.length() );
  for ( int d = 0; d < v.length(); d++ )
    {
      if ( v[d] != Teuchos::ScalarTraits<T>::zero() )
	{
	  result[num_nonzeros] = d;
	  num_nonzeros++;
	}
    }
  // chop off unwanted memory
  result.resize( num_nonzeros );
}

void lhs_indices( int num_dims, int num_pts, int duplication, int seed,
		  IntMatrix &result );

/// Construct the improved distributed hypercube sample
void ilhs( int num_dims, int num_pts, int duplication, int seed,
	   RealMatrix &result );

/// Construct basic random hypercube sample
void lhs( int num_dims, int num_pts, int seed,
	  RealMatrix &result );

template<typename T>
class index_sorter {
public:
  index_sorter(const T &values){ values_ = values; }
  bool operator()( int lhs, int rhs) const {
    return values_[lhs] < values_[rhs];
  }
private:
  T values_;
};

template<typename T>
class magnitude_index_sorter {
public:
  magnitude_index_sorter(const T &values){ values_ = values; }
  bool operator()( int lhs, int rhs) const {
    return std::abs( values_[lhs] ) > std::abs( values_[rhs] );
  }
private:
  T values_;
};

// sorts in ascending order
template<typename O, typename T>
void argsort( const Teuchos::SerialDenseVector<O,T> &values,
	      IntVector &result )
{
  std::vector<O> indices( values.length() );
  for ( O i = 0; i < values.length(); i++ )
    indices[i] = i;

  std::sort( indices.begin(), indices.end(), index_sorter< Teuchos::SerialDenseVector<O,T> >( values ) );

  result.sizeUninitialized( values.length() );
  for ( O i = 0; i < values.length(); i++ )
    result[i] = indices[i];
}

template<typename O, typename T>
void argsort( Teuchos::SerialDenseVector<O,T> &v,
	      Teuchos::SerialDenseVector<O,T> &result_0,
	      IntVector &result_1 )
{
  argsort( v, result_1 );
  result_0.sizeUninitialized( v.length() );
  for ( O i = 0; i < v.length(); i++ )
    result_0[i] = v[result_1[i]];
}

// sorts in descending order absolute value of entries
template<typename O, typename T>
void magnitude_argsort( const Teuchos::SerialDenseVector<O,T> &values,
			IntVector &result )
{
  std::vector<O> indices( values.length() );
  for ( O i = 0; i < values.length(); i++ )
    indices[i] = i;

  std::sort( indices.begin(), indices.end(), magnitude_index_sorter< Teuchos::SerialDenseVector<O,T> >( values ) );

  result.sizeUninitialized( values.length() );
  for ( O i = 0; i < values.length(); i++ )
    result[i] = indices[i];
}

/**
 * \brief find the interval containing a target value. Assumes data is
 * in ascending order
 */
template<typename O, typename T>
int binary_search( T target, Teuchos::SerialDenseVector<O,T> &data )
{
  O low = 0, high = data.length()-1, mid;
  while ( low <= high )
    {
      mid = low + ( high - low ) / 2;
      if ( target < data[mid] ) high = mid - 1;
      else if ( target > data[mid] ) low = mid + 1;
      else return mid;
    }
  if ( high < 0 ) return 0;
  else if ( high < low ) return high;
  else return low;
}

class LinearInterpolant1D
{
private:
  RealVector pts_, vals_;

public:
  LinearInterpolant1D( RealVector &pts, RealVector &vals )
  {
    pts_ = pts; vals_ = vals;
  };

  ~LinearInterpolant1D(){};

  void interpolate( RealVector &pts, RealVector &result )
  {
    int num_pts = pts.length();
    if ( result.length() != num_pts )
      result.sizeUninitialized( num_pts );
    for ( int i = 0; i < num_pts; i++ )
      {
	// enforce constant interpolation when interpolation is outside the
	// range of pts_
	if ( pts[i] <= pts_[0] )
	  result[i] = vals_[0];
	else if ( pts[i] >= pts_[pts_.length()-1] )
	  result[i] = vals_[pts_.length()-1];
	else
	  {
	    // assumes binary search returns index of the closest point in pts_
	    // to the left of pts[i]
	    int index = binary_search( pts[i], pts_ );
	    result[i] = vals_[index] + ( ( vals_[index+1] - vals_[index] ) /
					 ( pts_[index+1] - pts_[index] ) )
	      * ( pts[i] - pts_[index] );
	  }
      }
  };
};

/**
 * \brief Reverse the contents of a vector.
 *
 * Useful for changing an ordered array to/from ascending/descending order
 */
template<typename O, typename T>
void reverse( Teuchos::SerialDenseVector<O,T> &v )
{
  O n = v.length();
  Teuchos::SerialDenseVector<O,T> tmp( v );
  for ( O i = 0; i < n; i++ )
    v[i] = tmp[n-i-1];
}

template <typename O, typename T>
struct VectorEqual{
  /*
   * \brief Determine if two vectors of doubles are equal
   */
  typedef Teuchos::SerialDenseVector<O,T> VectorType;
  bool operator()( const VectorType &v1, const VectorType &v2 ) const{
    if ( v1.length() != v2.length() )
      return false;
    else{
      Real tol = std::numeric_limits<T>::epsilon();
      Real dist = 0.;
      for ( int i = 0; i < v1.length(); i++ )
	dist += ( v1[i]-v2[i] )*( v1[i]-v2[i] );
      dist = std::sqrt( dist );
      if ( dist > tol )
	return false;
    }
    return true;
  }
};

template<typename VectorType >
struct VectorHash{
  int operator()( const VectorType &v ) const{
    return (int)boost::hash_range(v.values(), v.values() + v.length() );
  }
};

/*
 * \brief Get the columns in A that are not in B
 */
template <typename O, typename T>
void set_difference_matrix_columns( const Teuchos::SerialDenseMatrix<O,T> &A,
				    const Teuchos::SerialDenseMatrix<O,T> &B,
				    IntVector &result ){
  if ( B.numCols() == 0 ){
    range( result, 0, A.numCols() );
    return;
  }

  int num_rows = A.numRows(), num_cols = A.numCols();
  if ( num_rows != B.numRows() ){
    std::string msg = "set_difference_matrix_columns: A and B are inconsistent";
    throw(std::runtime_error( msg ) );
  }
  typedef Teuchos::SerialDenseVector<O,T> VectorType;

  // Hash columns of the matrix B
  boost::unordered_set< VectorType,VectorHash<VectorType>,
			VectorEqual<O,T> > col_set;
  for ( int i = 0; i < B.numCols(); i++ ){
    VectorType col( Teuchos::View, const_cast<T*>(B[i]), num_rows );
    col_set.insert( col );
  }

  // Find columns of A not in B
  int num_result = 0;
  typename boost::unordered_set< VectorType,VectorHash<VectorType>,
				 VectorEqual<O,T> >::const_iterator it;
  result.sizeUninitialized( num_cols );
  for ( int i = 0; i < num_cols; i++ ){
    VectorType col( Teuchos::View, const_cast<T*>(A[i]), num_rows );
    it = col_set.find( col );
    if ( it == col_set.end() ){
      // Column is not in B
      result[num_result] = i;
      num_result++;
    }
  }

  // Remove unused memory
  result.resize( num_result );
}

template<typename MatrixType>
void extract_submatrix_from_column_indices( const MatrixType &A,
					    const IntVector &column_indices,
					    MatrixType &submatrix ){
  int num_rows = A.numRows(), num_indices = column_indices.length();
  reshape( submatrix, num_rows, num_indices );

  for ( int j = 0; j < num_indices; j++ )
    for ( int i = 0; i < num_rows; i++ )
      submatrix(i,j) = A(i,column_indices[j]);
}

template<typename MatrixType>
void copy_matrix( const MatrixType &source, MatrixType &dest,
	   int num_rows, int num_cols, int start_row=0, int start_col=0 ){

  MatrixType source_subset( Teuchos::View, source, num_rows, num_cols,
			    start_row, start_col );
  reshape( dest, num_rows, num_cols );
  dest.assign( source_subset );
}

template<typename VectorType>
void copy_vector( const VectorType &source, VectorType &dest,
	   int num_rows, int start_row=0 ){
  VectorType source_subset( Teuchos::View, source.values()+start_row, num_rows );
  resize( dest, num_rows );
  dest.assign( source_subset );
}

template<typename MatrixType>
void hstack( const MatrixType &source1, const MatrixType &source2,
	     MatrixType &dest ){
  if ( ( source1.numRows() != source2.numRows() ) &&
       ( ( source1.numCols()!=0 ) && ( source2.numCols()!=0 ) ) )
    throw( std::runtime_error("hstack: matrices are inconsistent") );
  int num_rows = source1.numRows() ? source1.numRows() : source2.numRows();
  reshape( dest, num_rows, source1.numCols()+source2.numCols() );
  int cntr = 0;
  for ( int j = 0; j < source1.numCols(); j++, cntr++ )
    for ( int i = 0; i < num_rows; i++ )
      dest(i,cntr) = source1(i,j);
  for ( int j = 0; j < source2.numCols(); j++, cntr++ )
    for ( int i = 0; i < num_rows; i++ )
      dest(i,cntr) = source2(i,j);
}

/**
 * Convert a static array to a "Vector.hpp" of vectors.
 * @param a array to be converted
 * @param n the number of vectors
 * @param m the size of each "Vector.hpp"
 * @return v the "Vector.hpp" of vectors
 */
template <typename O, typename T>
void convert(T a[], int m, int n, std::vector< Teuchos::SerialDenseVector<O,T> > &v)
{
  v.resize( m );
  for ( int i = 0; i < m; i++ )
    {
      Teuchos::SerialDenseVector<O,T> tmp( Teuchos::View, a + i * n, n );
      v[i] = tmp;
    }
};

/**
 * Convert a static array to a weakly ordered multiset of vectors.
 * Set allows for multiple keys with the same value.
 * @param a array to be converted
 * @param n the number of vectors
 * @param m the size of each "Vector.hpp"
 * @return s the set
 */
template <typename O, typename T>
void convert(T a[], int m, int n, std::set< Teuchos::SerialDenseVector<O,T> > &s)
{
  for ( int i = 0; i < m; i++ )
    {
      Teuchos::SerialDenseVector<O,T> v( Teuchos::View, a+i*n, n );
      s.insert( v );
    }
};

/**
 * \brief Convert a std::vector of vectors to a matrix
 *
 * Each element of the std::vector becomes a column of the matrix
 */
template <typename O, typename T>
void convert( const std::vector< Teuchos::SerialDenseVector<O,T> > &V,
	      Teuchos::SerialDenseMatrix<O,T> &M )
{
  M.shapeUninitialized( V[0].length(), (int)V.size() );
  for ( int i = 0; i < (int)V.size(); i++ )
    {
      for ( int j = 0; j < V[0].length(); j++ )
	{
	  M(j,i) = V[i][j];
	}
    };
};

/**
 * Add a set of vectors together.
 * @param vectors set of vectors to be added
 * @param result the accumualted values
 */
template< typename T, typename Operator>
void accumulate( const std::vector< std::vector<T> > &vectors,
		 std::vector<T> &result, Operator op)
{
  if( !vectors.empty() )
    {
	//invariant: all vectors are of the same size
      result = vectors[0] ;
      int N = result.size() ;
      for( int i = 1 ; i < vectors.size() ; ++i )
	{
	  #ifdef DEBUG
	  if ( vectors[i].size() != N )
	    {
	      throw(std::runtime_error("Accumulate() vectors must all have the same size"));
	    }
	  #endif
	  std::transform( result.begin(), result.end(), vectors[i].begin(),
			  result.begin(), op );
	}
    }
  else
    {
      result.clear();
    }
};

template< typename T >
bool is_nan_or_inf( T x )
{
  if ( x != x ||
       x >= std::numeric_limits<T>::infinity() ||
       x <= -std::numeric_limits<T>::infinity() ||
       x >= std::numeric_limits<T>::max() ||
       x <= -std::numeric_limits<T>::max() )
    return true;
  return false;
}

template <typename O, typename T>
bool has_nan_or_inf( const Teuchos::SerialDenseMatrix<O,T> &matrix )
{
  for ( O j = 0 ; j < matrix.numCols(); j++ )
    {
      for ( O i = 0 ; i < matrix.numRows(); i++ )
	{
	  if ( is_nan_or_inf<T>( matrix(i,j ) ) )
	    {
	      disp( matrix(i,j) );
	      return true;
	    }
	}
    }
  return false;
}

void get_column_norms( RealMatrix &A, RealVector &result );

}  // namespace Pecos

#endif


#include <fstream>
#include<boost/shared_ptr.hpp>
// CGAL headers
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Polygon_2.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/partition_2.h>
#include <CGAL/Partition_traits_2.h>
#include<CGAL/create_straight_skeleton_2.h>
#include<CGAL/create_offset_polygons_2.h>
#include <CGAL/linear_least_squares_fitting_2.h>
#include <CGAL/extremal_polygon_2.h>
#include <CGAL/minkowski_sum_2.h>
#include <boost/config.hpp>
#include <boost/version.hpp>
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <CGAL/IO/WKT.h>
#endif

// Qt headers
#include <QtGui>
#include <QString>
#include <QFileDialog>
#include <QInputDialog>
#include <QGraphicsLineItem>

// GraphicsView items and event filters (input classes)
#include <CGAL/Qt/GraphicsViewPolylineInput.h>
#include <CGAL/Qt/PolygonGraphicsItem.h>
#include <CGAL/Qt/PolygonWithHolesGraphicsItem.h>
#include <CGAL/Qt/LineGraphicsItem.h>

// the two base classes
#include "ui_Polygon_2.h"
#include <CGAL/Qt/DemosMainWindow.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;
typedef K::Segment_2 Segment_2;
typedef K::Line_2 Line_2;

typedef CGAL::Polygon_2<K,std::list< Point_2 > > Polygon2; // it must be a list for the partition
typedef CGAL::Polygon_with_holes_2<K,std::list< Point_2 > > Polygon_with_holes_2;

typedef CGAL::Straight_skeleton_2<K> Ss ;

typedef boost::shared_ptr<Ss> SsPtr ;

typedef boost::shared_ptr<Polygon2> PolygonPtr ;

typedef std::vector<PolygonPtr> PolygonPtr_vector ;

class MainWindow :
  public CGAL::Qt::DemosMainWindow,
  public Ui::Polygon_2
{
  Q_OBJECT

private:

  enum PartitionAlgorithm {YMonotone, ApproximateConvex, OptimalConvex} ;

  CGAL::Qt::Converter<K> convert;
  Polygon2 poly, kgon;
  Polygon_with_holes_2 selfmink;
  QGraphicsScene scene;

  CGAL::Qt::PolygonGraphicsItem<Polygon2> * pgi;

  CGAL::Qt::GraphicsViewPolylineInput<K> * pi;

  CGAL::Qt::PolygonWithHolesGraphicsItem<Polygon_with_holes_2> * minkgi;

  std::list<Polygon2> partitionPolygons;
  std::list<CGAL::Qt::PolygonGraphicsItem<Polygon2>* >  partitionGraphicsItems;
  std::list<QGraphicsLineItem* >  skeletonGraphicsItems;
  std::list<QGraphicsLineItem* >  offsetGraphicsItems;
  CGAL::Qt::LineGraphicsItem<K>* lgi;

  CGAL::Qt::PolygonGraphicsItem<Polygon2> * kgongi;

public:
  MainWindow();

public Q_SLOTS:

  void processInput(CGAL::Object o);

  void on_actionClear_triggered();

  void on_actionLoadPolygon_triggered();
  void on_actionSavePolygon_triggered();

  void on_actionSelfMinkowskiSum_triggered();
  void on_actionRecenter_triggered();
  void on_actionMaximumAreaKGon_triggered();
  void on_actionInnerSkeleton_triggered();
  void on_actionOuterOffset_triggered();
  void on_actionLinearLeastSquaresFitting_triggered();
  void on_actionLinearLeastSquaresFittingOfSegments_triggered();
  void on_actionCreateInputPolygon_toggled(bool);

  void on_actionYMonotonePartition_triggered();
  void on_actionApproximateConvexPartition_triggered();
  void on_actionOptimalConvexPartition_triggered();
  void partition(PartitionAlgorithm);

  void clearPartition();
  void clearMinkowski();
  void clearSkeleton();
  void clearOffset();
  void clear();

  virtual void open(QString);
Q_SIGNALS:
  void changed();
};

MainWindow::MainWindow()
  : DemosMainWindow()
{
  setupUi(this);

  this->graphicsView->setAcceptDrops(false);

  minkgi = nullptr;
  // Add a GraphicItem for the Polygon2
  pgi = new CGAL::Qt::PolygonGraphicsItem<Polygon2>(&poly);

  QObject::connect(this, SIGNAL(changed()),
                   pgi, SLOT(modelChanged()));

  scene.addItem(pgi);

  kgongi =  new CGAL::Qt::PolygonGraphicsItem<Polygon2>(&kgon);
  kgongi->setEdgesPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  kgongi->hide();
  scene.addItem(kgongi);

  lgi = new CGAL::Qt::LineGraphicsItem<K>();
  lgi->setPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  lgi->hide();
  scene.addItem(lgi);
  assert(lgi->scene() == &scene);
  // Setup input handlers. They get events before the scene gets them
  pi = new CGAL::Qt::GraphicsViewPolylineInput<K>(this, &scene, 0, true);

  this->actionCreateInputPolygon->setChecked(true);
  QObject::connect(pi, SIGNAL(generate(CGAL::Object)),
                   this, SLOT(processInput(CGAL::Object)));

  //
  // Manual handling of actions
  //
  QObject::connect(this->actionQuit, SIGNAL(triggered()),
                   this, SLOT(close()));

  //
  // Setup the scene and the view
  //
  scene.setItemIndexMethod(QGraphicsScene::NoIndex);
  scene.setSceneRect(-100, -100, 100, 100);
  this->graphicsView->setScene(&scene);
  this->graphicsView->setMouseTracking(true);

  // Uncomment the following line to get antialiasing by default.
//   actionUse_Antialiasing->setChecked(true);

  // Turn the vertical axis upside down
  this->graphicsView->scale(1, -1);

  // The navigation adds zooming and translation functionality to the
  // QGraphicsView
  this->addNavigation(this->graphicsView);

  this->setupStatusBar();
  this->setupOptionsMenu();
  this->addAboutDemo(":/cgal/help/about_Polygon_2.html");
  this->addAboutCGAL();
  this->setupExportSVG(action_Export_SVG, graphicsView);

  this->addRecentFiles(this->menuFile, this->actionQuit);
  connect(this, SIGNAL(openRecentFile(QString)),
          this, SLOT(open(QString)));
}

void
MainWindow::processInput(CGAL::Object o)
{
  this->actionCreateInputPolygon->setChecked(false);
  std::list<Point_2> points;
  if(CGAL::assign(points, o)){
    if((points.size() == 1)&& poly.size()>0){

    } else {
      poly.clear();
      if(points.front() == points.back()){
        points.pop_back();
      }
      poly.insert(poly.vertices_begin(), points.begin(), points.end());
    }
    Q_EMIT( changed());
  }
}

/*
 *  Qt Automatic Connections
 *  https://doc.qt.io/qt-5/designer-using-a-ui-file.html#automatic-connections
 *
 *  setupUi(this) generates connections to the slots named
 *  "on_<action_name>_<signal_name>"
 */

void
MainWindow::on_actionClear_triggered()
{
  poly.clear();
  clear();
  this->actionCreateInputPolygon->setChecked(true);
  Q_EMIT( changed());
}

void
MainWindow::on_actionLoadPolygon_triggered()
{
  QString fileName = QFileDialog::getOpenFileName(this,
                                                  tr("Open Polygon File"),
                                                  ".",
                                                  tr( "Polyline files (*.polygons.cgal);;"
                                                      "WSL files (*.wsl);;"
                                                    #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                                      "WKT files (*.wkt *.WKT);;"
                                                    #endif
                                                      "All file (*)"));
  if(! fileName.isEmpty()){
    open(fileName);
  }
}

void
MainWindow::open(QString fileName)
{
  this->actionCreateInputPolygon->setChecked(false);
  std::ifstream ifs(qPrintable(fileName));
  poly.clear();
  if(fileName.endsWith(".wkt", Qt::CaseInsensitive))
  {
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
    CGAL::Polygon_with_holes_2<K> P;
    CGAL::read_polygon_WKT(ifs, P);
    poly = Polygon2(P.outer_boundary().begin(),
                    P.outer_boundary().end());
#endif
  }
  else
  {
    ifs >> poly;
  }
  clear();

  this->addToRecentFiles(fileName);
  Q_EMIT( changed());
}

void
MainWindow::on_actionSavePolygon_triggered()
{
  QString fileName = QFileDialog::getSaveFileName(this,
                                                  tr("Save Polygon"),
                                                  ".",
                                                  tr( "Polyline files (*.polygons.cgal);;"
                                                    #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                                      "WKT files (*.wkt *.WKT);;"
                                                    #endif
                                                      "All file (*)"));
  if(! fileName.isEmpty()){
    std::ofstream ofs(qPrintable(fileName));
    if(fileName.endsWith(".wkt", Qt::CaseInsensitive))
    {
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
      CGAL::Polygon_2<K> P(poly.begin(),
                           poly.end());
      CGAL::Polygon_with_holes_2<K> Pwh(P);
      CGAL::write_polygon_WKT(ofs, Pwh);
#endif
    }
    else
      ofs << poly;
  }
}

void
MainWindow::on_actionCreateInputPolygon_toggled(bool checked)
{
  poly.clear();
  clear();
  if(checked){
    scene.installEventFilter(pi);
  } else {
    scene.removeEventFilter(pi);
  }
  Q_EMIT( changed());
}

void
MainWindow::on_actionRecenter_triggered()
{
  this->graphicsView->setSceneRect(pgi->boundingRect());
  this->graphicsView->fitInView(pgi->boundingRect(), Qt::KeepAspectRatio);
}

void
MainWindow::on_actionSelfMinkowskiSum_triggered()
{
  if(poly.size()>0){
    if(! poly.is_simple()){
      return;
    }

    selfmink = minkowski_sum_2 (poly, poly);

    minkgi = new CGAL::Qt::PolygonWithHolesGraphicsItem<Polygon_with_holes_2>(&selfmink);

    minkgi->setVerticesPen(QPen(Qt::red, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
    scene.addItem(minkgi);
    minkgi->setEdgesPen(QPen(Qt::darkRed, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  }
}

void
MainWindow::on_actionInnerSkeleton_triggered()
{
  if(poly.size()>0){
    if(! poly.is_simple()){
      return;
    }
    clear();
    if(! poly.is_counterclockwise_oriented()){
      poly.reverse_orientation();
    }
    SsPtr iss = CGAL::create_interior_straight_skeleton_2(poly.vertices_begin(), poly.vertices_end());

    CGAL::Straight_skeleton_2<K> const& ss = *iss;

    typedef Ss::Vertex_const_handle     Vertex_const_handle ;
    typedef Ss::Halfedge_const_handle   Halfedge_const_handle ;
    typedef Ss::Halfedge_const_iterator Halfedge_const_iterator ;

    Halfedge_const_handle null_halfedge ;
    Vertex_const_handle   null_vertex ;

    for ( Halfedge_const_iterator i = ss.halfedges_begin(); i != ss.halfedges_end(); ++i )
      {
        if ( i->is_bisector() ){
          Segment_2 s(i->opposite()->vertex()->point(), i->vertex()->point());
          skeletonGraphicsItems.push_back(new QGraphicsLineItem(convert(s)));
          scene.addItem(skeletonGraphicsItems.back());
          skeletonGraphicsItems.back()->setPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
        }
      }

  }

}

void
MainWindow::on_actionOuterOffset_triggered()
{
  if(poly.size()>0 && poly.is_simple())
  {
    clear();

    if(! poly.is_counterclockwise_oriented())
      poly.reverse_orientation();

    double w = poly.bbox().xmax() - poly.bbox().xmin() ;
    double h = poly.bbox().ymax() - poly.bbox().ymin() ;
    double s = (std::max)(w,h);
    double def_offset = s * 0.05 ;

    bool ok;
    const double off = QInputDialog::getDouble( this
                                              , tr("Offset distance")
                                              , tr("Offset distance:")
                                              , def_offset
                                              , 0.0
                                              , s
                                              , 2
                                              , &ok
                                              );
    if ( ok )
    {
      PolygonPtr_vector lContours = create_exterior_skeleton_and_offset_polygons_2(off,poly) ;

      PolygonPtr_vector::const_iterator frame ;

      double max_area = 0.0 ;

      for( PolygonPtr_vector::const_iterator cit = lContours.begin(); cit != lContours.end(); ++ cit )
      {
        double area = (*cit)->area();
        if ( area > max_area )
        {
          max_area = area ;
          frame = cit ;
        }
      }

      for( PolygonPtr_vector::const_iterator cit = lContours.begin(); cit != lContours.end(); ++ cit )
      {
        if ( cit != frame )
        {
          Polygon2 const& lContour = **cit ;
          lContour.area();
          for ( Polygon2::Edge_const_iterator eit = lContour.edges_begin(); eit != lContour.edges_end(); ++ eit )
          {
            Segment_2 s(eit->source(), eit->target());
            offsetGraphicsItems.push_back(new QGraphicsLineItem(convert(s)));
            scene.addItem(offsetGraphicsItems.back());
            offsetGraphicsItems.back()->setPen(QPen(Qt::red, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
          }
        }
      }
    }
  }
}

void
MainWindow::on_actionMaximumAreaKGon_triggered()
{
  if( (poly.size()>2) && poly.is_convex()){
    clear();

    kgon.clear();
    std::vector<Point_2> points(poly.vertices_begin(),
                                poly.vertices_end());
    CGAL::maximum_area_inscribed_k_gon_2(points.begin(),
                                         points.end(),
                                         3,
                                         std::back_inserter(kgon));

    kgongi->modelChanged();
    kgongi->show();
  } else {
    std::cout << "The polygon must be convex" << std::endl;
  }
}

void
MainWindow::on_actionLinearLeastSquaresFitting_triggered()
{
  if(poly.size()>2){
    clear();

    Line_2 line;
    CGAL::linear_least_squares_fitting_2(poly.vertices_begin(),
                                         poly.vertices_end(),
                                         line,
                                         CGAL::Dimension_tag<0>());

    lgi->setLine(line);
    lgi->show();
  }
}

void
MainWindow::on_actionLinearLeastSquaresFittingOfSegments_triggered()
{
  if(poly.size()>2){
    clear();

    Line_2 line;
    CGAL::linear_least_squares_fitting_2(poly.edges_begin(),
                                         poly.edges_end(),
                                         line,
                                         CGAL::Dimension_tag<1>());

    lgi->setLine(line);
    lgi->show();
  }
}

void
MainWindow::on_actionYMonotonePartition_triggered()
{
  partition(YMonotone);
}

void
MainWindow::on_actionOptimalConvexPartition_triggered()
{
  partition(OptimalConvex);
}

void
MainWindow::on_actionApproximateConvexPartition_triggered()
{
  partition(ApproximateConvex);
}

void
MainWindow::partition(PartitionAlgorithm pa)
{
  if(poly.size()>0){
    clear();
    if(! poly.is_counterclockwise_oriented()){
      poly.reverse_orientation();
    }
    switch (pa) {
    case YMonotone :
      CGAL::y_monotone_partition_2(poly.vertices_begin(), poly.vertices_end(), std::back_inserter(partitionPolygons));
      break;
    case ApproximateConvex:
      CGAL::approx_convex_partition_2(poly.vertices_begin(), poly.vertices_end(), std::back_inserter(partitionPolygons));
      break;
    default:
      CGAL::optimal_convex_partition_2(poly.vertices_begin(), poly.vertices_end(), std::back_inserter(partitionPolygons));
      break;
    }
    for(std::list<Polygon2>::iterator it = partitionPolygons.begin();
        it != partitionPolygons.end();
        ++it){
      partitionGraphicsItems.push_back(new CGAL::Qt::PolygonGraphicsItem<Polygon2>(&(*it)));
      scene.addItem(partitionGraphicsItems.back());
      partitionGraphicsItems.back()->setEdgesPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
    }
  }
}

void
MainWindow::clearPartition()
{
  partitionPolygons.clear();
  for(std::list<CGAL::Qt::PolygonGraphicsItem<Polygon2>* >::iterator it = partitionGraphicsItems.begin();
      it != partitionGraphicsItems.end();
      ++it){
    scene.removeItem(*it);
  }
  partitionGraphicsItems.clear();
}

void
MainWindow::clearMinkowski()
{
  if(minkgi != nullptr){
    scene.removeItem(minkgi);
    delete minkgi;
    minkgi = nullptr;
  }
}

void
MainWindow::clearSkeleton()
{ for(std::list<QGraphicsLineItem* >::iterator it = skeletonGraphicsItems.begin();
      it != skeletonGraphicsItems.end();
      ++it){
    scene.removeItem(*it);
  }
  skeletonGraphicsItems.clear();
}

void
MainWindow::clearOffset()
{ for(std::list<QGraphicsLineItem* >::iterator it = offsetGraphicsItems.begin();
      it != offsetGraphicsItems.end();
      ++it){
    scene.removeItem(*it);
  }
  offsetGraphicsItems.clear();
}

void
MainWindow::clear()
{
  clearPartition();
  clearMinkowski();
  clearSkeleton();
  clearOffset();
  lgi->hide();
  kgongi->hide();
}

#include "Polygon_2.moc"
#include <CGAL/Qt/resources.h>

int main(int argc, char **argv)
{
  QApplication app(argc, argv);

  app.setOrganizationDomain("geometryfactory.com");
  app.setOrganizationName("GeometryFactory");
  app.setApplicationName("Polygon_2 demo");

  // Import resources from libCGAL (Qt5).
  // See https://doc.qt.io/qt-5/qdir.html#Q_INIT_RESOURCE
  CGAL_QT_INIT_RESOURCES;
  Q_INIT_RESOURCE(Polygon_2);

  MainWindow mainWindow;
  mainWindow.show();
  return app.exec();
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_random.h>
#include <OpenTissue/core/math/big/big_types.h>

//#define USE_ATLAS
#include <OpenTissue/core/math/big/big_svd.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

template< typename matrix_type,typename vector_type>
void test(matrix_type const & A, vector_type  & x, vector_type const & b, vector_type const & y)
{
  using std::min;
  using std::max;

  typedef typename matrix_type::value_type real_type;
  typedef typename matrix_type::size_type  size_type;

  real_type const tol = boost::numeric_cast<real_type>(0.01);

  OpenTissue::math::big::svd(A,x,b);

  for(size_type i = 0; i < x.size();++i)
    BOOST_CHECK_CLOSE( real_type( x(i) ), real_type( y(i) ), tol );

  {
    matrix_type invA;

    OpenTissue::math::big::svd_invert(A,invA);

    size_type m = A.size1();
    size_type n = A.size2();
    matrix_type I;
    I.resize(m,n,false);
    ublas::noalias(I) = ublas::prod(A,invA);

    size_type K = min (m, n );
    for(size_type i = 0; i < K;++i)
      BOOST_CHECK_CLOSE( real_type( I(i,i) ), real_type( 1.0 ), tol );

    for(size_type i = 0; i < I.size1(); ++i)
      for(size_type j = 0; j < I.size2(); ++j)
        if(i!=j)
          BOOST_CHECK( fabs( I(i,j) ) <  10e-10 );
  }

}

BOOST_AUTO_TEST_SUITE(opentissue_math_big_svd);

BOOST_AUTO_TEST_CASE(logic_and_valid_arguments_testing)
{
  typedef ublas::compressed_matrix<double> matrix_type;
  typedef ublas::vector<double>            vector_type;
  typedef vector_type::size_type           size_type;

  {
    matrix_type A;
    vector_type x,b;

    BOOST_CHECK_THROW( OpenTissue::math::big::svd(A,x,b), std::invalid_argument );
  }
  {
    matrix_type A;
    vector_type x,b;
    A.resize(4,4,false);

    BOOST_CHECK_THROW( OpenTissue::math::big::svd(A,x,b), std::invalid_argument );
  }
  {
    matrix_type A;
    vector_type x,b;
    A.resize(4,4,false);
    b.resize(4,false);

    BOOST_CHECK_THROW( OpenTissue::math::big::svd(A,x,b), std::invalid_argument );
  }
  {
    matrix_type A;
    vector_type x,b;
    A.resize(4,4,false);
    x.resize(4,false);

    BOOST_CHECK_THROW( OpenTissue::math::big::svd(A,x,b), std::invalid_argument );
  }
  {
    matrix_type A;
    vector_type x,b;
    A.resize(4,4,false);
    x.resize(4,false);
    b.resize(4,false);
    A(0,0) = A(1,1) = A(2,2) = A(3,3) = 1.0;

    BOOST_CHECK_NO_THROW( OpenTissue::math::big::svd(A,x,b) );
  }
  {
    matrix_type A;
    vector_type x,b;
    x.resize(4,false);
    b.resize(4,false);

    BOOST_CHECK_THROW( OpenTissue::math::big::svd(A,x,b), std::invalid_argument );
  }
  {
    matrix_type A;
    vector_type x,b;
    A.resize(4,4,false);
    x.resize(3,false);
    b.resize(4,false);

    BOOST_CHECK_THROW( OpenTissue::math::big::svd(A,x,b), std::invalid_argument );
  }
  {
    matrix_type A;
    vector_type x,b;
    A.resize(4,4,false);
    x.resize(4,false);
    b.resize(3,false);

    BOOST_CHECK_THROW( OpenTissue::math::big::svd(A,x,b), std::invalid_argument );
  }
  {
    matrix_type A;
    vector_type x,b;
    A.resize(4,4,false);
    x.resize(3,false);
    b.resize(3,false);

    BOOST_CHECK_THROW( OpenTissue::math::big::svd(A,x,b), std::invalid_argument );
  }
}

BOOST_AUTO_TEST_CASE(random_test_case)
{

  typedef ublas::compressed_matrix<double> matrix_type;
  typedef ublas::vector<double>            vector_type;
  typedef vector_type::size_type           size_type;

  for(size_type tst=0;tst<1000;++tst)
  {
    size_type N = 10;

    matrix_type A;
    A.resize(N,N,false);

    vector_type x;
    x.resize(N,false);

    vector_type b;
    b.resize(N,false);

    vector_type y;
    y.resize(N,false);

    matrix_type R;
    R.resize(N,N,false);

    OpenTissue::math::Random<double> value(0.0,1.0);
    for(size_t i=0;i<R.size1();++i)
    {
      b(i) = value();
      x(i) = value();
      y(i) = value();
      for(size_t j=0;j<R.size2();++j)
        R(i,j) = value();
    }
    ublas::noalias(A) = ublas::sparse_prod<matrix_type>( ublas::trans(R), R );
    // forcing A to become PD matrix (it should be non-singular at all times)
    for(size_t i=0;i<R.size1();++i)
      A(i,i) += 0.5;

    ublas::noalias(b) = ublas::prod(A,x);
    y.assign(x);

    x.clear();
    test(A,x,b,y);
  }

}

BOOST_AUTO_TEST_SUITE_END();


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#include <rokko/rokko.hpp>
#include <rokko/lapack.h>
#include <boost/lexical_cast.hpp>

typedef rokko::localized_vector<std::complex<double> > vector_t;
typedef rokko::localized_matrix<std::complex<double>, rokko::matrix_col_major> matrix_t;

int main(int argc, char *argv[]) {
  int info;
  int m = 10;
  int n = 6;
  if (argc > 2) {
    m = boost::lexical_cast<int>(argv[1]);
    n = boost::lexical_cast<int>(argv[2]);
  }
  std::cout << "m = " << m << "\nn = " << n << std::endl;
  int k = std::min(m, n);

  // generate random martix
  matrix_t a = matrix_t::Random(m, n);
  std::cout << "Input random column vectors A:\n" << a << std::endl;

  // orthonormalization
  matrix_t mat = a;
  vector_t tau(k);
  info = LAPACKE_zgeqrf(LAPACK_COL_MAJOR, m, n, &mat(0, 0), m, &tau(0));
  matrix_t r = mat;
  for (int i = 0; i < m; ++i)
    for (int j = 0; j < i; ++j)
      r(i, j) = 0;
  std::cout << "Upper triangle matrix R:\n" << r << std::endl;
  info = LAPACKE_zungqr(LAPACK_COL_MAJOR, m, k, k, &mat(0, 0), m, &tau(0));
  matrix_t q(m, k);
  for (int i = 0; i < m; ++i)
    for (int j = 0; j < k; ++j)
      q(i, j) = mat(i, j);
  std::cout << "Orthonormalized column vectors Q:\n" << q << std::endl;

  // check orthonormality
  matrix_t check = q.adjoint() * q;
  std::cout << "Check orthogonality:\n" << check << std::endl;
  std::cout << "Error: | I - Qt Q | = " <<  (matrix_t::Identity(n, n) - check).norm() << std::endl;

  // check decomposition
  std::cout << "Reconstruction of original matrix QR:\n" << q * r << std::endl;
  std::cout << "Error: | A - QR | = " <<  (a - q * r).norm() << std::endl;
}


#include <StatSim/Math.hpp>

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

#include <cmath>
#include <numbers>

namespace StatSim {
	double BinomialPMF(int tryCount, int occurCount, double probability) {
		const auto numberOfCases = [](int n, int r) {
			if (n - r < r) {
				r = n - r;
			}

			boost::multiprecision::cpp_int result = 1;
			for (int i = 0; i < r; ++i) {
				result *= n - i;
			}
			for (int i = 0; i < r; ++i) {
				result /= i + 1;
			}
			return result;
		}(tryCount, occurCount);

		const boost::multiprecision::cpp_bin_float_100 probabilityEx(probability);
		const auto occurProbability = boost::multiprecision::pow(probabilityEx, occurCount);
		const auto notOccurProbability = boost::multiprecision::pow(1 - probabilityEx, tryCount - occurCount);

		const auto result = static_cast<boost::multiprecision::cpp_bin_float_100>(numberOfCases) * occurProbability * notOccurProbability;
		return static_cast<double>(result);
	}
	double NormalCDF(double value, double mean, double standardDeviation) {
		return 0.5 * std::erfc((mean - value) / standardDeviation / std::numbers::sqrt2_v<double>);
	}
}


#include<bits/stdc++.h>
#include <boost/multiprecision/cpp_int.hpp>
#define ll long long int
#define watch(x) cout << (#x) << " is " << (x) << endl  //watch function print variable and value for debugging
#define count_ones __builtin_popcountll                  // count_ones(9) is equal to 2 valid for ll also
#define fast_io ios_base::sync_with_stdio(false); cin.tie(NULL);
#define fo(i,n) for(ll i=0;i<n;i++)
using boost::multiprecision::cpp_int;
using namespace std;

cpp_int boost_factorial(int num)
{
    cpp_int fact = 1;
    for (int i=num; i>1; --i)
        fact *= i;
    return fact;
}

int main() {
 #ifndef ONLINE_JUDGE
	    freopen("input.txt", "r", stdin);
        freopen("output.txt", "w", stdout);
 #endif
       fast_io

       ll t;
       cin>>t;
       while(t--)
       {
       	ll n;
       	cin>>n;
       	cout<<boost_factorial(n)<<"\n";

       }

   return 0;
 }


#include "point_to_plane.h"
#include <Eigen/Dense>

using namespace probreg;

Pt2PlResult probreg::computeTwistForPointToPlane(const Matrix3X& model,
                                                 const Matrix3X& target,
                                                 const Matrix3X& target_normal,
                                                 const Vector& weight) {
    Matrix6 ata = Matrix6::Zero();
    Vector6 atb = Vector6::Zero();
    Float r_sum = 0.0;

    for (auto k = 0; k < model.cols(); ++k){
        const auto& vertex_k = model.col(k);
        const auto& target_k = target.col(k);
        const auto& normal_k = target_normal.col(k);
        const auto& weight_k = weight[k];
        const Float residual = normal_k.dot(target_k - vertex_k);
        const Vector6 jac = (Vector6() << vertex_k.cross(normal_k), normal_k).finished();
        ata += weight_k * jac * jac.transpose();
        atb += weight_k * residual * jac;
        r_sum += weight_k * weight_k * residual * residual;
    }
    return std::make_pair(ata.selfadjointView<Eigen::Upper>().ldlt().solve(atb), r_sum);
}


#include<iostream>
#include <math.h>
#include <vector>
#include <random>
#include <iomanip>
#include <Eigen/Core>
#include <algorithm>
using namespace std;
//高斯密度函数
float gussian_fx(float x){
    float weight=1/(sqrt(2*M_PI));
    float exow=-pow(x,2)/2;
    return (weight*exp(exow));
};
//求X<x 的概率
float guassian_pb(float x, float u,float sigma2){
    float detx=0.01;
    float pb=0;
    float MINX=-5;
    while (MINX<=(x-u)/sqrt(sigma2)){
        MINX+=detx;
        pb+=detx*gussian_fx(MINX);
    };
    return pb;
};
float guassian_pb_from_data(vector<float >data,float x){
    static int i=0,j=0;
    while(i<data.size()){
        if(data[i]<=x){i++;j++;}
        else{i++;};
    }
    return float(j)/data.size();
}
//最大似然估计，接受参数为向量，返回pair first为均值，second为方差
pair<float,float> MaxLikelihood(vector<float > &xi){
    float hatu=0; float hatsigma2=0;
    for (int i = 0; i <xi.size() ; ++i) {
        hatu+=xi[i];
    }
    for (int j = 0; j <xi.size() ; ++j) {
        hatsigma2+=pow(xi[j]-(hatu/xi.size()),2);
    }
    pair<float ,float > us={hatu/xi.size(),hatsigma2/xi.size()};
    return us;
};

//生成正态分布，mu均值，sigma2方差，默认个数500
vector<float >NormalGenerating(float mu,float sigma2,int length=500){
    random_device rd;
    std::mt19937 gen(rd());
    uniform_real_distribution<float > ud(0.0,1.0);
    int i=0;
    vector<float > seed;
    while(i<length) {
        float x = ud(gen);
        float y = ud(gen);
        float temp;
        if (i % 2) {
            temp =cos(2 * M_PI * x) * sqrt(-2 * log(1 - y));
        }
        else{
            temp=sin(2*M_PI*x)*sqrt(-2*log(1-y));
        }
        float ns=temp*sqrt(sigma2)+mu;
        i++;
        seed.push_back(ns);
    }
    return seed;
}
//生成正态分布，mu均值，sigma2方差，默认个数500
vector<float >NormalGenerating2(float mu,float sigma2,int length=500){
    random_device rd;
    std::mt19937 gen(rd());
    normal_distribution<float > normal(mu,sqrt(sigma2));
    vector<float >seed;
    while(length--){
        seed.push_back(normal(gen));
    }
    return seed;
}

//需要显示初始化
struct MultiNormal{
    int length=500;
    float u=0;
    float sigma2=0;
    vector<float> Gussiandata;
    MultiNormal(vector<float> data){
        u=MaxLikelihood(data).first;
        sigma2=MaxLikelihood(data).second;
        Gussiandata=data;
    }
};
//混合高斯分布，将多个正态分布相加
vector<float>MultiNormalAdding(vector <MultiNormal> &kargs){
    vector<float > reciver(500,0);
    int k=kargs.size();
    for (int loc = 0; loc <500 ; ++loc) {
        int j=0;
        while(j<k) {
            reciver[loc]+=kargs[j].Gussiandata[loc];
            j++;
        }
    }
    return reciver;
}
//混合高斯分布，将多个正态分布混合
vector<float>MultiNormalMerging(vector <MultiNormal> &kargs){
    int k=kargs.size();
    int Alllength=0;
    int i=0;
    while(i<k){
        Alllength+=kargs[i].Gussiandata.size();
        i++;
    }
    vector<float > reciver(Alllength,0);
    int j=0;
    int det=0;
    while(j<k){
        for (int l = 0; l <kargs[j].Gussiandata.size() ; ++l) {
            int pos=j-1;
            if(pos==-1)pos=0;
            reciver[l+j*kargs[pos].Gussiandata.size()]=kargs[j].Gussiandata[l];
        }
        j++;
    }
    return reciver;
}

//生成统计数据
vector<vector<float >> CalNormalData(vector<float > &multgassian,int quration,int GMM=2){
    vector<float > temp=multgassian;
    sort(temp.begin(),temp.end());
    float mindata=temp.front();
    float maxdata=temp.back();
    float middle=(mindata+maxdata)/2;
    cout<<middle<<endl;
    //cout<<temp.size()<<endl;
    //for(auto &item:temp)cout<<item<<endl;
    int qu=quration;//分区数
    float step=(maxdata-mindata)/qu;//分区数;
    vector<vector<float >> SortedHistData;
    int i=0;
    int pos=0;
    while(i<qu){
        vector<float> tempdata;
        while(mindata+i*step<=temp[pos]&&temp[pos]<=mindata+(i+1)*step){
            tempdata.push_back(temp[pos]);
            pos++;
        }
        if(tempdata.size()==0) tempdata.push_back(0);//防止该分区为空
        SortedHistData.push_back(tempdata);
        tempdata.clear();
        i++;
    }
    Eigen::MatrixXf QKI(quration,GMM);
    Eigen::MatrixXf SITA(GMM,2);//同列0表示mu 1表示sigma2
    SITA<<middle,middle,1,1;
    vector<float> cof;
    for (int i = 0; i <quration ; ++i) {
        for (int j = 0; j <GMM ; ++j) { //混合个数
            QKI(i,j)=guassian_pb(SortedHistData[i].back(),SITA(j,0),SITA(j,1))
                     -guassian_pb(SortedHistData[i].front(),SITA(j,0),SITA(j,1));
        }
    }
    for (int k = 0; k <quration ; ++k) {
        float temp=0;
        for (int l = 0; l <GMM ; ++l) {
            temp+=QKI(k,l);

        }
        cof.push_back(temp);
    }
    for (int m = 0; m <quration ; ++m) {
        for (int n = 0; n <GMM ; ++n) {
            if(!cof[m])QKI(m,n)=0;
            else QKI(m,n)=QKI(m,n)/cof[m];
        }

    }
    //cout<<QKI<<endl;

    return SortedHistData;

}

int main(){
    vector<float > seed1=NormalGenerating(0,2);
    vector<float > seed2=NormalGenerating2(10,2);
    //vector<float > seed3=NormalGenerating2(15,2);
    MultiNormal No1(seed1);
    MultiNormal No2(seed2);
   // MultiNormal No3(seed3);
    vector<MultiNormal> Multiseeds;
    Multiseeds.push_back(No1);
    Multiseeds.push_back(No2);
    //Multiseeds.push_back(No3);
    vector<float > merge=MultiNormalMerging(Multiseeds);
    //for(auto &item:merge)cout<<item<<endl;
    //cout<<"the mixed guassian mean is u="<<MaxLikelihood(merge).first<<endl<<"the mixed guassian variance is simga="<<MaxLikelihood(merge).second<<endl;
//    for(auto &item:CalNormalData(merge,20)){
//        for(auto &j:item){
//            cout<<j<<"  "<<endl;
//        }
//        cout<<endl;
//    }
    CalNormalData(merge,20);
    return 0;
}


#include <iostream>

#include <boost/multiprecision/gmp.hpp>

#include "headers/euler.hpp"

const int EXPANSIONS_BEGIN = 1;
const int EXPANSIONS_NUM = 1000;

typedef boost::multiprecision::mpz_int big_int;
typedef euler::fraction<big_int> fraction;

fraction expandSqrt(int n, int depth = 0) {
  if (depth == n) {
    return fraction(1, 2);
  }

  fraction cd = expandSqrt(n, depth + 1);

  big_int a = cd.denom + (depth ? 0 : cd.numer);
  big_int b = depth ? (2 * cd.denom + cd.numer) : cd.denom;

  return fraction(a, b);
}

int main(int argc, char** argv) {
  int termsWithHigherDigitCountNumers = 0;

  for (int i = EXPANSIONS_BEGIN; i <= EXPANSIONS_NUM; i++) {
    fraction expandedTerm = expandSqrt(i);

    if (euler::countDigits(expandedTerm.numer) > euler::countDigits(expandedTerm.denom)) {
      termsWithHigherDigitCountNumers++;
    }
  }

  std::cout << "Total expansions between " << EXPANSIONS_BEGIN << " and " << EXPANSIONS_NUM
            << " of sqrt(2) with higher counts of digits in numerator than denominator: "
            << termsWithHigherDigitCountNumers << std::endl;

  return 0;
}


#include <opencv2/opencv.hpp>
#include <string>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace cv;

string file_1 = "./LK1.png";  // first image
string file_2 = "./LK2.png";  // second image

class OpticalFlowTracker{
public:
    OpticalFlowTracker(const Mat &img1_,
                       const Mat &img2_,
                       const vector<KeyPoint> &kp1_,
                       vector<KeyPoint> &kp2_,
                       vector<bool> &success_,
                       bool inverse_ = true,
                       bool has_initial_ = false):
                       img1(img1_), img2(img2_), kp1(kp1_), kp2(kp2_), success(success_), inverse(inverse_),
                       has_initial(has_initial_) {}

    void calculateOpticalFlow(const Range &range);

private:
    const Mat &img1;
    const Mat &img2;
    const vector<KeyPoint> &kp1;
    vector<KeyPoint> &kp2;
    vector<bool> &success;
    bool inverse = true;
    bool has_initial = false;
};

void OpticalFlowMultiLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint> &kp1,
    vector<KeyPoint> &kp2,
    vector<bool> &success,
    bool inverse = false
);

void OpticalFlowSingleLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint> &kp1,
    vector<KeyPoint> &kp2,
    vector<bool> &success,
    bool inverse = false, bool has_initial = false);

inline float GetPixelValue(const Mat &img, double x, double y){
    if (x < 0) x = 0;
    else if (x > img.cols - 1) x = img.cols - 1;
    if (y < 0) y = 0;
    else if (y > img.rows - 1) y = img.rows - 1;

    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    float pixelValue = (1-xx)*(1-yy)*data[0] + (xx)*(1-yy)*data[1]
                        + (1-xx)*(yy)*data[img.step] + (xx)*(yy)*data[img.step+1];
    return pixelValue;
}

int main(int argc, char **argv){
    cv::Mat img_1 = cv::imread(file_1, 0);
    cv::Mat img_2 = cv::imread(file_2, 0);

    // 从img1中提取特征点，使用GFTT特征
    vector<KeyPoint> kp_1;
    Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20);
    detector->detect(img_1, kp_1);

    // OpenCV LK光流
    vector<Point2f> pts_1, pts_2;
    for (auto &kp:kp_1){
        pts_1.push_back(kp.pt);
    }
    vector<uchar> status;
    vector<float> error;
    cv::calcOpticalFlowPyrLK(img_1, img_2, pts_1, pts_2, status, error);
    // 画图：By OpenCV LK光流
    Mat img_2_CV;
    cv::cvtColor(img_2,img_2_CV, CV_GRAY2BGR);
    for (int i=0; i<pts_2.size(); i++){
        if (status[i]){
            cv::circle(img_2_CV, pts_2[i], 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img_2_CV, pts_1[i], pts_2[i], cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("By OpenCV", img_2_CV);

    // 单层LK光流
    vector<KeyPoint> kp_2_single;
    vector<bool> success_single;
    OpticalFlowSingleLevel(img_1, img_2, kp_1, kp_2_single, success_single);
    // 画图：单层LK光流
    Mat img_2_single;
    cv::cvtColor(img_2,img_2_single, CV_GRAY2BGR);
    for (int i=0; i<kp_2_single.size(); i++){
        if (success_single[i]){
            cv::circle(img_2_single, kp_2_single[i].pt, 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img_2_single, kp_1[i].pt, kp_2_single[i].pt, cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("By single", img_2_single);

    // 反向单层LK光流
    vector<KeyPoint> kp_2_single_inverse;
    vector<bool> success_single_inverse;
    OpticalFlowSingleLevel(img_1, img_2, kp_1, kp_2_single_inverse, success_single_inverse, true);
    // 画图：单层LK光流
    Mat img_2_single_inverse;
    cv::cvtColor(img_2,img_2_single_inverse, CV_GRAY2BGR);
    for (int i=0; i<kp_2_single_inverse.size(); i++){
        if (success_single_inverse[i]){
            cv::circle(img_2_single_inverse, kp_2_single_inverse[i].pt, 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img_2_single_inverse, kp_1[i].pt, kp_2_single_inverse[i].pt, cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("By single_inverse", img_2_single_inverse);

    // 多层LK光流
    vector<KeyPoint> kp_2_multi;
    vector<bool> success_multi;
    OpticalFlowMultiLevel(img_1, img_2, kp_1, kp_2_multi, success_multi, true);
    // OpticalFLowMultiLevel(img_1, img_2, kp_1, kp_2_multi, success_multi, true);
    // 画图：多层LK光流
    Mat img_2_multi;
    cv::cvtColor(img_2,img_2_multi, CV_GRAY2BGR);
    for (int i=0; i<kp_2_multi.size(); i++){
        if (success_multi[i]){
            cv::circle(img_2_multi, kp_2_multi[i].pt, 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img_2_multi, kp_1[i].pt, kp_2_multi[i].pt, cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("By multi", img_2_multi);

    cv::waitKey(0);
    return 0;
}

void OpticalFlowSingleLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint> &kp1,
    vector<KeyPoint> &kp2,
    vector<bool> &success,
    bool inverse, bool has_initial){

    kp2.resize(kp1.size());
    success.resize(kp1.size());
    OpticalFlowTracker tracker(img1, img2, kp1, kp2, success, inverse, has_initial);
    parallel_for_(Range(0, kp1.size()),
        std::bind(&OpticalFlowTracker::calculateOpticalFlow, &tracker, placeholders::_1));
}

void OpticalFlowMultiLevel(
    const Mat &img1,
    const Mat &img2,
    const vector<KeyPoint> &kp1,
    vector<KeyPoint> &kp2,
    vector<bool> &success,
    bool inverse) {

    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = {1.0, 0.5, 0.25, 0.125};

    vector<Mat> pyr1, pyr2;
    for (int i = 0; i < pyramids; i++){
        if(i==0){
            pyr1.push_back(img1);
            pyr2.push_back(img2);
        }
        else{
            Mat img1_ptr, img2_ptr;
            cv::resize(pyr1[i-1], img1_ptr, Size(), pyramid_scale, pyramid_scale);
            cv::resize(pyr2[i-1], img2_ptr, Size(), pyramid_scale, pyramid_scale);
            pyr1.push_back(img1_ptr);
            pyr2.push_back(img2_ptr);
        }
    }

    vector<KeyPoint> kp1_ptr, kp2_ptr;
    for (auto &kp: kp1){
        auto kp_top = kp;
        kp_top.pt *= (pow(pyramid_scale, pyramids - 1));
        kp1_ptr.push_back(kp_top);
        kp2_ptr.push_back(kp_top);
    }

    for(int level = pyramids - 1; level >= 0; level--){
        success.clear();
        OpticalFlowSingleLevel(pyr1[level], pyr2[level], kp1_ptr, kp2_ptr, success, inverse, true);

        if (level > 0){
            for (auto &kp:kp1_ptr){
                kp.pt /= pyramid_scale;
            }
            for (auto &kp:kp2_ptr){
                kp.pt /= pyramid_scale;
            }
        }
    }

    for(auto &kp: kp2_ptr){
        kp2.push_back(kp);
    }
}

// void OpticalFlowTracker::calculateOpticalFlow(const Range &range){
//     int half_patch_size = 4;
//     int iterations = 10;
//     for(size_t i = range.start; i < range.end; i++){
//         auto kp = kp1[i];
//         double dx = 0, dy = 0;
//         if (has_initial){
//             dx = kp2[i].pt.x - kp.pt.x;
//             dy = kp2[i].pt.y - kp.pt.y;
//         }

//         double cost = 0, lastCost = 0;
//         bool succ = true;

//         // Gauss-Newton iterations
//         Eigen::Matrix2d H = Eigen::Matrix2d::Zero();
//         Eigen::Vector2d b = Eigen::Vector2d::Zero();
//         Eigen::Vector2d J;

//         for (int iter = 0; iter < iterations; iter++){

//             cost = 0;
//             if (inverse == false){
//                 H = Eigen::Matrix2d::Zero();
//                 b = Eigen::Vector2d::Zero();
//             }
//             else{
//                 b = Eigen::Vector2d::Zero();
//             }

//             for (int x = -half_patch_size; x < half_patch_size; x++){
//                 for (int y = -half_patch_size; y < half_patch_size; y++) {
//                     double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y) -
//                                    GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);;  // Jacobian
//                     if (inverse == false) {
//                         J = -1.0 * Eigen::Vector2d(
//                             0.5 * (GetPixelValue(img2, kp.pt.x + dx + x + 1, kp.pt.y + dy + y) -
//                                    GetPixelValue(img2, kp.pt.x + dx + x - 1, kp.pt.y + dy + y)),
//                             0.5 * (GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y + 1) -
//                                    GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y - 1))
//                         );
//                     } else if (iter == 0) {
//                         // in inverse mode, J keeps same for all iterations
//                         // NOTE this J does not change when dx, dy is updated, so we can store it and only compute error
//                         J = -1.0 * Eigen::Vector2d(
//                             0.5 * (GetPixelValue(img1, kp.pt.x + x + 1, kp.pt.y + y) -
//                                    GetPixelValue(img1, kp.pt.x + x - 1, kp.pt.y + y)),
//                             0.5 * (GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y + 1) -
//                                    GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y - 1))
//                         );
//                     }
//                     // compute H, b and set cost;
//                     b += -error * J;
//                     cost += error * error;
//                     if (inverse == false || iter == 0) {
//                         // also update H
//                         H += J * J.transpose();
//                     }
//                 }
//             }

//             Eigen::Vector2d update = H.ldlt().solve(b);

//             if (std::isnan(update[0])) {
//                 // sometimes occurred when we have a black or white patch and H is irreversible
//                 cout << "update is nan" << endl;
//                 succ = false;
//                 break;
//             }

//             if (iter > 0 && cost > lastCost) {
//                 break;
//             }

//             // update dx, dy
//             dx += update[0];
//             dy += update[1];
//             lastCost = cost;
//             succ = true;

//             if (update.norm() < 1e-2) {
//                 // converge
//                 break;
//             }
//         }

//         success[i] = succ;

//         // set kp2
//         kp2[i].pt = kp.pt + Point2f(dx, dy);
//     }
// }

void OpticalFlowTracker::calculateOpticalFlow(const Range &range) {
    // parameters
    int half_patch_size = 4;
    int iterations = 10;
    for (size_t i = range.start; i < range.end; i++) {
        auto kp = kp1[i];
        double dx = 0, dy = 0; // dx,dy need to be estimated
        if (has_initial) {
            dx = kp2[i].pt.x - kp.pt.x;
            dy = kp2[i].pt.y - kp.pt.y;
        }

        double cost = 0, lastCost = 0;
        bool succ = true; // indicate if this point succeeded

        // Gauss-Newton iterations
        Eigen::Matrix2d H = Eigen::Matrix2d::Zero();    // hessian
        Eigen::Vector2d b = Eigen::Vector2d::Zero();    // bias
        Eigen::Vector2d J;  // jacobian
        for (int iter = 0; iter < iterations; iter++) {
            if (inverse == false) {
                H = Eigen::Matrix2d::Zero();
                b = Eigen::Vector2d::Zero();
            } else {
                // only reset b
                b = Eigen::Vector2d::Zero();
            }

            cost = 0;

            // compute cost and jacobian
            for (int x = -half_patch_size; x < half_patch_size; x++)
                for (int y = -half_patch_size; y < half_patch_size; y++) {
                    double error = GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y) -
                                   GetPixelValue(img2, kp.pt.x + x + dx, kp.pt.y + y + dy);;  // Jacobian
                    if (inverse == false) {
                        J = -1.0 * Eigen::Vector2d(
                            0.5 * (GetPixelValue(img2, kp.pt.x + dx + x + 1, kp.pt.y + dy + y) -
                                   GetPixelValue(img2, kp.pt.x + dx + x - 1, kp.pt.y + dy + y)),
                            0.5 * (GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y + 1) -
                                   GetPixelValue(img2, kp.pt.x + dx + x, kp.pt.y + dy + y - 1))
                        );
                    } else if (iter == 0) {
                        // in inverse mode, J keeps same for all iterations
                        // NOTE this J does not change when dx, dy is updated, so we can store it and only compute error
                        J = -1.0 * Eigen::Vector2d(
                            0.5 * (GetPixelValue(img1, kp.pt.x + x + 1, kp.pt.y + y) -
                                   GetPixelValue(img1, kp.pt.x + x - 1, kp.pt.y + y)),
                            0.5 * (GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y + 1) -
                                   GetPixelValue(img1, kp.pt.x + x, kp.pt.y + y - 1))
                        );
                    }
                    // compute H, b and set cost;
                    b += -error * J;
                    cost += error * error;
                    if (inverse == false || iter == 0) {
                        // also update H
                        H += J * J.transpose();
                    }
                }

            // compute update
            Eigen::Vector2d update = H.ldlt().solve(b);

            if (std::isnan(update[0])) {
                // sometimes occurred when we have a black or white patch and H is irreversible
                cout << "update is nan" << endl;
                succ = false;
                break;
            }

            if (iter > 0 && cost > lastCost) {
                break;
            }

            // update dx, dy
            dx += update[0];
            dy += update[1];
            lastCost = cost;
            succ = true;

            if (update.norm() < 1e-2) {
                // converge
                break;
            }
        }

        success[i] = succ;

        // set kp2
        kp2[i].pt = kp.pt + Point2f(dx, dy);
    }
}


/**
 * @file
 * @brief  NPDE UnstableBVP. Solution of source-free heat equation and
 * computation of H1 seminorms on different triangular meshes and refinement
 * levels
 * @author Julien Gacon, Ralf Hiptmair, Amélie Loher
 * @date   March 2019
 * @copyright MIT License
 */
#include "unstablebvp.h"
// General includes
#include <fstream>
#include <iomanip>
#include <memory>
// Math includes
#include <cmath>
// Eigen
#include <Eigen/Core>
// Lehrfempp
#include <lf/mesh/mesh.h>
#include <lf/refinement/refinement.h>

int main() {
  // Define the number of refinement levels we want for our mesh
  const int reflevels = 7;

  // Do the computations for all three possible mesh locations:
  // Above x2 = 0 (top), intersecting it (center), or below it (bottom)
  // Store the data in a Eigen Matrix for nice display later on
  Eigen::MatrixXd h1_seminorms(reflevels + 1, 3), h1_diffs(reflevels + 1, 3);

  // 0 for top (-> first column), 1 for center, 2 for bottom
  int type_idx = 0;
  for (auto mesh_type : {"top", "center", "bottom"}) {
    // Get a hierachy of refined meshes
    std::shared_ptr<lf::refinement::MeshHierarchy> multi_mesh_p =
        UnstableBVP::createMeshHierarchy(reflevels, mesh_type);
    lf::refinement::MeshHierarchy &multi_mesh{*multi_mesh_p};

    // Number of levels
    const int L = multi_mesh.NumLevels();

    // Computing the difference u_L - u_k, k=0...L-1 is hard, since we would
    // the functions are defined on different meshes.
    // To avoid this caveat we can approximate using the triangle inequality:
    // abs(norm(u_L) - norm(u_k)) <= norm(u_L - u_k)
    // This should also display the right convergence properties.

    // H1 seminorm of most refined solution (level L-1)
    double h1_uL =
        UnstableBVP::solveTemperatureDistribution(multi_mesh.getMesh(L - 1));

    // Store H1 seminorms and the difference of the seminorms to uL
    h1_seminorms(reflevels, type_idx) = h1_uL;
    h1_diffs(reflevels, type_idx) = 0;

    // Compute H1 seminorm for all levels 0..L-2
    for (int level = 0; level < L - 1; ++level) {
      // Get the mesh pointer
      std::shared_ptr<const lf::mesh::Mesh> mesh_p = multi_mesh.getMesh(level);

      // Get the seminorm
      double h1 = UnstableBVP::solveTemperatureDistribution(mesh_p);

      // Store
      h1_seminorms(level, type_idx) = h1;
      h1_diffs(level, type_idx) = std::abs(h1_uL - h1);
    }
    ++type_idx;
  }

  // Print to terminal
  std::cout << std::left << std::setfill('-');
  std::cout << std::setw(39) << "#  -- Above x2 = 0 " << std::setw(30)
            << "|-- Intersecting x2 = 0 " << std::setw(30)
            << "|-- Below x2 = 0 "
            << "\n"
            << std::right << std::setfill(' ');
  std::cout << "#" << std::setw(8) << "Level" << std::setw(10) << "|u_k|"
            << std::setw(20) << "||u_L| - |u_k||" << std::setw(10) << "|u_k|"
            << std::setw(20) << "||u_L| - |u_k||" << std::setw(10) << "|u_k|"
            << std::setw(20) << "||u_L| - |u_k||"
            << "\n";

  for (int l = 0; l < reflevels; ++l) {
    std::cout << std::setw(9) << l << std::setw(10) << h1_seminorms(l, 0)
              << std::setw(20) << h1_diffs(l, 0) << std::setw(10)
              << h1_seminorms(l, 1) << std::setw(20) << h1_diffs(l, 1)
              << std::setw(10) << h1_seminorms(l, 2) << std::setw(20)
              << h1_diffs(l, 2) << "\n";
  }
  std::cout << std::setw(9) << reflevels << std::setw(10)
            << h1_seminorms(reflevels, 0) << std::setw(20) << 0 << std::setw(10)
            << h1_seminorms(reflevels, 1) << std::setw(20) << 0 << std::setw(10)
            << h1_seminorms(reflevels, 2) << std::setw(20) << 0 << "\n";

  return 0;
}


#include "my_pearce_not_recursive.cpp"
#include "include_and_types.cpp"
#include <boost/property_map/dynamic_property_map.hpp>
#include <boost/graph/graphml.hpp>
#include <boost/property_map/dynamic_property_map.hpp>

using namespace boost;

int main(int, char*[])
{
    /*This is an example of how to modify this code for describing a graph "by hand" without using graphml parser and
    stdin*/
    /*typedef std::pair<int, int> Edge;

    const int num_nodes = 8;

    enum nodes { A, B, C, D, E, F, G, H, I, L, M, N};
    char name[] = "ABCDEFGHILMN";

    Edge edge_array[] = { Edge(A, B),
                          Edge(B, C), Edge(B, H),
                          Edge(C, D), Edge(C, G),
                          Edge(D, E),
                          Edge(E, F), Edge(E, C),
                          Edge(G, F), Edge(G, D),
                          Edge(H, A), Edge(H, G),
                          Edge(I, L),
                          Edge(M, N), Edge(N, M)
    };

    int num_arcs = sizeof(edge_array) / sizeof(Edge);
    Graph g(edge_array, edge_array + num_arcs, num_nodes);*/

    //Reading graph from stdin
    Graph g;
    dynamic_properties dp;
    read_graphml(std::cin, g, dp);

    //Graph printing
    std::cout << "A directed graph:" << std::endl;
    print_graph(g, get(vertex_index,g));
    std::cout << std::endl;

    //Instantiation of the PearceNR class object
    PearceNR <typeBool, typeInt> pearcenr(&g);
    std::vector<int>* rindex = pearcenr.pearce_not_recursive_scc();

    //Results printing
    IndexMap index=get(vertex_index,g);
    for (int i = 0; i != num_vertices(g); ++i){
        std::cout << index[i] << " -> " << (*rindex)[i] << std::endl;
    }
    return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef META_MATH_IS_POWER_OF_2_INCLUDE
#define META_MATH_IS_POWER_OF_2_INCLUDE

#include <boost/numeric/meta_math/least_significant_one_bit.hpp>

namespace meta_math {

template <unsigned long X>
struct is_power_of_2
{
    static const bool value= X == least_significant_one_bit<X>::value;
};

} // namespace meta_math

#endif // META_MATH_IS_POWER_OF_2_INCLUDE