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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

/*

    @ingroup group-constant

    Generates constant Minlog2 used in logarithm/exponential computations
    boost::simd::log2(x) return  0 if x is less than Minlog2 (underflow)

    @par Semantic:

    @code
    T r = Minlog2<T>();
    @endcode

    is similar to:

    @code
    if T is double
    r = -1023.0;
    else if T is float
    r = -127.0;

    @endcode

*/

namespace boost { namespace simd
{
  namespace tag
  {
    struct minlog2_ : boost::dispatch::constant_value_<minlog2_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,minlog2_,boost::dispatch::constant_value_<minlog2_>);
      BOOST_SIMD_REGISTER_CONSTANT(0, 0xc2fe0000UL, 0xc08ff00000000000ULL);
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag,minlog2_);
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::minlog2_,minlog2);
  }

  template<typename T> BOOST_FORCEINLINE auto Minlog2()
  BOOST_NOEXCEPT_DECLTYPE(detail::minlog2( boost::dispatch::as_<T>{}))
  {
    return detail::minlog2( boost::dispatch::as_<T>{} );
  }
} }

#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


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

#include "precomp.hpp"

// Eigen
#include <Eigen/Core>

// OpenCV
#include <opencv2/core/eigen.hpp>
#include <opencv2/sfm/numeric.hpp>
#include <opencv2/sfm/projection.hpp>

// libmv headers
#include "libmv/multiview/projection.h"

#include <iostream>

namespace cv
{
namespace sfm
{

template<typename T>
void
homogeneousToEuclidean(const Mat & X_, Mat & x_)
{
  int d = X_.rows - 1;

  const Mat_<T> & X_rows = X_.rowRange(0,d);
  const Mat_<T> h = X_.row(d);

  const T * h_ptr = h[0], *h_ptr_end = h_ptr + h.cols;
  const T * X_ptr = X_rows[0];
  T * x_ptr = x_.ptr<T>(0);
  for (; h_ptr != h_ptr_end; ++h_ptr, ++X_ptr, ++x_ptr)
  {
    const T * X_col_ptr = X_ptr;
    T * x_col_ptr = x_ptr, *x_col_ptr_end = x_col_ptr + d * x_.step1();
    for (; x_col_ptr != x_col_ptr_end; X_col_ptr+=X_rows.step1(), x_col_ptr+=x_.step1() )
      *x_col_ptr = (*X_col_ptr) / (*h_ptr);
  }
}

void
homogeneousToEuclidean(InputArray X_, OutputArray x_)
{
  // src
  const Mat X = X_.getMat();

  // dst
   x_.create(X.rows-1, X.cols, X.type());
  Mat x = x_.getMat();

  // type
  if( X.depth() == CV_32F )
  {
    homogeneousToEuclidean<float>(X,x);
  }
  else
  {
    homogeneousToEuclidean<double>(X,x);
  }
}

void
euclideanToHomogeneous(InputArray x_, OutputArray X_)
{
  const Mat x = x_.getMat();
  const Mat last_row = Mat::ones(1, x.cols, x.type());
  vconcat(x, last_row, X_);
}

template<typename T>
void
projectionFromKRt(const Mat_<T> &K, const Mat_<T> &R, const Mat_<T> &t, Mat_<T> P)
{
  hconcat( K*R, K*t, P );
}

void
projectionFromKRt(InputArray K_, InputArray R_, InputArray t_, OutputArray P_)
{
  const Mat K = K_.getMat(), R = R_.getMat(), t = t_.getMat();
  const int depth = K.depth();
  CV_Assert((K.cols == 3 && K.rows == 3) && (t.cols == 1 && t.rows == 3) && (K.size() == R.size()));
  CV_Assert((depth == CV_32F || depth == CV_64F) && depth == R.depth() && depth == t.depth());

  P_.create(3, 4, depth);

  Mat P = P_.getMat();

  // type
  if( depth == CV_32F )
  {
    projectionFromKRt<float>(K, R, t, P);
  }
  else
  {
    projectionFromKRt<double>(K, R, t, P);
  }

}

template<typename T>
void
KRtFromProjection( const Mat_<T> &P_, Mat_<T> K_, Mat_<T> R_, Mat_<T> t_ )
{
  libmv::Mat34 P;
  libmv::Mat3 K, R;
  libmv::Vec3 t;

  cv2eigen( P_, P );

  libmv::KRt_From_P( P, &K, &R, &t );

  eigen2cv( K, K_ );
  eigen2cv( R, R_ );
  eigen2cv( t, t_ );
}

void
KRtFromProjection( InputArray P_, OutputArray K_, OutputArray R_, OutputArray t_ )
{
  const Mat P = P_.getMat();
  const int depth = P.depth();
  CV_Assert((P.cols == 4 && P.rows == 3) && (depth == CV_32F || depth == CV_64F));

  K_.create(3, 3, depth);
  R_.create(3, 3, depth);
  t_.create(3, 1, depth);

  Mat K = K_.getMat(), R = R_.getMat(), t = t_.getMat();

  // type
  if( depth == CV_32F )
  {
    KRtFromProjection<float>(P, K, R, t);
  }
  else
  {
    KRtFromProjection<double>(P, K, R, t);
  }
}

template<typename T>
T
depthValue( const Mat_<T> &R_, const Mat_<T> &t_, const Mat_<T> &X_ )
{
  Matx<T,3,3> R(R_);
  Vec<T,3> t(t_);

  if ( X_.rows == 3)
  {
    Vec<T,3> X(X_);
    return (R*X)(2) + t(2);
  }
  else
  {
    Vec<T,4> X(X_);
    Vec<T,3> Xe;
    homogeneousToEuclidean(X,Xe);
    return depthValue<T>( Mat(R), Mat(t), Mat(Xe) );
  }
}

double
depth( InputArray R_, InputArray t_, InputArray X_)
{
  const Mat R = R_.getMat(), t = t_.getMat(), X = X_.getMat();
  const int depth = R.depth();
  CV_Assert( R.rows == 3 && R.cols == 3 && t.rows == 3 && t.cols == 1 );
  CV_Assert( (X.rows == 3 && X.cols == 1) || (X.rows == 4 && X.cols == 1) );
  CV_Assert( depth == CV_32F || depth == CV_64F );

  double depth_value = 0.0;

  if ( depth == CV_32F )
  {
    depth_value = static_cast<double>(depthValue<float>(R, t, X));
  }
  else
  {
    depth_value = depthValue<double>(R, t, X);
  }

  return depth_value;
}

} /* namespace sfm */
} /* namespace cv */


#include <iostream>
#include <vector>
#include <fstream>
#include <list>
#include <set>
#include <stack>
#include <algorithm>
#include <boost/date_time.hpp>

typedef std::vector<std::vector<unsigned long>> Graph;

std::stack<unsigned long> topological(Graph vertices, unsigned long vertice_count){

    std::cout << ("finding topological") << std::endl;
    std::set<unsigned long> notfound;
    for(unsigned long i=0;i<vertices.size();i++) notfound.emplace(i);
    std::set<unsigned long> found;
    std::stack<unsigned long> topo;
    std::stack<std::pair<unsigned long, bool>> stack;

    while (topo.size() < vertices.size()){
        if (stack.empty()){
            auto p = notfound.begin();
            stack.emplace(*p, false);
        }
        std::pair<unsigned long,bool> holder = stack.top();
        stack.pop();
        unsigned long i = holder.first;
        bool backtracking = holder.second;
        auto pos = found.find(i);
        if (not backtracking and pos == found.end()){
            notfound.erase(notfound.find(i));
            found.emplace(i);
            std::vector<unsigned long> children = vertices[i];
            bool pisfound = false;
            for(auto p = children.begin(); p != children.end(); pisfound ? p = children.erase(p) : p++ ){ // remove found children
                pisfound = false;
                if (found.find(*p) != found.end()){
                    pisfound = true;
                }
            }
            if(not children.empty()){
                stack.emplace(i, true);
                for (unsigned long v : children){
                    stack.emplace(v, false);
                }
            } else {
                topo.emplace(i);
            }
        } else if (backtracking){
            topo.emplace(i);
        }

    }
    return topo;
}

void load_vertices(const std::string &filename, bool get_r,
        Graph &vertices, Graph &vertices_r
                ,unsigned long &vertice_count, unsigned long &edge_count){

    std::cout << "reading file" << std::endl;
    std::fstream file(filename, std::ios::in);

    file >> vertice_count >> edge_count;

    vertices.reserve(static_cast<unsigned long>(vertice_count));
    vertices_r.reserve(static_cast<unsigned long>(vertice_count));
    for(unsigned long _ = 0; _ < vertice_count; _++){
        vertices.emplace_back(std::vector<unsigned long>());
        if (get_r) vertices_r.emplace_back(std::vector<unsigned long>());
    }

    unsigned long to, fro;
    while (file >> to >> fro) {
        vertices[to].emplace_back(fro);
        if (get_r) vertices_r[fro].emplace_back(to);
    }
    file.close();
}

std::vector<std::vector<unsigned long>> findComponents(Graph vertices, unsigned long vertice_count, std::stack<unsigned long> order){
    std::cout << "finding components" << std::endl;
    std::vector<unsigned long> nodecomp;
    nodecomp.reserve(static_cast<unsigned long>(vertice_count));
    for(unsigned long _ = 0; _ < vertice_count; _++) nodecomp.emplace_back(ULONG_MAX);
    unsigned long comp = 0;

    while (not order.empty()){
        unsigned long v = order.top();
        order.pop();

        if (nodecomp[v] == ULONG_MAX){
            std::vector<unsigned long> children = vertices[v];
            nodecomp[v] = comp;
            while (not children.empty()){
                auto p = children.begin();
                unsigned long c = *p;
                children.erase(p);
                if (nodecomp[c] == ULONG_MAX){
                    nodecomp[c] = comp;
                    std::vector<unsigned long> subch = vertices[c];
                    for (unsigned long i : subch) children.emplace_back(i);

                }
            }
            comp++;
        }

    }
    std::vector<std::vector<unsigned long>> result;
    result.reserve(static_cast<unsigned long>(comp));
    for(unsigned long _ = 0; _ < comp; _++) result.emplace_back(std::vector<unsigned long>());
    for(unsigned long i = 0; i < nodecomp.size(); i++){
        unsigned long v = nodecomp[i];
        result[v].emplace_back(i);
    }
    return result;

}

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

    std::string filename = "L7Skandinavia";
    if (argc > 1)
        filename = argv[1];

    Graph vertices;
    Graph vertices_r;
    unsigned long vertice_count = 0;
    unsigned long edge_count = 0;

    boost::posix_time::ptime start = boost::posix_time::microsec_clock::local_time();
    load_vertices(filename, true, vertices, vertices_r, vertice_count, edge_count);
     boost::posix_time::ptime end = boost::posix_time::microsec_clock::local_time();
    std::cout << "loaded " << vertices.size() << " vertices in " << (end-start).total_microseconds()/1000000. << " seconds" << std::endl;

    start = boost::posix_time::microsec_clock::local_time();
    std::stack<unsigned long> topo = topological(vertices, vertice_count);
    end = boost::posix_time::microsec_clock::local_time();
    auto ostart = start;
    std::cout << "found topological order in " << (end-start).total_microseconds()/1000000. << " seconds" << std::endl;

    start = boost::posix_time::microsec_clock::local_time();
    auto strongs = findComponents(vertices_r, vertice_count, topo);
    end = boost::posix_time::microsec_clock::local_time();
    std::cout << "found components in " << (end-start).total_microseconds()/1000000. << " seconds" << std::endl;
    std::cout << "total time: " << (end-ostart).total_microseconds()/1000000 << " seconds" << std::endl;

    std::cout << "grafen " << filename << " har " << strongs.size() << " sterkt sammenhengende komponenter." << std::endl;
    if (strongs.size() < 100){
        std::cout << "Komponent\t\tNoder i komponenten" << std::endl;
        for(unsigned long i = 0; i < strongs.size(); i++){
            std::vector<unsigned long> v = strongs[i];
            std::cout << i << "\t\t\t\t";
            for(unsigned long j = 0; j < v.size(); j++){
                std::cout << v[j];
                if (j+1 < v.size()) std::cout << ", ";
            }
            std::cout << std::endl;
        }
    }

    return EXIT_SUCCESS;

}


#include "NumberMpq.h"

#include "NumberClI.h"
#include "NumberClRA.h"
#include "NumberMpz.h"

#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>

namespace carl {

#ifdef USE_CLN_NUMBERS
		Number<mpq_class>::Number(const Number<cln::cl_RA>& n) : Number(n.toString()) {}
		Number<mpq_class>::Number(const Number<cln::cl_I>& n) : Number(n.toString()) {}
#endif

	//TODO: probably also add possibility to construct from fraction strings; is there a reason for this complicated method? instead of using mpq_class constructor??
	Number<mpq_class>::Number(const std::string& s) {
		std::vector<std::string> strs;
		boost::split(strs, s, boost::is_any_of("."));

		if(strs.size() > 2)
		{
		    throw std::invalid_argument("More than one delimiter in the string.");
		}
		mpq_class result;
		if(!strs.front().empty())
		{
		    result += mpq_class(strs.front());
		}
		if(strs.size() > 1)
		{
		    //if(strs.back().size() > )
		    result += (mpq_class(strs.back())/carl::pow(mpz_class(10),static_cast<unsigned>(strs.back().size())));
		}
		mData = result;
	}

	//constructs a/b:
	Number<mpq_class>::Number(const Number<mpz_class>& a,const Number<mpz_class>& b) { mData = mpq_class(a.getValue(),b.getValue()); }

	Number<mpq_class>::Number(const Number<mpz_class>& n) { mData = mpq_class(n.getValue()); }
	Number<mpq_class>::Number(const mpz_class& n) { mData = mpq_class(n); }

	//TODO: doesn't mpq_class have a standard "output" as well?
	std::string Number<mpq_class>::toString(bool _infix) const
	{
		std::stringstream s;
		bool negative = (mData < mpq_class(0));
		if(negative) s << "(-" << (_infix ? "" : " ");
		if(_infix) s << this->abs();
		else
		{
		    mpz_class d = mData.get_den();
		    if(constant_one<mpz_class>::get() != mData) {
			mpz_class abs1, abs2;
			mpz_abs(abs1.get_mpz_t(), mData.get_num().get_mpz_t());
			mpz_abs(abs2.get_mpz_t(), d.get_mpz_t());
			s << "(/ " << abs1 << " " << abs2 << ")";
		    } else {
			s << this->abs().mData;
			}
		}
		if(negative)
		    s << ")";
		return s.str();
	}

	 bool Number<mpq_class>::sqrt_exact(Number<mpq_class>& b) const
	    {
		if( mpq_sgn(mData.__get_mp()) < 0 ) return false;
		mpz_class den = mData.get_den();
		mpz_class num = mData.get_num();
		mpz_class root_den;
		mpz_class root_den_rem;
		mpz_sqrtrem(root_den.__get_mp(), root_den_rem.__get_mp(), den.__get_mp());
		if( !Number( root_den_rem ).isZero() )
		    return false;

		mpz_class root_num;
		mpz_class root_num_rem;
		mpz_sqrtrem(root_num.__get_mp(), root_num_rem.__get_mp(), num.__get_mp());
		if( !Number( root_num_rem ).isZero() )
		    return false;

		mpq_class resNum;
		mpq_set_z(resNum.get_mpq_t(), root_num.get_mpz_t());
		mpq_class resDen;
		mpq_set_z(resDen.get_mpq_t(), root_den.get_mpz_t());

		mpq_class fraction;
		mpq_div(fraction.get_mpq_t(), resNum.get_mpq_t(), resDen.get_mpq_t());

		b = Number<mpq_class>(fraction);
		return true;
	    }

	    Number<mpq_class> Number<mpq_class>::sqrt() const {
		std::pair<Number<mpq_class>,Number<mpq_class>> r = this->sqrt_safe();
		return (r.first + r.second) / 2;
	    }

	    std::pair<Number<mpq_class>,Number<mpq_class>> Number<mpq_class>::sqrt_safe() const
	    {
		assert( mpq_sgn(mData.__get_mp()) > 0 );
		mpz_class den = mData.get_den();
		mpz_class num = mData.get_num();
		mpz_class root_den;
		mpz_class root_den_rem;
		mpz_sqrtrem(root_den.__get_mp(), root_den_rem.__get_mp(), den.__get_mp());

		mpz_class root_num;
		mpz_class root_num_rem;
		mpz_sqrtrem(root_num.__get_mp(), root_num_rem.__get_mp(), num.__get_mp());

		mpq_class lower;
		mpq_class upper;

		lower = root_num;
		if(root_den_rem == 0)
		    lower /= root_den;
		else
		    lower /= root_den+1;

		if(root_num_rem == 0)
		    upper = root_num;
		else
		    upper = root_num+1;

		upper /= root_den;

		return std::make_pair(Number(lower),Number(upper));
	    }

	    std::pair<Number<mpq_class>, Number<mpq_class>> Number<mpq_class>::sqrt_fast() const
	    {
		assert(mData >= 0);
	#if 1
		return sqrt_safe(); //NOTE: there was something (probably) equivalent to sqrt_safe() here, so this replacement should be ok
	#else
		mpq_class exact_root;
		if (carl::sqrtp(mData, exact_root)) {
		    // root can be computed exactly.
		    return std::make_pair(exact_root, exact_root);
		} else {
		    // compute an approximation with sqrt(). we can assume that the surrounding integers contain the actual root.
		    mpf_class af = sqrt(mpf_class(mData));
		    mpq_class lower(af - carl::constant_one<mpf_class>::get());
		    mpq_class upper(af + carl::constant_one<mpf_class>::get());
		    assert(lower * lower < mData);
		    assert(upper * upper > mData);
		    return std::make_pair(lower, upper);
		}
	#endif
	    }

}


#ifndef MIA_SPACIAL_HPP
#define MIA_SPACIAL_HPP

#include <array>
#include <cstddef>
#include <functional>
#include <tuple>
#include <type_traits>
#include <utility>
#include <gsl/gsl_assert>
#include <gsl/gsl_util>
#include <boost/hana/equal.hpp>
#include <boost/hana/ext/std/array.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/unpack.hpp>
#include <range/v3/utility/common_type.hpp>
#include <range/v3/utility/concepts.hpp>
#include <jegp/utility.hpp>
#include <mia/concepts.hpp>
#include <mia/units.hpp>

namespace mia {

enum class Axis { x, y, z };

template <Axis A, class Qty>
class Coordinate;

namespace detail {

    template <Axis A>
    struct Mp_coordinate {
        template <class Qty>
        using fn = Coordinate<A, Qty>;
    };

} // namespace detail

template <Axis A, class Qty>
class Coordinate
  : public Unit_alias<detail::Mp_coordinate<A>::template fn, Qty> {
public:
    static_assert(A >= Axis::x);

    static constexpr Axis axis{A};

    using Unit_alias<detail::Mp_coordinate<A>::template fn, Qty>::Unit_alias;
};

template <class Qty>
using Abscissa = Coordinate<Axis::x, Qty>;
template <class Qty>
using Ordinate = Coordinate<Axis::y, Qty>;
template <class Qty>
using Applicate = Coordinate<Axis::z, Qty>;

} // namespace mia

namespace ranges {

template <mia::Axis A, class Qty1, class Qty2>
struct common_type<mia::Coordinate<A, Qty1>, mia::Coordinate<A, Qty2>> {
    using type = mia::Coordinate<A, common_type_t<Qty1, Qty2>>;
};

} // namespace ranges

namespace std {

template <mia::Axis A, class Qty>
struct hash<mia::Coordinate<A, Qty>>
  : mia::detail::Unit_alias_hash<
        mia::detail::Mp_coordinate<A>::template fn, Qty> {
};

} // namespace std

namespace mia {

enum class Coordinates;

template <Coordinates C, class Qty>
class Point {
public:
    static_assert(C >= Coordinates{1});
    static_assert(Quantity<Qty>{});

    static constexpr Coordinates coordinates{C};
    using quantity = Qty;

private:
    static constexpr boost::hana::range<int, 0, jegp::underlying(coordinates)>
        ints{};

    static constexpr std::array axes{boost::hana::unpack(
        ints, [](auto... int_c) { return std::array{Axis{int_c()}...}; })};

public:
    Point()             = default;
    Point(const Point&) = default;
    Point(Point&&)      = default;
    ~Point()            = default;
    Point& operator=(const Point&) = default;
    Point& operator=(Point&&) = default;

    template <
        Axis... A, class... Qty2,
        CONCEPT_REQUIRES_(
            std::conjunction_v<
                std::bool_constant<boost::hana::equal(axes, std::array{A...})>,
                ranges::Constructible<
                    Coordinate<A, quantity>, const Coordinate<A, Qty2>&>...>)>
    explicit constexpr Point(const Coordinate<A, Qty2>&... coord) noexcept(
        std::conjunction_v<std::is_nothrow_constructible<
            Coordinate<A, quantity>, const Coordinate<A, Qty2>&>...>)
      : coords{coord...}
    {
    }

    template <
        class Qty2, CONCEPT_REQUIRES_(ranges::Constructible<quantity, Qty2>{})>
    constexpr Point(const Point<coordinates, Qty2>& pt) noexcept(
        std::is_nothrow_copy_constructible_v<Qty2>&&
            std::is_nothrow_constructible_v<quantity, Qty2>)
      : coords{boost::hana::unpack(ints, [&](auto... int_c) {
            return Coords{pt.template get<int_c>()...};
        })}
    {
    }

    constexpr Abscissa<quantity>& x() noexcept
    {
        return std::get<jegp::underlying(Axis::x)>(coords);
    }
    constexpr const Abscissa<quantity>& x() const noexcept
    {
        return std::get<jegp::underlying(Axis::x)>(coords);
    }

    template <Axis A = Axis::y, CONCEPT_REQUIRES_(A <= axes.back())>
    constexpr Ordinate<quantity>& y() noexcept
    {
        return std::get<jegp::underlying(A)>(coords);
    }
    template <Axis A = Axis::y, CONCEPT_REQUIRES_(A <= axes.back())>
    constexpr const Ordinate<quantity>& y() const noexcept
    {
        return std::get<jegp::underlying(A)>(coords);
    }

    template <Axis A = Axis::z, CONCEPT_REQUIRES_(A <= axes.back())>
    constexpr Applicate<quantity>& z() noexcept
    {
        return std::get<jegp::underlying(A)>(coords);
    }
    template <Axis A = Axis::z, CONCEPT_REQUIRES_(A <= axes.back())>
    constexpr const Applicate<quantity>& z() const noexcept
    {
        return std::get<jegp::underlying(A)>(coords);
    }

    template <std::size_t I>
    constexpr Coordinate<Axis{I}, quantity>& get() noexcept
    {
        return std::get<I>(coords);
    }
    template <std::size_t I>
    constexpr const Coordinate<Axis{I}, quantity>& get() const noexcept
    {
        return std::get<I>(coords);
    }

private:
    static constexpr auto coords_tuple = [](auto... int_c) {
        return std::tuple<Coordinate<axes[int_c], quantity>...>{};
    };

    using Coords = decltype(boost::hana::unpack(ints, coords_tuple));

    Coords coords;
};

template <Axis... A, class Qty>
explicit Point(Coordinate<A, Qty>...)->Point<Coordinates{sizeof...(A)}, Qty>;

template <class Qty>
using Point2d = Point<Coordinates{2}, Qty>;
template <class Qty>
using Point3d = Point<Coordinates{3}, Qty>;

template <
    std::size_t I, Coordinates C, class Qty,
    CONCEPT_REQUIRES_(I < jegp::underlying(C))>
constexpr Coordinate<Axis{I}, Qty>& get(Point<C, Qty>& pt) noexcept
{
    return pt.template get<I>();
}
template <
    std::size_t I, Coordinates C, class Qty,
    CONCEPT_REQUIRES_(I < jegp::underlying(C))>
constexpr const Coordinate<Axis{I}, Qty>& get(const Point<C, Qty>& pt) noexcept
{
    return pt.template get<I>();
}

template <
    Axis A, Coordinates C, class Qty,
    CONCEPT_REQUIRES_(Axis::x <= A && A < static_cast<Axis>(C))>
constexpr Coordinate<A, Qty>& get(Point<C, Qty>& pt) noexcept
{
    return pt.template get<jegp::underlying(A)>();
}
template <
    Axis A, Coordinates C, class Qty,
    CONCEPT_REQUIRES_(Axis::x <= A && A < static_cast<Axis>(C))>
constexpr const Coordinate<A, Qty>& get(const Point<C, Qty>& pt) noexcept
{
    return pt.template get<jegp::underlying(A)>();
}

template <
    class Coord, Coordinates C, class Qty,
    CONCEPT_REQUIRES_(
        std::is_same_v<Coord, Coordinate<Coord::axis, Qty>> &&
        (Axis::x <= Coord::axis && Coord::axis < static_cast<Axis>(C)))>
constexpr Coord& get(Point<C, Qty>& pt) noexcept
{
    return pt.template get<jegp::underlying(Coord::axis)>();
}
template <
    class Coord, Coordinates C, class Qty,
    CONCEPT_REQUIRES_(
        std::is_same_v<Coord, Coordinate<Coord::axis, Qty>> &&
        (Axis::x <= Coord::axis && Coord::axis < static_cast<Axis>(C)))>
constexpr const Coord& get(const Point<C, Qty>& pt) noexcept
{
    return pt.template get<jegp::underlying(Coord::axis)>();
}

template <
    Coordinates C, class Qty1, class Qty2,
    CONCEPT_REQUIRES_(ranges::EqualityComparable<Qty1, Qty2>{})>
constexpr auto operator==(
    const Point<C, Qty1>& l,
    const Point<C, Qty2>&
        r) noexcept(noexcept(std::declval<Qty1>() == std::declval<Qty2>()))
{
    return boost::hana::unpack(
        boost::hana::range_c<int, 0, jegp::underlying(C)>, [&](auto... int_c) {
            return (... && (get<int_c>(l) == get<int_c>(r)));
        });
}
template <Coordinates C, class Qty1, class Qty2>
constexpr auto operator!=(
    const Point<C, Qty1>& l,
    const Point<C, Qty2>& r) noexcept(noexcept(!(l == r)))
    -> decltype(!(l == r))
{
    return !(l == r);
}

} // namespace mia

namespace std {

template <mia::Coordinates C, class Qty>
struct tuple_size<mia::Point<C, Qty>>
  : integral_constant<size_t, jegp::underlying(C)> {
};

template <size_t I, mia::Coordinates C, class Qty>
struct tuple_element<I, mia::Point<C, Qty>> {
    static_assert(I < jegp::underlying(C));

    using type = mia::Coordinate<mia::Axis{I}, Qty>;
};

} // namespace std

namespace ranges {

template <mia::Coordinates C, class Qty1, class Qty2>
struct common_type<mia::Point<C, Qty1>, mia::Point<C, Qty2>> {
    using type = mia::Point<C, common_type_t<Qty1, Qty2>>;
};

} // namespace ranges

namespace mia::detail {

template <class Tuple, class = void>
struct Tuple_hash
  : Tuple_hash<Tuple, std::make_index_sequence<std::tuple_size_v<Tuple>>> {
};

template <class Tuple, std::size_t... I>
struct Tuple_hash<Tuple, std::index_sequence<I...>>
  : private std::hash<std::tuple_element_t<I, Tuple>>... {
    constexpr auto operator()(const Tuple& t) const
        noexcept(noexcept(jegp::hash_combine(get<I>(t)...)))
            -> decltype(jegp::hash_combine(get<I>(t)...))
    {
        return jegp::hash_combine(get<I>(t)...);
    }
};

} // namespace mia::detail

namespace std {

template <mia::Coordinates C, class Qty>
struct hash<mia::Point<C, Qty>> : mia::detail::Tuple_hash<mia::Point<C, Qty>> {
};

} // namespace std

namespace mia {

template <Axis A, class Qty>
class Dimension;

namespace detail {

    template <Axis A>
    struct Mp_dimension {
        template <class Qty>
        using fn = Dimension<A, Qty>;
    };

} // namespace detail

template <Axis A, class Qty>
class Dimension : public Unit_alias<detail::Mp_dimension<A>::template fn, Qty> {
public:
    static_assert(A >= Axis::x);

    static constexpr Axis axis{A};

    using Unit_alias<detail::Mp_dimension<A>::template fn, Qty>::Unit_alias;
};

template <class Qty>
using Width = Dimension<Axis::x, Qty>;
template <class Qty>
using Height = Dimension<Axis::y, Qty>;
template <class Qty>
using Depth = Dimension<Axis::z, Qty>;

} // namespace mia

namespace ranges {

template <mia::Axis A, class Qty1, class Qty2>
struct common_type<mia::Dimension<A, Qty1>, mia::Dimension<A, Qty2>> {
    using type = mia::Dimension<A, common_type_t<Qty1, Qty2>>;
};

} // namespace ranges

namespace std {

template <mia::Axis A, class Qty>
struct hash<mia::Dimension<A, Qty>>
  : mia::detail::Unit_alias_hash<
        mia::detail::Mp_dimension<A>::template fn, Qty> {
};

} // namespace std

namespace mia {

enum class Dimensions;

inline namespace literals {
    inline namespace dimensions_literals { //

        constexpr Dimensions operator""_D(unsigned long long d)
        {
            return gsl::narrow_cast<Dimensions>(d);
        }

    } // namespace dimensions_literals
} // namespace literals

template <Dimensions D, class Qty>
class Size {
public:
    static_assert(D >= 1_D);
    static_assert(Quantity<Qty>{});

    static constexpr Dimensions dimensions{D};
    using quantity = Qty;

private:
    static constexpr boost::hana::range<int, 0, jegp::underlying(dimensions)>
        ints{};

    static constexpr std::array axes{boost::hana::unpack(
        ints, [](auto... int_c) { return std::array{Axis{int_c()}...}; })};

public:
    Size(const Size&) = default;
    Size(Size&&)      = default;
    ~Size()           = default;
    Size& operator=(const Size&) = default;
    Size& operator=(Size&&) = default;

    template <
        Axis... A, class... Qty2,
        CONCEPT_REQUIRES_(
            std::conjunction_v<
                std::bool_constant<boost::hana::equal(axes, std::array{A...})>,
                ranges::Constructible<
                    Dimension<A, quantity>, const Dimension<A, Qty2>&>...>)>
    explicit constexpr Size(const Dimension<A, Qty2>&... dim)
      : dims{[&] {
            (..., Expects((dim > Dimension<A, Qty2>{})));
            return Dims{dim...};
        }()}
    {
    }

    template <
        class Qty2, CONCEPT_REQUIRES_(ranges::Constructible<quantity, Qty2>{})>
    constexpr Size(const Size<dimensions, Qty2>& sz) noexcept(
        std::is_nothrow_copy_constructible_v<Qty2>&&
            std::is_nothrow_constructible_v<quantity, Qty2>)
      : dims{boost::hana::unpack(ints, [&](auto... int_c) {
            return Dims{sz.template get<int_c>()...};
        })}
    {
    }

    constexpr void w(const Width<quantity>& w)
    {
        Expects(w > Width<quantity>{});
        std::get<jegp::underlying(Axis::x)>(dims) = w;
    }
    template <Axis A = Axis::y, CONCEPT_REQUIRES_(A <= axes.back())>
    constexpr void h(const Height<quantity>& h)
    {
        Expects(h > Height<quantity>{});
        std::get<jegp::underlying(A)>(dims) = h;
    }
    template <Axis A = Axis::z, CONCEPT_REQUIRES_(A <= axes.back())>
    constexpr void d(const Depth<quantity>& d)
    {
        Expects(d > Depth<quantity>{});
        std::get<jegp::underlying(A)>(dims) = d;
    }

    template <Axis A, CONCEPT_REQUIRES_(Axis::x <= A && A <= axes.back())>
    constexpr void set(const Dimension<A, quantity>& dim)
    {
        Expects((dim > Dimension<A, quantity>{}));
        std::get<jegp::underlying(A)>(dims) = dim;
    }

    constexpr Width<quantity> w() const
        noexcept(std::is_nothrow_copy_constructible_v<Width<quantity>>)
    {
        return std::get<jegp::underlying(Axis::x)>(dims);
    }
    template <Axis A = Axis::y, CONCEPT_REQUIRES_(A <= axes.back())>
    constexpr Height<quantity> h() const
        noexcept(std::is_nothrow_copy_constructible_v<Height<quantity>>)
    {
        return std::get<jegp::underlying(A)>(dims);
    }
    template <Axis A = Axis::z, CONCEPT_REQUIRES_(A <= axes.back())>
    constexpr Depth<quantity> d() const
        noexcept(std::is_nothrow_copy_constructible_v<Depth<quantity>>)
    {
        return std::get<jegp::underlying(A)>(dims);
    }

    template <std::size_t I>
    constexpr Dimension<Axis{I}, quantity> get() const noexcept(
        std::is_nothrow_copy_constructible_v<Dimension<Axis{I}, quantity>>)
    {
        return std::get<I>(dims);
    }

private:
    static constexpr auto dims_tuple = [](auto... int_c) {
        return std::tuple<Dimension<axes[int_c], quantity>...>{};
    };

    using Dims = decltype(boost::hana::unpack(ints, dims_tuple));

    Dims dims;
};

template <Axis... A, class Qty>
explicit Size(Dimension<A, Qty>...)->Size<Dimensions{sizeof...(A)}, Qty>;

template <class Qty>
using Size2d = Size<2_D, Qty>;
template <class Qty>
using Size3d = Size<3_D, Qty>;

template <
    std::size_t I, Dimensions D, class Qty,
    CONCEPT_REQUIRES_(I < jegp::underlying(D))>
constexpr Dimension<Axis{I}, Qty> get(const Size<D, Qty>& sz) noexcept(
    std::is_nothrow_copy_constructible_v<Dimension<Axis{I}, Qty>>)
{
    return sz.template get<I>();
}
template <
    Axis A, Dimensions D, class Qty,
    CONCEPT_REQUIRES_(Axis::x <= A && A < static_cast<Axis>(D))>
constexpr Dimension<A, Qty> get(const Size<D, Qty>& sz) noexcept(
    std::is_nothrow_copy_constructible_v<Dimension<A, Qty>>)
{
    return sz.template get<jegp::underlying(A)>();
}
template <
    class Dim, Dimensions D, class Qty,
    CONCEPT_REQUIRES_(
        std::is_same_v<Dim, Dimension<Dim::axis, Qty>> &&
        (Axis::x <= Dim::axis && Dim::axis < static_cast<Axis>(D)))>
constexpr Dim get(const Size<D, Qty>& sz) noexcept(
    std::is_nothrow_copy_constructible_v<Dim>)
{
    return sz.template get<jegp::underlying(Dim::axis)>();
}

template <
    Dimensions D, class Qty1, class Qty2,
    CONCEPT_REQUIRES_(ranges::EqualityComparable<Qty1, Qty2>{})>
constexpr auto operator==(
    const Size<D, Qty1>& l,
    const Size<D, Qty2>&
        r) noexcept(noexcept(std::declval<Qty1>() == std::declval<Qty2>()))
{
    return boost::hana::unpack(
        boost::hana::range_c<int, 0, jegp::underlying(D)>, [&](auto... int_c) {
            return (... && (get<int_c>(l) == get<int_c>(r)));
        });
}
template <Dimensions D, class Qty1, class Qty2>
constexpr auto operator!=(
    const Size<D, Qty1>& l,
    const Size<D, Qty2>& r) noexcept(noexcept(!(l == r))) -> decltype(!(l == r))
{
    return !(l == r);
}

} // namespace mia

namespace std {

template <mia::Dimensions D, class Qty>
struct tuple_size<mia::Size<D, Qty>>
  : integral_constant<size_t, jegp::underlying(D)> {
};

template <size_t I, mia::Dimensions D, class Qty>
struct tuple_element<I, mia::Size<D, Qty>> {
    static_assert(I < jegp::underlying(D));

    using type = mia::Dimension<mia::Axis{I}, Qty>;
};

} // namespace std

namespace ranges {

template <mia::Dimensions D, class Qty1, class Qty2>
struct common_type<mia::Size<D, Qty1>, mia::Size<D, Qty2>> {
    using type = mia::Size<D, common_type_t<Qty1, Qty2>>;
};

} // namespace ranges

namespace std {

template <mia::Dimensions D, class Qty>
struct hash<mia::Size<D, Qty>> : mia::detail::Tuple_hash<mia::Size<D, Qty>> {
};

} // namespace std

#endif // MIA_SPACIAL_HPP


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

#include <iostream>
#include <cmath>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <typename Vector>
void test(Vector&, const char* name)
{
    using mtl::sum; using mtl::product; using mtl::one_norm;

    Vector a(3), b(3), res(3);
    a= 1, 2, 3; b= 4, 5, 6; res= -3, 6, -3;

    std::cout << name << ": cross(" << a << ", " << b << ") is " << cross(a, b) << '\n';

    MTL_THROW_IF(one_norm(Vector(cross(a, b) - res)) > 0.0001, mtl::runtime_error("Wrong cross product with dimension 3!\n"));

    Vector c(7), d(7);
    c= 1, 2, 3, 4, 5, 6, 7;
    d= 9, 8, 7, 6, 5, 4, 3;

    std::cout << "cross(" << c << ", " << d << ") is " << cross(c, d) << '\n';

    // What is the result???
}

int main(int, char**)
{
    using namespace mtl;
    using mtl::vec::parameters;

    dense_vector<float>   u;
    dense_vector<double>  x;
    dense_vector<std::complex<double> >  xc;

    std::cout << "Testing vector operations\n";

    test(u, "test float");
    test(x, "test double");

    test(xc, "test complex<double>");

    dense_vector<float, parameters<row_major> >   ur(5);
    //test(ur, "test float in row vector");

    return 0;
}


// This file is part of the dune-xt-common project:
//   https://github.com/dune-community/dune-xt-common
// Copyright 2009-2018 dune-xt-common developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   Felix Schindler (2012 - 2017)
//   René Fritze     (2012 - 2016, 2018)
//   Sven Kaulmann   (2012 - 2013)
//   Tobias Leibner  (2014, 2018)

#ifndef DUNE_XT_COMMON_RANGES_HH
#define DUNE_XT_COMMON_RANGES_HH

#include <type_traits>

#include <boost/serialization/static_warning.hpp>
#include <boost/iterator/iterator_facade.hpp>

#include <dune/xt/common/debug.hh>
#include <dune/xt/common/math.hh>

namespace Dune {
namespace XT {
namespace Common {

//! get a vector with values in [start : increment : end)
template <class T, class sequence = std::vector<T>>
typename std::enable_if<!std::is_enum<T>::value, sequence>::type
value_range(const T start, const T end, const T increment = Epsilon<T>::value)
{
  // sadly, no overloaded version of std::abs is available for
  // unsigned long long, so we compute the absolute value of increment
  // ourselves
  const auto incrementAbs = abs(increment);
  DXT_ASSERT(incrementAbs > 0);
  sequence ret(typename sequence::size_type(((end > start) ? end - start : start - end) / incrementAbs), start);
  typename sequence::size_type i = 0;
  std::generate(std::begin(ret), std::end(ret), [&]() { return T(start + (increment * i++)); });
  return ret;
}

//! signature for enumeration Types T
template <class T, class sequence = std::vector<typename internal::Absretval<T>::type>>
typename std::enable_if<std::is_enum<T>::value, sequence>::type value_range(
    const T start,
    const T end,
    const typename internal::Absretval<T>::type increment = Epsilon<typename internal::Absretval<T>::type>::value)
{
  typedef typename internal::Absretval<T>::type R;
  return value_range(static_cast<R>(start), static_cast<R>(end), increment);
}

//! get a vector with values in [0 : Epsilon<T> : end)
template <class T, class sequence = std::vector<T>>
typename std::enable_if<!std::is_enum<T>::value, sequence>::type value_range(const T end)
{
  return value_range(T(0), end);
}

//! get a vector with values in [0 : Epsilon<T> : end)
template <class T, class sequence = std::vector<typename internal::Absretval<T>::type>>
typename std::enable_if<std::is_enum<T>::value, sequence>::type value_range(const T end)
{
  return value_range(T(0), end);
}

} // namespace Common
} // namespace XT
} // namespace Dune

#endif // DUNE_XT_COMMON_RANGES_HH


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <vector>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/adjacency_list.hpp>

int
main()
{
  using namespace boost;
  typedef adjacency_list < vecS, vecS, undirectedS > Graph;
  typedef graph_traits < Graph >::vertex_descriptor Vertex;

  const int N = 6;
  Graph G(N);
  add_edge(0, 1, G);
  add_edge(1, 4, G);
  add_edge(4, 0, G);
  add_edge(2, 5, G);

  std::vector<int> c(num_vertices(G));
  int num = connected_components
    (G, make_iterator_property_map(c.begin(), get(vertex_index, G), c[0]));

  std::cout << std::endl;
  std::vector < int >::iterator i;
  std::cout << "Total number of components: " << num << std::endl;
  for (i = c.begin(); i != c.end(); ++i)
    std::cout << "Vertex " << i - c.begin()
      << " is in component " << *i << std::endl;
  std::cout << std::endl;
  return EXIT_SUCCESS;
}


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

#define BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp> // Boost.Test
#include <boost/math/special_functions/sign.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

BOOST_AUTO_TEST_CASE( test_main )
{
   BOOST_CHECK_EQUAL((boost::math::signbit)(1.0L), 0.0L);
   BOOST_CHECK_EQUAL((boost::math::signbit)(-1.0L), 1.0L);
   BOOST_CHECK_EQUAL((boost::math::sign)(1.0L), 1.0L);
   BOOST_CHECK_EQUAL((boost::math::sign)(-1.0L), -1.0L);
   BOOST_CHECK_EQUAL((boost::math::changesign)(1.0L), -1.0L);
   BOOST_CHECK_EQUAL((boost::math::changesign)(-1.0L), 1.0L);

   BOOST_CHECK_EQUAL((boost::math::fpclassify)(1.0L), FP_NORMAL);
   BOOST_CHECK_EQUAL((boost::math::isnan)(1.0L), false);
   BOOST_CHECK_EQUAL((boost::math::isinf)(1.0L), false);
   BOOST_CHECK_EQUAL((boost::math::isnormal)(1.0L), true);
   BOOST_CHECK_EQUAL((boost::math::isfinite)(1.0L), true);
} // BOOST_AUTO_TEST_CASE( test_main )


#include <mesh_array_io_matlab.h>

#include <tiny.h>

#include <util_string_helper.h>

#include <boost/tokenizer.hpp>
#include <boost/filesystem.hpp>

#include <cassert>
#include <fstream>

namespace mesh_array
{

  template<typename T>
  void read_matlab(
                   std::string const & filename
                   , T3Mesh & mesh
                   , VertexAttribute<T,T3Mesh> & X
                   , VertexAttribute<T,T3Mesh> & Y
                   , VertexAttribute<T,T3Mesh> & Z
                   )
  {
    boost::filesystem::path  mypath(filename);

    std::string const ext = boost::filesystem::extension( mypath );

    assert( ext == ".m" || !"read_matlab(): illegal file name extension");

    bool const tst = boost::filesystem::exists( mypath );

    assert( tst|| !"read_matlab(): file did not exist");

    typedef boost::tokenizer< boost::char_separator<char> > Tokenizer;
    boost::char_separator<char> sep(" XYZT=[];");

    std::ifstream file(filename.c_str());

    assert (file.is_open() || !"read_matlab(): unable to open file");

    std::vector<T> px;
    std::vector<T> py;
    std::vector<T> pz;
    std::vector<size_t> I;
    std::vector<size_t> J;
    std::vector<size_t> K;

    // matlab format is (remeber indexing in matlab is one-based )
    // X = [ x1 x2 ... xN];
    // Y = [ y1 y2 ... yN];
    // Z = [ z1 z2 ... zN];
    // T = [ i1 j1 k1; i2 j2 k2; ... iK jK kK ];

    std::string line;

    assert( !file.eof() || !"read_matlab(): EOF");
    getline(file,line);
    {
      Tokenizer tokens(line, sep);
      for (Tokenizer::iterator tok = tokens.begin(); tok != tokens.end(); ++tok)
      {
        T const x = util::to_value<T>( *tok );

        assert( is_number(x) || !"read_matlab(): nan");
        assert( is_finite(x) || !"read_matlab(): inf");

        px.push_back( x );
      }
    }
    assert( !file.eof() || !"read_matlab(): EOF");
    getline(file,line);
    {
      Tokenizer tokens(line, sep);
      for (Tokenizer::iterator tok = tokens.begin(); tok != tokens.end(); ++tok)
      {
        T const y = util::to_value<T>( *tok );

        assert( is_number(y) || !"read_matlab(): nan");
        assert( is_finite(y) || !"read_matlab(): inf");

        py.push_back( y );
      }
    }
    assert( !file.eof() || !"read_matlab(): EOF");
    getline(file,line);
    {
      Tokenizer tokens(line, sep);
      for (Tokenizer::iterator tok = tokens.begin(); tok != tokens.end(); ++tok)
      {
        T const z = util::to_value<T>( *tok );

        assert( is_number(z) || !"read_matlab(): nan");
        assert( is_finite(z) || !"read_matlab(): inf");

        pz.push_back( z );
      }
    }
    assert( !file.eof() || !"read_matlab(): EOF");
    getline(file,line);
    {
      Tokenizer tokens(line, sep);
      for (Tokenizer::iterator tok = tokens.begin(); tok != tokens.end();)
      {
        size_t const i = util::to_value<size_t>( *tok ) - 1u;
        ++tok;
        size_t const j = util::to_value<size_t>( *tok ) - 1u;
        ++tok;
        size_t const k = util::to_value<size_t>( *tok ) - 1u;
        ++tok;

        I.push_back(i);
        J.push_back(j);
        K.push_back(k);
      }
    }

    file.close();

    size_t const cntV = px.size();
    size_t const cntT = I.size();

    mesh.clear();

    mesh.set_capacity(cntV,cntT);

    X.bind(mesh);
    Y.bind(mesh);
    Z.bind(mesh);

    for(size_t idx=0u; idx < cntV; ++idx)
    {
      Vertex v = mesh.push_vertex();
      X(v) = px[idx];
      Y(v) = py[idx];
      Z(v) = pz[idx];
    }
    for(size_t idx=0u; idx< cntT; ++idx)
    {
      Vertex const & vi = mesh.vertex( I[idx] );
      Vertex const & vj = mesh.vertex( J[idx] );
      Vertex const & vk = mesh.vertex( K[idx] );

      mesh.push_triangle(vi, vj, vk);
    }
  }

  template
  void read_matlab<float>(
                   std::string const & filename
                   , T3Mesh & mesh
                   , VertexAttribute<float,T3Mesh> & X
                   , VertexAttribute<float,T3Mesh> & Y
                   , VertexAttribute<float,T3Mesh> & Z
                   );

  template
  void read_matlab<double>(
                   std::string const & filename
                   , T3Mesh & mesh
                   , VertexAttribute<double,T3Mesh> & X
                   , VertexAttribute<double,T3Mesh> & Y
                   , VertexAttribute<double,T3Mesh> & Z
                   );

  template<typename T>
  void read_matlab(
                   std::string const & filename
                   , T4Mesh & mesh
                   , VertexAttribute<T,T4Mesh> & X
                   , VertexAttribute<T,T4Mesh> & Y
                   , VertexAttribute<T,T4Mesh> & Z
                   )
  {
    boost::filesystem::path  mypath(filename);

    std::string const ext = boost::filesystem::extension( mypath );

    assert( ext == ".m" || !"read_matlab(): illegal file name extension");

    bool const tst = boost::filesystem::exists( mypath );

    assert( tst|| !"read_matlab(): file did not exist");

    typedef boost::tokenizer< boost::char_separator<char> > Tokenizer;
    boost::char_separator<char> sep(" XYZT=[];");

    std::ifstream file(filename.c_str());

    assert (file.is_open() || !"read_matlab(): unable to open file");

    std::vector<T> px;
    std::vector<T> py;
    std::vector<T> pz;
    std::vector<size_t> I;
    std::vector<size_t> J;
    std::vector<size_t> K;
    std::vector<size_t> M;

    // matlab format is (remeber indexing in matlab is one-based )
    // X = [ x1 x2 ... xN];
    // Y = [ y1 y2 ... yN];
    // Z = [ z1 z2 ... zN];
    // T = [ i1 j1 k1 m1; i2 j2 k2 m2; ... iK jK kK mK ];

    std::string line;

    assert( !file.eof() || !"read_matlab(): EOF");
    getline(file,line);
    {
      Tokenizer tokens(line, sep);
      for (Tokenizer::iterator tok = tokens.begin(); tok != tokens.end(); ++tok)
      {
        T const x = util::to_value<T>( *tok );

        assert( is_number(x) || !"read_matlab(): nan");
        assert( is_finite(x) || !"read_matlab(): inf");

        px.push_back( x );
      }
    }
    assert( !file.eof() || !"read_matlab(): EOF");
    getline(file,line);
    {
      Tokenizer tokens(line, sep);
      for (Tokenizer::iterator tok = tokens.begin(); tok != tokens.end(); ++tok)
      {
        T const y = util::to_value<T>( *tok );

        assert( is_number(y) || !"read_matlab(): nan");
        assert( is_finite(y) || !"read_matlab(): inf");

        py.push_back( y );
      }
    }
    assert( !file.eof() || !"read_matlab(): EOF");
    getline(file,line);
    {
      Tokenizer tokens(line, sep);
      for (Tokenizer::iterator tok = tokens.begin(); tok != tokens.end(); ++tok)
      {
        T const z = util::to_value<T>( *tok );

        assert( is_number(z) || !"read_matlab(): nan");
        assert( is_finite(z) || !"read_matlab(): inf");

        pz.push_back( z );
      }
    }
    assert( !file.eof() || !"read_matlab(): EOF");
    getline(file,line);
    {
      Tokenizer tokens(line, sep);
      for (Tokenizer::iterator tok = tokens.begin(); tok != tokens.end();)
      {
        size_t const i = util::to_value<size_t>( *tok ) - 1u;
        ++tok;
        size_t const j = util::to_value<size_t>( *tok ) - 1u;
        ++tok;
        size_t const k = util::to_value<size_t>( *tok ) - 1u;
        ++tok;
        size_t const m = util::to_value<size_t>( *tok ) - 1u;
        ++tok;

        I.push_back(i);
        J.push_back(j);
        K.push_back(k);
        M.push_back(m);
      }
    }

    file.close();

    size_t const cntV = px.size();
    size_t const cntT = I.size();

    mesh.clear();

    mesh.set_capacity(cntV,cntT);

    X.bind(mesh);
    Y.bind(mesh);
    Z.bind(mesh);

    for(size_t idx=0u; idx < cntV; ++idx)
    {
      Vertex v = mesh.push_vertex();
      X(v) = px[idx];
      Y(v) = py[idx];
      Z(v) = pz[idx];
    }
    for(size_t idx=0u; idx< cntT; ++idx)
    {
      Vertex const & vi = mesh.vertex( I[idx] );
      Vertex const & vj = mesh.vertex( J[idx] );
      Vertex const & vk = mesh.vertex( K[idx] );
      Vertex const & vm = mesh.vertex( M[idx] );

      mesh.push_tetrahedron(vi, vj, vk, vm);
    }
  }

  template
  void read_matlab<float>(
                   std::string const & filename
                   , T4Mesh & mesh
                   , VertexAttribute<float,T4Mesh> & X
                   , VertexAttribute<float,T4Mesh> & Y
                   , VertexAttribute<float,T4Mesh> & Z
                   );

  template
  void read_matlab<double>(
                   std::string const & filename
                   , T4Mesh & mesh
                   , VertexAttribute<double,T4Mesh> & X
                   , VertexAttribute<double,T4Mesh> & Y
                   , VertexAttribute<double,T4Mesh> & Z
                   );

  template<typename T>
  void write_matlab(
                    std::string const & filename
                    , T3Mesh const & mesh
                    , VertexAttribute<T,T3Mesh> const & X
                    , VertexAttribute<T,T3Mesh> const & Y
                    , VertexAttribute<T,T3Mesh> const & Z
                    )
  {
    boost::filesystem::path  mypath(filename);

    std::string const ext = boost::filesystem::extension( mypath );

    assert( ext == ".m" || !"write_matlab(): illegal file name extension");

    std::ofstream file (filename.c_str());

    assert( file.is_open() || !"write_matlab(): could not open file");

    file.precision(30);

    size_t const cntV = mesh.vertex_size();

    file << "X = [";
    for(size_t idx = 0u; idx < cntV; ++idx)
    {
      Vertex const & vertex = mesh.vertex(idx);
      file << " " << X(vertex);
    }
    file << " ];" << std::endl;

    file << "Y = [";
    for(size_t idx = 0u; idx < cntV; ++idx)
    {
      Vertex const & vertex = mesh.vertex(idx);
      file << " " << Y(vertex);
    }
    file << " ];" << std::endl;

    file << "Z = [";
    for(size_t idx = 0u; idx < cntV; ++idx)
    {
      Vertex const & vertex = mesh.vertex(idx);
      file << " " << Z(vertex);
    }
    file << " ];" << std::endl;

    size_t const cntT = mesh.triangle_size();

    file << "T = [";
    for(size_t idx = 0u; idx < cntT; ++idx)
    {
      Triangle const & triangle = mesh.triangle(idx);
      size_t const i = triangle.i() + 1u;
      size_t const j = triangle.j() + 1u;
      size_t const k = triangle.k() + 1u;
      file << " " << i << " " << j << " " << k << ";";
    }
    file << " ];" << std::endl;

    file.flush();
    file.close();
  }

  template
  void write_matlab<float>(
                    std::string const & filename
                    , T3Mesh const & mesh
                    , VertexAttribute<float,T3Mesh> const & X
                    , VertexAttribute<float,T3Mesh> const & Y
                    , VertexAttribute<float,T3Mesh> const & Z
                    );

  template
  void write_matlab<double>(
                    std::string const & filename
                    , T3Mesh const & mesh
                    , VertexAttribute<double,T3Mesh> const & X
                    , VertexAttribute<double,T3Mesh> const & Y
                    , VertexAttribute<double,T3Mesh> const & Z
                    );

  template<typename T>
  void write_matlab(
                    std::string const & filename
                    , T4Mesh const & mesh
                    , VertexAttribute<T,T4Mesh> const & X
                    , VertexAttribute<T,T4Mesh> const & Y
                    , VertexAttribute<T,T4Mesh> const & Z
                    )
  {
    boost::filesystem::path  mypath(filename);

    std::string const ext = boost::filesystem::extension( mypath );

    assert( ext == ".m" || !"write_matlab(): illegal file name extension");

    std::ofstream file (filename.c_str());

    assert( file.is_open() || !"write_matlab(): could not open file");

    file.precision(30);

    size_t const cntV = mesh.vertex_size();

    file << "X = [";
    for(size_t idx = 0u; idx < cntV; ++idx)
    {
      Vertex const & vertex = mesh.vertex(idx);
      file << " " << X(vertex);
    }
    file << " ];" << std::endl;

    file << "Y = [";
    for(size_t idx = 0u; idx < cntV; ++idx)
    {
      Vertex const & vertex = mesh.vertex(idx);
      file << " " << Y(vertex);
    }
    file << " ];" << std::endl;

    file << "Z = [";
    for(size_t idx = 0u; idx < cntV; ++idx)
    {
      Vertex const & vertex = mesh.vertex(idx);
      file << " " << Z(vertex);
    }
    file << " ];" << std::endl;

    size_t const cntT = mesh.tetrahedron_size();

    file << "T = [";
    for(size_t idx = 0u; idx < cntT; ++idx)
    {
      Tetrahedron const & tetrahedron = mesh.tetrahedron(idx);
      size_t const i = tetrahedron.i() + 1u;
      size_t const j = tetrahedron.j() + 1u;
      size_t const k = tetrahedron.k() + 1u;
      size_t const m = tetrahedron.m() + 1u;
      file << " " << i << " " << j << " " << k << " " << m << ";";
    }
    file << " ];" << std::endl;

    file.flush();
    file.close();
  }

  template
  void write_matlab<float>(
                    std::string const & filename
                    , T4Mesh const & mesh
                    , VertexAttribute<float,T4Mesh> const & X
                    , VertexAttribute<float,T4Mesh> const & Y
                    , VertexAttribute<float,T4Mesh> const & Z
                    );

  template
  void write_matlab<double>(
                    std::string const & filename
                    , T4Mesh const & mesh
                    , VertexAttribute<double,T4Mesh> const & X
                    , VertexAttribute<double,T4Mesh> const & Y
                    , VertexAttribute<double,T4Mesh> const & Z
                    );

} // end namespace mesh_array


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2014 by Synge Todo <wistaria@comp-phys.org>
*
* 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)
*
*****************************************************************************/

// C++ version of TITPACK Ver.2 by H. Nishimori

#ifndef TITPACK_SMALL_HPP
#define TITPACK_SMALL_HPP

#include "common.hpp"
#include "hamiltonian.hpp"
#include <vector>
#include <boost/tuple/tuple.hpp>

//
// matrix elements
//

template<typename MATRIX>
void elm3(hamiltonian const& hop, MATRIX& elemnt) {
  elemnt.set_zeros();
  for (int k = 0; k < hop.num_bonds(); ++k) {
    int isite1, isite2;
    boost::tie(isite1, isite2) = hop.site_pair(k);
    int is = (1 << isite1) + (1 << isite2);
    double wght = hop.bond_weight(k);
    double diag = 0.5 * wght * hop.z_ratio(k);
    #pragma omp parallel for schedule(static)
    for (int i = 0; i < hop.dimension(); ++i) {
      if (elemnt.is_gindex_myrow(i)) {
        int ibit = hop.config(i) & is;
        if (ibit == 0 || ibit == is) {
          elemnt.update_global(i, i, -diag);
        } else {
          elemnt.update_global(i, i, +diag);
          int newcfg = hop.config2index(hop.config(i) ^ is);
          elemnt.update_global(i, newcfg, -wght);
        }
      }
    }
  }
}

template<typename MATRIX>
void elm3_sx(subspace const& ss, int i1, int i2, MATRIX& elemnt) {
  elemnt.set_zeros();
  if (i1 < 0 || i1 >= ss.num_sites() || i2 < 0 || i2 >= ss.num_sites() || i1 == i2) {
    std::cerr << " #(W01)# Wrong site number given to xcorr\n";
    return;
  }
  int is = (1 << i1) + (1 << i2);
  #pragma omp parallel for schedule(static)
  for (int i = 0; i < ss.dimension(); ++i) {
    if (elemnt.is_gindex_myrow(i)) {
      int ibit = ss.config(i) & is;
      if (ibit != 0 && ibit != is) {
        int newcfg = ss.config2index(ss.config(i) ^ is);
        elemnt.update_global(i, newcfg, 0.25);
      }
    }
  }
}

//
// check of the eigenvector and eigenvalue
//
// elemnt    @ nonzero elements
// x         @ eigenvector to be checked
// xindex
// return value: Hexpec <x*H*x>

template<typename MATRIX>
double check3(MATRIX const& elemnt, MATRIX const& x, int xindex, MATRIX& y) {
  int idim = elemnt.rows();
  double dnorm = 0;
  for (int j = 0; j < idim; ++j) {
    dnorm += x(j, xindex) * x(j, xindex);
  }
  if (dnorm < 1e-30) {
    std::cerr << " #(W18)# Null vector given to check3\n";
    return 0;
  }
  for (int i = 0; i < idim; ++i) {
    y(i, xindex) = 0;
    for (int j = 0; j < idim; ++j) y(i, xindex) += elemnt(i, j) * x(j, xindex);
  }
  double prd = 0;
  for (int i = 0; i < idim; ++i) prd += y(i, xindex) * x(i, xindex);
  std::cout << "---------------------------- Information from check3\n"
            << "<x*H*x> = "<< prd << std::endl
            << "H*x(j)/x(j) (j=min(idim/3,13)-1,idim,max(1,idim/20))";
  int count = 0;
  for (int i = std::min((int)(idim / 3), 13) - 1; i < idim; i += std::max(1,idim/20), ++count) {
    if (count % 4 == 0) std::cout << std::endl;
    std::cout << '\t' << y(i, xindex) / x(i, xindex);
  }
  std::cout << std::endl << "---------------------------------------------------\n";
  return prd;
}

template<typename MATRIX>
double check3_mpi(MATRIX const& elemnt, MATRIX const& x, int xindex, MATRIX& y) {
  int idim = elemnt.get_n_global();
  double dnorm = dot_product(x, false, xindex, x, false, xindex);
  product_v(1.0, elemnt, false, x, false, xindex, 0.0, y, false, xindex);
  double prd = dot_product(x, false, xindex, y, false, xindex) / dnorm;

  if (x.is_gindex(0, xindex)) {
    std::cout << "---------------------------- Information from check3\n"
              << "<x*H*x> = "<< prd << std::endl
              << "H*x(j)/x(j) (j=min(idim/3,13)-1,idim,max(1,idim/20))";
  }
  std::cout << std::flush;
  MPI_Barrier(elemnt.get_grid().get_comm());
  int count = 0;
  for (int i = std::min((int)(idim / 3), 13) - 1; i < idim; i += std::max(1,idim/20), ++count) {
    if (x.is_gindex(i, xindex)) {
      if (count % 4 == 0) std::cout << std::endl;
      std::cout << '\t' << y.get_global(i, xindex) / x.get_global(i, xindex);
    }
    std::cout << std::flush;
    MPI_Barrier(elemnt.get_grid().get_comm());
  }
  if (x.is_gindex(0, xindex)) {
    std::cout << std::endl << "---------------------------------------------------\n";
  }
  std::cout << std::flush;
  return prd;
}

template<typename MATRIX>
void xcorr3_mpi(subspace const& ss, std::vector<int> const& npair, MATRIX const& x, int xindex,
                std::vector<double>& sxx, MATRIX& sx, MATRIX& y) {
  double dnorm = dot_product(x, false, xindex, x, false, xindex);
  int nbond = npair.size() / 2;
  for (int k = 0; k < nbond; ++k) {
    int i1 = npair[k * 2];
    int i2 = npair[k * 2 + 1];
    elm3_sx(ss, i1, i2, sx);
    product_v(1.0, sx, false, x, false, xindex, 0.0, y, false, xindex);
    sxx[k] =dot_product(x, false, xindex, y, false, xindex) / dnorm;
  }
}

#endif


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

/** \file LinearSolver.cpp
    \brief This file defines the python exports for the LinearSolver class.
  */

#include <aslam-tsvd-solver/aslam-tsvd-solver.h>
#include <boost/shared_ptr.hpp>
#include <numpy_eigen/boost_python_headers.hpp>
#include <sm/PropertyTree.hpp>

using namespace boost::python;
using namespace aslam::backend;
using namespace sm;

void exportLinearSolver() {
  typedef AslamTruncatedSvdSolver LinearSolver;
  typedef AslamTruncatedSvdSolver::Options LinearSolverOptions;

  /// Export LinearSolverOptions structure
  class_<LinearSolverOptions>("LinearSolverOptions", init<>())
    .def_readwrite("columnScaling", &LinearSolverOptions::columnScaling)
    .def_readwrite("epsNorm", &LinearSolverOptions::epsNorm)
    .def_readwrite("epsSVD", &LinearSolverOptions::epsSVD)
    .def_readwrite("epsQR", &LinearSolverOptions::epsQR)
    .def_readwrite("svdTol", &LinearSolverOptions::svdTol)
    .def_readwrite("qrTol", &LinearSolverOptions::qrTol)
    .def_readwrite("verbose", &LinearSolverOptions::verbose)
    ;

  /// Function for querying the options
  LinearSolverOptions& (LinearSolver::*getOptions)() =
    &LinearSolver::getOptions;

  /// Export LinearSolver class
  class_<LinearSolver, boost::shared_ptr<LinearSolver>,
    bases<LinearSystemSolver>, boost::noncopyable>("LinearSolver",
    init<const LinearSolverOptions&>())
    .def(init<const PropertyTree&>())
    .def("getOptions", getOptions, return_internal_reference<>())
    ;
}


// Copyright 2014 Vinzenz Feenstra
//
// 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 GUARD_LSL_RUNTIME_QUATERNION_HH_INCLUDED
#define GUARD_LSL_RUNTIME_QUATERNION_HH_INCLUDED

#include <boost/math/constants/constants.hpp>
#include <lsl/runtime/vector.hh>

namespace lsl {
namespace runtime {
    struct Quaternion {
        double x, y, z, s;
    };

    inline Quaternion identity(Quaternion) {
        return {0., 0., 0., 1.};
    }

    inline Quaternion add(Quaternion l, Quaternion r) {
        return Quaternion{
            l.x + r.x,
            l.y + r.y,
            l.z + r.z,
            l.s + l.s
        };
    }

    inline Quaternion sub(Quaternion l, Quaternion r) {
        return Quaternion{
            l.x - r.x,
            l.y - r.y,
            l.z - r.z,
            l.s - r.s
        };
    }

    inline Quaternion mul(Quaternion l, Quaternion r) {
        return Quaternion{
            r.s * l.x + r.x * l.s + r.y * l.z - r.z * l.y,
            r.s * l.y + r.y * l.s + r.z * l.x - r.x * l.z,
            r.s * l.z + r.z * l.s + r.x * l.y - r.y * l.x,
            r.s * l.s - r.x * l.x - r.y * l.y - r.z * l.z
        };
    }

    inline Vector mul(Vector l, Quaternion r) {
        Quaternion vq{l.x, l.y, l.z, 0.};
        Quaternion nq{-r.x, -r.y, -r.z, r.s};
        Quaternion result = mul(nq, mul(vq, r));
        return{result.x, result.y, result.z};
    }

    inline Quaternion mul(Quaternion l, double r) {
        return {l.x * r, l.y * r, l.z * r, l.s * r};
    }

    inline Quaternion div(Quaternion l, Quaternion r) {
        r.s = -r.s;
        return mul(l, r);
    }

    inline Vector div(Vector l, Quaternion r) {
        r.s = -r.s;
        return mul(l, r);
    }

    inline Vector operator * (Vector const & a, Quaternion const & b) {
        return mul(a, b);
    }
    inline Quaternion operator * (Quaternion const & a, Quaternion const & b) {
        return mul(a, b);
    }
    inline Quaternion operator * (Quaternion const & a, double b) {
        return mul(a, b);
    }
    inline Quaternion operator / (Quaternion const & a, Quaternion const & b) {
        return div(a, b);
    }
    inline Vector operator / (Vector const & a, Quaternion const & b) {
        return div(a, b);
    }
    inline Quaternion operator + (Quaternion const & a, Quaternion const & b) {
        return add(a, b);
    }
    inline Quaternion operator - (Quaternion const & a, Quaternion const & b) {
        return sub(a, b);
    }

    inline bool operator==(Quaternion l, Quaternion r) {
        return l.x == r.x
            && l.y == r.y
            && l.z == r.z
            && l.s == r.s;
    }

    inline bool operator!=(Quaternion l, Quaternion r) {
        return !(l == r);
    }

    inline Quaternion from_euler(Vector euler) {
        double c1 = cos(euler.x * 0.5);
        double c2 = cos(euler.y * 0.5);
        double c3 = cos(euler.z * 0.5);

        double s1 = sin(euler.x * 0.5);
        double s2 = sin(euler.y * 0.5);
        double s3 = sin(euler.z * 0.5);

        Quaternion b{
            s1 * c2 * c3 + c1 * s2 * s3,
            c1 * s2 * c3 - s1 * c2 * s3,
            s1 * s2 * c3 + c1 * c2 * s3,
            c1 * c2 * c3 - s1 * s2 * s3,
        };

        auto a = Quaternion{0., 0., s3, c3}
                 * Quaternion{0., s2, 0., c2}
                 * Quaternion{s1, 0., 0., s1};
        auto c = a + b;
        auto d = a - b;
        static double const err = 0.00001;
        if(   (fabs(c.x) > err && fabs(d.x) > err)
           || (fabs(c.y) > err && fabs(d.y) > err)
           || (fabs(c.z) > err && fabs(d.z) > err)
           || (fabs(c.s) > err && fabs(d.s) > err)) {
            return b;
        }
        return a;
    }

    static double const FP_MAG_THRESHOLD        = 0.0000001;
    static double const GIMBAL_THRESHOLD        = 0.000436;
    static double const ONE_PART_IN_A_MILLION   = 0.000001;

    inline Vector to_euler(Quaternion r) {
        using namespace boost::math::double_constants;

        double sx = 2. * (r.x * r.s - r.y * r.z);
        double sy = 2. * (r.y * r.s + r.x * r.z);

        double ys = r.s * r.s - r.y * r.y;
        double xz = r.x * r.x - r.z * r.z;

        double cx = ys - xz;
        double cy = sqrt(sx * sx + cx * cx);

        if(cy > GIMBAL_THRESHOLD) {
            return Vector{
                atan2(sx, cx),
                atan2(sy, cy),
                atan2(2 * (r.z * r.s - r.x * r.y), ys + xz)
            };
        }

        if(sy > 0) {
            return Vector{
                0.,
                half_pi,
                2. * atan2(r.z + r.x, r.s + r.y)
            };
        }
        return Vector{
            0.,
            half_pi * -1.,
            atan2(r.z - r.x, r.s - r.y)
        };
    }

    inline double mag(Quaternion q) {
        return sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.s * q.s);
    }

    inline Quaternion normalize(Quaternion q) {

        double m = mag(q);
        if(m < FP_MAG_THRESHOLD) {
            return Quaternion{0., 0., 0., 1.};
        }
        if(fabs(1. - m) > ONE_PART_IN_A_MILLION) {
            double oomag = 1. / m;
            return Quaternion{
                q.x * oomag,
                q.y * oomag,
                q.z * oomag,
                q.s * oomag
            };
        }
        return q;
    }

    inline bool operator<(Quaternion l, Quaternion r) {
        return mag(l) < mag(r);
    }
}
}

#endif //GUARD_LSL_RUNTIME_QUATERNION_HH_INCLUDED


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_POW_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_POW_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/function/scalar/is_flint.hpp>
#include <boost/simd/function/scalar/is_inf.hpp>
#include <boost/simd/function/scalar/is_ltz.hpp>
#include <boost/simd/function/scalar/is_negative.hpp>
#include <boost/simd/function/scalar/is_odd.hpp>
#include <boost/simd/function/scalar/pow_abs.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( pow_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    inline A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      auto ltza0 = is_ltz(a0);
      auto isinfa1 = is_inf(a1);
      if( a0 == Mone<A0>() && isinfa1 ) return One<A0>();
      if( ltza0 && !is_flint(a1) && !is_inf(a1) ) return Nan<A0>();
      A0 z = pow_abs(a0, a1);
      if (isinfa1) return z;
      return  (is_negative(a0) && is_odd(a1)) ? -z : z;
    }
  };

   BOOST_DISPATCH_OVERLOAD ( pow_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          , bs::std_tag
                          )
  {
    inline A0 operator() ( A0 a0, A0 a1, std_tag const&) const BOOST_NOEXCEPT
    {
      return std::pow(a0, a1);
    }
  };
} } }

#endif


#include <geometry.h>
#include <tiny_math_types.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>

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(inside_sphere_test)
{
  typedef tiny::MathTypes<double>   MT;
  typedef MT::vector3_type          V;
  typedef MT::real_type             T;
  typedef MT::value_traits          VT;

  V const center  = V::zero();
  T const radius  = VT::one();

  geometry::Sphere<V> const sphere = geometry::make_sphere(center , radius );

  BOOST_CHECK(geometry::is_valid(sphere));

  {
    V    const p    = V::make( 0.0, 0.0, 0.0);
    bool const test = geometry::inside_sphere( p,  sphere);
    BOOST_CHECK( test );
  }
  {
    V    const p    = V::make( 1.0, 0.0, 0.0);
    bool const test = geometry::inside_sphere( p,  sphere);
    BOOST_CHECK( test );
  }
  {
    V    const p    = V::make( 2.0, 0.0, 0.0);
    bool const test = geometry::inside_sphere( p,  sphere);
    BOOST_CHECK( !test );
  }

}

BOOST_AUTO_TEST_SUITE_END();


#ifndef WAVEO1_SOLVER_HPP
#define WAVEO1_SOLVER_HPP

#include "mfem.hpp"
using namespace mfem;

#include <Eigen/Core>

#include "../core/config.hpp"
#include "../core/pardiso.hpp"

#include "../mymfem/utilities.hpp"

#include "test_cases_factory.hpp"
#include "coefficients.hpp"
#include "discretisation.hpp"
#include "observer.hpp"

class WaveO1Solver
{
public:
    WaveO1Solver (const nlohmann::json& config);

    WaveO1Solver (const nlohmann::json& config,
                std::shared_ptr<WaveO1TestCases>& testCase);

    WaveO1Solver (const nlohmann::json& config,
                std::string mesh_dir,
                const int lx,
                const int lt,
                const bool load_init_mesh=false);

    WaveO1Solver (const nlohmann::json& config,
                std::shared_ptr<WaveO1TestCases>& testCase,
                std::string mesh_dir,
                const int lx,
                const int lt,
                const bool load_init_mesh=false);

    WaveO1Solver (const nlohmann::json& config,
                std::string mesh_dir,
                const int lx,
                const bool load_init_mesh=false);

    WaveO1Solver (const nlohmann::json& config,
                std::shared_ptr<WaveO1TestCases>& testCase,
                std::string mesh_dir,
                const int lx,
                const bool load_init_mesh=false);

    ~ WaveO1Solver ();

    // sets meshes, discretization, observer
    void set(std::string mesh_dir,
             const int lx,
             const int lt=-2,
             const bool load_init_mesh=false);

    void set(std::string mesh_dir,
             const bool load_init_mesh=false);
    void set(int lx, int lt=-2);
    void set_meshes(int lx, int lt=-2);

    // used for the full-tensor version
    std::tuple<int, double, double, Eigen::VectorXd>
    operator()();

    // used for the sparse-grids handler
    void operator()(std::unique_ptr<BlockVector>&);

    void run();
    void init ();
    void solve (BlockVector *W);

private:
    void assemble_system();

    void solve_one_time_slab (const int n, BlockVector *W);
    void assemble_rhs_initial();
    void assemble_rhs(int);
    void copy_slabW_to_W(const int n, const BlockVector *,
                         BlockVector *) const;

public:
    void finalize () const;

    // error computation
    Eigen::VectorXd compute_error(BlockVector *W) const;

    // energy computation
    void compute_energy(BlockVector *W) const;

    // solution at end-time
    std::shared_ptr <GridFunction>
    get_pressureSol_at_endTime (BlockVector *W) const;

    std::shared_ptr <GridFunction>
    get_velocitySol_at_endTime (BlockVector *W) const;

    void set_pressureSol_at_endTime ();

    void set_velocitySol_at_endTime ();

    void dump_sol_at_time_steps(BlockVector *W) const;

    void dump_sol_at_time_stamps(BlockVector *W) const;

    void measure_signal (BlockVector *W) const;

    // for L2-projection
    std::tuple<int, double, double, Eigen::VectorXd>
    projection();
    void projection(std::unique_ptr<BlockVector>&);

    void init_projection ();

private:
    void assemble_projection_system();
    void assemble_projection_rhs(int) const;

public:
    void solve_projection (BlockVector *W);

    inline std::shared_ptr<Mesh> get_tMesh() const {
        return m_tMesh;
    }

    inline std::shared_ptr<Mesh> get_xMesh() const {
        return m_xMesh;
    }

    inline std::shared_ptr<WaveO1TestCases>
    get_test_case() const {
        return m_testCase;
    }

    inline std::shared_ptr<WaveO1XtDG>
    get_discretisation() const {
        return m_discr;
    }

    inline std::shared_ptr<WaveO1Observer>
    get_observer() const {
        return m_observer;
    }

    inline std::pair<double, double>
    get_meshChars() const
    {
        double ht_min, ht_max, kappat_min, kappat_max;
        m_tMesh->GetCharacteristics(ht_min, ht_max,
                                    kappat_min, kappat_max);

        double hx_min, hx_max, kappax_min, kappax_max;
        m_xMesh->GetCharacteristics(hx_min, hx_max,
                                    kappax_min, kappax_max);

        return {std::move(ht_max), std::move(hx_max)};
    }

    inline int get_ndofs() const {
        return std::move(m_W->Size());
    }

private:
    const nlohmann::json& m_config;
    std::string m_mesh_dir;
    std::string m_mesh_elem_type;

    double m_endTime;
    std::shared_ptr<Mesh> m_tMesh;
    std::shared_ptr<Mesh> m_xMesh;
    bool m_load_init_mesh;
    int m_lx0;

    std::shared_ptr<WaveO1TestCases> m_testCase;
    std::shared_ptr<WaveO1XtDG> m_discr;
    std::shared_ptr<WaveO1Observer> m_observer;

    bool m_bool_error;
    std::string m_errorType;

    std::string m_meshFormat;

    std::string m_solOutputAt;
    Vector m_timeStamps;

    bool m_bool_measureSignal;
    Vector m_xMeasurementPoint;

    bool m_bool_energy;

    Array<int> m_blockOffsets;
    Array<int> m_slab_blockOffsets;

    BlockOperator *m_waveOp = nullptr;
    SparseMatrix *m_waveMat = nullptr;
    SparseMatrix *m_waveProjMat = nullptr;

    std::unique_ptr <BlockVector> m_W;
    mutable std::unique_ptr <BlockVector> m_slabW;
    mutable std::unique_ptr <BlockVector> m_slabB;

    std::shared_ptr <GridFunction> m_pressure;
    std::shared_ptr <GridFunction> m_velocity;

#ifdef PARDISO_HPP
    std::unique_ptr<PardisoSolver> m_pardisoSolver;
#endif
    mutable bool m_pardiso_finalized = true;
};

#endif /// WaveO1_SOLVER_HPP


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

#include <boost/simd/ieee/functions/exponentbits.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/simd/include/constants/maxexponent.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/splat.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( exponentbits_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<floating_<A0>,X>))
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type  result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename meta::scalar_of<A0>::type             s_type;
      typedef typename meta::scalar_of<result_type>::type sint_type;
      const sint_type me = Maxexponent<s_type>();
      const sint_type nmb= Nbmantissabits<s_type>();
      const result_type Mask = splat<result_type>((2*me+1)<<nmb);
      return (b_and(Mask, a0));
    }
  };

} } }

#endif


/* -*- c++ -*- */
/*
 * Copyright 2006,2010-2012 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include "costas_loop_cc_impl.h"
#include <gnuradio/io_signature.h>
#include <gnuradio/expj.h>
#include <gnuradio/sincos.h>
#include <gnuradio/math.h>
#include <boost/format.hpp>

namespace gr {
  namespace digital {

    costas_loop_cc::sptr
    costas_loop_cc::make(float loop_bw, int order, bool use_snr)
    {
      return gnuradio::get_initial_sptr
	(new costas_loop_cc_impl(loop_bw, order, use_snr));
    }

    static int ios[] = { sizeof(gr_complex), sizeof(float), sizeof(float), sizeof(float) };
    static std::vector<int> iosig(ios, ios+sizeof(ios)/sizeof(int));

    costas_loop_cc_impl::costas_loop_cc_impl(float loop_bw, int order, bool use_snr)
      : sync_block("costas_loop_cc",
                   io_signature::make(1, 1, sizeof(gr_complex)),
                   io_signature::makev(1, 4, iosig)),
	blocks::control_loop(loop_bw, 1.0, -1.0),
	d_order(order), d_error(0), d_noise(1.0), d_phase_detector(NULL)
    {
      // Set up the phase detector to use based on the constellation order
      switch(d_order) {
      case 2:
        if(use_snr)
          d_phase_detector = &costas_loop_cc_impl::phase_detector_snr_2;
	else
          d_phase_detector = &costas_loop_cc_impl::phase_detector_2;
	break;

      case 4:
        if(use_snr)
          d_phase_detector = &costas_loop_cc_impl::phase_detector_snr_4;
	else
          d_phase_detector = &costas_loop_cc_impl::phase_detector_4;
	break;

      case 8:
        if(use_snr)
          d_phase_detector = &costas_loop_cc_impl::phase_detector_snr_8;
	else
          d_phase_detector = &costas_loop_cc_impl::phase_detector_8;
	break;

      default:
	throw std::invalid_argument("order must be 2, 4, or 8");
	break;
      }

      message_port_register_in(pmt::mp("noise"));
      set_msg_handler(
        pmt::mp("noise"),
        boost::bind(&costas_loop_cc_impl::handle_set_noise,
                    this, _1));
    }

    costas_loop_cc_impl::~costas_loop_cc_impl()
    {
    }

    float
    costas_loop_cc_impl::phase_detector_8(gr_complex sample) const
    {
      /* This technique splits the 8PSK constellation into 2 squashed
	 QPSK constellations, one when I is larger than Q and one
	 where Q is larger than I. The error is then calculated
	 proportionally to these squashed constellations by the const
	 K = sqrt(2)-1.

	 The signal magnitude must be > 1 or K will incorrectly bias
	 the error value.

	 Ref: Z. Huang, Z. Yi, M. Zhang, K. Wang, "8PSK demodulation for
	 new generation DVB-S2", IEEE Proc. Int. Conf. Communications,
	 Circuits and Systems, Vol. 2, pp. 1447 - 1450, 2004.
      */

      float K = (sqrt(2.0) - 1);
      if(fabsf(sample.real()) >= fabsf(sample.imag())) {
	return ((sample.real()>0 ? 1.0 : -1.0) * sample.imag() -
		(sample.imag()>0 ? 1.0 : -1.0) * sample.real() * K);
      }
      else {
	return ((sample.real()>0 ? 1.0 : -1.0) * sample.imag() * K -
		(sample.imag()>0 ? 1.0 : -1.0) * sample.real());
      }
    }

    float
    costas_loop_cc_impl::phase_detector_4(gr_complex sample) const
    {
      return ((sample.real()>0 ? 1.0 : -1.0) * sample.imag() -
	      (sample.imag()>0 ? 1.0 : -1.0) * sample.real());
    }

    float
    costas_loop_cc_impl::phase_detector_2(gr_complex sample) const
    {
      return (sample.real()*sample.imag());
    }

    float
    costas_loop_cc_impl::phase_detector_snr_8(gr_complex sample) const
    {
      float K = (sqrt(2.0) - 1);
      float snr = abs(sample)*abs(sample) / d_noise;
      if(fabsf(sample.real()) >= fabsf(sample.imag())) {
	return ((blocks::tanhf_lut(snr*sample.real()) * sample.imag()) -
          (blocks::tanhf_lut(snr*sample.imag()) * sample.real() * K));
      }
      else {
	return ((blocks::tanhf_lut(snr*sample.real()) * sample.imag() * K) -
          (blocks::tanhf_lut(snr*sample.imag()) * sample.real()));
      }
    }

    float
    costas_loop_cc_impl::phase_detector_snr_4(gr_complex sample) const
    {
      float snr = abs(sample)*abs(sample) / d_noise;
      return ((blocks::tanhf_lut(snr*sample.real()) * sample.imag()) -
              (blocks::tanhf_lut(snr*sample.imag()) * sample.real()));
    }

    float
    costas_loop_cc_impl::phase_detector_snr_2(gr_complex sample) const
    {
      float snr = abs(sample)*abs(sample) / d_noise;
      return blocks::tanhf_lut(snr*sample.real()) * sample.imag();
    }

    float
    costas_loop_cc_impl::error() const
    {
      return d_error;
    }

    void
    costas_loop_cc_impl::handle_set_noise(pmt::pmt_t msg)
    {
      if(pmt::is_real(msg)) {
        d_noise = pmt::to_double(msg);
        d_noise = powf(10.0f, d_noise/10.0f);
      }
    }

    int
    costas_loop_cc_impl::work(int noutput_items,
			      gr_vector_const_void_star &input_items,
			      gr_vector_void_star &output_items)
    {
      const gr_complex *iptr = (gr_complex *) input_items[0];
      gr_complex *optr = (gr_complex *) output_items[0];
      float *freq_optr  = output_items.size() >= 2 ? (float *) output_items[1] : NULL;
      float *phase_optr = output_items.size() >= 3 ? (float *) output_items[2] : NULL;
      float *error_optr = output_items.size() >= 4 ? (float *) output_items[3] : NULL;

      gr_complex nco_out;

      std::vector<tag_t> tags;
      get_tags_in_range(tags, 0, nitems_read(0),
                        nitems_read(0)+noutput_items,
                        pmt::intern("phase_est"));

      for(int i = 0; i < noutput_items; i++) {
        if(tags.size() > 0) {
          if(tags[0].offset-nitems_read(0) == (size_t)i) {
            d_phase = (float)pmt::to_double(tags[0].value);
            tags.erase(tags.begin());
          }
        }

        nco_out = gr_expj(-d_phase);
        optr[i] = iptr[i] * nco_out;

        d_error = (*this.*d_phase_detector)(optr[i]);
        d_error = gr::branchless_clip(d_error, 1.0);

        advance_loop(d_error);
        phase_wrap();
        frequency_limit();

        if (freq_optr != NULL)
          freq_optr[i] = d_freq;
        if (phase_optr != NULL)
          phase_optr[i] = d_phase;
        if (error_optr != NULL)
          error_optr[i] = d_error;
      }

      return noutput_items;
    }

    void
    costas_loop_cc_impl::setup_rpc()
    {
#ifdef GR_CTRLPORT
      // Getters
      add_rpc_variable(
          rpcbasic_sptr(new rpcbasic_register_get<costas_loop_cc, float>(
	      alias(), "error",
	      &costas_loop_cc::error,
	      pmt::mp(-2.0f), pmt::mp(2.0f), pmt::mp(0.0f),
	      "", "Error signal of loop", RPC_PRIVLVL_MIN,
              DISPTIME | DISPOPTSTRIP)));

      add_rpc_variable(
          rpcbasic_sptr(new rpcbasic_register_get<control_loop, float>(
	      alias(), "frequency",
	      &control_loop::get_frequency,
	      pmt::mp(0.0f), pmt::mp(2.0f), pmt::mp(0.0f),
	      "", "Frequency Est.", RPC_PRIVLVL_MIN,
              DISPTIME | DISPOPTSTRIP)));

      add_rpc_variable(
          rpcbasic_sptr(new rpcbasic_register_get<control_loop, float>(
	      alias(), "phase",
	      &control_loop::get_phase,
	      pmt::mp(0.0f), pmt::mp(2.0f), pmt::mp(0.0f),
	      "", "Phase Est.", RPC_PRIVLVL_MIN,
              DISPTIME | DISPOPTSTRIP)));

      add_rpc_variable(
          rpcbasic_sptr(new rpcbasic_register_get<control_loop, float>(
	      alias(), "loop_bw",
	      &control_loop::get_loop_bandwidth,
	      pmt::mp(0.0f), pmt::mp(2.0f), pmt::mp(0.0f),
	      "", "Loop bandwidth", RPC_PRIVLVL_MIN,
              DISPTIME | DISPOPTSTRIP)));

      // Setters
      add_rpc_variable(
          rpcbasic_sptr(new rpcbasic_register_set<control_loop, float>(
	      alias(), "loop_bw",
	      &control_loop::set_loop_bandwidth,
	      pmt::mp(0.0f), pmt::mp(1.0f), pmt::mp(0.0f),
	      "", "Loop bandwidth",
	      RPC_PRIVLVL_MIN, DISPNULL)));
#endif /* GR_CTRLPORT */
    }

  } /* namespace digital */
} /* namespace gr */