//---------------------------------------------------------------------------//
//!
//! \file   Utility_Measurement.hpp
//! \author Alex Robinson
//! \brief  The measurement class declaration. This object is based of of the
//!         measurement class in the boost::units examples
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_MEASUREMENT_HPP
#define UTILITY_MEASUREMENT_HPP

// Boost Includes
#include <boost/units/config.hpp>
#include <boost/units/operators.hpp>
#include <boost/typeof/typeof.hpp>

// Trilinos Includes
#include <Teuchos_ScalarTraits.hpp>

// FRENSIE Includes
#include "Utility_PrintableObject.hpp"
#include "Utility_ContractException.hpp"

namespace Utility{

/*! The measurement class
 *
 * This class wraps is meant to be used as any typical scalar type
 * (e.g. double). It is designed to keep track of and propagate the
 * uncertainty of a value (like one would with a measured quantity).
 */
template<typename T>
class Measurement : public PrintableObject
{

private:

  // The scalar traits typedef
  typedef Teuchos::ScalarTraits<T> ST;

public:

  //! The typedef for this type
  typedef Measurement<T> ThisType;

  //! The typedef for the value type
  typedef T ValueType;

  //! Constructor
  Measurement( const ValueType& value = ValueType(),
	       const ValueType& uncertainty = ValueType() );

  //! Copy constructor
  Measurement( const ThisType& other_measurement );

  //! Destructor
  ~Measurement()
  { /* ... */ }

  //! Print method
  void print( std::ostream& os ) const;

  //! Return the value of the measurement
  const ValueType& getValue() const;

  //! Return the uncertainty of the measurement
  const ValueType& getUncertainty() const;

  //! Return the relative uncertainty of the measurement
  const ValueType getRelativeUncertainty() const;

  //! Return the lower bound of the measurement
  const ValueType getLowerBound() const;

  //! Return the upper bound of the measurement
  const ValueType getUpperBound() const;

  //! Implicit conversion to value type
  operator ValueType() const;

  //! In-place addition operator
  ThisType& operator+=( const ValueType& value );

  //! In-place addition operator
  ThisType& operator+=( const ThisType& other_measurement );

  //! In-place subtraction operator
  ThisType& operator-=( const ValueType& value );

  //! In-place subtraction operator
  ThisType& operator-=( const ThisType& other_measurement );

  //! In-place multiplication operator
  ThisType& operator*=( const ValueType& value );

  //! In-place multiplication operator
  ThisType& operator*=( const ThisType& other_measurement );

  //! In-place division operator
  ThisType& operator/=( const ValueType& value );

  //! In-place division operator
  ThisType& operator/=( const ThisType& other_measurement );

private:

  // The measurement value
  ValueType d_value;

  // The measurement uncertainty
  ValueType d_uncertainty;
};

//! Addition operator
template<typename T>
inline Measurement<T> operator+( T lhs, const Measurement<T>& rhs )
{
  // Make sure nested conditions are met
  testNestedConditionsBegin(1);

  return (Measurement<T>( lhs, T(0) ) += rhs);

  testNestedConditionsEnd(1);
}

//! Addition operator
template<typename T>
inline Measurement<T> operator+( const Measurement<T>& lhs, T rhs )
{
  // Make sure nested conditions are met
  testNestedConditionsBegin(1);

  return (Measurement<T>( lhs ) += Measurement<T>( rhs, T(0) ));

  testNestedConditionsEnd(1);
}

//! Addition operator
template<typename T>
inline Measurement<T> operator+( const Measurement<T>& lhs,
				 const Measurement<T>& rhs )
{
  // Make sure nested conditions are met
  testNestedConditionsBegin(1);

  return (Measurement<T>( lhs ) += rhs );

  testNestedConditionsEnd(1);
}

//! Subtraction operator
template<typename T>
inline Measurement<T> operator-( T lhs, const Measurement<T>& rhs )
{
  // Make sure nested conditions are met
  testNestedConditionsBegin(1);

  return (Measurement<T>( lhs, T(0) ) -= rhs);

  testNestedConditionsEnd(1);
}

//! Subtraction operator
template<typename T>
inline Measurement<T> operator-( const Measurement<T>& lhs, T rhs )
{
  // Make sure nested conditions are met
  testNestedConditionsBegin(1);

  return (Measurement<T>( lhs ) -= Measurement<T>( rhs, T(0) ));

  testNestedConditionsEnd(1);
}

//! Subtraction operator
template<typename T>
inline Measurement<T> operator-( const Measurement<T>& lhs,
				 const Measurement<T>& rhs )
{
  // Make sure nested conditions are met
  testNestedConditionsBegin(1);

  return (Measurement<T>( lhs ) -= rhs );

  testNestedConditionsEnd(1);
}

//! Multiplication operator
template<typename T>
inline Measurement<T> operator*( T lhs, const Measurement<T>& rhs )
{
  // Make sure nested conditions are met
  testNestedConditionsBegin(1);

  return (Measurement<T>( lhs, T(0) ) *= rhs);

  testNestedConditionsEnd(1);
}

//! Multiplication operator
template<typename T>
inline Measurement<T> operator*( const Measurement<T>& lhs, T rhs )
{
  // Make sure nested conditions are met
  testNestedConditionsBegin(1);

  return (Measurement<T>( lhs ) *= Measurement<T>( rhs, T(0) ));

  testNestedConditionsEnd(1);
}

//! Multiplication operator
template<typename T>
inline Measurement<T> operator*( const Measurement<T>& lhs,
				 const Measurement<T>& rhs )
{
  // Make sure nested conditions are met
  testNestedConditionsBegin(1);

  return (Measurement<T>( lhs ) *= rhs );

  testNestedConditionsEnd(1);
}

//! Division operator
template<typename T>
inline Measurement<T> operator/( T lhs, const Measurement<T>& rhs )
{
  // Make sure nested conditions are met
  testNestedConditionsBegin(1);

  // Make sure nested conditions are met
  return (Measurement<T>( lhs, T(0) ) /= rhs);

  testNestedConditionsEnd(1);
}

//! Division operator
template<typename T>
inline Measurement<T> operator/( const Measurement<T>& lhs, T rhs )
{
  // Make sure nested conditions are met
  testNestedConditionsBegin(1);

  return (Measurement<T>( lhs ) /= Measurement<T>( rhs, T(0) ));

  testNestedConditionsEnd(1);
}

//! Division operator
template<typename T>
inline Measurement<T> operator/( const Measurement<T>& lhs,
				 const Measurement<T>& rhs )
{
  // Make sure nested conditions are met
  testNestedConditionsBegin(1);

  return (Measurement<T>( lhs ) /= rhs );

  testNestedConditionsEnd(1);
}

//! Overload of sqrt for a measurement
template<typename T>
inline Measurement<T> sqrt( const Measurement<T>& x )
{
  // Make sure the measurement is valid
  testPrecondition( x.getValue() >= 0.0 );

  const T new_value = std::sqrt( x.getValue() );

  const T propagated_uncertainty = 0.5*(new_value/x.getValue())*
    x.getUncertainty();

  // Make sure reasonable values have been calculated
  testPostcondition( !Teuchos::ScalarTraits<T>::isnaninf( new_value ) );
  testPostcondition( !Teuchos::ScalarTraits<T>::isnaninf(
						    propagated_uncertainty ) );

  return Measurement<T>( new_value, propagated_uncertainty );
}

//! Overload of pow for a measurement
template<typename T, typename ExponentType>
inline Measurement<T> pow( const Measurement<T>& x,
			   const ExponentType exponent )
{
  const T new_value = std::pow( x.getValue(), exponent );

  const T propagated_uncertainty = fabs(exponent*(new_value/x.getValue()))*
    x.getUncertainty();

  // Make sure reasonable values have been calculated
  testPostcondition( !Teuchos::ScalarTraits<T>::isnaninf( new_value ) );
  testPostcondition( !Teuchos::ScalarTraits<T>::isnaninf(
						    propagated_uncertainty ) );
  testPostcondition( propagated_uncertainty >= 0.0 );

  return Measurement<T>( new_value, propagated_uncertainty );
}

} // end Utility namespace

namespace boost{

namespace units{

//! Specialization of the boost::units::power_typeof_helper
template<typename Y, long N, long D>
struct power_typeof_helper<Utility::Measurement<Y>,static_rational<N,D> >
{
  typedef Utility::Measurement<typename power_typeof_helper<Y,static_rational<N,D> >::type> type;

  static type value( const Utility::Measurement<Y>& x)
  {
    const static_rational<N,D> rational;

    const Y rational_power = Y(rational.numerator())/Y(rational.denominator());

    return Utility::pow( x, rational_power );
  }
};

//! Specialization of the boost::units::root_typeof_helper
template<typename Y, long N, long D>
struct root_typeof_helper<Utility::Measurement<Y>,static_rational<N,D> >
{
  typedef Utility::Measurement<typename root_typeof_helper<Y,static_rational<N,D> >::type> type;

  static type value( const Utility::Measurement<Y>& x )
  {
    const static_rational<N,D> rational;

    // Compute D/N instead of N/D since we're interested in the root
    const Y rational_power = Y(rational.denominator())/Y(rational.numerator());

    return Utility::pow( x, rational_power );
  }
};

} // end units namespace

} // end boost namespace

namespace Teuchos{

//! Partial specialization of Teuchos::ScalarTraits for the Measurement class
template<typename T>
struct ScalarTraits<Utility::Measurement<T> >
{
  typedef Utility::Measurement<T> Measurement;
  typedef T magnitudeType;
  typedef typename Teuchos::ScalarTraits<T>::halfPrecision halfPrecision;
  typedef typename Teuchos::ScalarTraits<T>::doublePrecision doublePrecision;

  static const bool isComplex = Teuchos::ScalarTraits<T>::isComplex;
  static const bool isOrdinal = Teuchos::ScalarTraits<T>::isOrdinal;
  static const bool isComparable = Teuchos::ScalarTraits<T>::isComparable;
  static const bool hasMachineParameters = Teuchos::ScalarTraits<T>::hasMachineParameters;

  static inline magnitudeType eps() { return ScalarTraits<magnitudeType>::eps(); }
  static inline magnitudeType sfmin() { return ScalarTraits<magnitudeType>::sfmin(); }
  static inline magnitudeType base() { return ScalarTraits<magnitudeType>::base(); }
  static inline magnitudeType prec() { return ScalarTraits<magnitudeType>::prec(); }
  static inline magnitudeType t() { return ScalarTraits<magnitudeType>::t(); }
  static inline magnitudeType rnd() { return ScalarTraits<magnitudeType>::rnd(); }
  static inline magnitudeType emin() { return ScalarTraits<magnitudeType>::emin(); }
  static inline magnitudeType rmin() { return ScalarTraits<magnitudeType>::rmin(); }
  static inline magnitudeType emax() { return ScalarTraits<magnitudeType>::emax(); }
  static inline magnitudeType rmax() { return ScalarTraits<magnitudeType>::rmax(); }
  static inline magnitudeType magnitude(Measurement a) { return ScalarTraits<magnitudeType>::magnitude( a.getValue() ); }
  static inline Measurement zero() { return Measurement( ScalarTraits<magnitudeType>::zero(), 0.0 ); }
  static inline Measurement one() { return Measurement( ScalarTraits<magnitudeType>::zero(), 1.0 ); }
  static inline Measurement conjugate(Measurement a){ return Measurement( ScalarTraits<magnitudeType>::conjugate(a.getValue()), ScalarTraits<magnitudeType>::conjugate(a.getUncertainty()) ); }
  static inline Measurement real(Measurement a){ return Measurement( ScalarTraits<magnitudeType>::real(a.getValue()), ScalarTraits<magnitudeType>::real(a.getUncertainty()) ); }
  static inline Measurement imag(Measurement a){ return Measurement( ScalarTraits<magnitudeType>::imag(a.getValue()), ScalarTraits<magnitudeType>::imag(a.getUncertainty()) ); }
  static inline Measurement nan() { return Measurement( ScalarTraits<magnitudeType>::nan(), ScalarTraits<magnitudeType>::nan() ); }
  static inline bool isnaninf(Measurement a){ return ScalarTraits<magnitudeType>::isnaninf(a.getValue()) || ScalarTraits<magnitudeType>::isnaninf(a.getUncertainty()); }
  static inline void seedrandom(unsigned int s) { ScalarTraits<magnitudeType>::seedrandom(s); }
  static inline Measurement random() { return Measurement( ScalarTraits<magnitudeType>::random(), 0.0 ); }
  static inline std::string name() { return std::string("Measurement<")+std::string(ScalarTraits<magnitudeType>::name())+std::string(">"); }
  static inline Measurement squareroot(Measurement a) { return Utility::sqrt(a); }
  static inline Measurement pow(Measurement a, Measurement b) { return Utility::pow( a, b.getValue() ); }

};

} // end Teuchos namespace

// Register the Measurement class with boost typeof for auto-like type
// deduction when used with the boost::units library
#if BOOST_UNITS_HAS_BOOST_TYPEOF

BOOST_TYPEOF_REGISTER_TEMPLATE(Utility::Measurement, 1)

#endif // end BOOST_UNITS_HAS_BOOST_TYPEOF

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "Utility_Measurement_def.hpp"

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

#endif // end UTILITY_MEASUREMENT_HPP

//---------------------------------------------------------------------------//
// end Utility_Measurement.hpp
//---------------------------------------------------------------------------//


#define _LIBCPP_ENABLE_THREAD_SAFETY_ANNOTATIONS
#include "min_dist.h"

#include <algorithm>
#include <boost/histogram/ostream.hpp>
#include <cstdint>
#include <fmt/core.h>
#include <fstream>
#include <gsl/gsl>
#include <iomanip>
#include <iostream>
#include <limits>
#include <mutex>
#include <opencv2/core/base.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/persistence.hpp>
#include <opencv2/core/types.hpp>
#include <opencv2/imgcodecs.hpp>
#include <optional>
#include <sens_loc/analysis/distance.h>
#include <sens_loc/io/histogram.h>
#include <sens_loc/util/correctness_util.h>
#include <sens_loc/util/thread_analysis.h>
#include <stdexcept>
#include <string_view>
#include <util/batch_visitor.h>
#include <util/common_structures.h>
#include <util/statistic_visitor.h>

using namespace std;

namespace {

template <cv::NormTypes NT>
struct element_access {
    using type                 = float;
    static constexpr int dtype = CV_32F;
};
template <>
struct element_access<cv::NormTypes::NORM_HAMMING> {
    using type                 = int;
    static constexpr int dtype = CV_32S;
};
template <>
struct element_access<cv::NormTypes::NORM_HAMMING2> {
    using type                 = int;
    static constexpr int dtype = CV_32S;
};

struct distance_stat_data {
    distance_stat_data() = default;

    void insert_distances(gsl::span<float> distances) noexcept {
        lock_guard l{_process_mutex};
        _global_min_distances.insert(end(_global_min_distances),
                                     begin(distances), end(distances));
    }

    vector<float> extract() noexcept {
        lock_guard    l{_process_mutex};
        vector<float> r       = move(_global_min_distances);
        _global_min_distances = vector<float>();
        return r;
    };

  private:
    mutex                               _process_mutex;
    vector<float> _global_min_distances GUARDED_BY(_process_mutex);
};

/// Calculate the minimal distance between descriptors within one image
/// and save this minimal distance.
/// This gives an overall idea of the spread of descriptors within an image.
template <cv::NormTypes NT>
class min_descriptor_distance {
  public:
    static const int data_type = element_access<NT>::dtype;
    using access_type          = typename element_access<NT>::type;

    min_descriptor_distance(distance_stat_data& data)
        : accumulated_data{data} {}

    void operator()(int /*idx*/,
                    optional<vector<cv::KeyPoint>> keypoints,  // NOLINT
                    optional<cv::Mat>              descriptors) noexcept {
        Expects(!keypoints.has_value());
        Expects(descriptors.has_value());

        if (descriptors->rows == 0)
            return;

        cv::Mat distances;
        cv::batchDistance(*descriptors, *descriptors, distances, data_type,
                          cv::noArray(),
                          /*normType=*/NT);

        // Calculate the minimal distances within that image.
        vector<float> local_min_distances;
        local_min_distances.reserve(distances.rows);
        for (decltype(distances.rows) row = 0; row < distances.rows; ++row) {
            // Make the distance to itself maximal, as that is known to
            // be zero and needs to be ignored.
            distances.at<access_type>(row, row) =
                numeric_limits<access_type>::max();

            cv::Mat r = distances.row(row);
            auto    d = gsl::narrow<float>(
                *min_element(r.begin<access_type>(), r.end<access_type>()));

            local_min_distances.push_back(d);
        }

        accumulated_data.insert_distances(local_min_distances);
    }

    /// Postprocess the findings of the minimal distances for each image to
    /// a coherent statistical finding. This will overwrite previous analysis
    /// and should only be called once.
    size_t postprocess(const optional<string>& stat_file,
                       const optional<string>& min_dist_histo) noexcept {
        // Lock the mutex, just in case. This method is not expected to be
        // run in parallel, but it could.
        vector<float> global_distances = accumulated_data.extract();

        sort(begin(global_distances), end(global_distances));
        const auto                   bins = 25UL;
        sens_loc::analysis::distance distance_stat{
            global_distances, bins, "Minimal Intra Image Descriptor Distances"};

        if (stat_file) {
            cv::FileStorage stat_out{*stat_file,
                                     cv::FileStorage::WRITE |
                                         cv::FileStorage::FORMAT_YAML};
            stat_out.writeComment("This file contains the statistical data for "
                                  "the distance to the closest descriptor.");
            write(stat_out, "descriptor_distance",
                  distance_stat.get_statistic());
            stat_out.release();
        } else {
            cout << "==== Descriptor Distances\n"
                 << "min:       " << distance_stat.min() << "\n"
                 << "max:       " << distance_stat.max() << "\n"
                 << "Mean:      " << distance_stat.mean() << "\n"
                 << "Median:    " << distance_stat.median() << "\n"
                 << "Variance:  " << distance_stat.variance() << "\n"
                 << "StdDev:    " << distance_stat.stddev() << "\n"
                 << "Skewness:  " << distance_stat.skewness() << "\n";
        }
        if (min_dist_histo) {
            ofstream gnuplot_data{*min_dist_histo};
            gnuplot_data << sens_loc::io::to_gnuplot(distance_stat.histogram())
                         << endl;
        } else {
            cout << distance_stat.histogram() << "\n";
        }
        return global_distances.size();
    }

  private:
    // Data required for the parallel processing.
    distance_stat_data& accumulated_data;
};

template <cv::NormTypes NT>
int analyze_min_distance_impl(sens_loc::util::processing_input in,
                              const optional<string>&          stat_file,
                              const optional<string>&          min_dist_histo) {
    using namespace sens_loc::apps;

    using visitor = statistic_visitor<min_descriptor_distance<NT>,
                                      required_data::descriptors>;

    distance_stat_data data;
    auto               f =
        parallel_visitation(in.start, in.end, visitor{in.input_pattern, data});

    size_t n_elements = f.postprocess(stat_file, min_dist_histo);

    return n_elements > 0UL ? 0 : 1;
}
}  // namespace

namespace sens_loc::apps {
int analyze_min_distance(util::processing_input  in,
                         cv::NormTypes           norm_to_use,
                         const optional<string>& stat_file,
                         const optional<string>& min_dist_histo) {

#define SWITCH_CV_NORM(NORM_NAME)                                              \
    if (norm_to_use == cv::NormTypes::NORM_##NORM_NAME)                        \
        return analyze_min_distance_impl<cv::NormTypes::NORM_##NORM_NAME>(     \
            in, stat_file, min_dist_histo);
    SWITCH_CV_NORM(L1)
    SWITCH_CV_NORM(L2)
    SWITCH_CV_NORM(L2SQR)
    SWITCH_CV_NORM(HAMMING2)
    SWITCH_CV_NORM(HAMMING)
#undef SWITCH_CV_NORM
    UNREACHABLE("unexpected norm type");  // LCOV_EXCL_LINE
}
}  // namespace sens_loc::apps


/**************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                            *
 * All rights reserved.                                                   *
 *                                                                        *
 * This file is part of the mfmg library. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the  *
 * top-level directory                                                    *
 *                                                                        *
 * SPDX-License-Identifier: BSD-3-Clause                                  *
 *************************************************************************/

#define BOOST_TEST_MODULE restriction

#include <mfmg/dealii/amge_host.hpp>
#include <mfmg/dealii/dealii_mesh_evaluator.hpp>

#include <deal.II/base/conditional_ostream.h>
#include <deal.II/distributed/tria.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/lac/la_parallel_vector.h>

#include <boost/property_tree/info_parser.hpp>
#include <boost/test/data/test_case.hpp>

#include <random>

#include "laplace.hpp"
#include "main.cc"

namespace utf = boost::unit_test;
namespace bdata = boost::unit_test::data;
namespace tt = boost::test_tools;

template <int dim>
class DummyMeshEvaluator : public mfmg::DealIIMeshEvaluator<dim>
{
public:
  DummyMeshEvaluator(dealii::DoFHandler<dim> &dof_handler,
                     dealii::AffineConstraints<double> &constraints)
      : mfmg::DealIIMeshEvaluator<dim>(dof_handler, constraints)
  {
  }

  virtual ~DummyMeshEvaluator() override = default;

  void
  evaluate_global(dealii::DoFHandler<dim> &,
                  dealii::AffineConstraints<double> &,
                  dealii::TrilinosWrappers::SparseMatrix &) const override final
  {
  }

  void evaluate_agglomerate(dealii::DoFHandler<dim> &,
                            dealii::AffineConstraints<double> &,
                            dealii::SparsityPattern &,
                            dealii::SparseMatrix<double> &) const override final
  {
  }
};

BOOST_AUTO_TEST_CASE(restriction_matrix, *utf::tolerance(1e-14))
{
  unsigned int constexpr dim = 2;

  MPI_Comm comm = MPI_COMM_WORLD;
  dealii::parallel::distributed::Triangulation<dim> triangulation(comm);
  dealii::GridGenerator::hyper_cube(triangulation);
  triangulation.refine_global(2);
  dealii::FE_Q<dim> fe(1);
  dealii::DoFHandler<dim> dof_handler(triangulation);
  dof_handler.distribute_dofs(fe);
  dealii::AffineConstraints<double> constraints;
  DummyMeshEvaluator<dim> evaluator(dof_handler, constraints);
  mfmg::AMGe_host<dim, mfmg::DealIIMeshEvaluator<dim>,
                  dealii::LinearAlgebra::distributed::Vector<double>>
      amge(comm, dof_handler);

  auto const locally_owned_dofs = dof_handler.locally_owned_dofs();
  unsigned int const n_local_rows = locally_owned_dofs.n_elements();

  // Fill the eigenvectors
  unsigned int const eigenvectors_size = 3;
  std::vector<dealii::Vector<double>> eigenvectors(
      n_local_rows, dealii::Vector<double>(eigenvectors_size));
  for (unsigned int i = 0; i < n_local_rows; ++i)
    for (unsigned int j = 0; j < eigenvectors_size; ++j)
      eigenvectors[i][j] =
          n_local_rows * eigenvectors_size + i * eigenvectors_size + j;

  // Fill dof_indices_maps
  std::vector<std::vector<dealii::types::global_dof_index>> dof_indices_maps(
      n_local_rows,
      std::vector<dealii::types::global_dof_index>(eigenvectors_size));
  std::default_random_engine generator;
  std::uniform_int_distribution<int> distribution(0, dof_handler.n_dofs() - 1);
  for (unsigned int i = 0; i < n_local_rows; ++i)
  {
    // We don't want dof_indices to have repeated values in a row
    std::set<int> dofs_set;
    unsigned int j = 0;
    while (dofs_set.size() < eigenvectors_size)
    {
      int dof_index = distribution(generator);
      if ((dofs_set.count(dof_index) == 0) &&
          (locally_owned_dofs.is_element(dof_index)))
      {
        dof_indices_maps[i][j] = dof_index;
        dofs_set.insert(dof_index);
        ++j;
      }
    }
  }

  // Fill diag_elements
  std::vector<std::vector<double>> diag_elements(
      n_local_rows, std::vector<double>(eigenvectors_size));
  std::map<unsigned int, double> count_elem;
  for (unsigned int i = 0; i < n_local_rows; ++i)
    for (unsigned int j = 0; j < eigenvectors_size; ++j)
      count_elem[dof_indices_maps[i][j]] += 1.0;
  for (unsigned int i = 0; i < n_local_rows; ++i)
    for (unsigned int j = 0; j < eigenvectors_size; ++j)
      diag_elements[i][j] = 1. / count_elem[dof_indices_maps[i][j]];

  // Fill n_local_eigenvectors
  std::vector<unsigned int> n_local_eigenvectors(n_local_rows, 1);

  // Fill system_sparse_matrix
  dealii::TrilinosWrappers::SparseMatrix system_sparse_matrix(
      locally_owned_dofs, comm);
  for (auto const index : locally_owned_dofs)
    system_sparse_matrix.set(index, index, 1.0);
  system_sparse_matrix.compress(dealii::VectorOperation::insert);

  // NOTE have to extract diagonal entries from system matrix, cannot rely on
  // DummyMeshEvaluator::get_diagonal()
  dealii::IndexSet locally_relevant_dofs;
  dealii::DoFTools::extract_locally_relevant_dofs(dof_handler,
                                                  locally_relevant_dofs);
  dealii::LinearAlgebra::distributed::Vector<double> locally_owned_global_diag(
      locally_owned_dofs, comm);
  for (auto const val : locally_owned_dofs)
    locally_owned_global_diag[val] = system_sparse_matrix.diag_element(val);
  locally_owned_global_diag.compress(dealii::VectorOperation::insert);

  dealii::LinearAlgebra::distributed::Vector<double>
      locally_relevant_global_diag(locally_owned_dofs, locally_relevant_dofs,
                                   comm);
  locally_relevant_global_diag = locally_owned_global_diag;

  dealii::TrilinosWrappers::SparseMatrix restriction_sparse_matrix;
  amge.compute_restriction_sparse_matrix(
      eigenvectors, diag_elements, dof_indices_maps, n_local_eigenvectors,
      locally_relevant_global_diag, restriction_sparse_matrix);

  // Check that the matrix was built correctly
  auto restriction_locally_owned_dofs =
      restriction_sparse_matrix.locally_owned_range_indices();
  unsigned int pos = 0;
  for (auto const index : restriction_locally_owned_dofs)
  {
    for (unsigned int j = 0; j < eigenvectors_size; ++j)
      BOOST_TEST(restriction_sparse_matrix(index, dof_indices_maps[pos][j]) ==
                 diag_elements[pos][j] * eigenvectors[pos][j]);
    ++pos;
  }
}

template <int dim>
class Source : public dealii::Function<dim>
{
public:
  Source() = default;

  virtual ~Source() override = default;

  virtual double value(dealii::Point<dim> const &p,
                       unsigned int const component = 0) const override final;
};

template <int dim>
double Source<dim>::value(dealii::Point<dim> const &, unsigned int const) const
{
  return 0.;
}

template <int dim>
class ConstantMaterialProperty : public dealii::Function<dim>
{
public:
  ConstantMaterialProperty() = default;

  virtual ~ConstantMaterialProperty() override = default;

  virtual double value(dealii::Point<dim> const &,
                       unsigned int const = 0) const override final
  {
    return 1.;
  }
};

template <int dim>
class TestMeshEvaluator : public mfmg::DealIIMeshEvaluator<dim>
{
public:
  TestMeshEvaluator(dealii::DoFHandler<dim> &dof_handler,
                    dealii::AffineConstraints<double> &constraints,
                    dealii::TrilinosWrappers::SparseMatrix const &matrix)
      : mfmg::DealIIMeshEvaluator<dim>(dof_handler, constraints),
        _matrix(matrix)
  {
  }

  virtual ~TestMeshEvaluator() override = default;

  void evaluate_global(dealii::DoFHandler<dim> &,
                       dealii::AffineConstraints<double> &,
                       dealii::TrilinosWrappers::SparseMatrix &system_matrix)
      const override final
  {
    // TODO this is pretty expansive, we should use a shared pointer
    system_matrix.copy_from(_matrix);
  }

  void evaluate_agglomerate(
      dealii::DoFHandler<dim> &dof_handler,
      dealii::AffineConstraints<double> &constraints,
      dealii::SparsityPattern &system_sparsity_pattern,
      dealii::SparseMatrix<double> &system_matrix) const override final
  {
    unsigned int const fe_degree = 1;
    dealii::FE_Q<dim> fe(fe_degree);
    dof_handler.distribute_dofs(fe);

    dealii::IndexSet locally_relevant_dofs;
    dealii::DoFTools::extract_locally_relevant_dofs(dof_handler,
                                                    locally_relevant_dofs);

    // Compute the constraints
    constraints.clear();
    constraints.reinit(locally_relevant_dofs);
    dealii::DoFTools::make_hanging_node_constraints(dof_handler, constraints);
    // The test requires us not to put boundary conditions
    // dealii::VectorTools::interpolate_boundary_values(
    //     dof_handler, 1, dealii::Functions::ZeroFunction<dim>(), constraints);
    constraints.close();

    // Build the system sparsity pattern and reinitialize the system sparse
    // matrix
    dealii::DynamicSparsityPattern dsp(dof_handler.n_dofs());
    dealii::DoFTools::make_sparsity_pattern(dof_handler, dsp, constraints);
    system_sparsity_pattern.copy_from(dsp);
    system_matrix.reinit(system_sparsity_pattern);

    // Fill the system matrix
    dealii::QGauss<dim> const quadrature(fe_degree + 1);
    dealii::FEValues<dim> fe_values(
        fe, quadrature,
        dealii::update_values | dealii::update_gradients |
            dealii::update_quadrature_points | dealii::update_JxW_values);
    unsigned int const dofs_per_cell = fe.dofs_per_cell;
    unsigned int const n_q_points = quadrature.size();
    dealii::FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
    std::vector<dealii::types::global_dof_index> local_dof_indices(
        dofs_per_cell);
    for (auto cell :
         dealii::filter_iterators(dof_handler.active_cell_iterators(),
                                  dealii::IteratorFilters::LocallyOwnedCell()))
    {
      cell_matrix = 0;
      fe_values.reinit(cell);

      for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          for (unsigned int j = 0; j < dofs_per_cell; ++j)
            cell_matrix(i, j) += fe_values.shape_grad(i, q_point) *
                                 fe_values.shape_grad(j, q_point) *
                                 fe_values.JxW(q_point);

      cell->get_dof_indices(local_dof_indices);
      constraints.distribute_local_to_global(cell_matrix, local_dof_indices,
                                             system_matrix);
    }
  }

private:
  dealii::TrilinosWrappers::SparseMatrix const &_matrix;
};

// FIXME relaxed tolerance from 1e-14 to 1e-4 for this test to pass while using
// ARPACK's regular mode instead of shift-and-invert
BOOST_DATA_TEST_CASE(weight_sum, bdata::make({"lapack", "lanczos"}),
                     eigensolver)
{
  // Check that the weight sum is equal to one
  unsigned int constexpr dim = 2;
  using DVector = dealii::LinearAlgebra::distributed::Vector<double>;
  using MeshEvaluator = mfmg::DealIIMeshEvaluator<dim>;

  MPI_Comm comm = MPI_COMM_WORLD;

  dealii::ConditionalOStream pcout(
      std::cout, dealii::Utilities::MPI::this_mpi_process(comm) == 0);

  Source<dim> source;

  auto params = std::make_shared<boost::property_tree::ptree>();
  boost::property_tree::info_parser::read_info("hierarchy_input.info", *params);
  params->put("eigensolver.type", eigensolver);
  params->put("eigensolver.number of eigenvectors", 1);
  auto agglomerate_ptree = params->get_child("agglomeration");
  int n_eigenvectors =
      params->get<int>("eigensolver.number of eigenvectors", 1);
  double tolerance = params->get<double>("eigensolver.tolerance", 1e-14);

  params->put("laplace.n_refinements", 4);
  std::shared_ptr<dealii::Function<dim>> material_property =
      std::make_shared<ConstantMaterialProperty<dim>>();
  auto laplace_ptree = params->get_child("laplace");
  auto fe_degree = laplace_ptree.get<unsigned>("fe_degree", 1);
  Laplace<dim, DVector> laplace(comm, fe_degree);
  laplace.setup_system(laplace_ptree);
  laplace.assemble_system(source, *material_property);

  TestMeshEvaluator<dim> evaluator(laplace._dof_handler, laplace._constraints,
                                   laplace._system_matrix);
  mfmg::AMGe_host<dim, MeshEvaluator, DVector> amge(
      comm, laplace._dof_handler, params->get_child("eigensolver"));

  auto locally_relevant_global_diag = evaluator.get_diagonal();

  dealii::TrilinosWrappers::SparseMatrix restrictor_matrix;
  amge.setup_restrictor(agglomerate_ptree, n_eigenvectors, tolerance, evaluator,
                        locally_relevant_global_diag, restrictor_matrix);

  // Multiply the matrix by three because all the eigenvectors are 1/3. So we
  // are left with the weights.
  restrictor_matrix *= 3.;
  unsigned int const size = restrictor_matrix.n();
  auto domain_dofs = restrictor_matrix.locally_owned_domain_indices();
  DVector e(domain_dofs, comm);
  auto range_dofs = restrictor_matrix.locally_owned_range_indices();
  DVector ee(range_dofs, comm);
  for (unsigned int i = 0; i < size; ++i)
  {
    e = 0;
    if (domain_dofs.is_element(i))
      e[i] = 1.;
    e.compress(::dealii::VectorOperation::insert);
    restrictor_matrix.vmult(ee, e);
    BOOST_TEST(ee.l1_norm() == 1., tt::tolerance(2e-4));
  }
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file DiscreteBayesNet.cpp
 * @date Feb 15, 2011
 * @author Duy-Nguyen Ta
 * @author Frank Dellaert
 */

#include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/inference/FactorGraph-inst.h>

#include <boost/make_shared.hpp>
#include <boost/range/adaptor/reversed.hpp>

namespace gtsam {

  // Instantiate base class
  template class FactorGraph<DiscreteConditional>;

  /* ************************************************************************* */
  bool DiscreteBayesNet::equals(const This& bn, double tol) const
  {
    return Base::equals(bn, tol);
  }

  /* ************************************************************************* */
//  void DiscreteBayesNet::add_front(const Signature& s) {
//    push_front(boost::make_shared<DiscreteConditional>(s));
//  }

  /* ************************************************************************* */
  void DiscreteBayesNet::add(const Signature& s) {
    push_back(boost::make_shared<DiscreteConditional>(s));
  }

  /* ************************************************************************* */
  double DiscreteBayesNet::evaluate(const DiscreteConditional::Values & values) const {
    // evaluate all conditionals and multiply
    double result = 1.0;
    for(DiscreteConditional::shared_ptr conditional: *this)
      result *= (*conditional)(values);
    return result;
  }

  /* ************************************************************************* */
  DiscreteFactor::sharedValues DiscreteBayesNet::optimize() const {
    // solve each node in turn in topological sort order (parents first)
    DiscreteFactor::sharedValues result(new DiscreteFactor::Values());
    for (auto conditional: boost::adaptors::reverse(*this))
      conditional->solveInPlace(*result);
    return result;
  }

  /* ************************************************************************* */
  DiscreteFactor::sharedValues DiscreteBayesNet::sample() const {
    // sample each node in turn in topological sort order (parents first)
    DiscreteFactor::sharedValues result(new DiscreteFactor::Values());
    for (auto conditional: boost::adaptors::reverse(*this))
      conditional->sampleInPlace(*result);
    return result;
  }

/* ************************************************************************* */
} // namespace


#include "foot_step_planner.hpp"
#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace Eigen;
int main(int argc, char const *argv[])
{
    footStepPlanner(2.0,2.0,0.30);
    planAlongSpline();
    system("sleep 0.1");
    system("gnuplot-x11 -persist show.plt");
    system("gnuplot-x11 -persist spline.plt");
    return 0;
}


#include <kdop_mesh_reorder.h>
#include <kdop_make_tree.h>
#include <mesh_array.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#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 <vector>

BOOST_AUTO_TEST_SUITE(kdop);

BOOST_AUTO_TEST_CASE(kdop_make_tree)
{
  typedef tiny::MathTypes<float> MT;
  typedef MT::vector3_type       V;
  typedef MT::real_type          T;

  mesh_array::T3Mesh surface;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sX;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sY;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sZ;

  T      const width   = 2.0f;
  T      const height  = .50f;
  T      const depth   = 2.0f;

  mesh_array::make_box<MT>( width, height, depth, surface, sX, sY, sZ);

  mesh_array::T4Mesh mesh_in;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> X_in;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Y_in;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Z_in;
  mesh_array::tetgen(surface, sX, sY, sZ, mesh_in, X_in, Y_in, Z_in);

  mesh_array::T4Mesh mesh_out;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> X_out;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Y_out;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Z_out;
  kdop::mesh_reorder( mesh_in, X_in, Y_in, Z_in, mesh_out, X_out, Y_out, Z_out );

  kdop::Tree<T,8> tree = kdop::make_tree<V,8,T>( 8000, mesh_out, X_out, Y_out, Z_out, kdop::sequential() );
}

BOOST_AUTO_TEST_SUITE_END();


//   Copyright 2015 Patrick Putnam
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
#ifndef RECOMBINATION_GENERATOR_HPP_
#define RECOMBINATION_GENERATOR_HPP_

#include "clotho/utility/random_generator.hpp"
#include "clotho/recombination/recombination_def.hpp"
#include "clotho/recombination/sequence_bias_parameter.hpp"

#include <boost/random/bernoulli_distribution.hpp>

// Base Sequence Bias allows one to specify
// whether a parental sequence is more likely
// to be the base sequence to be passed along
// For example, the maternally inheritted sequence
// is more likely to be passed along than the
// paternal.

namespace clotho {
namespace utility {

template < class URNG, class Sequence, class Classifier, class T0, class T1 >
class random_generator< URNG, clotho::recombine::recombination< Sequence, Classifier, T0, T1 > > {
public:
    typedef URNG                                                                rng_type;
    typedef clotho::recombine::recombination< Sequence, Classifier, T0, T1 >    result_type;

    typedef clotho::utility::random_generator< URNG, Classifier >       classifier_generator_type;
    typedef typename classifier_generator_type::result_type             classifier_type;

    typedef double real_type;
    typedef boost::random::bernoulli_distribution< real_type >          dist_type;

    random_generator( rng_type & rng, boost::property_tree::ptree & config ) :
        m_rng( &rng )
        , m_cgen( rng, config )
        , m_dist( 0.5 )
    {
        parseConfig( config );
    }

    random_generator( rng_type & rng, classifier_generator_type & cgen, real_type p = 0.5 ) :
        m_rng( &rng )
        , m_cgen( cgen )
        , m_dist( p )
    {    }

    result_type operator()() {
        classifier_type cfier = m_cgen();
        return result_type( cfier, m_dist( *m_rng ) );
    }

protected:

    void parseConfig( boost::property_tree::ptree & config ) {
//        boost::property_tree::ptree lconfig,bblock;
//        lconfig = config.get_child( REC_BLOCK_K, lconfig );
//        bblock = lconfig.get_child( SEQUENCE_BIAS_BLOCK_K, bblock );
//
//        real_type p = bblock.get< real_type >( P_K, m_dist.p());
//        bblock.put( P_K, p );
//        lconfig.put_child( SEQUENCE_BIAS_BLOCK_K, bblock);
//        config.put_child( REC_BLOCK_K, lconfig );

        sequence_bias_parameter< real_type > param( config );

        typename dist_type::param_type tmp(param.m_bias);
        m_dist.param( tmp );
    }

    rng_type    * m_rng;
    classifier_generator_type m_cgen;
    dist_type   m_dist;
};

}   // namespace utility {
}   // namespace clotho {

#endif  // RECOMBINATION_GENERATOR_HPP_


#include "gtest/gtest.h"
#include "Triangulate.h"
#include <Eigen/Core>

using namespace Eigen;
using namespace Geotree;

namespace {
  class TriangulateTest : public ::testing::Test {
  };

  TEST_F(TriangulateTest, basic)
  {
    std::vector<Point2D> face;

    face.push_back(Point2D({0,0}));
    face.push_back(Point2D({1,0}));
    face.push_back(Point2D({0,1}));

    std::vector<Point2D> split;
    std::vector<std::vector<Point2D>> splits;

    Point2D p0 = {0.1, 0.1};
    Point2D p1 = {0.1, 0.2};
    Point2D p2 = {0.2, 0.1};

    split.push_back(p0);
    split.push_back(p1);
    split.push_back(p2);
    splits.push_back(split);

    Triangulate tri(face, splits);

    EXPECT_TRUE(0);
  }
}


// Author(s): Jeroen Keiren
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//
/// \file parser_test.cpp
/// \brief Regression test for parsing a data specification.

#define BOOST_TEST_MODULE parser_test
#include <boost/test/included/unit_test_framework.hpp>
#include <iostream>

#include "mcrl2/data/basic_sort.h"
#include "mcrl2/data/parse.h"
#include "mcrl2/data/pos.h"
#include "mcrl2/data/standard.h"
#include "mcrl2/data/standard_utility.h"

using namespace mcrl2;

void parser_test()
{
  const std::string text(
    "sort S;\n"
    "cons s:S;\n"
    "map f:S -> List(S);\n"
  );

  data::data_specification spec(data::parse_data_specification(text));

  std::cerr << "number of sorts " << spec.sorts().size() << "\n";
  BOOST_CHECK(spec.sorts().size() == 7); // Bool, Pos, S, List(S), S->List(S), Nat, @NatPair.
  BOOST_CHECK(spec.constructors(data::basic_sort("S")).size() == 1);
  std::cerr << "number of functions " << spec.mappings().size() << "\n";
  BOOST_CHECK(spec.mappings().size() == 104);

  BOOST_CHECK(data::parse_data_expression("2") == data::sort_pos::pos(2));
  BOOST_CHECK(data::parse_data_expression("0") == data::sort_nat::nat(0));
  BOOST_CHECK(data::parse_sort_expression("Nat") == data::sort_nat::nat());
//  BOOST_CHECK(data::parse_data_expression("-1") == data::sort_int::int_(-1));
//  BOOST_CHECK(data::parse_data_expression("1/2") == data::sort_real::real_(1, 2));
}

// This test triggers a sort normalization problem.
void test_user_defined_sort()
{
  using namespace data;

  std::string text = "sort D = struct d1 | d2;\n";
  data_specification data_spec = parse_data_specification(text);
  sort_expression s = parse_sort_expression("D", data_spec);
}

void test_whr()
{
  using namespace data;
  data_expression x = parse_data_expression("exists n: Pos . n == 0 whr n = 1 end");
}

// returns true if parsing succeeded
bool parse_sort(const data::data_specification& data_spec, const std::string& text)
{
  try
  {
    data::sort_expression s = data::parse_sort_expression(text, data_spec);
  }
  catch (mcrl2::runtime_error&)
  {
    return false;
  }
  return true;
}

// if expected_result is false, parsing is expected to fail
void test_sort_expression(const data::data_specification& data_spec, const std::string& text, bool expected_result)
{
  bool result = parse_sort(data_spec, text);
  if (result != expected_result)
  {
    std::cout << "ERROR: parsing the sort expression '" << text << "' produced the result " << std::boolalpha << result << std::endl;
  }
  BOOST_CHECK(result == expected_result);
}

void test_ticket_1267()
{
  data::data_specification data_spec = data::parse_data_specification("sort A;");
  test_sort_expression(data_spec, "A -> A # A", false);
  test_sort_expression(data_spec, "(A -> A) # A", false);
  test_sort_expression(data_spec, "A # A", false);
  test_sort_expression(data_spec, "(A # A)", false);

  test_sort_expression(data_spec, "(A # A) -> A", true);
  test_sort_expression(data_spec, "A -> ((A # A) -> (A -> A))", true);
  test_sort_expression(data_spec, "A", true);
  test_sort_expression(data_spec, "A # A -> A", true);
  test_sort_expression(data_spec, "A -> A # A -> A -> A", true);
  test_sort_expression(data_spec, "A -> (A -> A)", true);
  test_sort_expression(data_spec, "(A -> A) -> A", true);
  test_sort_expression(data_spec, "A # A -> (A -> A)", true);
  test_sort_expression(data_spec, "A # (A -> A) -> (A -> A)", true);
  test_sort_expression(data_spec, "(A -> A) # A -> (A -> A)", true);
  test_sort_expression(data_spec, "A # A # A -> A", true);
  test_sort_expression(data_spec, "A -> A -> A -> A", true);
}

BOOST_AUTO_TEST_CASE(test_main)
{
  parser_test();
  test_user_defined_sort();
  test_whr();
  test_ticket_1267();
}


// Copyright 2019-2021 Cambridge Quantum Computing
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/range/iterator_range_core.hpp>
#include <catch2/catch.hpp>
#include <map>
#include <vector>

#include "Graphs/TreeSearch.hpp"
#include "Graphs/Utils.hpp"

namespace tket {
namespace graphs {
namespace tests {
namespace test_TreeSearch {

template <typename VertexListT>
static auto get_graph() {
  using Graph =
      boost::adjacency_list<boost::listS, VertexListT, boost::bidirectionalS>;

  constexpr unsigned num_vertices = 20;
  Graph g(num_vertices);

  std::vector<utils::vertex<Graph>> VERTEX(num_vertices);
  for (unsigned i = 0; i < num_vertices; ++i) {
    VERTEX[i] = boost::vertex(i, g);
  }

  auto make_cycle = [&g, &VERTEX](unsigned from, unsigned to) {
    const unsigned delta = to - from;
    for (unsigned i = 0; i < delta; ++i) {
      unsigned i_plus_one = (i + 1) % delta;
      unsigned v1 = i + from;
      unsigned v2 = i_plus_one + from;
      boost::add_edge(VERTEX[v1], VERTEX[v2], g);
    }
  };

  // cycle for vertices 0...9
  make_cycle(0, 10);

  // cycle for vertices 10...14
  make_cycle(10, 15);

  // edge between two cycles
  boost::add_edge(VERTEX[9], VERTEX[10], g);
  boost::add_edge(VERTEX[12], VERTEX[8], g);

  // disconnected cycle 15...19
  make_cycle(15, 20);

  return g;
}

SCENARIO("BFS get_parents") {
  GIVEN("Using implicit vecS indices") {
    auto g = get_graph<boost::vecS>();

    auto bfs = run_bfs(0, g);
    auto parents = bfs.get_parents();

    for (unsigned i = 1; i < 10; ++i) {
      CHECK(parents[i] == (i - 1) % 10);
    }
    CHECK(parents[10] == 9);
    for (unsigned i = 11; i < 15; ++i) {
      CHECK(parents[i] == i - 1);
    }
    for (unsigned i = 15; i < 20; ++i) {
      CHECK(parents[i] == i);
    }
  }
  GIVEN("Using explicit indices") {
    auto g = get_graph<boost::listS>();
    unsigned num_vertices = boost::num_vertices(g);
    std::map<utils::vertex<decltype(g)>, unsigned> VERTEX;

    for (unsigned i = 0; i < num_vertices; ++i) {
      VERTEX[boost::vertex(i, g)] = num_vertices - 1 - i;
    }

    auto bfs = run_bfs(boost::vertex(0, g), g, VERTEX);
    auto parents = bfs.get_parents();

    auto is_parent = [&VERTEX, &parents, num_vertices](
                         unsigned node, unsigned child) {
      return VERTEX[parents[num_vertices - child - 1]] ==
             num_vertices - node - 1;
    };
    for (unsigned i = 1; i < 10; ++i) {
      CHECK(is_parent((i - 1) % 10, i));
      // CHECK(parents[num_vertices - i] == num_vertices - (i-1)%10);
    }
    CHECK(is_parent(9, 10));
    for (unsigned i = 11; i < 15; ++i) {
      CHECK(is_parent(i - 1, i));
      // CHECK(parents[num_vertices - i] == num_vertices - (i-1));
    }
    for (unsigned i = 15; i < 20; ++i) {
      CHECK(is_parent(i, i));
      // CHECK(parents[num_vertices - i] == num_vertices - i);
    }
  }
  GIVEN("Using temporary object") {
    auto g = get_graph<boost::vecS>();
    auto parents = run_bfs(0, g).get_parents();
    auto g2 = get_graph<boost::vecS>();
    auto bfs = run_bfs(0, g);
    auto parents2 = bfs.get_parents();

    const unsigned num_vertices = boost::num_vertices(g);

    for (unsigned i = 0; i < num_vertices; ++i) {
      CHECK(parents[i] == parents2[i]);
    }
  }
}

SCENARIO("BFS get_dists") {
  GIVEN("Using implicit vecS indices") {
    auto g = get_graph<boost::vecS>();

    auto dists = run_bfs(0, g).get_dists();
    CHECK(dists[0] == 0);
    CHECK(dists[3] == 3);
    CHECK(dists[12] == 12);

    auto bfs = run_bfs(4, g);
    const unsigned num_vertices = boost::num_vertices(g);
    for (unsigned i = 0; i < num_vertices; ++i) {
      CHECK(bfs.get_dists()[i] == bfs.get_dist(i));
    }
  }
}

SCENARIO("BFS path to root") {
  auto g = get_graph<boost::vecS>();

  auto path = run_bfs(11, g).path_to_root(4);
  decltype(path) res{4, 3, 2, 1, 0, 9, 8, 12, 11};

  for (unsigned i = 0; i < res.size(); ++i) {
    CHECK(res[i] == path[i]);
  }
  auto path2 = run_bfs(11, g).path_from_root(4);
  decltype(path) res2{11, 12, 8, 9, 0, 1, 2, 3, 4};

  for (unsigned i = 0; i < res2.size(); ++i) {
    CHECK(res2[i] == path2[i]);
  }
}

SCENARIO("BFS depth") {
  auto g = get_graph<boost::vecS>();

  auto bfs = run_bfs(4, g);

  CHECK(bfs.max_depth() == 10);
  CHECK(bfs.max_depth_vertex() == 14);
}

SCENARIO("DFS get_parents") {
  GIVEN("Using implicit vecS indices") {
    auto g = get_graph<boost::vecS>();

    auto dfs = run_dfs(0, g);
    auto parents = dfs.get_parents();

    for (unsigned i = 1; i < 10; ++i) {
      CHECK(parents[i] == (i - 1) % 10);
    }
    CHECK(parents[10] == 9);
    for (unsigned i = 11; i < 15; ++i) {
      CHECK(parents[i] == i - 1);
    }
    CHECK(parents[15] == 15);
    for (unsigned i = 16; i < 20; ++i) {
      CHECK(parents[i] == i - 1);
    }
  }
  GIVEN("Using explicit indices") {
    auto g = get_graph<boost::listS>();
    unsigned num_vertices = boost::num_vertices(g);
    std::map<utils::vertex<decltype(g)>, unsigned> VERTEX;

    for (unsigned i = 0; i < num_vertices; ++i) {
      VERTEX[boost::vertex(i, g)] = num_vertices - 1 - i;
    }

    auto dfs = run_dfs(boost::vertex(0, g), g, VERTEX);
    auto parents = dfs.get_parents();

    auto is_parent = [&VERTEX, &parents, num_vertices](
                         unsigned node, unsigned child) {
      return VERTEX[parents[num_vertices - child - 1]] ==
             num_vertices - node - 1;
    };
    for (unsigned i = 1; i < 10; ++i) {
      CHECK(is_parent((i - 1) % 10, i));
    }
    CHECK(is_parent(9, 10));
    for (unsigned i = 11; i < 15; ++i) {
      CHECK(is_parent(i - 1, i));
    }
    CHECK(is_parent(15, 15));
    for (unsigned i = 16; i < 20; ++i) {
      CHECK(is_parent(i - 1, i));
    }
  }
  GIVEN("Using temporary object") {
    auto g = get_graph<boost::vecS>();
    auto parents = run_dfs(0, g).get_parents();
    auto g2 = get_graph<boost::vecS>();
    auto dfs = run_dfs(0, g);
    auto parents2 = dfs.get_parents();

    const unsigned num_vertices = boost::num_vertices(g);

    for (unsigned i = 0; i < num_vertices; ++i) {
      CHECK(parents[i] == parents2[i]);
    }
  }
}

SCENARIO("DFS get_dists") {
  GIVEN("Using implicit vecS indices") {
    auto g = get_graph<boost::vecS>();

    auto dists = run_dfs(0, g).get_dists();
    CHECK(dists[0] == 0);
    CHECK(dists[3] == 3);
    CHECK(dists[12] == 12);

    auto dfs = run_dfs(4, g);
    const unsigned num_vertices = boost::num_vertices(g);
    for (unsigned i = 0; i < num_vertices; ++i) {
      CHECK(dfs.get_dists()[i] == dfs.get_dist(i));
    }
  }
}

SCENARIO("DFS path to root") {
  auto g = get_graph<boost::vecS>();

  auto path = run_dfs(11, g).path_to_root(4);
  decltype(path) res{4, 3, 2, 1, 0, 9, 8, 12, 11};

  for (unsigned i = 0; i < res.size(); ++i) {
    CHECK(res[i] == path[i]);
  }
  auto path2 = run_dfs(11, g).path_from_root(4);
  decltype(path) res2{11, 12, 8, 9, 0, 1, 2, 3, 4};

  for (unsigned i = 0; i < res2.size(); ++i) {
    CHECK(res2[i] == path2[i]);
  }
}

SCENARIO("DFS depth") {
  auto g = get_graph<boost::vecS>();

  auto dfs = run_dfs(4, g);

  CHECK(dfs.max_depth() == 10);
  CHECK(dfs.max_depth_vertex() == 14);
}

SCENARIO("longest simplest path") {
  auto g = get_graph<boost::vecS>();

  auto res = longest_simple_path(g);
  unsigned i = 0;
  for (unsigned v : res) {
    CHECK(v == i);
    ++i;
  }
  CHECK(res.size() == 15);
  CHECK(res[0] == 0);
}

}  // namespace test_TreeSearch
}  // namespace tests
}  // namespace graphs
}  // namespace tket


#include "BooleanEncoding.h"
#include "ParserState.h"
#include "Conversions.h"
#include "FunctionInstantiator.h"

#define BOOST_SPIRIT_USE_PHOENIX_V3
#include <boost/spirit/include/qi.hpp>

namespace smtrat {
namespace parser {

	struct EncodingInstantiator: public IndexedFunctionInstantiator {
		bool operator()(const std::vector<std::size_t>& indices, const std::vector<types::TermType>& arguments, types::TermType& result, TheoryError& errors) const {
			std::vector<FormulaT> args;
			if (!convert(arguments, args)) return false;
			return apply(indices, args, result, errors);
		}
		virtual bool apply(const std::vector<std::size_t>& indices, const std::vector<FormulaT>& arguments, types::TermType& result, TheoryError& errors) const = 0;
	};

	void at_most_k_helper(size_t k, const std::vector<FormulaT>& set, FormulasT& working, long long int prev_idx, FormulasT& results) {
		for (size_t i = prev_idx+1; i < set.size(); i++) {
			working.push_back(set[i].negated());
			if (working.size() == k+1) {
				results.emplace_back(carl::FormulaType::OR, working);
			} else {
				at_most_k_helper(k, set, working, i, results);
			}
			working.pop_back();
		}
	}
	FormulaT at_most_k(size_t k, const std::vector<FormulaT>& set) {
		if (k >= set.size()) {
			return FormulaT(carl::FormulaType::TRUE);
		} else {
			FormulasT working;
			FormulasT results;
			at_most_k_helper(k, set, working, -1, results);
			return FormulaT(carl::FormulaType::AND, std::move(results));
		}
	}

	struct AtMostInstantiator: public EncodingInstantiator {
		bool apply(const std::vector<std::size_t>& indices, const std::vector<FormulaT>& arguments, types::TermType& result, TheoryError& errors) const {
			if (arguments.size() == 0) {
				errors.next() << "The operator \"at-most\" expects at least one argument.";
				return false;
			}
			if (indices.size() != 1) {
				errors.next() << "The operator \"at-most\" expects exactly one index.";
				return false;
			}

			result = at_most_k(indices[0], arguments); // TODO better encoding, see http://www.cs.toronto.edu/~fbacchus/csc2512/at_most_k.pdf
			return true;
		}
	};

	BooleanEncodingTheory::BooleanEncodingTheory(ParserState* state): AbstractTheory(state) {
		state->registerFunction("at-most", new AtMostInstantiator());
	}

}
}


////////////////////////////////////////////////////////////////////////
// ENCODING IMAGES USING AVCONV or FFMPEG
//
// Manuel Martinez (manuel.martinez@kit.edu)
//
// license: LGPLv3
//
// FLAGS: -g -std=c++11 `pkg-config opencv --cflags --libs` -lavcodec -lavutil -lboost_system -lboost_program_options

#include <uSnippets/libav.hpp>
#include <fstream>
#include <opencv/highgui.h>
#include <boost/program_options.hpp>

using namespace std;
using namespace uSnippets;

static inline double getPSNR(const cv::Mat& I1, const cv::Mat& I2) {

    cv::Mat s1;
    cv::absdiff(I1, I2, s1);       // |I1 - I2|
    s1.convertTo(s1, CV_32F);  // cannot make a square on 8 bits
    s1 = s1.mul(s1);           // |I1 - I2|^2

    cv::Scalar s = cv::sum(s1);        // sum elements per channel

    double sse = s.val[0] + s.val[1] + s.val[2]; // sum channels

    if (sse <= 1e-10) // for small values return zero
        return 0;

	double mse  = sse / (double)(I1.channels() * I1.total());
	double psnr = 10.0 * log10((255 * 255) / mse);
	return psnr;
}

struct WAVE {

	string fmt;

	vector<LibAV::MP2Pkg> read(string file) {

		vector<LibAV::MP2Pkg> out;

		char descriptor[5] = {0,0,0,0,0};
		uint32_t size;
		ifstream wav(file);
		wav.read(descriptor,4);
		wav.read((char *)&size,4);
		wav.read(descriptor,4);
		while (true) {
			wav.read(descriptor,4);
			if (!wav) break;
			wav.read((char *)&size,4);
			if (string(descriptor)=="fmt ") {
				fmt.resize(size);
				wav.read(&fmt[0],size);
			} else if (string(descriptor)=="data") {
				out.resize(size/sizeof(LibAV::MP2Pkg));
				for (uint i=0; i<out.size(); i++)
					wav.read((char *)&out[i],sizeof(LibAV::MP2Pkg));
			} else {
				wav.seekg(size,wav.cur);
			}
		}
		return out;
	}

	void write(string file, const vector<LibAV::MP2Pkg> data) {

		ofstream wav(file);
		wav.write("RIFF", 4);
		uint32_t size = 4+4+4+4+16+4+data.size()*sizeof(LibAV::MP2Pkg); wav.write((char *)&size, 4);
		wav.write("WAVE", 4);
		wav.write("fmt ", 4);
		size = 16; wav.write((char *)&size, 4);
		wav.write(&fmt[0],16);
		wav.write("data",4);
		size = data.size()*sizeof(LibAV::MP2Pkg); wav.write((char *)&size, 4);
		for (uint i=0; i<data.size(); i++)
			wav.write((char *)&data[i],sizeof(LibAV::MP2Pkg));
	}
};

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

	// PARSING PROGRAM OPTIONS
	namespace po = boost::program_options;
	po::options_description global("Options for the libav test file");
	global.add_options()
		("help,h", "produce this help message")
		("command", po::value<std::string>(), "command to execute, can be audio, video or ffv1")
		("subargs", po::value<std::vector<std::string> >(), "Arguments for command");

	po::positional_options_description pos;
	pos.add("command", 1).add("subargs",-1);

	po::parsed_options parsed = po::command_line_parser(argc, argv).options(global).positional(pos).allow_unregistered().run();
	po::variables_map vm;
	po::store(parsed, vm);

	if (vm.count("help") or not vm.count("command")) {
		cout << "Usage: libav {audio|video} options" << endl;
		return 0;
	}

	// Collect all the unrecognized options from the first pass. This will include the
	// (positional) command name, so we need to erase that.
	std::vector<std::string> opts = po::collect_unrecognized(parsed.options, po::include_positional);
	opts.erase(opts.begin());

	std::string cmd = vm["command"].as<std::string>();
	if (cmd == "audio") {

		po::options_description pod("audio options");
		pod.add_options()
			("file", po::value<std::string>(), "RAW 16bit stereo 44100 audio file");

		po::positional_options_description pos;
		pos.add("file", 1);

		po::store(po::command_line_parser(opts).options(pod).positional(pos).run(), vm);

		if (vm.count("file")) {
			WAVE wave;

			int bitrate = 192000;
			vector<LibAV::MP2Pkg> src = wave.read(vm["file"].as<std::string>());
			cout << "Compressing " << src.size() << " pkgs with bitrate " << bitrate << endl;
			string compressed = LibAV::mp2encode(src,bitrate);
			cout << "Compressed to size: " << compressed.size()/(1e-10+src.size()*sizeof(LibAV::MP2Pkg)) << endl;
			vector<LibAV::MP2Pkg> out = LibAV::mp2decode(compressed);
			cout << "Decompressed to npkg: " << out.size() << endl << endl;
			wave.write("out.wave",out);

		} else {
			cout << "Synthetic audio testing" << endl;

			int bitrate = 128000;
			for (int i=1; i<10; i++) {
				LibAV::MP2Pkg pkg;
				for (uint j=0; j<pkg.size(); j++) {
					pkg[j][0] = 1024*sin(j*3.14*2*440/44100);
					pkg[j][1] = 1024*sin(j*3.14*2*440/44100+2);
				}
				vector<LibAV::MP2Pkg> src(i,pkg);
				cout << "Compressing " << i << " pkgs with bitrate " << bitrate << endl;
				string compressed = LibAV::mp2encode(src,bitrate);
				cout << "Compressed to size: " << compressed.size()/(1e-10+i*sizeof(LibAV::MP2Pkg)) << endl;
				vector<LibAV::MP2Pkg> out = LibAV::mp2decode(compressed);
				cout << "Decompressed to npkg: " << out.size() << endl << endl;
			}
		}

	} else if (cmd == "video") {

		po::options_description pod("audio options");
		pod.add_options()
			("file,f", po::value<std::string>(), "RAW 16bit stereo 44100 audio file");

		po::positional_options_description pos;
		pos.add("file", 1);

		po::store(po::command_line_parser(opts).options(pod).positional(pos).run(), vm);

		shared_ptr<cv::VideoCapture> imgsrc;
		if (vm.count("file"))
			imgsrc = make_shared<cv::VideoCapture>(vm["file"].as<std::string>());
		else
			imgsrc = make_shared<cv::VideoCapture>(0);

		if (not imgsrc->isOpened()) exit(-1);

		vector<LibAV::ImgYUV480P> in, out;
		while ((cv::waitKey(10)&255) != 'q') {

			cv::Mat3b img;
			*imgsrc >> img;
			in.emplace_back(img);

			if (in.size()==200) {

				cout << "Coding " << in.size()*img.rows*img.cols*img.elemSize() << " bytes" << endl;
				std::string coded = LibAV::h264encode(in, "1", "ultrafast");
				cout << "Coded to " << coded.size() << " bytes  1-" << (in.size()*img.rows*img.cols*img.elemSize())/(1.*coded.size()) << " ratio" << endl;
				out = LibAV::h264decode(coded);
				cout << "Decoded to " << out.size() << " images " << endl;

				for (uint i=0; i<out.size(); i++) {

					cout << getPSNR(in[i].bgr(), out[i].bgr()) << endl;
					cv::imshow("cam1", out[i].bgr());
					cv::imshow("cam2", in[i].gray());
					cv::Mat1b cmp; cv::compare(in[i].gray(), out[i].gray(), cmp, cv::CMP_NE);
					cv::imshow("cam3", cmp);
					cv::waitKey(50);
				}

				in.clear();
			}
		}

	} else if (cmd == "ffv1") {
/*

		vector<cv::Mat1s> in, out;
		for (int i=0; i<15; i++) {
			in.push_back(cv::Mat1s(480,640));
			for (auto &v : in.back())
				v = rand()&0x1FFF;
		}

		cout << "Coding " << in.size()*in.front().rows*in.front().cols*in.front().elemSize() << " bytes" << endl;
		std::string coded = LibAV::ffv1encode(in);
		cout << "Coded to " << coded.size() << " bytes  1-" << (in.size()*in.front().rows*in.front().cols*in.front().elemSize())/(1.*coded.size()) << " ratio" << endl;
		out = LibAV::ffv1decode<int16_t>(coded);
		cout << "Decoded to " << out.size() << " images " << endl;
		cout << "Is equal: " << (in.size() == out.size()) << endl;*/

	} else {
		cout << "Usage: libav {audio|video} options" << endl;
		return 0;
	}
}