#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include <rigging/CompositeCamera.h>
#include <rigging/DerivedCamera.h>
#include <rigging/SimpleCamera.h>
using namespace rigging;

#define CHECK_EQUIVALENT_VECTORS(l,r) BOOST_CHECK_SMALL((l - r).norm(), 1e-5f)

BOOST_AUTO_TEST_SUITE(test_CompositeCamera)

BOOST_AUTO_TEST_CASE(stereo_test)
{
  // Make the camera rig itself.
  CompositeCamera_Ptr rig(new CompositeCamera(Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 1.0f, 0.0f), Eigen::Vector3f(0.0f, 0.0f, 1.0f)));

  // Add left eye and right eye cameras to the rig. These are respectively 1 unit to the left and right of the main camera
  // and point inwards at an angle of 45 degrees.
  Camera_CPtr leftCamera(new DerivedCamera(
    rig,
    Eigen::AngleAxisf(static_cast<float>(-M_PI/4.0f), Eigen::Vector3f(0.0f, 1.0f, 0.0f)).toRotationMatrix(),  // the rotation is in *camera space*, i.e. about v
    Eigen::Vector3f(1.0f, 0.0f, 0.0f)                                                     // the translation is in *camera space*, i.e. along u
  ));
  rig->add_secondary_camera("left", leftCamera);

  Camera_CPtr rightCamera(new DerivedCamera(
    rig,
    Eigen::AngleAxisf(static_cast<float>(M_PI/4.0f), Eigen::Vector3f(0.0f, 1.0f, 0.0f)).toRotationMatrix(),   // the rotation is in *camera space*, i.e. about v
    Eigen::Vector3f(-1.0f, 0.0f, 0.0f)                                                    // the translation is in *camera space*, i.e. along -u
  ));
  rig->add_secondary_camera("right", rightCamera);

  // Rotate the entire rig so that it faces downwards.
  rig->rotate(rig->u(), static_cast<float>(M_PI/2.0f));

  // Check the positions and orientations of the left and right eye cameras.
  const float ONE_OVER_ROOT_2 = 1.0f / sqrtf(2.0f);
  CHECK_EQUIVALENT_VECTORS(rig->get_secondary_camera("left")->p(), Eigen::Vector3f(-1.0f, 0.0f, 0.0f));
  CHECK_EQUIVALENT_VECTORS(rig->get_secondary_camera("left")->u(), Eigen::Vector3f(-ONE_OVER_ROOT_2, 0.0f, -ONE_OVER_ROOT_2));
  CHECK_EQUIVALENT_VECTORS(rig->get_secondary_camera("left")->v(), Eigen::Vector3f(0.0f, 1.0f, 0.0f));
  CHECK_EQUIVALENT_VECTORS(rig->get_secondary_camera("left")->n(), Eigen::Vector3f(ONE_OVER_ROOT_2, 0.0f, -ONE_OVER_ROOT_2));
  CHECK_EQUIVALENT_VECTORS(rig->get_secondary_camera("right")->p(), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
  CHECK_EQUIVALENT_VECTORS(rig->get_secondary_camera("right")->u(), Eigen::Vector3f(-ONE_OVER_ROOT_2, 0.0f, ONE_OVER_ROOT_2));
  CHECK_EQUIVALENT_VECTORS(rig->get_secondary_camera("right")->v(), Eigen::Vector3f(0.0f, 1.0f, 0.0f));
  CHECK_EQUIVALENT_VECTORS(rig->get_secondary_camera("right")->n(), Eigen::Vector3f(-ONE_OVER_ROOT_2, 0.0f, -ONE_OVER_ROOT_2));
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (C) 2003-2004 Jeremy B. Maitin-Shepard.
// Copyright (C) 2005-2009 Daniel James
// 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_UNORDERED_DETAIL_UTIL_HPP_INCLUDED
#define BOOST_UNORDERED_DETAIL_UTIL_HPP_INCLUDED

#include <cstddef>
#include <utility>
#include <algorithm>
#include <boost/limits.hpp>
#include <boost/iterator/iterator_categories.hpp>
#include <boost/preprocessor/seq/size.hpp>
#include <boost/preprocessor/seq/enum.hpp>
#include <boost/unordered/detail/fwd.hpp>

namespace boost { namespace unordered_detail {

    ////////////////////////////////////////////////////////////////////////////
    // convert double to std::size_t

    inline std::size_t double_to_size_t(double f)
    {
        return f >= static_cast<double>(
            (std::numeric_limits<std::size_t>::max)()) ?
            (std::numeric_limits<std::size_t>::max)() :
            static_cast<std::size_t>(f);
    }

    ////////////////////////////////////////////////////////////////////////////
    // primes

#define BOOST_UNORDERED_PRIMES \
    (5ul)(11ul)(17ul)(29ul)(37ul)(53ul)(67ul)(79ul) \
    (97ul)(131ul)(193ul)(257ul)(389ul)(521ul)(769ul) \
    (1031ul)(1543ul)(2053ul)(3079ul)(6151ul)(12289ul)(24593ul) \
    (49157ul)(98317ul)(196613ul)(393241ul)(786433ul) \
    (1572869ul)(3145739ul)(6291469ul)(12582917ul)(25165843ul) \
    (50331653ul)(100663319ul)(201326611ul)(402653189ul)(805306457ul) \
    (1610612741ul)(3221225473ul)(4294967291ul)

    template<class T> struct prime_list_template
    {
        static std::size_t const value[];

#if !defined(SUNPRO_CC)
        static std::ptrdiff_t const length;
#else
        static std::ptrdiff_t const length
            = BOOST_PP_SEQ_SIZE(BOOST_UNORDERED_PRIMES);
#endif
    };

    template<class T>
    std::size_t const prime_list_template<T>::value[] = {
        BOOST_PP_SEQ_ENUM(BOOST_UNORDERED_PRIMES)
    };

#if !defined(SUNPRO_CC)
    template<class T>
    std::ptrdiff_t const prime_list_template<T>::length
        = BOOST_PP_SEQ_SIZE(BOOST_UNORDERED_PRIMES);
#endif

#undef BOOST_UNORDERED_PRIMES

    typedef prime_list_template<std::size_t> prime_list;

    // no throw
    inline std::size_t next_prime(std::size_t num) {
        std::size_t const* const prime_list_begin = prime_list::value;
        std::size_t const* const prime_list_end = prime_list_begin +
            prime_list::length;
        std::size_t const* bound =
            std::lower_bound(prime_list_begin, prime_list_end, num);
        if(bound == prime_list_end)
            bound--;
        return *bound;
    }

    // no throw
    inline std::size_t prev_prime(std::size_t num) {
        std::size_t const* const prime_list_begin = prime_list::value;
        std::size_t const* const prime_list_end = prime_list_begin +
            prime_list::length;
        std::size_t const* bound =
            std::upper_bound(prime_list_begin,prime_list_end, num);
        if(bound != prime_list_begin)
            bound--;
        return *bound;
    }

    ////////////////////////////////////////////////////////////////////////////
    // pair_cast - because some libraries don't have the full pair constructors.

    template <class Dst1, class Dst2, class Src1, class Src2>
    inline std::pair<Dst1, Dst2> pair_cast(std::pair<Src1, Src2> const& x)
    {
        return std::pair<Dst1, Dst2>(Dst1(x.first), Dst2(x.second));
    }

    ////////////////////////////////////////////////////////////////////////////
    // insert_size/initial_size

#if !defined(BOOST_NO_STD_DISTANCE)
    using ::std::distance;
#else
    template <class ForwardIterator>
    inline std::size_t distance(ForwardIterator i, ForwardIterator j) {
        std::size_t x;
        std::distance(i, j, x);
        return x;
    }
#endif

    template <class I>
    inline std::size_t insert_size(I i, I j, boost::forward_traversal_tag)
    {
        return std::distance(i, j);
    }

    template <class I>
    inline std::size_t insert_size(I, I, boost::incrementable_traversal_tag)
    {
        return 1;
    }

    template <class I>
    inline std::size_t insert_size(I i, I j)
    {
        BOOST_DEDUCED_TYPENAME boost::iterator_traversal<I>::type
            iterator_traversal_tag;
        return insert_size(i, j, iterator_traversal_tag);
    }

    template <class I>
    inline std::size_t initial_size(I i, I j,
        std::size_t num_buckets = boost::unordered_detail::default_bucket_count)
    {
        return (std::max)(static_cast<std::size_t>(insert_size(i, j)) + 1,
            num_buckets);
    }

    ////////////////////////////////////////////////////////////////////////////
    // Node Constructors

#if defined(BOOST_UNORDERED_STD_FORWARD)

    template <class T, class... Args>
    inline void construct_impl(T*, void* address, Args&&... args)
    {
        new(address) T(std::forward<Args>(args)...);
    }

#if defined(BOOST_UNORDERED_CPP0X_PAIR)
    template <class First, class Second, class Key, class Arg0, class... Args>
    inline void construct_impl(std::pair<First, Second>*, void* address,
        Key&& k, Arg0&& arg0, Args&&... args)
    )
    {
        new(address) std::pair<First, Second>(k,
            Second(arg0, std::forward<Args>(args)...);
    }
#endif

#else

#define BOOST_UNORDERED_CONSTRUCT_IMPL(z, num_params, _)                       \
    template <                                                                 \
        class T,                                                               \
        BOOST_UNORDERED_TEMPLATE_ARGS(z, num_params)                           \
    >                                                                          \
    inline void construct_impl(                                                \
        T*, void* address,                                                     \
        BOOST_UNORDERED_FUNCTION_PARAMS(z, num_params)                         \
    )                                                                          \
    {                                                                          \
        new(address) T(                                                        \
            BOOST_UNORDERED_CALL_PARAMS(z, num_params));                       \
    }                                                                          \
                                                                               \
    template <class First, class Second, class Key,                            \
        BOOST_UNORDERED_TEMPLATE_ARGS(z, num_params)                           \
    >                                                                          \
    inline void construct_impl(                                                \
        std::pair<First, Second>*, void* address,                              \
        Key const& k, BOOST_UNORDERED_FUNCTION_PARAMS(z, num_params))          \
    {                                                                          \
        new(address) std::pair<First, Second>(k,                               \
            Second(BOOST_UNORDERED_CALL_PARAMS(z, num_params)));               \
    }

    BOOST_PP_REPEAT_FROM_TO(1, BOOST_UNORDERED_EMPLACE_LIMIT,
        BOOST_UNORDERED_CONSTRUCT_IMPL, _)

#undef BOOST_UNORDERED_CONSTRUCT_IMPL
#endif

    // hash_node_constructor
    //
    // Used to construct nodes in an exception safe manner.

    template <class Alloc, class Grouped>
    class hash_node_constructor
    {
        typedef hash_buckets<Alloc, Grouped> buckets;
        typedef BOOST_DEDUCED_TYPENAME buckets::node node;
        typedef BOOST_DEDUCED_TYPENAME buckets::real_node_ptr real_node_ptr;
        typedef BOOST_DEDUCED_TYPENAME buckets::value_type value_type;

        buckets& buckets_;
        real_node_ptr node_;
        bool node_constructed_;
        bool value_constructed_;

    public:

        hash_node_constructor(buckets& m) :
            buckets_(m),
            node_(),
            node_constructed_(false),
            value_constructed_(false)
        {
        }

        ~hash_node_constructor();
        void construct_preamble();

#if defined(BOOST_UNORDERED_STD_FORWARD)
        template <class... Args>
        void construct(Args&&... args)
        {
            construct_preamble();
            construct_impl((value_type*) 0, node_->address(),
                std::forward<Args>(args)...);
            value_constructed_ = true;
        }
#else

#define BOOST_UNORDERED_CONSTRUCT(z, num_params, _)                            \
        template <                                                             \
            BOOST_UNORDERED_TEMPLATE_ARGS(z, num_params)                       \
        >                                                                      \
        void construct(                                                        \
            BOOST_UNORDERED_FUNCTION_PARAMS(z, num_params)                     \
        )                                                                      \
        {                                                                      \
            construct_preamble();                                              \
            construct_impl(                                                    \
                (value_type*) 0, node_->address(),                             \
                BOOST_UNORDERED_CALL_PARAMS(z, num_params)                     \
            );                                                                 \
            value_constructed_ = true;                                         \
        }

        BOOST_PP_REPEAT_FROM_TO(1, BOOST_UNORDERED_EMPLACE_LIMIT,
            BOOST_UNORDERED_CONSTRUCT, _)

#undef BOOST_UNORDERED_CONSTRUCT

#endif
        template <class K, class M>
        void construct_pair(K const& k, M*)
        {
            construct_preamble();
            new(node_->address()) value_type(k, M());
            value_constructed_ = true;
        }

        value_type& value() const
        {
            BOOST_ASSERT(node_);
            return node_->value();
        }

        // no throw
        BOOST_DEDUCED_TYPENAME buckets::node_ptr release()
        {
            real_node_ptr p = node_;
            node_ = real_node_ptr();
            // node_ptr cast
            return buckets_.bucket_alloc().address(*p);
        }

    private:
        hash_node_constructor(hash_node_constructor const&);
        hash_node_constructor& operator=(hash_node_constructor const&);
    };

    // hash_node_constructor

    template <class Alloc, class Grouped>
    inline hash_node_constructor<Alloc, Grouped>::~hash_node_constructor()
    {
        if (node_) {
            if (value_constructed_) {
#if BOOST_WORKAROUND(__CODEGEARC__, BOOST_TESTED_AT(0x0613))
                struct dummy { hash_node<Alloc, Grouped> x; };
#endif
                boost::unordered_detail::destroy(node_->value_ptr());
            }

            if (node_constructed_)
                buckets_.node_alloc().destroy(node_);

            buckets_.node_alloc().deallocate(node_, 1);
        }
    }

    template <class Alloc, class Grouped>
    inline void hash_node_constructor<Alloc, Grouped>::construct_preamble()
    {
        if(!node_) {
            node_constructed_ = false;
            value_constructed_ = false;

            node_ = buckets_.node_alloc().allocate(1);
            buckets_.node_alloc().construct(node_, node());
            node_constructed_ = true;
        }
        else {
            BOOST_ASSERT(node_constructed_ && value_constructed_);
            boost::unordered_detail::destroy(node_->value_ptr());
            value_constructed_ = false;
        }
    }
}}

#endif


#include <boost/config.hpp>
#include <fstream>
#include <iostream>
#include <regex>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_traits.hpp>

using namespace boost;
typedef adjacency_list<listS,
                       vecS,
                       directedS,
                       no_property,
                       property<edge_weight_t, int>>
    graph_t;
typedef graph_traits<graph_t>::vertex_descriptor vertex_descriptor;
typedef graph_traits<graph_t>::edge_descriptor edge_descriptor;

typedef std::pair<int, int> Edge;

typedef std::pair<int, int> Point;
typedef std::vector<Point> Alignment;

std::vector<std::string> split(const std::string& input,
                               const std::string& regex) {
  std::regex re(regex);
  std::sregex_token_iterator first{input.begin(), input.end(), re, -1}, last;
  return {first, last};
}

float dist(Point a, Point b) {
  return sqrt(pow(b.first - a.first, 2) + pow(b.second - a.second, 2)) - 1;
}

Alignment shortestPath(const Alignment& a) {
  Alignment shortest;

  std::vector<Edge> edges;
  std::vector<int> weights;

  for(int i = 0; i < a.size(); ++i) {
    for(int j = 0; j < a.size(); ++j) {
      if(a[i] != a[j] && a[j].first - a[i].first >= 0
         && a[j].second - a[i].second >= 0) {
        edges.push_back(Edge(i, j));
        weights.push_back(dist(a[i], a[j]));
      }
    }
  }

  graph_t g(edges.begin(), edges.end(), weights.data(), a.size());
  property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);

  std::vector<vertex_descriptor> p(num_vertices(g));
  std::vector<int> d(num_vertices(g));
  vertex_descriptor s = vertex(0, g);

  dijkstra_shortest_paths(g, s, predecessor_map(&p[0]).distance_map(&d[0]));

  int v = a.size() - 1;
  while(v != 0) {
    shortest.push_back(a[v]);
    v = p[v];
  }
  shortest.push_back(a[v]);
  std::sort(shortest.begin(), shortest.end());

  Alignment shortestU;
  for(auto p : shortest)
    if(shortestU.empty() || shortestU.back().first != p.first)
      shortestU.push_back(p);

  return shortestU;
}

int main(int argc, char** argv) {
  if(argc != 4) {
    std::cerr << "Usage: ./align2steps source target alignment" << std::endl;
    exit(1);
  }

  std::ifstream srcStrm, trgStrm, alnStrm;
  srcStrm.open(argv[1]);
  trgStrm.open(argv[2]);
  alnStrm.open(argv[3]);

  int i = 0;
  std::string source, target, alignment;
  while(std::getline(srcStrm, source) && std::getline(trgStrm, target)
        && std::getline(alnStrm, alignment)) {
    auto srcToks = split(source, R"(\s)");
    auto trgToks = split(target, R"(\s)");
    auto alnToks = split(alignment, R"(\s|-)");

    Alignment alignment;
    for(int i = 0; i < alnToks.size(); i += 2)
      alignment.emplace_back(std::stoi(alnToks[i + 1]), std::stoi(alnToks[i]));

    // add end
    alignment.emplace_back(trgToks.size(), srcToks.size());

    auto shortest = shortestPath(alignment);

    int cTrg = 0, cSrc = 0;
    for(auto& p : shortest) {
      for(int i = cTrg; i < p.first; ++i)
        std::cout << trgToks[i] << " ";

      for(int i = cSrc; i < p.second; ++i)
        std::cout << "<step> ";

      cTrg = p.first;
      cSrc = p.second;
    }

    std::cout << std::endl;
    i++;
    if(i % 10000 == 0)
      std::cerr << i << " ";
  }
  std::cerr << std::endl;

  return 0;
}


//  ratio_test.cpp  ----------------------------------------------------------//

//  Copyright 2008 Howard Hinnant
//  Copyright 2008 Beman Dawes

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

#include <iostream>
#include <boost/ratio/ratio.hpp>
#include "duration.hpp"

namespace User1
{
// Example type-safe "physics" code interoperating with chrono::duration types
//  and taking advantage of the std::ratio infrastructure and design philosophy.

// length - mimics chrono::duration except restricts representation to double.
//    Uses boost::ratio facilities for length units conversions.

template <class Ratio>
class length
{
public:
    typedef Ratio ratio;
private:
    double len_;
public:

    length() : len_(1) {}
    length(const double& len) : len_(len) {}

    // conversions
    template <class R>
    length(const length<R>& d)
            : len_(d.count() * boost::ratio_divide<Ratio, R>::type::den /
                               boost::ratio_divide<Ratio, R>::type::num) {}

    // observer

    double count() const {return len_;}

    // arithmetic

    length& operator+=(const length& d) {len_ += d.count(); return *this;}
    length& operator-=(const length& d) {len_ -= d.count(); return *this;}

    length operator+() const {return *this;}
    length operator-() const {return length(-len_);}

    length& operator*=(double rhs) {len_ *= rhs; return *this;}
    length& operator/=(double rhs) {len_ /= rhs; return *this;}
};

// Sparse sampling of length units
typedef length<boost::ratio<1> >          meter;        // set meter as "unity"
typedef length<boost::centi>              centimeter;   // 1/100 meter
typedef length<boost::kilo>               kilometer;    // 1000  meters
typedef length<boost::ratio<254, 10000> > inch;         // 254/10000 meters
// length takes ratio instead of two integral types so that definitions can be made like so:
typedef length<boost::ratio_multiply<boost::ratio<12>, inch::ratio>::type>   foot;  // 12 inchs
typedef length<boost::ratio_multiply<boost::ratio<5280>, foot::ratio>::type> mile;  // 5280 feet

// Need a floating point definition of seconds
typedef boost_ex::chrono::duration<double> seconds;                         // unity
// Demo of (scientific) support for sub-nanosecond resolutions
typedef boost_ex::chrono::duration<double,  boost::pico> picosecond;  // 10^-12 seconds
typedef boost_ex::chrono::duration<double, boost::femto> femtosecond; // 10^-15 seconds
typedef boost_ex::chrono::duration<double,  boost::atto> attosecond;  // 10^-18 seconds

// A very brief proof-of-concept for SIUnits-like library
//  Hard-wired to floating point seconds and meters, but accepts other units (shown in testUser1())
template <class R1, class R2>
class quantity
{
    double q_;
public:
    typedef R1 time_dim;
    typedef R2 distance_dim;
    quantity() : q_(1) {}

    double get() const {return q_;}
    void set(double q) {q_ = q;}
};

template <>
class quantity<boost::ratio<1>, boost::ratio<0> >
{
    double q_;
public:
    quantity() : q_(1) {}
    quantity(seconds d) : q_(d.count()) {}  // note:  only User1::seconds needed here

    double get() const {return q_;}
    void set(double q) {q_ = q;}
};

template <>
class quantity<boost::ratio<0>, boost::ratio<1> >
{
    double q_;
public:
    quantity() : q_(1) {}
    quantity(meter d) : q_(d.count()) {}  // note:  only User1::meter needed here

    double get() const {return q_;}
    void set(double q) {q_ = q;}
};

template <>
class quantity<boost::ratio<0>, boost::ratio<0> >
{
    double q_;
public:
    quantity() : q_(1) {}
    quantity(double d) : q_(d) {}

    double get() const {return q_;}
    void set(double q) {q_ = q;}
};

// Example SI-Units
typedef quantity<boost::ratio<0>, boost::ratio<0> >  Scalar;
typedef quantity<boost::ratio<1>, boost::ratio<0> >  Time;         // second
typedef quantity<boost::ratio<0>, boost::ratio<1> >  Distance;     // meter
typedef quantity<boost::ratio<-1>, boost::ratio<1> > Speed;        // meter/second
typedef quantity<boost::ratio<-2>, boost::ratio<1> > Acceleration; // meter/second^2

template <class R1, class R2, class R3, class R4>
quantity<typename boost::ratio_subtract<R1, R3>::type, typename boost::ratio_subtract<R2, R4>::type>
operator/(const quantity<R1, R2>& x, const quantity<R3, R4>& y)
{
    typedef quantity<typename boost::ratio_subtract<R1, R3>::type, typename boost::ratio_subtract<R2, R4>::type> R;
    R r;
    r.set(x.get() / y.get());
    return r;
}

template <class R1, class R2, class R3, class R4>
quantity<typename boost::ratio_add<R1, R3>::type, typename boost::ratio_add<R2, R4>::type>
operator*(const quantity<R1, R2>& x, const quantity<R3, R4>& y)
{
    typedef quantity<typename boost::ratio_add<R1, R3>::type, typename boost::ratio_add<R2, R4>::type> R;
    R r;
    r.set(x.get() * y.get());
    return r;
}

template <class R1, class R2>
quantity<R1, R2>
operator+(const quantity<R1, R2>& x, const quantity<R1, R2>& y)
{
    typedef quantity<R1, R2> R;
    R r;
    r.set(x.get() + y.get());
    return r;
}

template <class R1, class R2>
quantity<R1, R2>
operator-(const quantity<R1, R2>& x, const quantity<R1, R2>& y)
{
    typedef quantity<R1, R2> R;
    R r;
    r.set(x.get() - y.get());
    return r;
}

// Example type-safe physics function
Distance
compute_distance(Speed v0, Time t, Acceleration a)
{
    return v0 * t + Scalar(.5) * a * t * t;  // if a units mistake is made here it won't compile
}

} // User1

// Exercise example type-safe physics function and show interoperation
// of custom time durations (User1::seconds) and standard time durations (std::hours).
// Though input can be arbitrary (but type-safe) units, output is always in SI-units
//   (a limitation of the simplified Units lib demoed here).

int main()
{
    //~ typedef boost::ratio<8, BOOST_INTMAX_C(0x7FFFFFFFD)> R1;
    //~ typedef boost::ratio<3, BOOST_INTMAX_C(0x7FFFFFFFD)> R2;
    typedef User1::quantity<boost::ratio_subtract<boost::ratio<0>, boost::ratio<1> >::type,
                             boost::ratio_subtract<boost::ratio<1>, boost::ratio<0> >::type > RR;
    //~ typedef boost::ratio_subtract<R1, R2>::type RS;
    //~ std::cout << RS::num << '/' << RS::den << '\n';

    std::cout << "*************\n";
    std::cout << "* testUser1 *\n";
    std::cout << "*************\n";
    User1::Distance d(( User1::mile(110) ));
    boost_ex::chrono::hours h((2));
    User1::Time t(( h ));
    //~ boost_ex::chrono::seconds sss=boost_ex::chrono::duration_cast<boost_ex::chrono::seconds>(h);
    //~ User1::seconds sss((120));
    //~ User1::Time t(( sss ));

    //typedef User1::quantity<boost::ratio_subtract<User1::Distance::time_dim, User1::Time::time_dim >::type,
    //                        boost::ratio_subtract<User1::Distance::distance_dim, User1::Time::distance_dim >::type > R;
    RR r=d / t;
    //r.set(d.get() / t.get());

    User1::Speed rc= r;

    User1::Speed s = d / t;
    std::cout << "Speed = " << s.get() << " meters/sec\n";
    User1::Acceleration a = User1::Distance( User1::foot(32.2) ) / User1::Time() / User1::Time();
    std::cout << "Acceleration = " << a.get() << " meters/sec^2\n";
    User1::Distance df = compute_distance(s, User1::Time( User1::seconds(0.5) ), a);
    std::cout << "Distance = " << df.get() << " meters\n";
    std::cout << "There are " << User1::mile::ratio::den << '/' << User1::mile::ratio::num << " miles/meter";
    User1::meter mt = 1;
    User1::mile mi = mt;
    std::cout << " which is approximately " << mi.count() << '\n';
    std::cout << "There are " << User1::mile::ratio::num << '/' << User1::mile::ratio::den << " meters/mile";
    mi = 1;
    mt = mi;
    std::cout << " which is approximately " << mt.count() << '\n';
    User1::attosecond as(1);
    User1::seconds sec = as;
    std::cout << "1 attosecond is " << sec.count() << " seconds\n";
    std::cout << "sec = as;  // compiles\n";
    sec = User1::seconds(1);
    as = sec;
    std::cout << "1 second is " << as.count() << " attoseconds\n";
    std::cout << "as = sec;  // compiles\n";
    std::cout << "\n";
  return 0;
}


#include <Eigen/Core>
#include "mex.h"

using namespace Eigen;
using namespace std;

// TODO openmp version

// NOTE: mxSetProperty and possibly mxGetProperty make copies, even with
// classdef < handle! lame! I believe mxGetField does NOT make a copy, though I
// haven't tested it recently

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{

    /* SETUP */
    if (nrhs != 5) { mexErrMsgTxt("wrong number of arguments\n"); }

    //// pull out inputs

    // data
    if (!mxGetField(prhs[0],0,"data")) { mexErrMsgTxt("data missing 'data' field\n"); }
    int8_t *alldata = (int8_t *) mxGetData(mxGetField(prhs[0],0,"data"));
    int bigT = mxGetN(mxGetField(prhs[0],0,"data")); // total length of the data
    int num_subparts = mxGetM(mxGetField(prhs[0],0,"data")); // number of sub-parts

    if (!mxGetField(prhs[0],0,"resources")) { mexErrMsgTxt("data missing 'resources' field\n"); }
    int8_t *allresources = (int8_t *) mxGetData(mxGetField(prhs[0],0,"resources"));

    if (!mxGetField(prhs[0],0,"starts")) { mexErrMsgTxt("data missing 'starts' field\n"); }
    int32_t *starts = (int32_t *) mxGetData(mxGetField(prhs[0],0,"starts"));
    int num_sequences = max(mxGetM(mxGetField(prhs[0],0,"starts")),
            mxGetM(mxGetField(prhs[0],0,"starts")));

    if (!mxGetField(prhs[0],0,"lengths")) { mexErrMsgTxt("data missing 'lengths' field\n"); }
    int32_t *lengths = (int32_t *) mxGetData(mxGetField(prhs[0],0,"lengths"));

    // parameters struct
    if (!mxGetField(prhs[1],0,"learns")) { mexErrMsgTxt("model missing 'learns' field\n"); }
    double *learns = mxGetPr(mxGetField(prhs[1],0,"learns"));
    int num_resources = max(mxGetM(mxGetField(prhs[1],0,"learns")),
            mxGetN(mxGetField(prhs[1],0,"learns")));

    if (!mxGetField(prhs[1],0,"forgets")) { mexErrMsgTxt("model missing 'forgets' field\n"); }
    double *forgets = mxGetPr(mxGetField(prhs[1],0,"forgets"));

    if (!mxGetField(prhs[1],0,"guesses")) { mexErrMsgTxt("model missing 'guesses' field\n"); }
    double *guess = mxGetPr(mxGetField(prhs[1],0,"guesses"));

    if (!mxGetField(prhs[1],0,"slips")) { mexErrMsgTxt("model missing 'slips' field\n"); }
    double *slip = mxGetPr(mxGetField(prhs[1],0,"slips"));

    if (!mxGetField(prhs[1],0,"prior")) { mexErrMsgTxt("model missing 'prior' field\n"); }
    double prior = mxGetScalar(mxGetField(prhs[1],0,"prior"));

    Array2d initial_distn;
    initial_distn << 1-prior, prior;

    MatrixXd As(2,2*num_resources);
    for (int n=0; n<num_resources; n++) {
        As.col(2*n) << 1-learns[n], learns[n];
        As.col(2*n+1) << forgets[n], 1-forgets[n];
    }

    Array2Xd Bn(2,2*num_subparts);
    for (int n=0; n<num_subparts; n++) {
        Bn.col(2*n) << 1-guess[n], slip[n]; // incorrect
        Bn.col(2*n+1) << guess[n], 1-slip[n]; // correct
    }

    //// outputs

    // rhs outputs
    Map<ArrayXXd,Aligned> all_trans_softcounts(mxGetPr(prhs[2]),2,2*num_resources);
    all_trans_softcounts.setZero();
    Map<Array2Xd,Aligned> all_emission_softcounts(mxGetPr(prhs[3]),2,2*num_subparts);
    all_emission_softcounts.setZero();
    Map<Array2d,Aligned> all_initial_softcounts(mxGetPr(prhs[4]));
    all_initial_softcounts.setZero();

    // lhs outputs
    Map<Array2Xd,Aligned> gamma_out(NULL,2,bigT);
    Map<Array2Xd,Aligned> alpha_out(NULL,2,bigT);
    double s_total_loglike = 0;
    double *total_loglike = &s_total_loglike;
    switch (nlhs) {
        case 3:
            plhs[2] = mxCreateDoubleMatrix(2,bigT,mxREAL);
            new (&gamma_out) Map<Array2Xd,Aligned>(mxGetPr(plhs[2]),2,bigT);
        case 2:
            plhs[1] = mxCreateDoubleMatrix(2,bigT,mxREAL);
            new (&alpha_out) Map<Array2Xd,Aligned>(mxGetPr(plhs[1]),2,bigT);
        case 1:
            plhs[0] = mxCreateDoubleScalar(0.);
            total_loglike = mxGetPr(plhs[0]);
    }

    /* COMPUTATION */

    for (int sequence_index=0; sequence_index < num_sequences; sequence_index++) {
        // NOTE: -1 because Matlab indexing starts at 1
        int32_t sequence_start = starts[sequence_index] - 1;
        int32_t T = lengths[sequence_index];

        int8_t *data = alldata + num_subparts*sequence_start;
        int8_t *resources = allresources + sequence_start;

        //// likelihoods
        Array2Xd likelihoods(2,T);
        likelihoods.setOnes();
        for (int t=0; t<T; t++) {
            for (int n=0; n<num_subparts; n++) {
                if (data[n+num_subparts*t] != 0) {
                    likelihoods.col(t) *= Bn.col(2*n + (data[n+num_subparts*t] == 2));
                }
            }
        }

        //// forward messages
        double loglike, norm;
        MatrixXd alpha(2,T);
        alpha.col(0) = initial_distn * likelihoods.col(0);
        norm = alpha.col(0).sum();
        alpha.col(0) /= norm;
        loglike = log(norm);
        for (int t=0; t<T-1; t++) {
            alpha.col(t+1) = (As.block(0,2*(resources[t]-1),2,2) * alpha.col(t)).array()
                * likelihoods.col(t+1);
            norm = alpha.col(t+1).sum();
            alpha.col(t+1) /= norm;
            loglike += log(norm);
        }

        //// backward messages and statistic counting
        ArrayXXd trans_softcounts(2,2*num_resources);
        trans_softcounts.setZero();
        Array2Xd emission_softcounts(2,2*num_subparts);
        emission_softcounts.setZero();
        Array2d init_softcounts; // no need to set zero

        Array2Xd gamma(2,T);
        gamma.col(T-1) = alpha.col(T-1);
        for (int n=0; n<num_subparts; n++) {
            if (data[n+num_subparts*(T-1)] != 0) {
                emission_softcounts.col(2*n + (data[n+num_subparts*(T-1)] == 2)) += gamma.col(T-1);
            }
        }
        for (int t=T-2; t>=0; t--) {
            Matrix2d A = As.block(0,2*(resources[t]-1),2,2);
            Array22d pair = A;
            pair.rowwise() *= alpha.col(t).transpose().array();
            pair.colwise() *= gamma.col(t+1);
            pair.colwise() /= (A*alpha.col(t)).array();

            trans_softcounts.block(0,2*(resources[t]-1),2,2) += pair;

            gamma.col(t) = pair.colwise().sum().transpose();
            // NOTE: we have to touch the data again here
            for (int n=0; n<num_subparts; n++) {
                if (data[n+num_subparts*t] != 0) {
                    emission_softcounts.col(2*n + (data[n+num_subparts*t] == 2)) += gamma.col(t);
                }
            }
        }
        init_softcounts = gamma.col(0);

        // NOTE: these will need to change to base types for openmp
        all_trans_softcounts += trans_softcounts;
        all_emission_softcounts += emission_softcounts;
        all_initial_softcounts += init_softcounts;

        switch (nlhs) {
            case 3:
                gamma_out.block(0,sequence_start,2,T) = gamma;
            case 2:
                alpha_out.block(0,sequence_start,2,T) = alpha;
            case 1:
                *total_loglike += loglike;
        }

    }
}


/**
 * \file CachedCosinusGeneratorFilter.cpp
 */

#include "CachedCosinusGeneratorFilter.h"

#include <boost/math/constants/constants.hpp>

#include <cmath>
#include <cstdint>
#include <cstring>

namespace ATK
{
  template<typename DataType_>
  CachedCosinusGeneratorFilter<DataType_>::CachedCosinusGeneratorFilter(int periods, int seconds)
  :Parent(0, 1), periods(periods), seconds(seconds)
  {
  }

  template<typename DataType_>
  void CachedCosinusGeneratorFilter<DataType_>::set_frequency(int periods, int seconds)
  {
    if(periods <= 0)
    {
      throw std::out_of_range("Periods must be strictly positive");
    }
    this->periods = periods;
    this->seconds = seconds;
    setup();
  }

  template<typename DataType_>
  std::pair<int, int> CachedCosinusGeneratorFilter<DataType_>::get_frequency() const
  {
    return std::make_pair(periods, seconds);
  }

  template<typename DataType_>
  void CachedCosinusGeneratorFilter<DataType_>::set_volume(DataType_ volume)
  {
    this->volume = volume;
  }

  template<typename DataType_>
  DataType_ CachedCosinusGeneratorFilter<DataType_>::get_volume() const
  {
    return volume;
  }

  template<typename DataType_>
  void CachedCosinusGeneratorFilter<DataType_>::set_offset(DataType_ offset)
  {
    this->offset = offset;
  }

  template<typename DataType_>
  DataType_ CachedCosinusGeneratorFilter<DataType_>::get_offset() const
  {
    return offset;
  }

  template<typename DataType_>
  void CachedCosinusGeneratorFilter<DataType_>::setup()
  {
    indice = 0;
    cache.resize(output_sampling_rate * seconds);
    for(gsl::index i = 0; i < cache.size(); ++i)
    {
      cache[i] = static_cast<DataType>(std::cos(2 * boost::math::constants::pi<double>() * (i+1) * periods / seconds / output_sampling_rate));
    }
  }

  template<typename DataType_>
  void CachedCosinusGeneratorFilter<DataType_>::process_impl(gsl::index size) const
  {
    DataType* ATK_RESTRICT output = outputs[0];
    gsl::index processed = 0;
    while(processed < size)
    {
      auto to_copy = std::min(size - processed, static_cast<gsl::index>(cache.size()) - indice);
      memcpy(reinterpret_cast<void*>(output + processed), reinterpret_cast<const void*>(cache.data() + indice), to_copy * sizeof(DataType_));
      indice += to_copy;
      processed += to_copy;
      if(indice >= cache.size())
      {
        indice = 0;
      }
    }
    for(gsl::index i = 0; i < size; ++i)
    {
      output[i] = static_cast<DataType>(offset + volume * output[i]);
    }
  }

#if ATK_ENABLE_INSTANTIATION
  template class CachedCosinusGeneratorFilter<float>;
#endif
  template class CachedCosinusGeneratorFilter<double>;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 the Delft University of Technology 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 HOLDER 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      140110    S. Hirsh          File created.
 *      140220    T. Roegiers       Small changes during codecheck.
 *
 *    References
 *    Hameduddin, I. Rotate vector(s) about axis, rodrigues_rot.m, available at
 *        http://www.mathworks.com/matlabcentral/fileexchange/34426-rotate-vectors-about-axis,
 *        2012, last accessed: 11th January, 2014.
 *    Murray, G. Rotation Matrices and Formulas java script, RotationMatrix.java available at
 *        https://sites.google.com/site/glennmurray/Home/rotation-matrices-and-formulas, 2011.
 *        last accessed: 20th January, 2014.
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Mathematics/BasicMathematics/rotationAboutArbitraryAxis.h"

namespace tudat
{
namespace unit_tests
{

//! Test suite for rotations about about arbitrary axes.
BOOST_AUTO_TEST_SUITE( test_RotationAboutArbitraryAxis )

BOOST_AUTO_TEST_CASE( test_RotationAboutArbitraryAxis_PointRotationWithCommonOrigin )
{

    //Benchmark data is obtained using Matlab Script (Hameduddin, 2012).

    //Set expected rotated vector.
    const Eigen::Vector3d expectedRotatedPosition = Eigen::Vector3d( 0.0, 1.414213562373095, 1.0 );

    //Set origin of rotation.
    const Eigen::Vector3d originOfRotation = Eigen::Vector3d( 0.0, 0.0, 0.0 );

    //Set angle of rotation [rad].
    const double angleOfRotation = mathematical_constants::PI / 4.0;

    //Set axis of rotation.
    const Eigen::Vector3d axisOfRotation = Eigen::Vector3d( 0.0, 0.0, 1.0 );

    //Set initial position of point.
    const Eigen::Vector3d initialPositionOfPoint = Eigen::Vector3d( 1.0, 1.0, 1.0 );

    //Compute rotated position.
    const Eigen::Vector3d computedRotatedPosition = basic_mathematics::
        computeRotationOfPointAboutArbitraryAxis( originOfRotation, angleOfRotation,
                                                  axisOfRotation, initialPositionOfPoint );

    // Compare computed and expected vectors.
    BOOST_CHECK_SMALL( computedRotatedPosition.x( ), std::numeric_limits< double >::epsilon( ) );

    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedRotatedPosition.segment( 1, 2 ),
                                       expectedRotatedPosition.segment( 1, 2),
                                       std::numeric_limits< double >::epsilon( ) );

}

BOOST_AUTO_TEST_CASE( test_RotationAboutArbitraryAxis_PointRotationWithDifferentOrigins )
{

    //Benchmark data is obtained using java script (Murray, 2013).

    //Set expected rotated vector.
    const Eigen::Vector3d expectedRotatedPosition = Eigen::Vector3d( 3.156561876696307,
                                                                     -5.97145870839039,
                                                                     -4.418680390057799 );
    //Set origin of rotation.
    const Eigen::Vector3d originOfRotation = Eigen::Vector3d( 4.0, 1.0, -1.0 );

    //Set angle of rotation [rad]
    const double angleOfRotation = 7.0;

    //Set axis of rotation.
    const Eigen::Vector3d axisOfRotation = Eigen::Vector3d( 2.0, -2.0, 3.0 );

    //Set initial position of point.
    const Eigen::Vector3d initialPositionOfPoint = Eigen::Vector3d( -1.0, -5.0, -1.0 );

    //Compute rotated position.
    const Eigen::Vector3d computedRotatedPosition = basic_mathematics::
        computeRotationOfPointAboutArbitraryAxis( originOfRotation,  angleOfRotation,
                                                  axisOfRotation, initialPositionOfPoint );

    // Compare computed and expected radiation pressure acceleration vectors.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedRotatedPosition,
                                       expectedRotatedPosition,
                                       std::numeric_limits<double>::epsilon( ) );

}

BOOST_AUTO_TEST_CASE( test_RotationAboutArbitraryAxis_PointRotationWithDifferentOrigins2 )
{

    //Benchmark data is obtained using java script (Murray, 2013).

    //Set expected rotated vector.
    const Eigen::Vector3d expectedRotatedPosition = Eigen::Vector3d( 15.71267522236938,
                                                                     -0.9723168350942417,
                                                                      9.449538267504582 );
    //Set origin of rotation.
    const Eigen::Vector3d originOfRotation = Eigen::Vector3d( 1.5, 3.8, 12.0 );

    //Set angle of rotation [rad]
    const double angleOfRotation = 3.0;

    //Set axis of rotation.
    const Eigen::Vector3d axisOfRotation = Eigen::Vector3d( -4.6, 6.75, 7.7 );

    //Set initial position of point.
    const Eigen::Vector3d initialPositionOfPoint = Eigen::Vector3d( -4.3, -5.2, 1.2 );

    //Compute rotated position.
    const Eigen::Vector3d computedRotatedPosition = basic_mathematics::
        computeRotationOfPointAboutArbitraryAxis( originOfRotation, angleOfRotation,
                                                  axisOfRotation, initialPositionOfPoint );

    // Compare computed and expected radiation pressure acceleration vectors.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedRotatedPosition, expectedRotatedPosition, 1.0e-14 );

}

BOOST_AUTO_TEST_CASE( test_RotationAboutArbitraryAxis_VectorRotationWithCommonOrigin )
{

    //Benchmark data is obtained using Matlab Script (Hameduddin, 2012).

    //Set expected rotated vector.
    const Eigen::Vector3d expectedRotatedVector = Eigen::Vector3d( -5.480598288892924,
                                                                    0.532794739754852,
                                                                    6.138336269794405 );
    //Set origin of rotation.
    const Eigen::Vector3d originOfRotation = Eigen::Vector3d( 0.0, 0.0, 0.0 );

    //Set angle of rotation [rad].
    const double angleOfRotation = 12.0;

    //Set axis of rotation.
    const Eigen::Vector3d axisOfRotation = Eigen::Vector3d( -1.0, -2.0, -3.0 );

    //Set initial position of vector tail.
    const Eigen::Vector3d initialPositionOfVectorTail = Eigen::Vector3d( 1.0, 3.0, 5.0 );

    //Set initial vector.
    const Eigen::Vector3d initialVector = Eigen::Vector3d( -6.0, 4.0, 4.0 );

    //Compute rotated position.
    const Eigen::Vector3d computedRotatedVector = basic_mathematics::
        computeRotationOfVectorAboutArbitraryAxis( originOfRotation, angleOfRotation,
                                                   axisOfRotation, initialPositionOfVectorTail,
                                                   initialVector );

    // Compare computed and expected radiation pressure acceleration vectors
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedRotatedVector, expectedRotatedVector, 1.0e-14 );

}

BOOST_AUTO_TEST_CASE( test_RotationAboutArbitraryAxis_VectorRotationWithDifferentOrigins )
{

    //Benchmark data is obtained using java script (Murray, 2013).

    //Set expected rotated vector.
    const Eigen::Vector3d expectedRotatedVector = Eigen::Vector3d( -7.15233485964669,
                                                                   -2.4444137866784175,
                                                                    8.308967883857736 );
    //Set origin of rotation.
    const Eigen::Vector3d originOfRotation = Eigen::Vector3d( 1.5, 3.8, 12.0 );

    //Set angle of rotation [rad].
    const double angleOfRotation = 3.0;

    //Set axis of rotation.
    const Eigen::Vector3d axisOfRotation = Eigen::Vector3d( -4.6, 6.75, 7.7 );

    //Set initial position of vector tail.
    const Eigen::Vector3d initialPositionOfVectorTail = Eigen::Vector3d( -4.3, -5.2, 1.2 );

    //Set initial vector.
    const Eigen::Vector3d initialVector = Eigen::Vector3d( 0.3, 11.2, 0.8 );

    //Compute rotated position.
    const Eigen::Vector3d computedRotatedVector = basic_mathematics::
        computeRotationOfVectorAboutArbitraryAxis( originOfRotation, angleOfRotation,
                                                   axisOfRotation, initialPositionOfVectorTail,
                                                   initialVector );

  // Compare computed and expected rotated vectors.
  TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedRotatedVector, expectedRotatedVector, 1.0e-14 );

}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


// 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.

// #define MTL_VERBOSE_TEST
#define MTL_HAS_STD_OUTPUT_OPERATOR

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

template <typename Matrix>
inline void fill_matrix(Matrix& A)
{
    mtl::mat::inserter<Matrix> ins(A, 3);
    ins[0][0] << 2;
    ins[0][1] << 9;
    ins[1][1] << 1;
    ins[1][2] << 5;
    ins[1][3] << 5;
    ins[1][4] << 1;
    ins[2][2] << 6;
    ins[2][3] << 9;
    ins[3][2] << 2;
    ins[3][3] << 4;
    ins[4][0] << 7;
    ins[4][4] << 3;
}

int main(int, char**)
{
    using namespace mtl;
    using mtl::io::tout;
    typedef mtl::dense_vector<double>         vector_type;
    typedef mtl::mat::ell_matrix<double>   matrix_type;
    matrix_type   A(5, 5);

    fill_matrix(A);

    tout << "A (internal)\n";
    A.print_internal(tout);

    tout << "A[2][3] = " << A[2][3] << '\n';
    tout << "A[2][4] = " << A[2][4] << '\n';
    tout << "A[2][0] = " << A[2][0] << '\n';

    MTL_THROW_IF(A[2][3] != 9.0, unexpected_result());
    MTL_THROW_IF(A[2][4] != 0.0, unexpected_result());
    MTL_THROW_IF(A[2][0] != 0.0, unexpected_result());

    tout << "A =\n" << A;
    tout << "nnz = " << A.nnz() << std::endl;

    mtl::compressed2D<double> B(5, 5);
    fill_matrix(B);
    tout << "B =\n" << B;
    MTL_THROW_IF(A.nnz() != B.nnz(), unexpected_result());

    vector_type res(5), res2(5), x(5);
    iota(x, 1);
    res2= B * x;
    tout << "B * x = " << res2 << '\n';

    res= A * x;
    tout << "A * x =\n" << res << '\n';

    res2-= res;
    MTL_THROW_IF(two_norm(res2) > 0.001, unexpected_result());

    matrix_type C;
    laplacian_setup(C, 3, 4);
    tout << "C =\n" << C << '\n';

    // lazy(res2)= B * res;
    // lazy(res2)= A * res;

    // std::cout << "index_evaluatable<CRS ...> is " << mtl::traits::index_evaluatable<lazy_assign<vector_type, mtl::mat_cvec_times_expr<mtl::compressed2D<double>, vector_type>, int> >::value << '\n';
    // std::cout << "index_evaluatable<Ell ...> is " << mtl::traits::index_evaluatable<lazy_assign<vector_type, mtl::mat_cvec_times_expr<matrix_type, vector_type>, int> >::value << '\n';

    return 0;
}


//////////////////////////////////////////////////////////////////////////////
// random::poisson::devroye::q::lemma1.hpp                        	        //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  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)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_POISSON_EXT_DEVROYE_Q_LEMMA1_HPP_ER_2010
#define BOOST_RANDOM_POISSON_EXT_DEVROYE_Q_LEMMA1_HPP_ER_2010
#include <string>
#include <boost/format.hpp>

namespace boost{
namespace random{
namespace poisson{
namespace devroye{
namespace q{
namespace lemma1{

    // Lemma 1, p.199
    // q(y) < upper_bound(y), if y >= -mean
    template<typename T,typename Int,typename IntT>
    T upper_bound(const Int& i_mean,const Int& i_y,const IntT& converter)
    {
        T z = IntT::convert( 0 );
        T y = IntT::convert( i_y );
        T yp1 = IntT::convert( i_y +1 );
        T pos_y = IntT::convert( i_y < 0 ? z : y );
        T num = ( - y * yp1 );
        T den = IntT::convert( 2 * i_mean ) + pos_y;
        return num / den;
    }

    template<typename T,typename Int,typename IntT>
    bool do_raise_error(const T& q,const T& tol,std::string& str,
        const Int& i_mean,const Int& i_y,const IntT& converter)
    {
        T ub1 = lemma1::upper_bound<T>(i_mean,i_y,converter);
        if( q > ( tol + ub1 ) ){
            std::string msg = " [ lemma1 : q = %1% < %2% has failed ]";
            boost::format f( msg ); f % q % ub1;
            str += f.str();
            return true;
        }else{
            return false;
        }
    }

}// lemma1
}// q
}// devroye
}// poisson
}// random
}// boost

#endif


#include <vsim/env/camera.hpp>

#include <iostream>
#include <fstream>

#include <Eigen/Geometry>

using namespace std ;
using namespace Eigen ;

namespace vsim {

Eigen::Matrix4f PerspectiveCamera::projectionMatrix() const {
    assert(abs(aspect_ - std::numeric_limits<float>::epsilon()) > static_cast<float>(0));

    float xfov = aspect_ * yfov_ ;
    float const d = 1/tan(xfov / static_cast<float>(2));

    Matrix4f result ;
    result.setZero() ;

    result(0, 0) = d / aspect_ ;
    result(1, 1) = d ;
    result(2, 2) =  (zfar_ + znear_) / (znear_ - zfar_);
    result(2, 3) =  2 * zfar_ * znear_ /(znear_ - zfar_) ;
    result(3, 2) = -1 ;

    return result;
}

void Camera::lookAt(const Vector3f &eye, const Vector3f &center, const Vector3f &up) {
    Vector3f f = (center - eye).normalized();
    Vector3f s = f.cross(up).normalized();
    Vector3f u = s.cross(f) ;

    mat_ << s.x(), s.y(), s.z(), -s.dot(eye),
            u.x(), u.y(), u.z(), -u.dot(eye),
            -f.x(), -f.y(), -f.z(), f.dot(eye),
            0, 0, 0, 1 ;
}

void Camera::lookAt(const Vector3f &eye, const Vector3f &center, float roll) {
    lookAt(eye, center, Vector3f(0, 1, 0)) ;

    Affine3f rot ;
    rot.setIdentity();
    rot.rotate(AngleAxisf(roll, Eigen::Vector3f::UnitZ())) ;
    mat_ = mat_ * rot.matrix() ;
}

}


#define DEBUG 1

#include <iostream>
#include <iomanip>
#include <string>
//#include <cmath>
#include "Hubbard2D.hpp"

#ifdef USE_SPECTRA
#include <MatOp/SparseGenMatProd.h>
#include <SymEigsSolver.h>
#endif

#include <boost/random.hpp>
#include <boost/limits.hpp>
#include <ietl/interface/eigen3.h>
#include <ietl/vectorspace.h>
#include <ietl/lanczos.h>

///check if a file exists
bool fileExists(const std::string& fname){
  std::ifstream f(fname.c_str());
  bool isGood = f.good();
  f.close();
  return isGood;
}

int main(int argc, const char * argv[]) {
  int ne; //# of electrons
  double U;

  if(argc == 1){
    std::cerr<<"Using all defaults"<<std::endl;
    ne=2; //# of electrons
    U=4;
  }
  else if(argc >5 || (argc-1)%2){
    std::cout<<"Wrong input format!"<<std::endl;
    std::cout<<"-ne [#num electrons] -U [# for U/t]"<<std::endl;
    return 1;
  }
  else{
    ne=2;
    U=4;
    for(int i=1;i<argc;i+=2){
     if(std::string(argv[i]) == "-ne")
       ne = std::atoi(argv[i+1]);
     if(std::string(argv[i]) == "-U")
       U = std::atof(argv[i+1]);
    }
  }
  std::cout << "2D "<<BASISSIZE/2<<" site Hubbard model with "
  << ne <<" up/down electrons and U/t="<<U<<std::endl;

  HubbardModel2D H(ne,ne,1,U);
  H.makeBasis();
  std::cout <<"Made Basis: " << H.getBasis()->size()<< std::endl;

  H.buildHubbard2D();
  std::cout <<"Built Matrix" << std::endl;
  SpMat Hmat = *(H.getH());
  std::cout << "# of non-zero elements:"<<(H.getH())->nonZeros() << std::endl;
  H.getH()->makeCompressed();

  std::ofstream outfile;
  if(fileExists("energies.dat")){
    outfile.open("energies.dat",std::ofstream::out |std::ofstream::app);
  }
  else{
     outfile.open("energies.dat",std::ofstream::out |std::ofstream::app);
     outfile<<"#ne U E0"<<std::endl;
  }
  outfile << std::setprecision(9);

  std::ofstream outfile_v;
  if(fileExists("ketvals.dat")){
    outfile_v.open("ketvals.dat",std::ofstream::out |std::ofstream::app);
  }
  else{
    outfile_v.open("ketvals.dat",std::ofstream::out |std::ofstream::app);
    outfile_v <<"#ne U dbl up|down antiferro"<<std::endl;
  }
  outfile_v << std::setprecision(9);

  //construct test states
  size_t spinOffset = BASISSIZE/2;

  lattice_t psi1;
  int i;
  for(i=0;i<ne;i++)
    psi1[i].flip();

  for(i=0;i<ne;i++)
    psi1[(i+spinOffset)%(BASISSIZE)].flip();

  lattice_t psi2;
  i=0;

  for(i=0;i<ne;i++)
    psi2[i].flip();

  for(i=0;i<ne;i++)
    psi2[(i+ne+spinOffset)%(BASISSIZE)].flip();

  lattice_t psi3;
  i=0;
  for(i=0;i<ne;i++)
    psi3[2*i].flip();
  for(i=0;i<ne;i++)
    psi3[(2*i+1+spinOffset)%(BASISSIZE)].flip();
  //std::cout<<psi1<<std::endl;
  //std::cout<<psi2<<std::endl;
  //std::cout<<psi3<<std::endl;

  std::string double_occ="|";
  for(i=0;i<ne;i++)
    double_occ+="↑↓|";
  std::string not_occ="|";
  for(i=0;i<ne;i++)
    not_occ+="↑|";
  for(i=0;i<ne;i++)
    not_occ+="↓|";
  std::string antiferro="|";
  for(i=0;i<ne;i++)
    antiferro+="↑|↓|";

  //**************************************
  //        generate eigenvalues
  //**************************************

  //Diag with specrta
#ifdef USE_SPECTRA
  // Construct matrix operation object using the wrapper class SparseGenMatProd
  using wrapper_t =Spectra::SparseGenMatProd<double,Eigen::RowMajor,long>;
  wrapper_t op(Hmat);

  // Construct eigen solver object, requesting the smallest eigenvalue
  Spectra::SymEigsSolver< double, Spectra::SMALLEST_ALGE , wrapper_t > eigs(&op, 1, 6);
  // Initialize and compute
  eigs.init();
  int nconv = eigs.compute();

#ifdef DEBUG
  if(eigs.info() != Spectra::SUCCESSFUL)
      std::cout <<"Warning something failed " <<nconv << std::endl;
#endif

  std::cout<< "E0="<<eigs.eigenvalues()[0]<<std::endl;

  std::cout<<double_occ<<" :"<<eigs.eigenvectors()(H.getState(psi1),0)<<std::endl; //psi1
  std::cout<<not_occ<<" :"<<eigs.eigenvectors()(H.getState(psi2),0)<<std::endl; //psi2
#else
  typedef boost::lagged_fibonacci607 Gen;
  Gen mygen;
  mygen.seed(0);

  typedef Eigen::SparseMatrix<double,Eigen::RowMajor,long> Matrix;
  typedef Eigen::VectorXd Vector;
  typedef ietl::vectorspace<Vector> Vecspace;

  // Creation of an iteration object:
  int max_iter = 10*H.getBasis()->size();
  double rel_tol = 500*std::numeric_limits<double>::epsilon();
  double abs_tol = std::pow(std::numeric_limits<double>::epsilon(),2./3);
  int n_lowest_eigenval = 1;
  std::vector<double> eigen;
  std::vector<double> err;
  std::vector<int> multiplicity;

  std::vector<double> groundStates;

  Vecspace vec(Hmat.cols());
  ietl::lanczos<Matrix,Vecspace> lanczos(Hmat,vec);
  ietl::lanczos_iteration_nlowest<double>
  iter(max_iter, n_lowest_eigenval, rel_tol, abs_tol);
  std::cout << "Running lanczos" << std::endl;
  try{
    lanczos.calculate_eigenvalues(iter,mygen);
    eigen = lanczos.eigenvalues();
    err = lanczos.errors();
    multiplicity = lanczos.multiplicities();
    std::cout<<"number of iterations: "<<iter.iterations()<<"\n";
    groundStates.push_back(eigen[0]);
  }
  catch (std::runtime_error& e) {
    std::cout << e.what() << "\n";
  }
  std::cout << "#        eigenvalue            error         multiplicity\n";
  for (int i=0;i<10;++i)
    std::cout << i+1 << "\t" << eigen[i] << "\t" << err[i] << "\t"
    << multiplicity[i] << "\n";

  outfile<<ne<<" " <<U<< " "<< groundStates[0]<<std::endl;
  //**************************************
  //        generate eigenvectors
  //**************************************
  // call of eigenvectors function follows:
  std::cout << "\nHead of Eigenvector for the lowest eigenvalue:\n\n";
  std::vector<double>::iterator start = eigen.begin();
  std::vector<double>::iterator end = eigen.begin()+1;
  std::vector<Vector> eigenvectors; // for storing the eigen vectors.
  ietl::Info<double> info; // (m1, m2, ma, eigenvalue, residualm, status).

  try {
    lanczos.eigenvectors(start,end,std::back_inserter(eigenvectors),info,mygen,max_iter);
  }
  catch (std::runtime_error& e) {
    std::cout << e.what() << "\n";
  }
  std::cout << eigenvectors[0].head(10)<<std::endl;
  std::cout << "Information about the eigenvector computations:\n\n";
  for(int i = 0; i < info.size(); i++) {
    std::cout << " m1(" << i+1 << "): " << info.m1(i) << ", m2(" << i+1 << "): "
    << info.m2(i) << ", ma(" << i+1 << "): " << info.ma(i) << " eigenvalue("
    << i+1 << "): " << info.eigenvalue(i) << " residual(" << i+1 << "): "
    << info.residual(i) << " error_info(" << i+1 << "): "
    << info.error_info(i) << "\n\n";
  }

  std::cout<<double_occ<<" :"<<eigenvectors[0](H.getState(psi1))<<std::endl; //psi1
  std::cout<<not_occ<<" :"<<eigenvectors[0](H.getState(psi2))<<std::endl; //psi2
  std::cout<<antiferro<<" :"<<eigenvectors[0](H.getState(psi3))<<std::endl; //psi3

  outfile_v << ne << " "<<U<<" " << eigenvectors[0](H.getState(psi1)) << " "
            <<eigenvectors[0](H.getState(psi2))<<" " <<eigenvectors[0](H.getState(psi3))<<std::endl;
#endif

  return 0;
}


#include "fast_particle_slam.h"
#include "robot_configuration.h"
#include "error_handling.h"
#include "occupancy_grid.inl"

#include <boost/range/algorithm/max_element.hpp>
#include <boost/range/adaptor/transformed.hpp>

#include <opencv2/imgproc.hpp>
#include <iostream>
#include <random>
#include <future>

// Based on Grisetti, Stachniss, Burgard
// "Improving Grid-based SLAM with Rao-Blackwellized Particle Filters by Adaptive Proposals and Selective Resampling"
// and their implementation at https://openslam.org/gmapping.html
SFastSlamParticle::SFastSlamParticle() : m_pose(rbt::pose<double>::zero()) {}

void SFastSlamParticle::updatePose(SScanLine const& scanline) {
    // 1. Update particles with probabilistic motion model
    auto poseSampled = sample_motion_model(m_pose, scanline.translation(), scanline.rotation());

    // 2. If not first update (and optionally: enough distance traveled since last update)
    //    scan match and update particle pose
    m_pose = m_occgrid.fit(poseSampled, scanline);

    // 3. Compute likelihood of resulting match
    // gmapping computes log likelihood, also skips distanceTransform and searches
    // in small kernel around expected obstacle
    m_fLogWeight += log_likelihood_field(m_pose, scanline, m_occgrid);

    LOG("Update Particle: poseSampled = " << poseSampled << " m_pose = " << m_pose << " m_fLogWeight = " << m_fLogWeight << "\n");
}

void SFastSlamParticle::updateMap(SScanLine const& scanline) {
    boost::for_each(scanline.m_vecscan, [&](auto const& scan) {
        m_occgrid.update(m_pose, scan.m_fRadAngle, scan.m_nDistance);
    });
}

CFastParticleSlamBase::CFastParticleSlamBase(int cParticles)
    : m_vecparticle(cParticles), m_itparticleBest(m_vecparticle.begin()), m_fNEff(1.0)
{}

static std::random_device s_rd;
void CFastParticleSlamBase::receivedSensorData(SScanLine const& scanline) {
     LOG("=== Update === ");
     LOG("t = " << scanline.translation() << " phi = " << scanline.rotation());

    {
        std::vector<std::future<void>> vecfuture;
        boost::for_each(m_vecparticle, [&](auto& p) {
            vecfuture.emplace_back(
                std::async(std::launch::async,
                    [&] {
                        p.updatePose(scanline);
                    }
                ));
        });
    }

    // 4. Normalize weights (see GridSlamProcessor::normalize())
    {
        // TODO: m_obsSigmaGain
        double const fGain = 1. / ( 3 * /* = m_obsSigmaGain */ m_vecparticle.size());
        auto const fMax = *boost::max_element(boost::adaptors::transform(m_vecparticle, std::mem_fn(&SFastSlamParticle::m_fLogWeight)));

        double fWeightSum = 0;
        boost::for_each(m_vecparticle, [&](auto& p) {
            p.m_fWeight = std::exp(fGain * (p.m_fLogWeight - fMax));
            fWeightSum += p.m_fWeight;
        });

        m_fNEff = 0;
        boost::for_each(m_vecparticle, [&](auto& p) {
            p.m_fWeight /= fWeightSum;
            m_fNEff += p.m_fWeight*p.m_fWeight;
        });
        m_fNEff = 1.0 / m_fNEff;
    }

    // 5. If neff < threshold, resample
    if(m_fNEff<0.5 * m_vecparticle.size()) {
        LOG("============ Resample ============");
        // Resampling
        // Thrun, Probabilistic robotics, p. 110
        auto const fStepSize = 1.0/m_vecparticle.size();
        auto const r = std::uniform_real_distribution<double>(0.0, fStepSize)(s_rd);
        auto c = m_vecparticle.front().m_fWeight;

        std::vector<int> veciparticle;
        for(int i = 0, m = 0; m<m_vecparticle.size(); ++m) {
            auto const u = r + m * fStepSize;
            while(c<u) {
                ++i;
                c += m_vecparticle[i].m_fWeight;
            }
            veciparticle.emplace_back(i);
            LOG("Keep particle " << i);
        }

        std::vector<SFastSlamParticle> vecparticle(m_vecparticle.size());
        auto itparticleOut = vecparticle.begin();
        for(auto itn = veciparticle.begin(); itn!=veciparticle.end(); ++itn) {
            if(boost::next(itn)==veciparticle.end() || *itn!=*boost::next(itn)) {
                *itparticleOut = std::move(m_vecparticle[*itn]);
            } else {
                *itparticleOut = m_vecparticle[*itn];
            }
            itparticleOut->m_fLogWeight = 0.0;
            ++itparticleOut;
        }
        std::swap(m_vecparticle, vecparticle);
    }

    m_itparticleBest = boost::max_element(
        boost::adaptors::transform(m_vecparticle, std::mem_fn(&SFastSlamParticle::m_fWeight))
    ).base();
    m_vecpose.emplace_back(m_itparticleBest->m_pose);

    {
        std::vector<std::future<void>> vecfuture;
        boost::for_each(m_vecparticle, [&](auto& p) {
            vecfuture.emplace_back(
                std::async(std::launch::async,
                    [&] {
                        p.updateMap(scanline);
                    }
                ));
        });
    }
}

cv::Mat CFastParticleSlamBase::getMapWithPoses() const {
    ASSERT(m_itparticleBest!=m_vecparticle.end());
    return m_itparticleBest->m_occgrid.ObstacleMapWithPoses(m_vecpose);
}

cv::Mat CFastParticleSlamBase::getMap() const {
    ASSERT(m_itparticleBest!=m_vecparticle.end());
    return m_itparticleBest->m_occgrid.ObstacleMap();
}

cv::Mat CFastParticleSlamBase::getMapWithPose() const {
    ASSERT(m_itparticleBest!=m_vecparticle.end());
    cv::Mat mat = m_itparticleBest->m_occgrid.ObstacleMap();
    cv::Mat matColor;
    cvtColor(mat, matColor, CV_GRAY2RGB);
    RenderRobotPose(matColor, m_vecpose.back(), cv::Scalar(255, 0, 0));
    return matColor;
}


// 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.

// Written by Cornelius Steinhardt

#ifndef ITL_QMR_INCLUDE
#define ITL_QMR_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/operation/trans.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

#include <boost/numeric/itl/krylov/base_solver.hpp>

namespace itl {

/// Quasi-Minimal Residual method
template < typename Matrix, typename Vector,typename LeftPreconditioner,
	   typename RightPreconditioner, typename Iteration >
int qmr(const Matrix& A, Vector& x, const Vector& b, LeftPreconditioner& L,
	const RightPreconditioner& R, Iteration& iter)
{
    mtl::vampir_trace<7008> tracer;
    using mtl::size;
    typedef typename mtl::Collection<Vector>::value_type Scalar;
    if (size(b) == 0) throw mtl::logic_error("empty rhs vector");

    const Scalar         zero= math::zero(Scalar()), one= math::one(Scalar());
    Scalar               beta, gamma(one), gamma_1, delta, eta(-one), ep(one), rho_1, theta(zero), theta_1;
    Vector               r(b - A * x), v_tld(r), y(solve(L, v_tld)), w_tld(r), z(adjoint_solve(R,w_tld)), v(resource(x)), w(resource(x)),
                         y_tld(resource(x)), z_tld(resource(x)), p(resource(x)), q(resource(x)), p_tld(resource(x)), d(resource(x)), s(resource(x));

    if (iter.finished(r))
	return iter;

    Scalar rho = two_norm(y), xi = two_norm(z);
    while(! iter.finished(rho)) {
	++iter;
        if (rho == zero)
	    return iter.fail(1, "qmr breakdown #1, rho=0");
        if (xi == zero)
            return iter.fail(2, "qmr breakdown #2, xi=0");

        v= v_tld / rho;
        y/= rho;
        w= w_tld / xi;
        z/= xi;

        delta = dot(z,y);
        if (delta == zero)
            return iter.fail(3, "qmr breakdown, delta=0 #3");

        y_tld = solve(R,y);
        z_tld = adjoint_solve(L,z);

	if (iter.first()) {
            p = y_tld;
            q = z_tld;
	} else {
            p = y_tld - ((xi * delta) / ep) * p;
            q = z_tld - ((rho* delta) / ep) * q;
        }

        p_tld = A * p;
        ep = dot(q, p_tld);
        if (ep == zero)
            return iter.fail(4, "qmr breakdown ep=0 #4");
        beta= ep / delta;
        if (beta == zero)
            return iter.fail(5, "qmr breakdown beta=0 #5");
        v_tld = p_tld - beta * v;
        y = solve(L,v_tld);
        rho_1 = rho;
	rho = two_norm(y);
        w_tld= trans(A)*q  - beta*w;
        z = adjoint_solve(R, w_tld);
        xi = two_norm(z);
        gamma_1 = gamma;
        theta_1 = theta;
        theta = rho / (gamma_1 * beta);
        gamma = one / (sqrt(one + theta * theta));

        if (gamma == zero)
            return iter.fail(6, "qmr breakdown gamma=0 #6");

        eta= -eta * rho_1 * gamma * gamma / (beta * gamma_1 * gamma_1);
	if (iter.first()) {
           d= eta * p;
	   s= eta * p_tld;
	} else {
            d= eta * p + (theta_1 * theta_1 * gamma * gamma) * d;
            s= eta * p_tld + (theta_1 * theta_1 * gamma * gamma) * s;
        }
        x += d;
        r -= s;
    }
    return iter;
}

/// Solver class for Quasi-minimal residual method; right preconditioner ignored (prints warning if not identity)
/** Methods inherited from \ref base_solver. **/
template < typename LinearOperator, typename Preconditioner= pc::identity<LinearOperator>,
	   typename RightPreconditioner= pc::identity<LinearOperator> >
class qmr_solver
  : public base_solver< qmr_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator >
{
    typedef base_solver< qmr_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator > base;
  public:
    /// Construct solver from a linear operator; generate (left) preconditioner from it
    explicit qmr_solver(const LinearOperator& A) : base(A), L(A), R(A) {}

    /// Construct solver from a linear operator and left preconditioner
    qmr_solver(const LinearOperator& A, const Preconditioner& L) : base(A), L(L), R(A) {}

    /// Construct solver from a linear operator and left preconditioner
    qmr_solver(const LinearOperator& A, const Preconditioner& L, const RightPreconditioner& R)
      : base(A), L(L), R(R) {}

    /// Solve linear system approximately as specified by \p iter
    template < typename HilbertSpaceX, typename HilbertSpaceB, typename Iteration >
    int solve(HilbertSpaceX& x, const HilbertSpaceB& b, Iteration& iter) const
    {
	return qmr(this->A, x, b, L, R, iter);
    }

  private:
    Preconditioner        L;
    RightPreconditioner   R;
};

} // namespace itl

#endif // ITL_QMR_INCLUDE