#ifndef IEOMPP_TYPES_EIGEN_DESCRIPTION_HPP_
#define IEOMPP_TYPES_EIGEN_DESCRIPTION_HPP_

#include "ieompp/description.hpp"
#include "ieompp/string.hpp"

#include <Eigen/Dense>
#include <Eigen/Sparse>

namespace ieompp
{
    template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
    struct TypeDescription<Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>> {
        static Description description()
        {
            return Description{{"type", "Eigen::Matrix"}, {"scalar", ""}}
                   + get_type_description<Scalar>().indent(2);
        }
    };

    template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
    struct InstanceDescription<Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>> {
        static Description
        description(const Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>& matrix)
        {
            return Description{
                {"rows",
                 compose(matrix.rows(), (Rows == Eigen::Dynamic) ? " (dynamic)" : " (fixed)")},
                {"cols",
                 compose(matrix.cols(), (Cols == Eigen::Dynamic) ? " (dynamic)" : " (fixed)")}};
        }
    };

    template <typename Scalar, int Options, typename Index>
    struct TypeDescription<Eigen::SparseMatrix<Scalar, Options, Index>> {
        static Description description()
        {
            return Description{{"type", "Eigen::SparseMatrix"}, {"scalar", ""}}
                   + get_type_description<Scalar>().indent(2) + Description{{"index", ""}}
                   + get_type_description<Index>().indent(2);
        }
    };

    template <typename Scalar, int Options, typename Index>
    struct InstanceDescription<Eigen::SparseMatrix<Scalar, Options, Index>> {
        static Description description(const Eigen::SparseMatrix<Scalar, Options, Index>& matrix)
        {
            return Description{{"rows", std::to_string(matrix.rows())},
                               {"cols", std::to_string(matrix.cols())}};
        }
    };
}

#endif


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/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 parse_test.cpp
/// \brief Add your file description here.

#include <string>
#include <sstream>
#include <boost/test/minimal.hpp>
#include "mcrl2/pbes/parse.h"

using namespace mcrl2;
using namespace mcrl2::pbes_system;

const std::string PBESSPEC =
  "pbes nu X(b: Bool) = exists n: Nat. Y(n) && val(b); \n"
  "     mu Y(n: Nat)  = X(n >= 10);                    \n"
  "                                                    \n"
  "init X(true);                                       \n"
  ;

const std::string VARSPEC =
  "datavar         \n"
  "  n: Nat;       \n"
  "                \n"
  "predvar         \n"
  "  X: Bool, Pos; \n"
  "  Y: Nat;       \n"
  ;

void test_parse()
{
  pbes p;
  std::stringstream s(PBESSPEC);
  s >> p;

  pbes_expression x = parse_pbes_expression("X(true, 2) && Y(n+1)", VARSPEC);
  std::cout << "x = " << pbes_system::pp(x) << std::endl;
}

void test_parse_pbes_expression()
{
  data::variable_vector vardecl;
  data::parse_variables("b: Bool; n: Nat;", std::back_inserter(vardecl));
  std::vector<propositional_variable> propvardecl;
  propositional_variable X = parse_propositional_variable("X(b: Bool, n: Nat)", vardecl);
  propositional_variable Y = parse_propositional_variable("Y(n: Nat)", vardecl);
  propvardecl.push_back(X);
  propvardecl.push_back(Y);
  pbes_expression x = parse_pbes_expression("X(true, 2) && Y(n+1)", vardecl, propvardecl);
  BOOST_CHECK(pbes_system::pp(x) == "X(true, 2) && Y(n + 1)");
}

int test_main(int argc, char* argv[])
{
  test_parse();
  test_parse_pbes_expression();

  return 0;
}


/* compact_size_type_test.cc
   Jeremy Barnes, 12 August 2009
   Copyright (c) 2009 Jeremy Barnes.  All rights reserved.

   Testing for the compact size type.
*/

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK

#include "jml/utils/parse_context.h"
#include "jml/utils/file_functions.h"
#include "jml/utils/guard.h"
#include "jml/db/persistent.h"
#include "jml/db/compact_size_types.h"
#include <boost/test/unit_test.hpp>
#include <boost/bind.hpp>
#include <sstream>
#include <boost/multi_array.hpp>
#include "jml/algebra/matrix_ops.h"
#include "jml/stats/distribution.h"

using namespace ML;
using namespace ML::DB;
using namespace std;

using boost::unit_test::test_suite;

#if 0
namespace boost {
namespace test_tools {
namespace tt_detail {

predicate_result
equal_impl(const ML::Stats::distribution<float> & d1,
           const ML::Stats::distribution<float> & d2)
{
    return (d1.size() == d2.size()
            && (d1 == d2).all());
}

} // namespace tt_detail
} // namespace test_tools
} // namespace boost

#endif

template<typename X>
void test_serialize_reconstitute(const X & x)
{
    ostringstream stream_out;

    {
        DB::Store_Writer writer(stream_out);
        writer << x;
        writer << std::string("END");
    }

    istringstream stream_in(stream_out.str());

    DB::Store_Reader reader(stream_in);
    X y;
    std::string s;

    try {
        reader >> y;
        reader >> s;
    } catch (const std::exception & exc) {
        cerr << "serialized representation:" << endl;

        string s = stream_out.str();
        for (unsigned i = 0;  i < s.size();  i += 16) {
            cerr << format("%04x | ", i);
            for (unsigned j = i;  j < i + 16;  ++j) {
                if (j < s.size())
                    cerr << format("%02x ", (int)*(unsigned char *)(&s[j]));
                else cerr << "   ";
            }

            cerr << "| ";

            for (unsigned j = i;  j < i + 16;  ++j) {
                if (j < s.size()) {
                    if (s[j] >= ' ' && s[j] <= 127)
                        cerr << s[i];
                    else cerr << '.';
                }
                else cerr << " ";
            }
            cerr << endl;
        }

        throw;
    }

    BOOST_CHECK_EQUAL(x, y);
    BOOST_CHECK_EQUAL(s, "END");
}

BOOST_AUTO_TEST_CASE( test_char )
{
    test_serialize_reconstitute('a');
}

BOOST_AUTO_TEST_CASE( test_multi_array )
{
    boost::multi_array<float, 2> A(boost::extents[3][3]);
    for (unsigned i = 0;  i < 3;  ++i)
        for (unsigned j = 0;  j < 3;  ++j)
            A[i][j] = j * j - i;

    test_serialize_reconstitute(A);
}

BOOST_AUTO_TEST_CASE( test_bool )
{
    ostringstream stream_out;
    {
        DB::Store_Writer writer(stream_out);
        writer << true;
    }

    BOOST_CHECK_EQUAL(stream_out.str().size(), 1);
    BOOST_CHECK_EQUAL(stream_out.str().at(0), 1);

    test_serialize_reconstitute(true);

    test_serialize_reconstitute(false);
}

BOOST_AUTO_TEST_CASE( test_distribution )
{
    distribution<float> dist;
    test_serialize_reconstitute(dist);

    dist.push_back(1.0);
    test_serialize_reconstitute(dist);

    dist.push_back(2.0);
    test_serialize_reconstitute(dist);
}


#ifndef AST_BOOLEXPR_HPP
#define AST_BOOLEXPR_HPP

#include <set>
#include <vector>
#include <sstream>
#include <string>
#include <memory>
#include <boost/format.hpp>

#include "boolean/Expr.hpp"
#include "ast/Located.hpp"
#include "ast/Common.hpp"

class Variable;
class BoolValue;
class BoolNot;
class BoolAnd;
class BoolOr;

class BoolVisitor {
public:
  virtual void visit(Variable& v) = 0;
  virtual void visit(BoolValue& v) = 0;
  virtual void visit(BoolNot& v) = 0;
  virtual void visit(BoolAnd& v) = 0;
  virtual void visit(BoolOr& v) = 0;
  virtual ~BoolVisitor() = default;
};

class BoolExpr : public LocatedBase {
public:
  BoolExpr(const Located& loc) : LocatedBase(loc) {}
  virtual void accept(BoolVisitor& v) = 0;
};

class BoolNot : public BoolExpr {
public:
  BoolNot(BoolNot& nt) : BoolExpr(nt.getLoc()), arg(nt.getArg()) {}
  BoolNot(const Located& loc_, Ptr<BoolExpr> arg_) : BoolExpr(loc_), arg(arg_) {}
  void accept(BoolVisitor& v) override { v.visit(*this); }
  const String pretty() const override {
    return (boost::format("not (%1%)") % arg->pretty()).str();
  }

  Ptr<BoolExpr> getArg() const { return arg; }

private:
  Ptr<BoolExpr> arg;
};

class BoolAnd : public BoolExpr {
public:
  BoolAnd(BoolAnd& a) : BoolExpr(a.getLoc()), lhs(a.getLhs()), rhs(a.getRhs()) {}
  BoolAnd(const Located& loc_, Ptr<BoolExpr> lhs_, Ptr<BoolExpr> rhs_)
    : BoolExpr(loc_), lhs(lhs_), rhs(rhs_) {}
  void accept(BoolVisitor& v) override { v.visit(*this); }
  const String pretty() const override {
    return (boost::format("(%1%) && (%2%)") % lhs->pretty() % rhs->pretty()).str();
  }

  Ptr<BoolExpr> getLhs() const { return lhs; }
  Ptr<BoolExpr> getRhs() const { return rhs; }

private:
  Ptr<BoolExpr> lhs;
  Ptr<BoolExpr> rhs;
};

class BoolOr : public BoolExpr {
public:
  BoolOr(BoolOr& a) : BoolExpr(a.getLoc()), lhs(a.getLhs()), rhs(a.getRhs()) {}
  BoolOr(const Located& loc_, Ptr<BoolExpr> lhs_, Ptr<BoolExpr> rhs_)
    : BoolExpr(loc_), lhs(lhs_), rhs(rhs_) {}
  void accept(BoolVisitor& v) override { v.visit(*this); }
  const String pretty() const override {
    return (boost::format("(%1%) || (%2%)") % lhs->pretty() % rhs->pretty()).str();
  }

  Ptr<BoolExpr> getLhs() const { return lhs; }
  Ptr<BoolExpr> getRhs() const { return rhs; }

private:
  Ptr<BoolExpr> lhs;
  Ptr<BoolExpr> rhs;
};

class Variable : public BoolExpr {
private:
  VarName name;
public:
  Variable(Variable& v) : BoolExpr(v.getLoc()), name(v.getName()) {}
  Variable(const Located& loc, const VarName& n) : BoolExpr(loc), name (n) {}
  void accept(BoolVisitor& v) override { v.visit(*this); }
  const String pretty() const override {
    std::ostringstream st;
    st << "x" << name.ix;
    return st.str();
  }

  const VarName& getName() const { return name; }
  VarName& getNameRef() { return name; }
};

class BoolValue : public BoolExpr {
private:
  bool value;
public:
  BoolValue (BoolValue& v) : BoolExpr(v.getLoc()), value(v.getValue()) {}
  BoolValue (const Located& loc, bool v) : BoolExpr(loc), value (v) {}

  void accept(BoolVisitor& v) override { v.visit(*this); }

  const String pretty() const override {
    return value ? "true" : "false";
  }

  bool getValue() const { return value; }
};

/**
 * Convert BoolExpr into equivalent compact representaion boolean::Expr
 *
 * @param expr BoolExpr object
 * @return boolean::Expr object
 */
extern boolean::Expr boolExprToExpr(BoolExpr& expr);

/**
 * Get all variables in BoolExpr
 */
extern std::set<VarName> boolExprGetAtomics(BoolExpr& expr);

/**
 * Convert BoolExpr into run time representation
 */
extern void toRtPredicate(BoolExpr& expr,
                          std::vector<uint8_t>& data);

#define RE_TRUE std::make_shared<BoolValue>(RE_LOC, true)
#define RE_FALSE std::make_shared<BoolValue>(RE_LOC, false)
#define RE_VAR(n) std::make_shared<Variable>(RE_LOC, make_varName(n))
#define RE_NOT(n) std::make_shared<BoolNot>(RE_LOC, n)
#define RE_AND(u,v) std::make_shared<BoolAnd>(RE_LOC, u ,v)
#define RE_OR(u,v) std::make_shared<BoolOr>(RE_LOC, u ,v)

#endif // AST_BOOLEXPR_HPP


#include "drake/math/fast_pose_composition_functions.h"

#include <limits>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "drake/common/test_utilities/eigen_matrix_compare.h"

namespace drake {
namespace math {
namespace  {
using Eigen::Matrix3d;

GTEST_TEST(TestFastPoseCompositionFunctions, UsingAVX) {
#ifdef __APPLE__
  constexpr bool kApple = true;
#else
  constexpr bool kApple = false;
#endif

  EXPECT_EQ(IsUsingPortableCompositionMethods(), kApple);
}

// Test the given RotationMatrix composition function for correct functionality
// (just a matrix multiply in this case) and that it still works when the output
// overlaps in memory with the inputs.
void TestRR(
    std::function<void(const double*, const double*, double*)> compose_RR) {
  Matrix3d M, N;
  M << 1, 5, 9, 2, 6, 10, 3, 7, 11;
  N << 13, 17, 21, 14, 18, 22, 15, 19, 23;
  const Matrix3d MM_expected = M * M;
  const Matrix3d MN_expected = M * N;

  Matrix3d MN;
  compose_RR(M.data(), N.data(), MN.data());

  // Should be a perfect match with integer elements.
  EXPECT_TRUE(CompareMatrices(MN, MN_expected, 0));

  // Now test in-place compositions.
  Matrix3d Mwork = M, Nwork = N;  // Copies to overwrite.

  // Results should be perfect match with integer elements.
  compose_RR(Mwork.data(), Nwork.data(), Mwork.data());  // Mwork=M*N
  EXPECT_TRUE(CompareMatrices(Mwork, MN_expected, 0));
  Mwork = M;                                             // Restore value.
  compose_RR(Mwork.data(), Nwork.data(), Nwork.data());  // Nwork=M*N
  EXPECT_TRUE(CompareMatrices(Nwork, MN_expected, 0));
  compose_RR(Mwork.data(), Mwork.data(), Mwork.data());  // Mwork=M*M
  EXPECT_TRUE(CompareMatrices(Mwork, MM_expected, 0));
}

/* Test the user-callable methods first. Those are ideally implemented
with SIMD instructions, however they may just punt to the plain C++ fallback
methods, which we'll test separately below. */
GTEST_TEST(TestFastPoseCompositionFunctions, TestRotationCompositions) {
  SCOPED_TRACE("testing ComposeRR()");
  TestRR(ComposeRR);
}

GTEST_TEST(TestFastPoseCompositionFunctions, TestPortableRotationCompositions) {
  SCOPED_TRACE("testing internal::ComposeRRPortable()");
  TestRR(internal::ComposeRRPortable);
}

}  // namespace
}  // namespace math
}  // namespace drake


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

#include <boost/hana/all_of.hpp>
#include <boost/hana/assert.hpp>
#include <boost/hana/not_equal.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;

int main() {
    static_assert(hana::not_equal(hana::make_tuple(1, 2), hana::make_tuple(3)), "");
    static_assert(hana::not_equal('x', 'y'), "");
    BOOST_HANA_CONSTANT_CHECK(hana::not_equal(hana::make_tuple(1, 2), 'y'));

    static_assert(hana::all_of(hana::make_tuple(1, 2, 3), hana::not_equal.to(5)), "");
}


//
// Copyright Karl Meerbergen 2008
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_NUMERIC_BINDINGS_MUMPS_MUMPS_DRIVER_4_8_0_HPP
#define BOOST_NUMERIC_BINDINGS_MUMPS_MUMPS_DRIVER_4_8_0_HPP

#include <smumps_c.h>
#include <cmumps_c.h>
#include <dmumps_c.h>
#include <zmumps_c.h>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/data_order.hpp>
#include <boost/numeric/bindings/index_base.hpp>
#include <boost/static_assert.hpp>
#include <complex>
#include <cassert>

//
// This file contains a C++ driver for MUMPS
//
// The templated class mumps<M> is a wrapper for the corresponding C struct.
// The class contains constructor and destructor that call mumps with JOB=-1 and JOB=-2
// respectively.
//
// The function driver() calls MUMPS. The user must set the parameters of the data manually.
//
// The following functions are very helpful in this respect, since they extract pointer and size
// data using the Boost Bindings.
//
// void matrix_integer_data() : to set the integer data of the matrix.
// void matrix_value_data() : to set the values of the matrix.
// void rhs_sol_value_data() : to set the right-hand side and solution vectors in the case of a dense solution vector.
//

namespace boost { namespace numeric { namespace bindings { namespace mumps {

  namespace detail {
    //
    // Type and Driver
    //
    template <class T>
    struct mumps_type {
    } ;

    template <class T>
    struct mumps_call {
    } ;

    template <class T>
    struct mumps_internal_value_type {
      typedef T type ;
    } ;

    //
    // Specialization for float
    //

    template <>
    struct mumps_type< float > {
      typedef SMUMPS_STRUC_C type ;
    } ;

    template <>
    struct mumps_call< float > {
      void operator() ( SMUMPS_STRUC_C& struc ) const {
        smumps_c( &struc ) ;
      }
    } ;

    //
    // Specialization for double
    //

    template <>
    struct mumps_type< double > {
      typedef DMUMPS_STRUC_C type ;
    } ;

    template <>
    struct mumps_call< double > {
      void operator() ( DMUMPS_STRUC_C& struc ) const {
        dmumps_c( &struc ) ;
      }
    } ;

    //
    // Specialization for complex<float>
    //

    template <>
    struct mumps_type< std::complex< float > > {
      typedef CMUMPS_STRUC_C type ;
    } ;

    template <>
    struct mumps_call< std::complex< float > > {
      void operator() ( CMUMPS_STRUC_C& struc ) const {
        cmumps_c( &struc ) ;
      }
    } ;

    template <>
    struct mumps_internal_value_type< std::complex<float> > {
      typedef mumps_complex type ;
    } ;

    //
    // Specialization for complex<double>
    //

    template <>
    struct mumps_type< std::complex< double > > {
      typedef ZMUMPS_STRUC_C type ;
    } ;

    template <>
    struct mumps_call< std::complex< double > > {
      void operator() ( ZMUMPS_STRUC_C& struc ) const {
        zmumps_c( &struc ) ;
      }
    } ;

    template <>
    struct mumps_internal_value_type< std::complex<double> > {
      typedef mumps_double_complex type ;
    } ;

    //
    // Symmetry map
    //

    template <class T>
    struct mumps_sym {
    } ;

    template <>
    struct mumps_sym< bindings::tag::symmetric > {
      static int const value = 2 ;
    } ;

    template <>
    struct mumps_sym< bindings::tag::general > {
      static int const value = 0 ;
    } ;

    //
    // Get index pointers
    //
    template <typename M>
    void indices( bindings::tag::row_major, int*& rows, int*& cols, M const& m ) {
      rows = const_cast<int*>( bindings::begin_index_major( m ) ) ;
      cols = const_cast<int*>( bindings::begin_index_minor( m ) ) ;
    }

    template <typename M>
    void indices( bindings::tag::column_major, int*& rows, int*& cols, M const& m ) {
      cols = const_cast<int*>( bindings::begin_index_major( m ) ) ;
      rows = const_cast<int*>( bindings::begin_index_minor( m ) ) ;
    }

    // Pointer Cast
    float* cast_2_mumps( float* p ) { return p ; }
    double* cast_2_mumps( double* p ) { return p ; }
    mumps_double_complex* cast_2_mumps( std::complex<double>* p ) { return reinterpret_cast<mumps_double_complex*>( p ) ; }
    mumps_complex* cast_2_mumps( std::complex<float>* p ) { return reinterpret_cast<mumps_complex*>( p ) ; }
  } // namespace detail

  //
  // Generic MUMPS data for any value_type
  //
  template <typename M>
  class mumps
  : public detail::mumps_type< typename bindings::value_type<M>::type >::type
  {
    public:
      typedef typename bindings::value_type<M>::type                                      value_type ;
      typedef typename detail::mumps_type< typename bindings::value_type<M>::type >::type c_struct_type ;

      //
      // Initialize MUMPS solver
      // Pass a communicator (comm=-987654 means choose default)
      // Pass 'par': default = 1: host is involved in factorization
      //
      mumps( int comm_fortran=-987654, int par=1 )
      {
        this->job = -1 ;
        this->par = par ;
        this->comm_fortran = comm_fortran ;
        this->sym = detail::mumps_sym< typename bindings::detail::property_at<M, tag::matrix_type >::type >::value ;
        detail::mumps_call<value_type>() ( *this ) ;
      }

      // Destroy the solver
      ~mumps() {
        this->job = -2 ;
        detail::mumps_call<value_type>() ( *this ) ;
      }

    private:
      // Disable assignment and Copy Constructor
      mumps& operator=( mumps const& that ) { return *this ; }

      mumps( mumps const& that ) {}
  } ;

  //
  // Copy the matrix integer data (matrix order, structure) to the MUMPS struct
  //
  template <typename M>
  void matrix_integer_data( mumps<M>& data, M& m ) {
    typedef typename bindings::result_of::index_base<M>::type index_b ;
    BOOST_STATIC_ASSERT(index_b::value == 1) ;
    data.n = bindings::size_row( m ) ;
    assert( bindings::size_column( m ) == data.n ) ;

    data.nz = bindings::end_value( m ) - bindings::begin_value( m ) ;
    detail::indices( bindings::data_order(m), data.irn, data.jcn, m ) ;

    data.nz_loc = bindings::end_value( m ) - bindings::begin_value( m ) ;
    detail::indices( bindings::data_order(m), data.irn_loc, data.jcn_loc, m ) ;
  } // matrix_integer_data()

  //
  // Copy the values pointer to the MUMPS struct
  //
  template <typename M>
  void matrix_value_data( mumps<M>& data, M& m ) {
    data.a = detail::cast_2_mumps( bindings::begin_value( m ) ) ;
    data.a_loc = detail::cast_2_mumps( bindings::begin_value( m ) ) ;
  } // matrix_value_data()

  //
  // Copy the right-hand side / solution pointer to the MUMPS struct
  // in case of a dense undistributed right-hand side and solution.
  //
  template <typename M, typename X>
  void rhs_sol_value_data( mumps<M>& data, X& x ) {
    data.rhs = detail::cast_2_mumps( bindings::begin_value( x ) ) ;
    data.nrhs = bindings::size_column( x ) ;
    data.lrhs = bindings::stride_major( x ) ;
  } // matrix_rhs_sol_value_data()

  //
  // Call the MUMPS driver for the given MUMPS struct.
  //
  template <typename M>
  int driver( mumps<M>& data ) {
    assert( data.job>=1 ? data.irn!=0 : true ) ;
    assert( data.job>=1 ? data.jcn!=0 : true ) ;
    assert( data.job>=2 ? data.a!=0 : true ) ;
    assert( data.job==3 || data.job==5 ? data.rhs!=0 : true ) ;
    detail::mumps_call<typename M::value_type>() ( static_cast<typename mumps<M>::c_struct_type&>( data ) ) ;
    return data.info[0] ;
  } // driver()

} } } } // namespace boost::numeric::bindings::mumps

#endif


--- host/lib/usrp/usrp2/usrp2_iface.hpp.orig	2021-04-22 08:57:07 UTC
+++ host/lib/usrp/usrp2/usrp2_iface.hpp
@@ -13,6 +13,8 @@
 #include <uhd/types/serial.hpp>
 #include <uhd/types/wb_iface.hpp>
 #include <uhd/usrp/mboard_eeprom.hpp>
+#include <boost/math/special_functions/round.hpp>
+#include <boost/math/special_functions/sign.hpp>
 #include <boost/utility.hpp>
 #include <functional>
 #include <memory>


#pragma once

#include <crab/analysis/graphs/sccg.hpp>
#include <crab/analysis/graphs/sccg_bgl.hpp>

#include <boost/graph/topological_sort.hpp>
#include <boost/range/iterator_range.hpp>

#include <unordered_map>

/*
   Topological order of a graph
 */

namespace crab {
namespace analyzer {
namespace graph_algo {

// res contains the reverse topological order of a graph g
// pre: g is a DAG.
template <typename G>
void rev_topo_sort(const G &g, std::vector<typename G::node_t> &res) {

  typedef std::unordered_map<typename G::node_t, boost::default_color_type>
      color_map_t;
  typedef boost::associative_property_map<color_map_t> property_color_map_t;

  color_map_t colormap;

  for (auto const &v : boost::make_iterator_range(vertices(g))) {
    colormap[v] = boost::default_color_type();
  }

  res.reserve(num_vertices(g));

  boost::topological_sort(g, std::back_inserter(res),
                          color_map(property_color_map_t(colormap)));
}

// res contains the topological order of g.
// pre: g is a DAG.
template <typename G>
void topo_sort(const G &g, std::vector<typename G::node_t> &res) {
  rev_topo_sort(g, res);
  std::reverse(res.begin(), res.end());
}

// return the reversed topological order of g with possibly cycles.
// XXX: related concept but this is not Bourdoncle's WTO
template <class G> std::vector<typename G::node_t> weak_rev_topo_sort(G g) {
  std::vector<typename G::node_t> sccg_order;
  scc_graph<G> scc_g(g, false /*postorder within scc*/);
  rev_topo_sort(scc_g, sccg_order);
  std::vector<typename G::node_t> order;
  for (auto &n : sccg_order) {
    auto &members = scc_g.get_component_members(n);
    order.insert(order.end(), members.begin(), members.end());
  }
  return order;
}

// return the topological order of g with possibly cycles.
// XXX: related concept but this is not Bourdoncle's WTO
template <class G> std::vector<typename G::node_t> weak_topo_sort(G g) {
  std::vector<typename G::node_t> sccg_order;
  scc_graph<G> scc_g(g, true /*preorder within scc*/);
  topo_sort(scc_g, sccg_order);
  std::vector<typename G::node_t> order;
  for (auto &n : sccg_order) {
    auto &members = scc_g.get_component_members(n);
    order.insert(order.end(), members.begin(), members.end());
  }
  return order;
}

} // end namespace graph_algo
} // end namespace analyzer
} // end namespace crab


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <sstream>
#include <fstream>
#include <iomanip>
#include <string>

#define BOOST_GEOMETRY_REPORT_OVERLAY_ERROR
#define BOOST_GEOMETRY_NO_BOOST_TEST
//#define BOOST_GEOMETRY_TIME_OVERLAY

#include <boost/program_options.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/timer.hpp>

#include <test_overlay_p_q.hpp>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/multi/multi.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>
#include <boost/geometry/domains/gis/io/wkt/wkt.hpp>
#include <boost/geometry/extensions/io/svg/svg_mapper.hpp>

template <typename MultiPolygon>
inline void make_polygon(MultiPolygon& mp, int count_x, int count_y, int index, int width_x)
{
    typedef typename bg::point_type<MultiPolygon>::type point_type;

    for(int j = 0; j < count_x; ++j)
    {
        for(int k = 0; k < count_y; ++k)
        {
            mp.push_back(MultiPolygon::value_type());
            mp.back().outer().push_back(point_type(width_x + j * 10 + 1, k * 10 + 1));
            mp.back().outer().push_back(point_type(width_x + j * 10 + width_x, k * 10 + 5 + index));
            mp.back().outer().push_back(point_type(width_x + j * 10 + 5 + index, k * 10 + 7));
            mp.back().outer().push_back(point_type(width_x + j * 10 + 1, k * 10 + 1));
        }
    }
}

template <typename MultiPolygon>
void test_intersects(int count_x, int count_y, int width_x, p_q_settings const& settings)
{
    MultiPolygon mp;

    make_polygon(mp, count_x, count_y, 0, width_x);

    bool const b = bg::intersects(mp);
    if (b)
    {
        std::cout << " YES";
    }

    if(settings.svg)
    {
        typedef typename bg::coordinate_type<MultiPolygon>::type coordinate_type;
        typedef typename bg::point_type<MultiPolygon>::type point_type;
        std::ostringstream filename;
        filename << "intersects_"
            << string_from_type<coordinate_type>::name()
            << ".svg";

        std::ofstream svg(filename.str().c_str());
        bg::svg_mapper<point_type> mapper(svg, 500, 500);
        mapper.add(mp);
        mapper.map(mp, "fill-opacity:0.5;fill:rgb(153,204,0);"
                "stroke:rgb(153,204,0);stroke-width:3");
    }
}

template <typename T, bool Clockwise, bool Closed>
void test_all(int count, int count_x, int count_y, int width_x, p_q_settings const& settings)
{
    boost::timer t;

    typedef bg::model::polygon
        <
            bg::model::d2::point_xy<T>, Clockwise, Closed
        > polygon;
    typedef bg::model::multi_polygon
        <
            polygon
        > multi_polygon;

    int index = 0;
    for(int i = 0; i < count; i++)
    {
        test_intersects<multi_polygon>(count_x, count_y, width_x, settings);
    }
    std::cout
        << " type: " << string_from_type<T>::name()
        << " time: " << t.elapsed()  << std::endl;
}

int main(int argc, char** argv)
{
    try
    {
        namespace po = boost::program_options;
        po::options_description description("=== intersects ===\nAllowed options");

        int width_x = 7;
        int count = 1;
        int count_x = 10;
        int count_y = 10;
        bool ccw = false;
        bool open = false;
        p_q_settings settings;

        description.add_options()
            ("help", "Help message")
            ("count", po::value<int>(&count)->default_value(1), "Number of tests")
            ("count_x", po::value<int>(&count_x)->default_value(10), "Triangle count in x-direction")
            ("count_y", po::value<int>(&count_y)->default_value(10), "Triangle count in y-direction")
            ("width_x", po::value<int>(&width_x)->default_value(7), "Width of triangle in x-direction")
            ("ccw", po::value<bool>(&ccw)->default_value(false), "Counter clockwise polygons")
            ("open", po::value<bool>(&open)->default_value(false), "Open polygons")
            ("wkt", po::value<bool>(&settings.wkt)->default_value(false), "Create a WKT of the inputs, for all tests")
            ("svg", po::value<bool>(&settings.svg)->default_value(false), "Create a SVG for all tests")
        ;

        po::variables_map varmap;
        po::store(po::parse_command_line(argc, argv, description), varmap);
        po::notify(varmap);

        if (varmap.count("help"))
        {
            std::cout << description << std::endl;
            return 1;
        }

        if (ccw && open)
        {
            test_all<double, false, false>(count, count_x, count_y, width_x, settings);
        }
        else if (ccw)
        {
            test_all<double, false, true>(count, count_x, count_y, width_x, settings);
        }
        else if (open)
        {
            test_all<double, true, false>(count, count_x, count_y, width_x, settings);
        }
        else
        {
            test_all<double, true, true>(count, count_x, count_y, width_x, settings);
        }

#if defined(HAVE_TTMATH)
        // test_all<ttmath_big, true, true>(seed, count, max, svg, level);
#endif
    }
    catch(std::exception const& e)
    {
        std::cout << "Exception " << e.what() << std::endl;
    }
    catch(...)
    {
        std::cout << "Other exception" << std::endl;
    }

    return 0;
}


/* Boost test/add.cpp
 * test with symbolic operations if the addition algorithm is correct
 *
 * Copyright 2002-2003 Guillaume Melquiond
 *
 * 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 <boost/numeric/interval/interval.hpp>
#include <boost/numeric/interval/arith.hpp>
#include <boost/numeric/interval/rounding.hpp>
#include <boost/numeric/interval/rounded_arith.hpp>
#include <boost/numeric/interval/utility.hpp>
#include <boost/numeric/interval/policies.hpp>
#include <boost/core/lightweight_test.hpp>
#include "bugs.hpp"

typedef enum { EXPR_VAR, EXPR_NEG, EXPR_UP, EXPR_DOWN, EXPR_ADD, EXPR_SUB } e_type;

struct expr;
struct pexpr {
  expr *ptr;
  expr* operator->() { return ptr; }
  pexpr(expr *p = NULL): ptr(p) { }
};

struct expr {
  e_type type;
  int var;
  pexpr e;
  pexpr e1, e2;
};

pexpr var(int v) {
  pexpr e = new expr;
  e->type = EXPR_VAR;
  e->var = v;
  return e;
}

pexpr operator+(pexpr, pexpr);
pexpr operator-(pexpr, pexpr);
pexpr operator-(pexpr);

pexpr operator+(pexpr a, pexpr b) {
  if (a->type == EXPR_NEG) return b - a->e;
  if (b->type == EXPR_NEG) return a - b->e;
  if (a->type == EXPR_VAR && b->type == EXPR_VAR && a->var > b->var) return b + a;
  pexpr c = new expr;
  c->type = EXPR_ADD;
  c->e1 = a;
  c->e2 = b;
  return c;
}

pexpr operator-(pexpr a, pexpr b) {
  if (b->type == EXPR_NEG) return a + b->e;
  pexpr c = new expr;
  c->type = EXPR_SUB;
  c->e1 = a;
  c->e2 = b;
  return c;
}

pexpr down(pexpr a) {
  pexpr e = new expr;
  e->type = EXPR_DOWN;
  e->e = a;
  return e;
}

pexpr up(pexpr a) {
  pexpr e = new expr;
  e->type = EXPR_UP;
  e->e = a;
  return e;
}

pexpr operator-(pexpr a) {
  if (a->type == EXPR_NEG) return a->e;
  if (a->type == EXPR_UP) return down(-a->e);
  if (a->type == EXPR_DOWN) return up(-a->e);
  if (a->type == EXPR_SUB) return a->e2 - a->e1;
  if (a->type == EXPR_ADD) return -a->e1 - a->e2;
  pexpr e = new expr;
  e->type = EXPR_NEG;
  e->e = a;
  return e;
}

bool operator==(pexpr a, pexpr b) {
  if (a->type != b->type) return false;
  if (a->type == EXPR_VAR) return a->var == b->var;
  if (a->type == EXPR_DOWN || a->type == EXPR_UP || a->type == EXPR_NEG)
    return a->e == b->e;
  return a->e1 == b->e1 && a->e2 == b->e2;
}

bool operator<=(pexpr, pexpr) { return true; }

namespace boost {
namespace numeric {
namespace interval_lib {

template<>
struct rounding_control<pexpr> {
  typedef enum { RND_U, RND_M, RND_D } rounding_mode;
  static rounding_mode mode;
  rounding_control() { mode = RND_M; }
  void get_rounding_mode(rounding_mode& m) { m = mode; }
  void set_rounding_mode(rounding_mode m)  { mode = m; }
  void upward()   { mode = RND_U; }
  void downward() { mode = RND_D; }
  pexpr force_rounding(pexpr a) {
    switch (mode) {
    case RND_U: return up(a);
    case RND_D: return down(a);
    default: throw "Unset rounding mode";
    }
  }
};

rounding_control<pexpr>::rounding_mode rounding_control<pexpr>::mode = RND_M;

} // namespace interval_lib
} // namespace numeric
} // namespace boost

template<class I>
bool test_neg() {
  I a(var(0), var(1));
  return equal(-a, I(-var(1), -var(0)));
}

template<class I>
bool test_add() {
  I a(var(0), var(1)), b(var(2), var(3));
  return equal(a + b, I(down(var(0) + var(2)), up(var(1) + var(3))));
}

template<class I>
bool test_add1() {
  I a(var(0), var(1));
  return equal(a + var(2), I(down(var(0) + var(2)), up(var(1) + var(2))));
}

template<class I>
bool test_add2() {
  I a(var(0), var(1));
  return equal(var(2) + a, I(down(var(0) + var(2)), up(var(1) + var(2))));
}

template<class I>
bool test_sub() {
  I a(var(0), var(1)), b(var(2), var(3));
  return equal(a - b, I(down(var(0) - var(3)), up(var(1) - var(2))));
}

template<class I>
bool test_sub1() {
  I a(var(0), var(1));
  return equal(a - var(2), I(down(var(0) - var(2)), up(var(1) - var(2))));
}

template<class I>
bool test_sub2() {
  I a(var(0), var(1));
  return equal(var(2) - a, I(down(var(2) - var(1)), up(var(2) - var(0))));
}

template<class I>
bool test_addeq() {
  I a(var(0), var(1)), b(var(2), var(3));
  return equal(a += b, I(down(var(0) + var(2)), up(var(1) + var(3))));
}

template<class I>
bool test_addeq1() {
  I a(var(0), var(1));
  return equal(a += var(2), I(down(var(0) + var(2)), up(var(1) + var(2))));
}

template<class I>
bool test_subeq() {
  I a(var(0), var(1)), b(var(2), var(3));
  return equal(a -= b, I(down(var(0) - var(3)), up(var(1) - var(2))));
}

template<class I>
bool test_subeq1() {
  I a(var(0), var(1));
  return equal(a -= var(2), I(down(var(0) - var(2)), up(var(1) - var(2))));
}

struct my_checking
{
  static pexpr pos_inf() { throw; }
  static pexpr neg_inf() { throw; }
  static pexpr nan() { throw; }
  static bool is_nan(const pexpr&) { return false; }
  static pexpr empty_lower() { throw; }
  static pexpr empty_upper() { throw; }
  static bool is_empty(const pexpr&, const pexpr&) { return false; }
};

template<class Rounding>
struct my_interval {
private:
  typedef boost::numeric::interval_lib::save_state<Rounding> my_rounding;
  typedef boost::numeric::interval_lib::policies<my_rounding, my_checking> my_policies;
public:
  typedef boost::numeric::interval<pexpr, my_policies> type;
};

int main() {
  typedef my_interval<boost::numeric::interval_lib::rounded_arith_std<pexpr> >::type I1;
  typedef my_interval<boost::numeric::interval_lib::rounded_arith_opp<pexpr> >::type I2;
  BOOST_TEST((test_neg<I1>()));
  BOOST_TEST((test_neg<I2>()));
  BOOST_TEST((test_add<I1>()));
  BOOST_TEST((test_add<I2>()));
  BOOST_TEST((test_add1<I1>()));
  BOOST_TEST((test_add1<I2>()));
  BOOST_TEST((test_add2<I1>()));
  BOOST_TEST((test_add2<I2>()));
  BOOST_TEST((test_sub<I1>()));
  BOOST_TEST((test_sub<I2>()));
  BOOST_TEST((test_sub1<I1>()));
  BOOST_TEST((test_sub1<I2>()));
  BOOST_TEST((test_sub2<I1>()));
  BOOST_TEST((test_sub2<I2>()));
  BOOST_TEST((test_addeq<I1>()));
  BOOST_TEST((test_addeq<I2>()));
  BOOST_TEST((test_addeq1<I1>()));
  BOOST_TEST((test_addeq1<I2>()));
  BOOST_TEST((test_subeq<I1>()));
  BOOST_TEST((test_subeq<I2>()));
  BOOST_TEST((test_subeq1<I1>()));
  BOOST_TEST((test_subeq1<I2>()));
  return boost::report_errors();
}


// (C) Copyright 2007-2009 Andrew Sutton
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GRAPH_CLUSTERING_COEFFICIENT_HPP
#define BOOST_GRAPH_CLUSTERING_COEFFICIENT_HPP

#include <boost/utility.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>

namespace boost
{
namespace detail
{
    template <class Graph>
    inline typename graph_traits<Graph>::degree_size_type
    possible_edges(const Graph& g, std::size_t k, directed_tag)
    {
        function_requires< GraphConcept<Graph> >();
        typedef typename graph_traits<Graph>::degree_size_type T;
        return T(k) * (T(k) - 1);
    }

    template <class Graph>
    inline typename graph_traits<Graph>::degree_size_type
    possible_edges(const Graph& g, size_t k, undirected_tag)
    {
        // dirty little trick...
        return possible_edges(g, k, directed_tag()) / 2;
    }

    // This template matches directedS and bidirectionalS.
    template <class Graph>
    inline typename graph_traits<Graph>::degree_size_type
    count_edges(const Graph& g,
                typename Graph::vertex_descriptor u,
                typename Graph::vertex_descriptor v,
                directed_tag)

    {
        function_requires< AdjacencyMatrixConcept<Graph> >();
        return (edge(u, v, g).second ? 1 : 0) +
                (edge(v, u, g).second ? 1 : 0);
    }

    // This template matches undirectedS
    template <class Graph>
    inline typename graph_traits<Graph>::degree_size_type
    count_edges(const Graph& g,
                typename Graph::vertex_descriptor u,
                typename Graph::vertex_descriptor v,
                undirected_tag)
    {
        function_requires< AdjacencyMatrixConcept<Graph> >();
        return edge(u, v, g).second ? 1 : 0;
    }
}

template <typename Graph, typename Vertex>
inline typename graph_traits<Graph>::degree_size_type
num_paths_through_vertex(const Graph& g, Vertex v)
{
    function_requires< AdjacencyGraphConcept<Graph> >();
    typedef typename graph_traits<Graph>::directed_category Directed;
    typedef typename graph_traits<Graph>::adjacency_iterator AdjacencyIterator;

    // TODO: There should actually be a set of neighborhood functions
    // for things like this (num_neighbors() would be great).

    AdjacencyIterator i, end;
    tie(i, end) = adjacent_vertices(v, g);
    std::size_t k = std::distance(i, end);
    return detail::possible_edges(g, k, Directed());
}

template <typename Graph, typename Vertex>
inline typename graph_traits<Graph>::degree_size_type
num_triangles_on_vertex(const Graph& g, Vertex v)
{
    function_requires< IncidenceGraphConcept<Graph> >();
    function_requires< AdjacencyGraphConcept<Graph> >();
    typedef typename graph_traits<Graph>::degree_size_type Degree;
    typedef typename graph_traits<Graph>::directed_category Directed;
    typedef typename graph_traits<Graph>::adjacency_iterator AdjacencyIterator;

    // TODO: I might be able to reduce the requirement from adjacency graph
    // to incidence graph by using out edges.

    Degree count(0);
    AdjacencyIterator i, j, end;
    for(tie(i, end) = adjacent_vertices(v, g); i != end; ++i) {
        for(j = boost::next(i); j != end; ++j) {
            count += detail::count_edges(g, *i, *j, Directed());
        }
    }
    return count;
} /* namespace detail */

template <typename T, typename Graph, typename Vertex>
inline T
clustering_coefficient(const Graph& g, Vertex v)
{
    T zero(0);
    T routes = T(num_paths_through_vertex(g, v));
    return (routes > zero) ?
        T(num_triangles_on_vertex(g, v)) / routes : zero;
}

template <typename Graph, typename Vertex>
inline double
clustering_coefficient(const Graph& g, Vertex v)
{ return clustering_coefficient<double>(g, v); }

template <typename Graph, typename ClusteringMap>
inline typename property_traits<ClusteringMap>::value_type
all_clustering_coefficients(const Graph& g, ClusteringMap cm)
{
    function_requires< VertexListGraphConcept<Graph> >();
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename graph_traits<Graph>::vertex_iterator VertexIterator;
    function_requires< WritablePropertyMapConcept<ClusteringMap,Vertex> >();
    typedef typename property_traits<ClusteringMap>::value_type Coefficient;

    Coefficient sum(0);
    VertexIterator i, end;
    for(tie(i, end) = vertices(g); i != end; ++i) {
        Coefficient cc = clustering_coefficient<Coefficient>(g, *i);
        put(cm, *i, cc);
        sum += cc;
    }
    return sum / Coefficient(num_vertices(g));
}

template <typename Graph, typename ClusteringMap>
inline typename property_traits<ClusteringMap>::value_type
mean_clustering_coefficient(const Graph& g, ClusteringMap cm)
{
    function_requires< VertexListGraphConcept<Graph> >();
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename graph_traits<Graph>::vertex_iterator VertexIterator;
    function_requires< ReadablePropertyMapConcept<ClusteringMap,Vertex> >();
    typedef typename property_traits<ClusteringMap>::value_type Coefficient;

    Coefficient cc(0);
    VertexIterator i, end;
    for(tie(i, end) = vertices(g); i != end; ++i) {
        cc += get(cm, *i);
    }
    return cc / Coefficient(num_vertices(g));
}

} /* namespace boost */

#endif


#include <hdf5.h>
#include <omp.h>
#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <cmath>
#include <cstdio>
#include <iostream>
#include <memory>
#include <regex>
#include <stdexcept>
#include <tuple>

#include "base/eigen2hdf.hpp"
#include "base/exceptions.hpp"
#include "brt_config.h"
#include "fft/fft2_r2c.hpp"
#include "ridgelet/rc_linearize.hpp"
#include "ridgelet/ridgelet_cell_array.hpp"
#include "ridgelet/ridgelet_frame.hpp"
#include "ridgelet/rt.hpp"
#include "spectral/quadrature/qhermitew.hpp"

typedef FFTr2c<PlannerR2C> fft_t;
// typedef FFTr2c<PlannerR2COD> fft_t;

unsigned int PLANNER_STRATEGY = FFTW_MEASURE;

typedef RT<double, RidgeletFrame, fft_t> RT_t;
typedef RT_t::rt_coeff_t rt_coeff_t;
typedef RidgeletCellArray<rt_coeff_t> rca_t;

namespace po = boost::program_options;

unsigned int ncoeffs(const RidgeletFrame& rf)
{
  unsigned int nc = 0;
  for (auto& lam : rf.lambdas()) {
    auto t = tgrid_dim(lam, rf);
    unsigned int TX = std::get<0>(t) * std::get<1>(t);
    nc += TX;
  }
  return nc;
}

std::vector<std::tuple<int, int>> load_directions_all(int K)
{
  std::vector<std::tuple<int, int>> vec(K * K);
  for (int i = 0; i < K; ++i) {
    for (int j = 0; j < K; ++j) {
      vec[i * K + j] = std::make_tuple(i, j);
    }
  }

  return vec;
}

std::vector<std::tuple<int, int>> load_directions_file(const std::string& fname = "config.yaml")
{
  if (!(boost::filesystem::is_regular_file(fname) || boost::filesystem::is_symlink(fname))) {
    throw std::runtime_error("No file called " + fname + "found! Exiting.");
  }
  YAML::Node config = YAML::LoadFile(fname);
  auto V = config["dirs"];

  std::vector<std::tuple<int, int>> vec;
  for (int k = 0; k < V.size(); ++k) {
    int i = V[k][0].as<int>();
    int j = V[k][1].as<int>();
    vec.push_back(std::make_tuple(i, j));
  }

  return vec;
}

std::vector<double> load_qs(const std::string& fname = "config.yaml")
{
  if (!(boost::filesystem::is_regular_file(fname) || boost::filesystem::is_symlink(fname))) {
    throw std::runtime_error("No file called " + fname + "found! Exiting.");
  }
  YAML::Node config = YAML::LoadFile(fname);
  auto V = config["qs"];

  std::vector<double> vec;
  for (int k = 0; k < V.size(); ++k) {
    vec.push_back(V[k].as<double>());
  }

  return vec;
}

int get_frame(const std::string& fname)
{
  std::regex my_regex(".*solution_vector([0-9]*)");
  std::smatch match;
  bool found = std::regex_search(fname, match, my_regex);
  if (found) {
    return atoi(match[1].str().c_str());
  } else {
    throw std::runtime_error("could not find frame number");
  }
}

template <typename T>
void dump_ac(int frame, const T& array)
{
  char buf[256];
  sprintf(buf, "active_coeffs%06d.h5", frame);

  hid_t h5f = H5Fcreate(buf, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  eigen2hdf::save(h5f, "C", array);
  H5Fclose(h5f);
}

int main(int argc, char* argv[])
{
  std::cout << "SOURCE::INFO::GIT_BRANCHNAME " << GIT_BNAME << "\n";
  std::cout << "SOURCE::INFO::GIT_SHA1       " << GIT_SHA1 << "\n";

  // disable buffering of printf
  std::setbuf(stdout, NULL);

  po::options_description options("options");
  options.add_options()
      ("help", "produce help message")
      ("save", "save set of active coefficients")
      ("alld", "compute rt in all directions")
      ("verbose,v", "vebose output")
      ("errors", "compute errors");

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, options), vm);
    po::notify(vm);
  } catch (std::exception& e) {
    if (vm.count("help"))
      std::cout << options << "\n";
    else
      std::cout << e.what() << "\n";
    return 1;
  }
  if (vm.count("help")) {
    std::cout << options << "\n";
    return 0;
  }

  typedef Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> array_t;
  // read input files from stdin and check if they exist
  std::string input_line;
  std::vector<std::string> fnames;
  while (std::cin) {
    std::getline(std::cin, input_line);
    // check for empty (e.g. terminating line)
    std::regex my_regex("^([[:space:]]*|)$");
    std::smatch my_match;
    std::regex_search(input_line, my_match, my_regex);
    // cout << "match size: " << my_match.size() << "\n";
    if (my_match.size() > 0) {
      break;
    } else {
      if (boost::filesystem::is_regular_file(input_line) ||
          boost::filesystem::is_symlink(input_line))
        fnames.push_back(input_line);
      else
        throw std::runtime_error("File " + input_line + " does not exist. Exiting.");
    }
  };

  std::cout << "Found " << fnames.size() << " input files\n";
  std::vector<int> frame_numbers(fnames.size());
  std::transform(fnames.begin(), fnames.end(), frame_numbers.begin(), get_frame);

  // load files
  std::shared_ptr<RidgeletFrame> rf_ptr;
  std::shared_ptr<RCLinearize> rcl_ptr;
  typedef std::vector<std::tuple<int, int>> vdirections_t;
  std::shared_ptr<vdirections_t> directions_ptr;
  std::vector<double> qs;                              // ratio of active coefficients.  q in (0, 1]
  directions_ptr = std::make_shared<vdirections_t>();  // initialize empty
  Eigen::Array<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> active_coeffs;

  int fc = 0;
  for (auto& fname : fnames) {
    array_t coeffs;
    hid_t h5f = H5Fopen(fname.c_str(), H5F_ACC_RDONLY, H5P_DEFAULT);
    eigen2hdf::load(h5f, "C", coeffs);
    int N = coeffs.rows();
    int L = coeffs.cols();
    int K = std::sqrt(N);
    ASSERT(K * K == N);
    int Lx = std::sqrt(L);
    ASSERT(Lx * Lx == L);
    H5Fclose(h5f);

    if (!rf_ptr) {
      int J = std::log2(2 * Lx) - 2;
      rf_ptr = std::make_shared<RidgeletFrame>(J, J, 1, 1);
      rcl_ptr = std::make_shared<RCLinearize>(*rf_ptr);
      fft_t fft;
      init_fftw(fft, PLANNER_STRATEGY, *rf_ptr);

      if (vm.count("alld")) {
        // read directions from config
        *directions_ptr = load_directions_all(K);
      } else {
        // make all directions
        if (vm.count("errors")) {
          std::cerr << "computing errors without using all directions is not implemented!"
                    << "\n";
          return 1;
        }
        *directions_ptr = load_directions_file("config.yaml");
      }
      qs = load_qs("config.yaml");
      std::cout << "what is inside dirs"
                << "\n";
      unsigned int nc = ncoeffs(*rf_ptr);
      active_coeffs.resize(qs.size(), nc);
      active_coeffs.setZero();
    }
    fft_t fft;
    RT_t rt(*rf_ptr);

    typedef RT_t::complex_array_t complex_array_t;

    // for later use
    boltzmann::QHermiteW quad(1.0, K);
    auto w = quad.wts();
    decltype(w) sqrtw(K);
    std::transform(w.begin(), w.end(), sqrtw.begin(), [](double x) { return std::sqrt(x); });

    // approximation error for reduced basis
    std::vector<double> errors(qs.size(), 0);

#pragma omp parallel default(none) shared(active_coeffs,  \
                                          coeffs,         \
                                          rt,             \
                                          rf_ptr,         \
                                          rcl_ptr,        \
                                          fft,            \
                                          L,              \
                                          Lx,             \
                                          K,              \
                                          directions_ptr, \
                                          qs,             \
                                          errors,         \
                                          sqrtw,          \
                                          vm)
    {
      complex_array_t Fhh(Lx, Lx);
      complex_array_t Fh(2 * Lx, 2 * Lx);
      rca_t f_rc(*rf_ptr);

#pragma omp for schedule(static)
      for (int k = 0; k < directions_ptr->size(); k++) {
        // read F..
        int j1 = std::get<0>(directions_ptr->operator[](k));
        int j2 = std::get<1>(directions_ptr->operator[](k));
        Eigen::Map<array_t> F(coeffs.data() + L * (j2 * K + j1), Lx, Lx);
        fft.ft(Fhh, F, false);
        Fh.setZero();
        ftcut(Fh, Lx, Lx) = Fhh;
        hf_zero(Fh);
        rt.rt(f_rc.coeffs(), Fh);

        std::vector<double> rclin = rcl_ptr->linearize(f_rc.coeffs());
        std::vector<double> tres = rcl_ptr->get_threshold(std::vector<double>(rclin), qs);

        for (unsigned int qi = 0; qi < tres.size(); ++qi) {
          // compute active set
          // find locations where std::abs(rclin) > tre
          for (unsigned int i = 0; i < rclin.size(); ++i) {
            if (std::abs(rclin[i]) > tres[qi])
              // synchronization is not necessary
              active_coeffs(qi, i) = 1;
          }
          // compute approximation errors
          if (vm.count("errors")) {
            rca_t reduced = f_rc;
            rcl_ptr->threshold(f_rc, tres[qi]);
            // irt
            complex_array_t Fh2(2 * Lx, 2 * Lx);
            rt.irt(Fh2, reduced);
            complex_array_t Fh(Lx, Lx);
            Fh = ftcut(Fh2, Lx, Lx);
            hf_zero(Fh);
            array_t F2(Lx, Lx);
            fft.ift(F2, Fh);
            double h = 1.0 / Lx;
            double loc_err = ((F - F2).cwiseAbs2()).sum() * h * h * sqrtw[j1] * sqrtw[j2];
#pragma omp critical
            {
              errors[qi] += loc_err;
            }
          }
        }
      }
    }

    // ---------- LOG FILE ----------
    //
    if (vm.count("errors")) {
      std::cout << fname << "@errors "
                << "\t";
      for (int i = 0; i < errors.size(); ++i) {
        std::cout << std::scientific << std::sqrt(errors[i]) << "\t";
      }
      std::cout << "\n";
    }
    // stats
    Eigen::ArrayXd nnz_ratio = active_coeffs.rowwise().sum().cast<double>();
    nnz_ratio /= active_coeffs.cols();
    std::cout << fname << "@nnz_ratio"
              << "\t";
    for (int i = 0; i < active_coeffs.rows(); ++i) {
      std::cout << nnz_ratio[i] << "\t";
    }

    // ---------- HDF export ----------
    if (vm.count("save-all")) {
      dump_ac(frame_numbers[fc], active_coeffs);
    } else if (vm.count("save") && (fc == 0 || fc == fnames.size() - 1)) {
      dump_ac(frame_numbers[fc], active_coeffs);
    }

    fc++;  // increment frame counter
  }        // end for files

  // if(vm.count("save")) {
  //   int frame_min = *std::min_element(frame_numbers.begin(), frame_numbers.end());
  //   int frame_max = *std::max_element(frame_numbers.begin(), frame_numbers.end());

  //   char buf[256];
  //   sprintf(buf, "active_coeffs%06d-%06d.h5", frame_min, frame_max);

  //   hid_t h5f = H5Fcreate(buf, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  //   eigen2hdf::save(h5f, "C", active_coeffs);
  //   H5Fclose(h5f);
  // }

  std::cout << "\n";

  return 0;
}


/*
 * This file is part of the Sequoia MSO Solver.
 *
 * Copyright 2012 Alexander Langer, Theoretical Computer Science,
 *                                  RWTH Aachen University
 *
 * 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.
 */
/**
 * @author Alexander Langer
 */
#include "structures/labeled_graph.h"
#include "structures/graph_factory.h"

#include <boost/graph/connected_components.hpp>

#include <iostream>
#include <fstream>

using namespace sequoia;
using namespace boost;

template <typename Graph, typename VertexIndexMap>
void test_connected(Graph &g, VertexIndexMap vm) {
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator_t;
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    typedef typename graph_traits<Graph>::vertices_size_type v_size_t;
    typedef iterator_property_map< typename std::vector<v_size_t>::iterator,
                                   VertexIndexMap
                                  > vertex_to_v_size_map_t;

    std::vector<v_size_t> component_vector(num_vertices(g));
    vertex_to_v_size_map_t component(component_vector.begin(), vm);
    std::vector<vertex_t> vertices_by_component(num_vertices(g));

    v_size_t num_components = connected_components(g, component);

    std::cout << num_components << std::endl;
}

int main(int argc, char **argv) {
    if (argc < 2) {
        std::cerr << "Usage: " << argv[0]
                << " infile [outfile]"
                << std::endl;
        return 1;
    }
    LabeledGraph g;
    try {
	GraphFactory<LabeledGraph>::load_graph(g, argv[1]);
    } catch (const std::exception &e) {
	std::cerr << "Error loading graph: " << e.what() << std::endl;
        exit(EXIT_FAILURE);
    }

    test_connected(g.impl(), boost::get(boost::vertex_index,g.impl()));
    return 0;
}