#ifndef _CELERITE2_INTERNAL_HPP_DEFINED_
#define _CELERITE2_INTERNAL_HPP_DEFINED_

#include <Eigen/Core>

namespace celerite2 {
namespace core {

#define UNUSED(x) (void)(x)

#define CAST_BASE(TYPE, VAR) Eigen::MatrixBase<TYPE> &VAR = const_cast<Eigen::MatrixBase<TYPE> &>(VAR##_out)

#define CAST_VEC(TYPE, VAR, ROWS)                                                                                \
  CAST_BASE(TYPE, VAR);                                                                                \
  VAR.derived().resize(ROWS)

#define CAST_MAT(TYPE, VAR, ROWS, COLS)                                                                                \
  CAST_BASE(TYPE, VAR);                                                                                \
  VAR.derived().resize(ROWS, COLS)

const int THE_WORKSPACE_VARIABLE_MUST_BE_ROW_MAJOR = 0;
#define ASSERT_ROW_MAJOR(TYPE) EIGEN_STATIC_ASSERT((TYPE::ColsAtCompileTime == 1) || TYPE::IsRowMajor, THE_WORKSPACE_VARIABLE_MUST_BE_ROW_MAJOR)

namespace internal {

template <bool do_update = true>
struct update_workspace {
  template <typename A, typename B>
  static void apply(Eigen::Index n, const Eigen::MatrixBase<A> &a, Eigen::MatrixBase<B> const &b_out) {
    CAST_BASE(B, b);
    b.row(n) = a;
  }
};

template <>
struct update_workspace<false> {
  template <typename A, typename B>
  static void apply(Eigen::Index n, const Eigen::MatrixBase<A> &a, Eigen::MatrixBase<B> const &b_out) {
    UNUSED(n);
    UNUSED(a);
    UNUSED(b_out);
  }
};

template <bool is_solve = false>
struct update_f {
  template <typename A, typename B, typename C, typename D>
  static void apply(const Eigen::MatrixBase<A> &a, const Eigen::MatrixBase<B> &b, const Eigen::MatrixBase<C> &c, Eigen::MatrixBase<D> const &d_out) {
    CAST_BASE(D, d);
    d.noalias() += a * b;
    UNUSED(c);
  }

  template <typename A, typename B, typename C, typename D, typename E, typename F, typename G>
  static void reverse(const Eigen::MatrixBase<A> &a, const Eigen::MatrixBase<B> &b, const Eigen::MatrixBase<C> &c, const Eigen::MatrixBase<D> &d,
                      Eigen::MatrixBase<E> const &e_out, Eigen::MatrixBase<F> const &f_out, Eigen::MatrixBase<G> const &g_out) {
    CAST_BASE(E, e);
    CAST_BASE(F, f);
    e.noalias() += b * d.transpose();
    f.noalias() += a * d;
    UNUSED(c);
    UNUSED(g_out);
  }
};

template <>
struct update_f<true> {
  template <typename A, typename B, typename C, typename D>
  static void apply(const Eigen::MatrixBase<A> &a, const Eigen::MatrixBase<B> &b, const Eigen::MatrixBase<C> &c, Eigen::MatrixBase<D> const &d_out) {
    CAST_BASE(D, d);
    d.noalias() += a * c;
    UNUSED(b);
  }

  template <typename A, typename B, typename C, typename D, typename E, typename F, typename G>
  static void reverse(const Eigen::MatrixBase<A> &a, const Eigen::MatrixBase<B> &b, const Eigen::MatrixBase<C> &c, const Eigen::MatrixBase<D> &d,
                      Eigen::MatrixBase<E> const &e_out, Eigen::MatrixBase<F> const &f_out, Eigen::MatrixBase<G> const &g_out) {
    CAST_BASE(E, e);
    CAST_BASE(G, g);
    e.noalias() += c * d.transpose();
    g.noalias() += a * d;
    UNUSED(b);
    UNUSED(f_out);
  }
};

template <bool is_solve = false>
struct update_z {
  template <typename A, typename B>
  static void apply(const Eigen::MatrixBase<A> &a, Eigen::MatrixBase<B> const &b_out) {
    CAST_BASE(B, b);
    b.noalias() += a;
  }
};

template <>
struct update_z<true> {
  template <typename A, typename B>
  static void apply(const Eigen::MatrixBase<A> &a, Eigen::MatrixBase<B> const &b_out) {
    CAST_BASE(B, b);
    b.noalias() -= a;
  }
};

template <bool is_solve, bool do_update = true, typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOut,
          typename Work>
void forward(const Eigen::MatrixBase<Input> &t,                // (N,)
             const Eigen::MatrixBase<Coeffs> &c,               // (J,)
             const Eigen::MatrixBase<LowRank> &U,              // (N, J)
             const Eigen::MatrixBase<LowRank> &V,              // (N, J)
             const Eigen::MatrixBase<RightHandSide> &Y,        // (N, Nrhs)
             Eigen::MatrixBase<RightHandSideOut> const &Z_out, // (N, Nrhs)
             Eigen::MatrixBase<Work> const &F_out              // (N, J * Nrhs)
) {
  ASSERT_ROW_MAJOR(Work);

  typedef typename LowRank::Scalar Scalar;
  typedef typename Eigen::internal::plain_row_type<RightHandSide>::type RowVector;
  typedef typename Eigen::internal::plain_col_type<Coeffs>::type CoeffVector;
  typedef typename Eigen::Matrix<Scalar, LowRank::ColsAtCompileTime, RightHandSide::ColsAtCompileTime> Inner;

  Eigen::Index N = U.rows(), J = U.cols(), nrhs = Y.cols();
  CAST_BASE(RightHandSideOut, Z); // Must already be the right shape
  CAST_BASE(Work, F);
  if (do_update) {
    F.derived().resize(N, J * nrhs);
    F.row(0).setZero();
  }

  CoeffVector p(J);
  Inner Fn(J, nrhs);
  Eigen::Map<typename Eigen::internal::plain_row_type<Work>::type> ptr(Fn.data(), 1, J * nrhs);

  // This will track the previous row allowing for inplace operations
  RowVector tmp = Y.row(0);

  Fn.setZero();
  for (Eigen::Index n = 1; n < N; ++n) {
    p = exp(c.array() * (t(n - 1) - t(n)));
    update_f<is_solve>::apply(V.row(n - 1).transpose(), tmp, Z.row(n - 1), Fn);
    tmp = Y.row(n);
    update_workspace<do_update>::apply(n, ptr, F);
    Fn = p.asDiagonal() * Fn;
    update_z<is_solve>::apply(U.row(n) * Fn, Z.row(n));
  }
}

template <bool is_solve, bool do_update = true, typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOut,
          typename Work>
void backward(const Eigen::MatrixBase<Input> &t,                // (N,)
              const Eigen::MatrixBase<Coeffs> &c,               // (J,)
              const Eigen::MatrixBase<LowRank> &U,              // (N, J)
              const Eigen::MatrixBase<LowRank> &V,              // (N, J)
              const Eigen::MatrixBase<RightHandSide> &Y,        // (N, Nrhs)
              Eigen::MatrixBase<RightHandSideOut> const &Z_out, // (N, Nrhs)
              Eigen::MatrixBase<Work> const &F_out              // (N, J * Nrhs)
) {
  ASSERT_ROW_MAJOR(Work);

  typedef typename LowRank::Scalar Scalar;
  typedef typename Eigen::internal::plain_row_type<RightHandSide>::type RowVector;
  typedef typename Eigen::internal::plain_col_type<Coeffs>::type CoeffVector;
  typedef typename Eigen::Matrix<Scalar, LowRank::ColsAtCompileTime, RightHandSide::ColsAtCompileTime> Inner;

  Eigen::Index N = U.rows(), J = U.cols(), nrhs = Y.cols();
  CAST_BASE(RightHandSideOut, Z); // Must already be the right shape
  CAST_BASE(Work, F);
  if (do_update) {
    F.derived().resize(N, J * nrhs);
    F.row(N - 1).setZero();
  }

  CoeffVector p(J);
  Inner Fn(J, nrhs);
  Eigen::Map<typename Eigen::internal::plain_row_type<Work>::type> ptr(Fn.data(), 1, J * nrhs);

  // This will track the previous row allowing for inplace operations
  RowVector tmp = Y.row(N - 1);

  Fn.setZero();
  for (Eigen::Index n = N - 2; n >= 0; --n) {
    p = exp(c.array() * (t(n) - t(n + 1)));
    update_f<is_solve>::apply(U.row(n + 1).transpose(), tmp, Z.row(n + 1), Fn);
    tmp = Y.row(n);
    update_workspace<do_update>::apply(n, ptr, F);
    Fn = p.asDiagonal() * Fn;
    update_z<is_solve>::apply(V.row(n) * Fn, Z.row(n));
  }
}

template <bool is_solve, typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename RightHandSideOptional, typename Work,
          typename RightHandSideInternal, typename InputOut, typename CoeffsOut, typename LowRankOut, typename RightHandSideOut>
void forward_rev(const Eigen::MatrixBase<Input> &t,                      // (N,)
                 const Eigen::MatrixBase<Coeffs> &c,                     // (J,)
                 const Eigen::MatrixBase<LowRank> &U,                    // (N, J)
                 const Eigen::MatrixBase<LowRank> &V,                    // (N, J)
                 const Eigen::MatrixBase<RightHandSideOptional> &Y,      // (N, Nrhs)
                 const Eigen::MatrixBase<RightHandSide> &Z,              // (N, Nrhs)
                 const Eigen::MatrixBase<Work> &F,                       // (N, J * Nrhs)
                 Eigen::MatrixBase<RightHandSideInternal> const &bZ_out, // (N, Nrhs)
                 Eigen::MatrixBase<InputOut> const &bt_out,              // (N,)
                 Eigen::MatrixBase<CoeffsOut> const &bc_out,             // (J,)
                 Eigen::MatrixBase<LowRankOut> const &bU_out,            // (N, J)
                 Eigen::MatrixBase<LowRankOut> const &bV_out,            // (N, J)
                 Eigen::MatrixBase<RightHandSideOut> const &bY_out       // (N, Nrhs)  -  Must be the right shape already (and zeroed)
) {
  ASSERT_ROW_MAJOR(Work);

  typedef typename LowRank::Scalar Scalar;
  typedef typename Eigen::internal::plain_col_type<Coeffs>::type CoeffVector;
  typedef typename Eigen::Matrix<Scalar, LowRank::ColsAtCompileTime, RightHandSide::ColsAtCompileTime> Inner;

  Eigen::Index N = U.rows(), J = U.cols(), nrhs = Y.cols();
  CAST_VEC(InputOut, bt, N);
  CAST_VEC(CoeffsOut, bc, J);
  CAST_MAT(LowRankOut, bU, N, J);
  CAST_MAT(LowRankOut, bV, N, J);
  CAST_BASE(RightHandSideOut, bY);
  CAST_BASE(RightHandSideInternal, bZ);

  Scalar dt, factor;
  CoeffVector p(J), bp(J);
  Inner Fn(J, nrhs), bF(J, nrhs);
  Eigen::Map<typename Eigen::internal::plain_row_type<Work>::type> ptr(Fn.data(), 1, J * nrhs);
  bF.setZero();
  for (Eigen::Index n = N - 1; n >= 1; --n) {
    dt  = t(n - 1) - t(n);
    p   = exp(c.array() * dt);
    ptr = F.row(n);

    // Reverse: update_z<is_solve>::apply(U.row(n) * Fn, Z.row(n));
    update_z<is_solve>::apply(bZ.row(n) * (p.asDiagonal() * Fn).transpose(), bU.row(n));
    update_z<is_solve>::apply(U.row(n).transpose() * bZ.row(n), bF);

    // Reverse: Fn = P.row(n - 1).asDiagonal() * Fn;
    bp.array() = (Fn * bF.transpose()).diagonal().array() * p.array();
    bc.noalias() += dt * bp;
    factor = (c.array() * bp.array()).sum();
    bt(n) -= factor;
    bt(n - 1) += factor;
    bF = p.asDiagonal() * bF;

    // Reverse: update_f<is_solve>::apply(V.row(n - 1).transpose(), Y.row(n - 1), Z.row(n - 1), Fn);
    update_f<is_solve>::reverse(V.row(n - 1), Y.row(n - 1), Z.row(n - 1), bF, bV.row(n - 1), bY.row(n - 1), bZ.row(n - 1));
  }
}

template <bool is_solve, typename Input, typename Coeffs, typename LowRank, typename RightHandSide, typename Work, typename RightHandSideInternal,
          typename InputOut, typename CoeffsOut, typename LowRankOut, typename RightHandSideOut>
void backward_rev(const Eigen::MatrixBase<Input> &t,                      // (N,)
                  const Eigen::MatrixBase<Coeffs> &c,                     // (J,)
                  const Eigen::MatrixBase<LowRank> &U,                    // (N, J)
                  const Eigen::MatrixBase<LowRank> &V,                    // (N, J)
                  const Eigen::MatrixBase<RightHandSide> &Y,              // (N, Nrhs)
                  const Eigen::MatrixBase<RightHandSide> &Z,              // (N, Nrhs)
                  const Eigen::MatrixBase<Work> &F,                       // (N, J * Nrhs)
                  Eigen::MatrixBase<RightHandSideInternal> const &bZ_out, // (N, Nrhs)
                  Eigen::MatrixBase<InputOut> const &bt_out,              // (N,)
                  Eigen::MatrixBase<CoeffsOut> const &bc_out,             // (J,)
                  Eigen::MatrixBase<LowRankOut> const &bU_out,            // (N, J)
                  Eigen::MatrixBase<LowRankOut> const &bV_out,            // (N, J)
                  Eigen::MatrixBase<RightHandSideOut> const &bY_out       // (N, Nrhs)  -  Must be the right shape already (and zeroed)
) {
  ASSERT_ROW_MAJOR(Work);

  typedef typename LowRank::Scalar Scalar;
  typedef typename Eigen::internal::plain_col_type<Coeffs>::type CoeffVector;
  typedef typename Eigen::Matrix<Scalar, LowRank::ColsAtCompileTime, RightHandSide::ColsAtCompileTime> Inner;

  Eigen::Index N = U.rows(), J = U.cols(), nrhs = Y.cols();
  CAST_VEC(InputOut, bt, N);
  CAST_VEC(CoeffsOut, bc, J);
  CAST_MAT(LowRankOut, bU, N, J);
  CAST_MAT(LowRankOut, bV, N, J);
  CAST_BASE(RightHandSideOut, bY);
  CAST_BASE(RightHandSideInternal, bZ);

  Scalar dt, factor;
  CoeffVector p(J), bp(J);
  Inner Fn(J, nrhs), bF(J, nrhs);
  Eigen::Map<typename Eigen::internal::plain_row_type<Work>::type> ptr(Fn.data(), 1, J * nrhs);
  bF.setZero();
  for (Eigen::Index n = 0; n <= N - 2; ++n) {
    dt  = t(n) - t(n + 1);
    p   = exp(c.array() * dt);
    ptr = F.row(n);

    // Reverse: update_z<is_solve>::apply(V.row(n) * Fn, Z.row(n));
    update_z<is_solve>::apply(bZ.row(n) * (p.asDiagonal() * Fn).transpose(), bV.row(n));
    update_z<is_solve>::apply(V.row(n).transpose() * bZ.row(n), bF);

    // Reverse: Fn = P.row(n).asDiagonal() * Fn;
    bp.array() = (Fn * bF.transpose()).diagonal().array() * p.array();
    bc.noalias() += dt * bp;
    factor = (c.array() * bp.array()).sum();
    bt(n + 1) -= factor;
    bt(n) += factor;
    bF = p.asDiagonal() * bF;

    // Reverse: update_f<is_solve>::apply(U.row(n + 1).transpose(), Y.row(n + 1), Z.row(n + 1), Fn);
    update_f<is_solve>::reverse(U.row(n + 1), Y.row(n + 1), Z.row(n + 1), bF, bU.row(n + 1), bY.row(n + 1), bZ.row(n + 1));
  }
}

} // namespace internal

} // namespace core
} // namespace celerite2

#endif // _CELERITE2_INTERNAL_HPP_DEFINED_


#pragma once
#include "../tags/SAT.hpp"
#include <boost/mpl/int.hpp>
#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/include/as_vector.hpp>
#include <boost/fusion/include/joint_view.hpp>
#include <boost/fusion/include/single_view.hpp>

#include <boost/proto/core.hpp>

namespace metaSMT {
  namespace transform {
    namespace proto = boost::proto;

    struct satClauseArity
      : proto::or_<
            proto::when<proto::terminal<proto::_>, boost::mpl::int_<1>()>
          , proto::otherwise<proto::fold<proto::_, boost::mpl::int_<0>(), boost::mpl::plus<proto::_state, satClauseArity>()> >
      > {};

    struct satClause
      : proto::or_<
        proto::when<
          proto::terminal<proto::_>
          , boost::fusion::single_view<
            proto::_value
          > (
            proto::_value
          )
        >
        , proto::when<
            proto::plus<satClause, satClause>
          , boost::fusion::joint_view<
              boost::add_const<satClause(proto::_left) >
            , boost::add_const<satClause(proto::_right) >
          >(satClause(proto::_left), satClause(proto::_right))
        >
        , proto::when<
            proto::negate< proto::negate<proto::_> >
          , satClause(proto::_child(proto::_child))
        >
        , proto::when<
            proto::negate< proto::terminal<proto::_> >
          , boost::fusion::single_view<
              proto::call<std::negate<SAT::tag::lit_tag> >(
                proto::_value(proto::_child)
              )
            > (
              proto::call<std::negate<SAT::tag::lit_tag> >(
                proto::_value(proto::_child)
              )
            )
        >
     > {};

  } /* transform */
} /* metaSMT */


#include "functions/variables.hh"
#include <boost/test/unit_test.hpp>

template <int N> struct Test {
  bool operator==(const Test &o) const { return true; }
};

template <int N> std::ostream &operator<<(std::ostream &s, Test<N>) {
  return s << "Test<" << N << ">";
}

BOOST_AUTO_TEST_CASE(test_variables) {
  Test<0> a;
  Test<1> b;
  Test<2> c;
  Test<3> d;
  Test<4> e;
  Test<5> f;

  using namespace manifolds;

  BOOST_CHECK_EQUAL(x(a, b, c, d, e, f), a);
  BOOST_CHECK_EQUAL(y(a, b, c, d, e, f), b);
  BOOST_CHECK_EQUAL(z(a, b, c, d, e, f), c);
  BOOST_CHECK_EQUAL(t(a, b, c, d, e, f), d);
  BOOST_CHECK_EQUAL(Variable<4>()(a, b, c, d, e, f), e);
  BOOST_CHECK_EQUAL(Variable<5>()(a, b, c, d, e, f), f);
}


//  Copyright John Maddock 2005-2008.
//  Copyright (c) 2006-2008 Johan Rade
//  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_MATH_FPCLASSIFY_HPP
#define BOOST_MATH_FPCLASSIFY_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <math.h>
#include <boost/config/no_tr1/cmath.hpp>
#include <boost/limits.hpp>
#include <boost/math/tools/real_cast.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/detail/fp_traits.hpp>
/*!
  \file fpclassify.hpp
  \brief Classify floating-point value as normal, subnormal, zero, infinite, or NaN.
  \version 1.0
  \author John Maddock
 */

/*

1. If the platform is C99 compliant, then the native floating point
classification functions are used.  However, note that we must only
define the functions which call std::fpclassify etc if that function
really does exist: otherwise a compiler may reject the code even though
the template is never instantiated.

2. If the platform is not C99 compliant, and the binary format for
a floating point type (float, double or long double) can be determined
at compile time, then the following algorithm is used:

        If all exponent bits, the flag bit (if there is one),
        and all significand bits are 0, then the number is zero.

        If all exponent bits and the flag bit (if there is one) are 0,
        and at least one significand bit is 1, then the number is subnormal.

        If all exponent bits are 1 and all significand bits are 0,
        then the number is infinity.

        If all exponent bits are 1 and at least one significand bit is 1,
        then the number is a not-a-number.

        Otherwise the number is normal.

        This algorithm works for the IEEE 754 representation,
        and also for several non IEEE 754 formats.

    Most formats have the structure
        sign bit + exponent bits + significand bits.

    A few have the structure
        sign bit + exponent bits + flag bit + significand bits.
    The flag bit is 0 for zero and subnormal numbers,
        and 1 for normal numbers and NaN.
        It is 0 (Motorola 68K) or 1 (Intel) for infinity.

    To get the bits, the four or eight most significant bytes are copied
    into an uint32_t or uint64_t and bit masks are applied.
    This covers all the exponent bits and the flag bit (if there is one),
    but not always all the significand bits.
    Some of the functions below have two implementations,
    depending on whether all the significand bits are copied or not.

3. If the platform is not C99 compliant, and the binary format for
a floating point type (float, double or long double) can not be determined
at compile time, then comparison with std::numeric_limits values
is used.

*/

#if defined(_MSC_VER) || defined(__BORLANDC__)
#include <float.h>
#endif

#ifdef BOOST_NO_STDC_NAMESPACE
  namespace std{ using ::abs; using ::fabs; }
#endif

namespace boost{

#if (defined(BOOST_HAS_FPCLASSIFY) || defined(isnan)) && !defined(BOOST_MATH_DISABLE_STD_FPCLASSIFY)
//
// This must not be located in any namespace under boost::math
// otherwise we can get into an infinite loop if isnan is
// a #define for "isnan" !
//
namespace math_detail{

template <class T>
inline bool is_nan_helper(T t, const boost::true_type&)
{
#ifdef isnan
   return isnan(t);
#else // BOOST_HAS_FPCLASSIFY
   return (BOOST_FPCLASSIFY_PREFIX fpclassify(t) == (int)FP_NAN);
#endif
}

template <class T>
inline bool is_nan_helper(T, const boost::false_type&)
{
   return false;
}

}

#endif // defined(BOOST_HAS_FPCLASSIFY) || defined(isnan)

namespace math{

namespace detail{

#ifdef BOOST_MATH_USE_STD_FPCLASSIFY
template <class T>
inline int fpclassify_imp BOOST_NO_MACRO_EXPAND(T t, const native_tag&)
{
   return (std::fpclassify)(t);
}
#endif

template <class T>
inline int fpclassify_imp BOOST_NO_MACRO_EXPAND(T t, const generic_tag<true>&)
{
   BOOST_MATH_INSTRUMENT_VARIABLE(t);

   // whenever possible check for Nan's first:
#if defined(BOOST_HAS_FPCLASSIFY)  && !defined(BOOST_MATH_DISABLE_STD_FPCLASSIFY)
   if(::boost::math_detail::is_nan_helper(t, ::boost::is_floating_point<T>()))
      return FP_NAN;
#elif defined(isnan)
   if(boost::math_detail::is_nan_helper(t, ::boost::is_floating_point<T>()))
      return FP_NAN;
#elif defined(_MSC_VER) || defined(__BORLANDC__)
   if(::_isnan(boost::math::tools::real_cast<double>(t)))
      return FP_NAN;
#endif
   // std::fabs broken on a few systems especially for long long!!!!
   T at = (t < T(0)) ? -t : t;

   // Use a process of exclusion to figure out
   // what kind of type we have, this relies on
   // IEEE conforming reals that will treat
   // Nan's as unordered.  Some compilers
   // don't do this once optimisations are
   // turned on, hence the check for nan's above.
   if(at <= (std::numeric_limits<T>::max)())
   {
      if(at >= (std::numeric_limits<T>::min)())
         return FP_NORMAL;
      return (at != 0) ? FP_SUBNORMAL : FP_ZERO;
   }
   else if(at > (std::numeric_limits<T>::max)())
      return FP_INFINITE;
   return FP_NAN;
}

template <class T>
inline int fpclassify_imp BOOST_NO_MACRO_EXPAND(T t, const generic_tag<false>&)
{
#ifdef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   if(std::numeric_limits<T>::is_specialized)
      return fp_classify_imp(t, mpl::true_());
#endif
   //
   // An unknown type with no numeric_limits support,
   // so what are we supposed to do we do here?
   //
   BOOST_MATH_INSTRUMENT_VARIABLE(t);

   return t == 0 ? FP_ZERO : FP_NORMAL;
}

template<class T>
int fpclassify_imp BOOST_NO_MACRO_EXPAND(T x, ieee_copy_all_bits_tag)
{
   typedef BOOST_DEDUCED_TYPENAME fp_traits<T>::type traits;

   BOOST_MATH_INSTRUMENT_VARIABLE(x);

   BOOST_DEDUCED_TYPENAME traits::bits a;
   traits::get_bits(x,a);
   BOOST_MATH_INSTRUMENT_VARIABLE(a);
   a &= traits::exponent | traits::flag | traits::significand;
   BOOST_MATH_INSTRUMENT_VARIABLE((traits::exponent | traits::flag | traits::significand));
   BOOST_MATH_INSTRUMENT_VARIABLE(a);

   if(a <= traits::significand) {
      if(a == 0)
         return FP_ZERO;
      else
         return FP_SUBNORMAL;
   }

   if(a < traits::exponent) return FP_NORMAL;

   a &= traits::significand;
   if(a == 0) return FP_INFINITE;

   return FP_NAN;
}

template<class T>
int fpclassify_imp BOOST_NO_MACRO_EXPAND(T x, ieee_copy_leading_bits_tag)
{
   typedef BOOST_DEDUCED_TYPENAME fp_traits<T>::type traits;

   BOOST_MATH_INSTRUMENT_VARIABLE(x);

   BOOST_DEDUCED_TYPENAME traits::bits a;
   traits::get_bits(x,a);
   a &= traits::exponent | traits::flag | traits::significand;

   if(a <= traits::significand) {
      if(x == 0)
         return FP_ZERO;
      else
         return FP_SUBNORMAL;
   }

   if(a < traits::exponent) return FP_NORMAL;

   a &= traits::significand;
   traits::set_bits(x,a);
   if(x == 0) return FP_INFINITE;

   return FP_NAN;
}

#if defined(BOOST_MATH_USE_STD_FPCLASSIFY) && defined(BOOST_MATH_NO_NATIVE_LONG_DOUBLE_FP_CLASSIFY)
template <>
inline int fpclassify_imp<long double> BOOST_NO_MACRO_EXPAND(long double t, const native_tag&)
{
   return boost::math::detail::fpclassify_imp(t, generic_tag<true>());
}
#endif

}  // namespace detail

template <class T>
inline int fpclassify BOOST_NO_MACRO_EXPAND(T t)
{
   typedef typename detail::fp_traits<T>::type traits;
   typedef typename traits::method method;
#ifdef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   if(std::numeric_limits<T>::is_specialized && detail::is_generic_tag_false(method()))
      return detail::fpclassify_imp(t, detail::generic_tag<true>());
   return detail::fpclassify_imp(t, method());
#else
   return detail::fpclassify_imp(t, method());
#endif
}

namespace detail {

#ifdef BOOST_MATH_USE_STD_FPCLASSIFY
    template<class T>
    inline bool isfinite_impl(T x, native_tag const&)
    {
        return (std::isfinite)(x);
    }
#endif

    template<class T>
    inline bool isfinite_impl(T x, generic_tag<true> const&)
    {
        return x >= -(std::numeric_limits<T>::max)()
            && x <= (std::numeric_limits<T>::max)();
    }

    template<class T>
    inline bool isfinite_impl(T x, generic_tag<false> const&)
    {
#ifdef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
      if(std::numeric_limits<T>::is_specialized)
         return isfinite_impl(x, mpl::true_());
#endif
       (void)x; // warning supression.
       return true;
    }

    template<class T>
    inline bool isfinite_impl(T x, ieee_tag const&)
    {
        typedef BOOST_DEDUCED_TYPENAME detail::fp_traits<T>::type traits;
        BOOST_DEDUCED_TYPENAME traits::bits a;
        traits::get_bits(x,a);
        a &= traits::exponent;
        return a != traits::exponent;
    }

#if defined(BOOST_MATH_USE_STD_FPCLASSIFY) && defined(BOOST_MATH_NO_NATIVE_LONG_DOUBLE_FP_CLASSIFY)
template <>
inline bool isfinite_impl<long double> BOOST_NO_MACRO_EXPAND(long double t, const native_tag&)
{
   return boost::math::detail::isfinite_impl(t, generic_tag<true>());
}
#endif

}

template<class T>
inline bool (isfinite)(T x)
{ //!< \brief return true if floating-point type t is finite.
   typedef typename detail::fp_traits<T>::type traits;
   typedef typename traits::method method;
   typedef typename boost::is_floating_point<T>::type fp_tag;
   return detail::isfinite_impl(x, method());
}

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

namespace detail {

#ifdef BOOST_MATH_USE_STD_FPCLASSIFY
    template<class T>
    inline bool isnormal_impl(T x, native_tag const&)
    {
        return (std::isnormal)(x);
    }
#endif

    template<class T>
    inline bool isnormal_impl(T x, generic_tag<true> const&)
    {
        if(x < 0) x = -x;
        return x >= (std::numeric_limits<T>::min)()
            && x <= (std::numeric_limits<T>::max)();
    }

    template<class T>
    inline bool isnormal_impl(T x, generic_tag<false> const&)
    {
#ifdef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
      if(std::numeric_limits<T>::is_specialized)
         return isnormal_impl(x, mpl::true_());
#endif
       return !(x == 0);
    }

    template<class T>
    inline bool isnormal_impl(T x, ieee_tag const&)
    {
        typedef BOOST_DEDUCED_TYPENAME detail::fp_traits<T>::type traits;
        BOOST_DEDUCED_TYPENAME traits::bits a;
        traits::get_bits(x,a);
        a &= traits::exponent | traits::flag;
        return (a != 0) && (a < traits::exponent);
    }

#if defined(BOOST_MATH_USE_STD_FPCLASSIFY) && defined(BOOST_MATH_NO_NATIVE_LONG_DOUBLE_FP_CLASSIFY)
template <>
inline bool isnormal_impl<long double> BOOST_NO_MACRO_EXPAND(long double t, const native_tag&)
{
   return boost::math::detail::isnormal_impl(t, generic_tag<true>());
}
#endif

}

template<class T>
inline bool (isnormal)(T x)
{
   typedef typename detail::fp_traits<T>::type traits;
   typedef typename traits::method method;
   typedef typename boost::is_floating_point<T>::type fp_tag;
   return detail::isnormal_impl(x, method());
}

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

namespace detail {

#ifdef BOOST_MATH_USE_STD_FPCLASSIFY
    template<class T>
    inline bool isinf_impl(T x, native_tag const&)
    {
        return (std::isinf)(x);
    }
#endif

    template<class T>
    inline bool isinf_impl(T x, generic_tag<true> const&)
    {
        (void)x; // in case the compiler thinks that x is unused because std::numeric_limits<T>::has_infinity is false
        return std::numeric_limits<T>::has_infinity
            && ( x == std::numeric_limits<T>::infinity()
                 || x == -std::numeric_limits<T>::infinity());
    }

    template<class T>
    inline bool isinf_impl(T x, generic_tag<false> const&)
    {
#ifdef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
      if(std::numeric_limits<T>::is_specialized)
         return isinf_impl(x, mpl::true_());
#endif
        (void)x; // warning supression.
        return false;
    }

    template<class T>
    inline bool isinf_impl(T x, ieee_copy_all_bits_tag const&)
    {
        typedef BOOST_DEDUCED_TYPENAME fp_traits<T>::type traits;

        BOOST_DEDUCED_TYPENAME traits::bits a;
        traits::get_bits(x,a);
        a &= traits::exponent | traits::significand;
        return a == traits::exponent;
    }

    template<class T>
    inline bool isinf_impl(T x, ieee_copy_leading_bits_tag const&)
    {
        typedef BOOST_DEDUCED_TYPENAME fp_traits<T>::type traits;

        BOOST_DEDUCED_TYPENAME traits::bits a;
        traits::get_bits(x,a);
        a &= traits::exponent | traits::significand;
        if(a != traits::exponent)
            return false;

        traits::set_bits(x,0);
        return x == 0;
    }

#if defined(BOOST_MATH_USE_STD_FPCLASSIFY) && defined(BOOST_MATH_NO_NATIVE_LONG_DOUBLE_FP_CLASSIFY)
template <>
inline bool isinf_impl<long double> BOOST_NO_MACRO_EXPAND(long double t, const native_tag&)
{
   return boost::math::detail::isinf_impl(t, generic_tag<true>());
}
#endif

}   // namespace detail

template<class T>
inline bool (isinf)(T x)
{
   typedef typename detail::fp_traits<T>::type traits;
   typedef typename traits::method method;
   typedef typename boost::is_floating_point<T>::type fp_tag;
   return detail::isinf_impl(x, method());
}

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

namespace detail {

#ifdef BOOST_MATH_USE_STD_FPCLASSIFY
    template<class T>
    inline bool isnan_impl(T x, native_tag const&)
    {
        return (std::isnan)(x);
    }
#endif

    template<class T>
    inline bool isnan_impl(T x, generic_tag<true> const&)
    {
        return std::numeric_limits<T>::has_infinity
            ? !(x <= std::numeric_limits<T>::infinity())
            : x != x;
    }

    template<class T>
    inline bool isnan_impl(T x, generic_tag<false> const&)
    {
#ifdef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
      if(std::numeric_limits<T>::is_specialized)
         return isnan_impl(x, mpl::true_());
#endif
        (void)x; // warning supression
        return false;
    }

    template<class T>
    inline bool isnan_impl(T x, ieee_copy_all_bits_tag const&)
    {
        typedef BOOST_DEDUCED_TYPENAME fp_traits<T>::type traits;

        BOOST_DEDUCED_TYPENAME traits::bits a;
        traits::get_bits(x,a);
        a &= traits::exponent | traits::significand;
        return a > traits::exponent;
    }

    template<class T>
    inline bool isnan_impl(T x, ieee_copy_leading_bits_tag const&)
    {
        typedef BOOST_DEDUCED_TYPENAME fp_traits<T>::type traits;

        BOOST_DEDUCED_TYPENAME traits::bits a;
        traits::get_bits(x,a);

        a &= traits::exponent | traits::significand;
        if(a < traits::exponent)
            return false;

        a &= traits::significand;
        traits::set_bits(x,a);
        return x != 0;
    }

}   // namespace detail

template<class T> bool (isnan)(T x)
{ //!< \brief return true if floating-point type t is NaN (Not A Number).
   typedef typename detail::fp_traits<T>::type traits;
   typedef typename traits::method method;
   typedef typename boost::is_floating_point<T>::type fp_tag;
   return detail::isnan_impl(x, method());
}

#ifdef isnan
template <> inline bool isnan BOOST_NO_MACRO_EXPAND<float>(float t){ return ::boost::math_detail::is_nan_helper(t, boost::true_type()); }
template <> inline bool isnan BOOST_NO_MACRO_EXPAND<double>(double t){ return ::boost::math_detail::is_nan_helper(t, boost::true_type()); }
template <> inline bool isnan BOOST_NO_MACRO_EXPAND<long double>(long double t){ return ::boost::math_detail::is_nan_helper(t, boost::true_type()); }
#endif

} // namespace math
} // namespace boost

#endif // BOOST_MATH_FPCLASSIFY_HPP


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

namespace Geotree
{

  FacesDivide::FacesDivide(Paths &paths)
    : paths(paths)
  {
    // add existing faces
    for (int face : this->paths.faces(MESH0))
      {
	int newindex = this->newMesh.add(paths.intersection.mesh0.getFace(face));
	FacePart facepart;
	facepart.faceindex = face;
	facepart.mesh = MESH0;
	facepart.newindex = newindex;
	faceparts.push_back(facepart);
      }

    for (int face : this->paths.faces(MESH1))
      {
	int newindex = this->newMesh.add(paths.intersection.mesh1.getFace(face));
	FacePart facepart;
	facepart.faceindex = face;
	facepart.mesh = MESH1;
	facepart.newindex = newindex;
	faceparts.push_back(facepart);
      }

    // for (FacePart facepart : this->faceparts)
    //   {
    // 	this->divide(facepart);
    //   }
  }

  void FacesDivide::divide(FacePart &facepart)
  {
    std::vector <std::set<Point>> facepaths = paths.getPaths(facepart.faceindex);

  }
}


#include <boost/test/unit_test.hpp>
#include "algorithms/math/basic_matrix_multiplication.hpp"

BOOST_AUTO_TEST_SUITE(TestBasicMatrixMultiplication)

BOOST_AUTO_TEST_CASE(empty_matrices)
{
    BOOST_CHECK(DS::Matrix<int>() ==
            BasicMatrixMultiplication(DS::Matrix<int>(), DS::Matrix<int>()));

    DS::Matrix<int> matrix(1, 1, 1);
    BOOST_CHECK(DS::Matrix<int>() ==
            BasicMatrixMultiplication(matrix, DS::Matrix<int>()));

    BOOST_CHECK(DS::Matrix<int>() ==
            BasicMatrixMultiplication(DS::Matrix<int>(), matrix));
}

BOOST_AUTO_TEST_CASE(matrices_with_wrong_dimensions)
{
    BOOST_CHECK(DS::Matrix<int>() ==
        BasicMatrixMultiplication(DS::Matrix<int>(1, 1, 1), DS::Matrix<int>(2, 3, 1)));
}

BOOST_AUTO_TEST_CASE(square_matrices)
{
    DS::Matrix<int> left(2, 2, 0);
    left.setRowValues(0, {1, 2});
    left.setRowValues(1, {3, 4});

    DS::Matrix<int> right(2, 2, 0);
    right.setRowValues(0, {5, 6});
    right.setRowValues(1, {7, 8});

    DS::Matrix<int> expected(2, 2, 0);
    expected.setRowValues(0, {19, 22});
    expected.setRowValues(1, {43, 50});

    BOOST_CHECK(expected == BasicMatrixMultiplication(left, right));
}

BOOST_AUTO_TEST_CASE(matrices)
{
    DS::Matrix<int> left(2, 3, 0);
    left.setRowValues(0, {3, 8, 1});
    left.setRowValues(1, {-2, 5, 1});

    DS::Matrix<int> right(3, 4, 0);
    right.setRowValues(0, {11, 9, 1, 0});
    right.setRowValues(1, {2, 0, 3, 15});
    right.setRowValues(2, {2, 6, 6, 7});

    DS::Matrix<int> expected(2, 4, 0);
    expected.setRowValues(0, {51, 33, 33, 127});
    expected.setRowValues(1, {-10, -12, 19, 82});

    BOOST_CHECK(expected == BasicMatrixMultiplication(left, right));
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_OPERATOR_FUNCTIONS_BITWISE_OR_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_BITWISE_OR_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <boost/proto/tags.hpp>

/*!
 * \ingroup boost_simd_operator
 * \defgroup boost_simd_operator_bitwise_or bitwise_or
 *
 * \par Description
 * return the bitwise or of the two parameters
 * the operands must share the same bit size and
 * the result type is the one of the first operand
 * Infix notation can be used with operator '|',
 * but this will not work in scalar mode if any
 * operand is floating point because of C++ limitations.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/bitwise_or.hpp>
 * \endcode
 *
 * \par Alias
 * \arg b_or
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0,class A1>
 *     meta::call<tag::bitwise_or_(A0,A1)>::type
 *     bitwise_or(const A0 & a0,const A1 & a1);
 * }
 * \endcode
 *
 * \param a0 the first parameter of bitwise_or
 * \param a1 the second parameter of bitwise_or
 *
 * \return a value of the type of the first parameter
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag bitwise_or_ of functor bitwise_or
     *        in namespace boost::simd::tag for toolbox boost.simd.operator
    **/
    struct bitwise_or_ : ext::elementwise_<bitwise_or_> { typedef ext::elementwise_<bitwise_or_> parent; };
  }

  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::bitwise_or_      , bitwise_or     , 2 )
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::bitwise_or_      , b_or           , 2 )
} }

namespace boost { namespace dispatch { namespace meta
{
  template<>
  struct hierarchy_of<boost::proto::tag::bitwise_or>
  {
    typedef boost::simd::tag::bitwise_or_ type;
  };
  template<>
  struct proto_tag<boost::simd::tag::bitwise_or_>
  {
    typedef boost::proto::tag::bitwise_or type;
  };
} } }

#include <boost/simd/operator/specific/common.hpp>

#endif


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file 2_local_search_from_stdin.cpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-09-20
 */
#define BOOST_TEST_MODULE two_local_search_stdin

#include "test_utils/read_tsplib.hpp"
#include "test_utils/2_local_search_logger.hpp"

#include "paal/local_search/2_local_search/2_local_search.hpp"
#include "paal/data_structures/cycle/simple_cycle.hpp"
#include "paal/data_structures/cycle/splay_cycle.hpp"
#include "paal/local_search/custom_components.hpp"
#include "paal/data_structures/components/components_replace.hpp"

#include <boost/test/unit_test.hpp>

#include <vector>
#include <string>

using std::vector;
using namespace paal::local_search;
using namespace paal;

template <typename Cycle> void test() {
    read_tsplib::TSPLIB_Directory::Graph graph(std::cin);
    read_tsplib::TSPLIB_Matrix mtx;

    graph.load(mtx);
    auto size = mtx.size();
    std::vector<int> v(size);
    std::iota(v.begin(), v.end(), 0);

    // create random solution
    std::random_shuffle(v.begin(), v.end());
    LOG_COPY_RANGE_DEL(v, ",");
    Cycle cycle(v.begin(), v.end());
    LOG_COPY_DEL(cycle.vbegin(), cycle.vend(), ",");

    // printing
    LOG_COPY_DEL(cycle.vbegin(), cycle.vend(), ",");
    LOGLN("Length before\t" << get_cycle_length(mtx, cycle));

    // setting logger
    auto logger = utils::make_two_ls_logger(mtx);

    // search
    two_local_search(cycle, paal::local_search::first_improving_strategy{},
                     logger, utils::always_false(),
                     get_default_two_local_components(mtx));
}

BOOST_AUTO_TEST_CASE(TSPLIB_simple) {
    test<paal::data_structures::simple_cycle<int>>();
}

BOOST_AUTO_TEST_CASE(TSPLIB) {
    test<paal::data_structures::splay_cycle<int>>();
}

using paal::local_search::search_components;

// TODO probably optimization does not optimize...
BOOST_AUTO_TEST_CASE(TSPLIB_cut) {
    read_tsplib::TSPLIB_Directory::Graph graph(std::cin);
    read_tsplib::TSPLIB_Matrix mtx;
    graph.load(mtx);
    auto size = mtx.size();
    std::vector<int> v(size);
    std::iota(v.begin(), v.end(), 0);

    // create random solution
    typedef data_structures::simple_cycle<int> Cycle;
    std::random_shuffle(v.begin(), v.end());
    Cycle cycle(v.begin(), v.end());
    int startLen = get_cycle_length(mtx, cycle);

    // creating local search
    auto lsc = get_default_two_local_components(mtx);
    typedef local_search::search_components_traits<puretype(lsc)>::GainT GainT;
    typedef local_search::gain_cut_small_improves<GainT, int> CIC;
    double epsilon = 0.001;
    CIC cut(lsc.get<paal::local_search::Gain>(), startLen, epsilon);
    auto cutLsc =
        data_structures::replace<local_search::Gain>(std::move(cut), lsc);

    // setting logger
    auto logger = utils::make_two_ls_logger(mtx);

    // printing
    LOGLN("Length before\t" << get_cycle_length(mtx, cycle));
    paal::local_search::first_improving_strategy strategy{};

    // search
    for (int j = 0; j < 20; ++j) {
        epsilon /= 2;
        LOGLN("epsilon = " << epsilon);
        cutLsc.get<local_search::Gain>().set_epsilon(epsilon);
        two_local_search(cycle, strategy, logger, utils::always_false(),
                         cutLsc);
    }

    LOGLN("Normal search at the end");
    two_local_search(cycle, strategy, logger, utils::always_false(), lsc);
}


#include <iostream>

#include <Eigen/CXX11/Tensor>

int main() {

    // Map a tensor of ints on top of stack-allocated storage.
    // 2 x 4 x 2 x 8
    int storage[128];
    // Eigen::TensorMap<int, 4> t_4d(storage, 2, 4, 2, 8);

    // The same storage can be viewed as a different tensor. You can
    // also pass the sizes as an array.
    // Eigen::TensorMap<int, 2> t_2d(storage, 16, 8);

    return 0;
}


/*
 * robustISAM2.cpp
 *
 *  Created on: Jul 13, 2012
 *      Author: niko
 */

#include <gtsam/slam/planarSLAM.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Marginals.h>

using namespace gtsam;

// additional boost features
#include "boost/program_options.hpp"
namespace po = boost::program_options;

#include "boost/foreach.hpp"
#define foreach BOOST_FOREACH

#include <boost/progress.hpp>
#include <boost/lexical_cast.hpp>

#include "betweenFactorSwitchable.h"
#include "switchVariableLinear.h"
#include "switchVariableSigmoid.h"
#include "betweenFactorMaxMix.h"
#include "timer.h"
using namespace vertigo;

// ===================================================================
struct Pose {
  int id;
  double x,y,th;
};

struct Edge {
  int i, j;
  double x, y, th;
  bool switchable;
  bool maxMix;
  Matrix covariance;
  double weight;
};

void writeResults(Values &results, string outputFile)
{
  ofstream resultFile(outputFile.c_str());

  Values::ConstFiltered<Pose2> result_poses = results.filter<Pose2>();
  foreach (const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, result_poses) {
    Pose2 p = key_value.value;
    resultFile << "VERTEX_SE2 " << p.x() << " " << p.y() << " " << p.theta() << endl;
  }

  {
    Values::ConstFiltered<SwitchVariableLinear> result_switches = results.filter<SwitchVariableLinear>();
    foreach (const Values::ConstFiltered<SwitchVariableLinear>::KeyValuePair& key_value, result_switches) {
      resultFile << "VERTEX_SWITCH " << key_value.value.value() << endl;
    }
  }

  {
    Values::ConstFiltered<SwitchVariableSigmoid> result_switches = results.filter<SwitchVariableSigmoid>();
    foreach (const Values::ConstFiltered<SwitchVariableSigmoid>::KeyValuePair& key_value, result_switches) {
      resultFile << "VERTEX_SWITCH " << key_value.value.value() << endl;
    }
  }
}

// ===================================================================
bool parseDataset(string inputFile, vector<Pose>&poses, vector<Edge>&edges,multimap<int, int> &poseToEdges)
{
	 cout << endl << "Parsing dataset " << inputFile << " ... " << endl;

	 // open the file
	 ifstream inFile(inputFile.c_str());
	 if (!inFile) {
		 cerr << "Error opening dataset file " << inputFile << endl;
		 return false;
	 }

	 // go through the dataset file
	 while (!inFile.eof()) {
		 // depending on the type of vertex or edge, read the data
		 string type;
		 inFile >> type;

		 if (type == "VERTEX_SE2") {
			 Pose p;
			 inFile  >> p.id >> p.x >> p.y >> p.th;
			 poses.push_back(p);
		 }

		 else if (type == "EDGE_SE2_SWITCHABLE" || type == "EDGE_SE2" || type == "EDGE_SE2_MAXMIX") {
			 double dummy;
			 Edge e;

			 // read pose IDs
			 inFile >> e.i >> e.j;

			 if (e.i>e.j) {
			   swap(e.i,e.j);
			 }

			 // read the switch variable ID (only needed in g2o, we dont need it here in the gtsam world)
			 if (type == "EDGE_SE2_SWITCHABLE") inFile >> dummy;
			 if (type == "EDGE_SE2_MAXMIX") inFile >> e.weight;

			 // read odometry measurement
			 inFile >> e.x >> e.y >> e.th;

			 // read information matrix
			 double info[6];
			 inFile >> info[0] >> info[1] >> info[2] >> info[3] >> info[4] >> info[5];
			 Matrix informationMatrix = Matrix_(3,3, info[0], info[1], info[2], info[1], info[3], info[4], info[2], info[4], info[5]);
			 e.covariance = inverse(informationMatrix);

			 if (type == "EDGE_SE2_SWITCHABLE") {
			   e.switchable=true;
			   e.maxMix=false;
			 }
			 else if (type == "EDGE_SE2_MAXMIX") {
			   e.switchable=false;
			   e.maxMix=true;
			 }
			 else {
			   e.switchable=false;
			   e.maxMix=false;
			 }

			 edges.push_back(e);

			 int id = edges.size()-1;
			 poseToEdges.insert(pair<int, int>(e.j, id));
		 }
		 // else just read the next item at next iteration until one of the if clauses is true
	 }

     return true;
}

// ===================================================================
int main(int argc, char *argv[])
{
    // === handle command line arguments ===
    string inputFile, outputFile;
    string method;
    int stop;
    double relinThresh;

    ISAM2Params isam2Params;

    po::options_description desc("Allowed options");
      desc.add_options()
      ("help", "Show this help message.")
      ("input,i", po::value<string>(&inputFile)->default_value("dataset.g2o"),"Load dataset from this file.")
      ("output,o", po::value<string>(&outputFile)->default_value("results.isam"),"Save results in this file.")
      ("stop", po::value<int>(&stop)->default_value(-1), "Stop after this many poses.")
      ("verbose,v", "verbose mode")
      ("sigmoid", "Use the sigmoid as the switch function Psi instead of a linear function.")
      ("dogleg", "Use Powell's dogleg method instead of Gauss-Newton.")
      ("qr", "Use QR factorization instead of Cholesky.")
      ("relinSkip", po::value<int>(&isam2Params.relinearizeSkip)->default_value(10), "Only relinearize any variables every relinearizeSkip calls to ISAM2::update (default: 10)")
      ("relinThresh", po::value<double>(&relinThresh)->default_value(0.1),"Only relinearize variables whose linear delta magnitude is greater than this threshold." )
      ("odoOnly", "Only use odometry edges, discard all other edges in the graph.")
    ;

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).allow_unregistered().run(),vm);
    po::notify(vm);

    if (vm.count("help")) {
      cout << desc << "\n";
      return 1;
    }

    bool verbose;
    if (vm.count("verbose")) verbose=true;
    else verbose=false;

    bool useSigmoid;
    if (vm.count("sigmoid")) useSigmoid=true;
    else useSigmoid = false;

    // === read and parse input file ===
    vector<Pose> poses;
    vector<Edge> edges;
    multimap<int, int> poseToEdges;
    if (!parseDataset(inputFile, poses, edges, poseToEdges)) {
    	cerr << "Error parsing the dataset."<< endl;
    	return 0;
    }
    cout << "Read " << poses.size() << " poses and " << edges.size() << " edges." << endl;

    // === set up iSAM2 ===
    //ISAM2Params isam2Params; already defined on top
    ISAM2DoglegParams doglegParams;
    ISAM2GaussNewtonParams gaussParams;

    if (vm.count("dogleg")) isam2Params.optimizationParams = doglegParams;
    else isam2Params.optimizationParams = gaussParams;

    if (vm.count("qr")) isam2Params.factorization = ISAM2Params::QR;

    if (vm.count("verbose")) {
    //  isam2Params.enableDetailedResults = true;
      isam2Params.evaluateNonlinearError = true;
    }
    isam2Params.relinearizeThreshold = relinThresh;

    ISAM2 isam2(isam2Params);

    // === go through the data and incrementally fee1d it into iSAM2 ===
    cout << "Running iSAM2 ..." << endl;

    // set up progress bar
    int progressLength = poses.size();
    if (stop>0 && stop<=poses.size()) progressLength=stop;
    boost::progress_display show_progress(progressLength);

    int counter=0;
    int switchCounter=-1;

    planarSLAM::Values globalInitialEstimate;
    Timer timer;

    // iterate through the poses and incrementally build and solve the graph, using iSAM2
    foreach (Pose p, poses) {

      planarSLAM::Graph graph;
      planarSLAM::Values initialEstimate;

    	 // find all the edges that involve this pose

      timer.tic("findEdges");
      pair<multimap<int, int>::iterator, multimap<int, int>::iterator > ret = poseToEdges.equal_range(p.id);
      timer.toc("findEdges");

    	for (multimap<int, int>::iterator it=ret.first; it!=ret.second; it++) {

    	  // look at the edge and see if it is switchable or not
    	  Edge e = edges[it->second];

    	  // see if this is an odometry edge, if yes, use it to initialize the current pose
    	  if (e.j==e.i+1) {
    	    timer.tic("initialize");
    	    Pose2 predecessorPose = isam2.calculateEstimate<Pose2>(planarSLAM::PoseKey(p.id-1));
    	    if (isnan(predecessorPose.x()) || isnan(predecessorPose.y()) || isnan(predecessorPose.theta())) {
    	      cout << "! Degenerated solution (NaN) detected. Solver failed." << endl;
    	      writeResults(globalInitialEstimate, outputFile);
    	      timer.print(cout);
    	      return 0;
    	    }
    	    initialEstimate.insertPose(p.id, predecessorPose * Pose2(e.x, e.y, e.th));
    	    globalInitialEstimate.insertPose(p.id,  predecessorPose * Pose2(e.x, e.y, e.th) );
    	    timer.toc("initialize");
    	  }

    	  timer.tic("addEdges");
    		if (!e.switchable && !e.maxMix) {
    		  if (!vm.count("odoOnly") || (e.j == e.i+1) ) {
    		    // this is easy, use the convenience functions of gtsam
    		    SharedNoiseModel odom_model = noiseModel::Gaussian::Covariance(e.covariance);
    		    graph.addOdometry(e.i, e.j, Pose2(e.x, e.y, e.th), odom_model);
    		  }
    		}
    		else if (e.switchable && !vm.count("odoOnly")) {
    		  if (!useSigmoid) {
            // create new switch variable
            initialEstimate.insert(Symbol('s',++switchCounter),SwitchVariableLinear(1.0));

            // create switch prior factor
            SharedNoiseModel switchPriorModel = noiseModel::Diagonal::Sigmas(Vector_(1, 1.0));
            boost::shared_ptr<PriorFactor<SwitchVariableLinear> > switchPriorFactor (new PriorFactor<SwitchVariableLinear> (Symbol('s',switchCounter), SwitchVariableLinear(1.0), switchPriorModel));
            graph.push_back(switchPriorFactor);

            // create switchable odometry factor
            SharedNoiseModel odom_model = noiseModel::Gaussian::Covariance(e.covariance);
            boost::shared_ptr<NonlinearFactor> switchableFactor(new BetweenFactorSwitchableLinear<Pose2>(planarSLAM::PoseKey(e.i), planarSLAM::PoseKey(e.j), Symbol('s', switchCounter), Pose2(e.x, e.y, e.th), odom_model));
            graph.push_back(switchableFactor);
    		  }
    		  else {

    		    // create new switch variable
    		    initialEstimate.insert(Symbol('s',++switchCounter),SwitchVariableSigmoid(10.0));

    		    // create switch prior factor
    		    SharedNoiseModel switchPriorModel = noiseModel::Diagonal::Sigmas(Vector_(1, 20.0));
    		    boost::shared_ptr<PriorFactor<SwitchVariableSigmoid> > switchPriorFactor (new PriorFactor<SwitchVariableSigmoid> (Symbol('s',switchCounter), SwitchVariableSigmoid(10.0), switchPriorModel));
    		    graph.push_back(switchPriorFactor);

    		    // create switchable odometry factor
    		    SharedNoiseModel odom_model = noiseModel::Gaussian::Covariance(e.covariance);
    		    boost::shared_ptr<NonlinearFactor> switchableFactor(new BetweenFactorSwitchableSigmoid<Pose2>(planarSLAM::PoseKey(e.i), planarSLAM::PoseKey(e.j), Symbol('s', switchCounter), Pose2(e.x, e.y, e.th), odom_model));
    		    graph.push_back(switchableFactor);
    		  }
    		}
    		else if (e.maxMix && !vm.count("odoOnly")) {
    		  // create mixture odometry factor
    		  SharedNoiseModel odom_model = noiseModel::Gaussian::Covariance(e.covariance);
    		  SharedNoiseModel null_model = noiseModel::Gaussian::Covariance(e.covariance / e.weight);

    		  boost::shared_ptr<NonlinearFactor> maxMixFactor(new BetweenFactorMaxMix<Pose2>(planarSLAM::PoseKey(e.i), planarSLAM::PoseKey(e.j), Pose2(e.x, e.y, e.th), odom_model, null_model, e.weight));
    		  graph.push_back(maxMixFactor);
    		}
    		timer.toc("addEdges");
    	}

    	if (p.id==0) {
    	  // add prior for first pose
    	  SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.01, 0.01, 0.01));
    		graph.addPrior(p.id, Pose2(p.x, p.y, p.th), prior_model);

    		// initial value for first pose
    		initialEstimate.insertPose(p.id, Pose2(p.x, p.y, p.th));
    	}

    	if (verbose) {
    	  timer.tic("update");
    	  ISAM2Result result = isam2.update(graph, initialEstimate);
    	  timer.toc("update");
    	  cout << "cliques: " << result.cliques << "\terr_before: " << *(result.errorBefore) << "\terr_after: " << *(result.errorAfter) << "\trelinearized: " << result.variablesRelinearized << endl;
    	}
    	else  {
    	  timer.tic("update");
    	  isam2.update(graph, initialEstimate);
    	  timer.toc("update");
    	}

    	if (!verbose) ++show_progress;
    	if ( (counter++ >= stop) && (stop>0)) break;

    	if (false) {
    	  timer.tic("marginals");

    	  gtsam::Marginals marginals(isam2.getFactorsUnsafe(), isam2.getLinearizationPoint());
    	  gtsam::Matrix cov = marginals.marginalCovariance(gtsam::Symbol('x', p.id));
    	  timer.toc("marginals");
    	  cout << cov << endl << endl;
    	}

/*
    	if (counter % 100 == 0) {
    	  Values results = isam2.calculateEstimate();
    	  char fileName[1024];
    	  sprintf(fileName, "results/results_%05d.isam", counter);
    	  writeResults(results, string(fileName));
    	}
*/
    }

    // === do final estimation ===
    //cout << endl;
    Values results = isam2.calculateBestEstimate();
    //results.print();

    timer.print(cout);

    // === write results ===

    cout << endl << endl << "Writing results." << endl;
    writeResults(results, outputFile);

}


/*
 * Copyright (C) 2017 Martin Lambers <marlam@marlam.de>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include "webcam-head-tracker.hpp"

#include <chrono>
#include <cstdlib>
#include <cstdio>
#include <cmath>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/video/tracking.hpp>

#include <dlib/opencv.h>
#include <dlib/image_processing.h>

/* Helpers for timing */

class timer
{
public:
    std::chrono::steady_clock::time_point t;
    void setNow() { t = std::chrono::steady_clock::now(); }
};

float duration(const timer& start, const timer& end)
{
    std::chrono::duration<long long, std::micro> time_span
        = std::chrono::duration_cast<std::chrono::duration<long long, std::micro>>(end.t - start.t);
    return time_span.count() / 1e3f;
}

/* Double Exponential Smoothing
 * This implements double exponential smoothing-based prediction
 * as described in Sec. 2 of "Double Exponential Smoothing: An alternative to
 * Kalman Filter-Based Predictive Tracking" by Joseph J. LaViola Jr.
 */

class DoubleExponentialSmoothing
{
private:
    bool _isInitialized;
    double _lastVecS[3], _lastVecS2[3];
    double _lastQuatS[4], _lastQuatS2[4];

    inline void copy3(double* result, const double* value)
    {
        result[0] = value[0];
        result[1] = value[1];
        result[2] = value[2];
    }

    inline void copy4(double* result, const double* value)
    {
        result[0] = value[0];
        result[1] = value[1];
        result[2] = value[2];
        result[3] = value[3];
    }

    inline double mix(double alpha, double x, double y)
    {
        return alpha * y + (1.0 - alpha) * x;
    }

    inline void mix3(double* result, double alpha, const double* v, const double* w)
    {
        result[0] = mix(alpha, v[0], w[0]);
        result[1] = mix(alpha, v[1], w[1]);
        result[2] = mix(alpha, v[2], w[2]);
    }

    inline void mix4(double* result, double alpha, const double* v, const double* w)
    {
        result[0] = mix(alpha, v[0], w[0]);
        result[1] = mix(alpha, v[1], w[1]);
        result[2] = mix(alpha, v[2], w[2]);
        result[3] = mix(alpha, v[3], w[3]);
    }

    inline double dot4(const double* v, const double* w)
    {
        return (v[0] * w[0] + v[1] * w[1] + v[2] * w[2] + v[3] * w[3]);
    }

    inline void normalize4(double* v)
    {
        double s = std::sqrt(dot4(v, v));
        v[0] /= s;
        v[1] /= s;
        v[2] /= s;
        v[3] /= s;
    }

    inline void slerp(double* result, double alpha, const double* q, const double* r)
    {
        double w[4] = { r[0], r[1], r[2], r[3] };
        double cosHalfAngle = dot4(q, r);
        if (cosHalfAngle < 0.0) {
            // quat(x, y, z, w) and quat(-x, -y, -z, -w) represent the same rotation
            w[0] = -w[0]; w[1] = -w[1]; w[2] = -w[2]; w[3] = -w[3];
            cosHalfAngle = -cosHalfAngle;
        }
        double tmpQ, tmpW;
        if (std::fabs(cosHalfAngle) >= 1.0) {
            // angle is zero => rotations are identical
            tmpQ = 1.0;
            tmpW = 0.0;
        } else {
            double halfAngle = acos(cosHalfAngle);
            double sinHalfAngle = sqrt(1.0 - cosHalfAngle * cosHalfAngle);
            if (std::fabs(sinHalfAngle) < 0.001) {
                // angle is 180 degrees => result is not clear
                tmpQ = 0.5;
                tmpW = 0.5;
            } else {
                tmpQ = std::sin((1.0 - alpha) * halfAngle) / sinHalfAngle;
                tmpW = sin(alpha * halfAngle) / sinHalfAngle;
            }
        }
        result[0] = q[0] * tmpQ + w[0] * tmpW;
        result[1] = q[1] * tmpQ + w[1] * tmpW;
        result[2] = q[2] * tmpQ + w[2] * tmpW;
        result[3] = q[3] * tmpQ + w[3] * tmpW;
    }

public:
    DoubleExponentialSmoothing() : _isInitialized(false) {}

    void step(const double* vec, const double* quat,
            double alpha, double tau, double* estimatedVec, double* estimatedQuat)
    {
        if (!_isInitialized) {
            copy3(_lastVecS, vec);
            copy3(_lastVecS2, vec);
            copy4(_lastQuatS, quat);
            copy4(_lastQuatS2, quat);
            _isInitialized = true;
        }
        double vecS[3], vecS2[3], quatS[4], quatS2[4];
        // Eq. (1), (2)
        mix3(vecS, alpha, _lastVecS, vec);
        mix3(vecS2, alpha, _lastVecS2, vecS);
        mix4(quatS, alpha, _lastQuatS, quat);
        mix4(quatS2, alpha, _lastQuatS2, quatS);
        copy3(_lastVecS, vecS);
        copy3(_lastVecS2, vecS2);
        copy4(_lastQuatS, quatS);
        copy4(_lastQuatS2, quatS2);
        // Eq. (6) for floor(tau) and ceil(tau)
        double floorTau = std::floor(tau);
        double betaFloorTau = 2.0 + alpha * floorTau / (1.0 - alpha);
        double estimatedVecFloorTau[3], estimatedQuatFloorTau[4];
        mix3(estimatedVecFloorTau, betaFloorTau, vecS2, vecS);
        mix4(estimatedQuatFloorTau, betaFloorTau, quatS2, quatS);
        normalize4(estimatedQuatFloorTau);
        double ceilTau = std::ceil(tau);
        double betaCeilTau = 2.0 + alpha * ceilTau / (1.0 - alpha);
        double estimatedVecCeilTau[3], estimatedQuatCeilTau[4];
        mix3(estimatedVecCeilTau, betaCeilTau, vecS2, vecS);
        mix4(estimatedQuatCeilTau, betaCeilTau, quatS2, quatS);
        normalize4(estimatedQuatCeilTau);
        // mix results for floor(tau) and ceil(tau)
        mix3(estimatedVec, tau - floorTau, estimatedVecCeilTau, estimatedVecFloorTau);
        slerp(estimatedQuat, tau - floorTau, estimatedQuatCeilTau, estimatedQuatFloorTau);
    }
};

/* WebcamHeadTracker */

WebcamHeadTracker::WebcamHeadTracker(unsigned int debugOptions) :
    _debugOptions(debugOptions),
    _isReady(false),
    _capture(NULL),
    _frame(NULL),
    _w(0), _h(0),
    _fps(0.0f),
    _fx(0.0f), _fy(0.0f),
    _cx(0.0f), _cy(0.0f),
    _k1(0.0f), _k2(0.0f), _p1(0.0f), _p2(0.0f), _k3(0.0f),
    _faceCascade(NULL),
    _faceModel(NULL),
    _filter(Filter_Double_Exponential),
    _kalmanFilter(NULL),
    _despFilter(NULL),
    _headPosition { 0.0f, 0.0f, 0.5f },
    _headOrientation { 0.0f, 0.0f, 0.0f, 0.0f }
{
}

WebcamHeadTracker::~WebcamHeadTracker()
{
    delete _capture;
    delete _frame;
    delete _faceCascade;
    delete _faceModel;
    delete _kalmanFilter;
    delete _despFilter;
}

bool WebcamHeadTracker::initWebcam()
{
    if (_capture)
        return isReady();
    _capture = new cv::VideoCapture(0);
    if (_capture && _capture->isOpened()) {
        _capture->set(cv::CAP_PROP_FRAME_WIDTH, 640);
        _capture->set(cv::CAP_PROP_FRAME_HEIGHT, 480);
        _frame = new cv::Mat;
        _w = _capture->get(cv::CAP_PROP_FRAME_WIDTH);
        _h = _capture->get(cv::CAP_PROP_FRAME_HEIGHT);
        _fps = _capture->get(cv::CAP_PROP_FPS);
        if (_fps <= 0.0f)
            _fps = 30.0f;
        const char* intrinsics;
        float fx, fy, cx, cy;
        if ((intrinsics = std::getenv("WEBCAM_INTRINSIC_PARAMETERS"))
                && std::sscanf(intrinsics, "%g,%g,%g,%g", &fx, &fy, &cx, &cy) == 4) {
            _fx = fx;
            _fy = fy;
            _cx = cx;
            _cy = cy;
        } else {
            _fx = 0.9f * _w;
            _fy = _fx;
            _cx = _w / 2.0f;
            _cy = _h / 2.0f;
        }
        const char* distCoeffs;
        float k1, k2, p1, p2, k3;
        if ((distCoeffs = std::getenv("WEBCAM_DISTORTION_COEFFICIENTS"))
                && std::sscanf(distCoeffs, "%g,%g,%g,%g,%g", &k1, &k2, &p1, &p2, &k3) == 5) {
            _k1 = k1;
            _k2 = k2;
            _p1 = p1;
            _p2 = p2;
            _k3 = k3;
        }
        return true;
    } else {
        return false;
    }
}

#define STRINGIFY(s) STRINGIFY_HELPER(s)
#define STRINGIFY_HELPER(s) #s

const char* WebcamHeadTracker::filePathFrontalFaceXml()
{
#ifdef HAARCASCADE_FRONTALFACE_ALT_XML
    static const char s[] = STRINGIFY(HAARCASCADE_FRONTALFACE_ALT_XML);
#else
    static const char s[] = "";
#endif
    return s;
}

const char* WebcamHeadTracker::filePathFaceLandmarksDat()
{
#ifdef SHAPE_PREDICTOR_68_FACE_LANDMARKS_DAT
    static const char s[] = STRINGIFY(SHAPE_PREDICTOR_68_FACE_LANDMARKS_DAT);
#else
    static const char s[] = "";
#endif
    return s;
}

bool WebcamHeadTracker::initPoseEstimator(const char* frontalFaceXml, const char* faceLandmarksDat)
{
    if (isReady())
        return true;

    _faceCascade = new cv::CascadeClassifier;
    if (!_faceCascade->load(frontalFaceXml)) {
        delete _faceCascade;
        _faceCascade = NULL;
        return false;
    }

    _faceModel = new dlib::shape_predictor;
    try {
        dlib::deserialize(faceLandmarksDat) >> *_faceModel;
    }
    catch (std::exception& e) {
        delete _faceCascade;
        _faceCascade = NULL;
        delete _faceModel;
        _faceModel = NULL;
        return false;
    }

    // See http://docs.opencv.org/trunk/dc/d2c/tutorial_real_time_pose.html
    // for information on this!
    _kalmanFilter = new cv::KalmanFilter;
    _kalmanFilter->init(18, 6, 0, CV_64F);
    cv::setIdentity(_kalmanFilter->processNoiseCov, cv::Scalar::all(1e-3));
    cv::setIdentity(_kalmanFilter->measurementNoiseCov, cv::Scalar::all(1e-1));
    cv::setIdentity(_kalmanFilter->errorCovPost, cv::Scalar::all(1));
    float dt = 1.0f / _fps;
    _kalmanFilter->transitionMatrix.at<double>(0, 3) = dt;
    _kalmanFilter->transitionMatrix.at<double>(1, 4) = dt;
    _kalmanFilter->transitionMatrix.at<double>(2, 5) = dt;
    _kalmanFilter->transitionMatrix.at<double>(3, 6) = dt;
    _kalmanFilter->transitionMatrix.at<double>(4, 7) = dt;
    _kalmanFilter->transitionMatrix.at<double>(5, 8) = dt;
    _kalmanFilter->transitionMatrix.at<double>(0, 6) = 0.5f * dt * dt;
    _kalmanFilter->transitionMatrix.at<double>(1, 7) = 0.5f * dt * dt;
    _kalmanFilter->transitionMatrix.at<double>(2, 8) = 0.5f * dt * dt;
    _kalmanFilter->transitionMatrix.at<double>(9, 12) = dt;
    _kalmanFilter->transitionMatrix.at<double>(10, 13) = dt;
    _kalmanFilter->transitionMatrix.at<double>(11, 14) = dt;
    _kalmanFilter->transitionMatrix.at<double>(12, 15) = dt;
    _kalmanFilter->transitionMatrix.at<double>(13, 16) = dt;
    _kalmanFilter->transitionMatrix.at<double>(14, 17) = dt;
    _kalmanFilter->transitionMatrix.at<double>(9, 15) = 0.5f * dt * dt;
    _kalmanFilter->transitionMatrix.at<double>(10, 16) = 0.5f * dt * dt;
    _kalmanFilter->transitionMatrix.at<double>(11, 17) = 0.5f * dt * dt;
    _kalmanFilter->measurementMatrix.at<double>(0, 0) = 1;
    _kalmanFilter->measurementMatrix.at<double>(1, 1) = 1;
    _kalmanFilter->measurementMatrix.at<double>(2, 2) = 1;
    _kalmanFilter->measurementMatrix.at<double>(3, 9) = 1;
    _kalmanFilter->measurementMatrix.at<double>(4, 10) = 1;
    _kalmanFilter->measurementMatrix.at<double>(5, 11) = 1;

    _despFilter = new DoubleExponentialSmoothing;

    _isReady = true;
    return true;
}

void WebcamHeadTracker::setFocalLengthsInPixels(float fx, float fy)
{
    _fx = fx;
    _fy = fy;
}

void WebcamHeadTracker::setPrincipalPointInPixels(float cx, float cy)
{
    _cx = cx;
    _cy = cy;
}

void WebcamHeadTracker::setDistortionCoefficients(float k1, float k2, float p1, float p2, float k3)
{
    _k1 = k1;
    _k2 = k2;
    _p1 = p1;
    _p2 = p2;
    _k3 = k3;
}

void WebcamHeadTracker::setFilter(enum Filter filter)
{
    _filter = filter;
}

void WebcamHeadTracker::getNewFrame()
{
    timer t0, t1;
    t0.setNow();
    *_capture >> *_frame;
    t1.setNow();
    if (_debugOptions & Debug_Timing) {
        fprintf(stderr, "WHT: acquiring webcam frame:  %4.1f ms\n", duration(t0, t1));
    }
}

static void rodriguesToQuaternion(const double* r, double* q)
{
    // Note: the OpenCV use of Rodrigues rotation vectors seems to differ from
    // other uses. They multiply the unit rotation axis with the rotation angle
    // instead of with the tangens of half the angle. This is all crap. Everyone
    // should always use quaternions ;)
    double angle = std::sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]);
    double axis[3] = { r[0] / angle, r[1] / angle, r[2] / angle};
    double sinHalfAngle = std::sin(angle / 2.0f);
    q[0] = axis[0] * sinHalfAngle;
    q[1] = axis[1] * sinHalfAngle;
    q[2] = axis[2] * sinHalfAngle;
    q[3] = std::cos(angle / 2.0f);
}

static void quaternionToRodrigues(const double* q, double* r)
{
    double halfAngle = std::acos(q[3]);
    double sinHalfAngle = std::sin(halfAngle);
    double angle = halfAngle * 2.0;
    double factor = angle / sinHalfAngle;
    r[0] = factor * q[0];
    r[1] = factor * q[1];
    r[2] = factor * q[2];
}

static void quaternionToEuler(const double* q, double* euler)
{
    double singularityTest = q[0] * q[1] + q[2] * q[3];
    if (singularityTest > 0.4999) {
        // north pole
        euler[0] = 2.0 * std::atan2(q[0], q[3]);
        euler[1] = M_PI_2;
        euler[2] = 0.0;
    } else if (singularityTest < -0.4999) {
        // south pole
        euler[0] = -2.0 * std::atan2(q[0], q[3]);
        euler[1] = -M_PI_2;
        euler[2] = 0.0f;
    } else {
        euler[0] = std::atan2(2.0 * (q[3] * q[0] + q[1] * q[2]), 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1]));
        euler[1] = std::asin(2.0 * (q[3] * q[1] - q[0] * q[2]));
        euler[2] = std::atan2(2.0 * (q[3] * q[2] + q[0] * q[1]), 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]));
    }
}

static void eulerToQuaternion(const double* euler, double* q)
{
    double x2 = euler[0] / 2.0;
    double y2 = euler[1] / 2.0;
    double z2 = euler[2] / 2.0;
    double sx2 = std::sin(x2);
    double cx2 = std::cos(x2);
    double sy2 = std::sin(y2);
    double cy2 = std::cos(y2);
    double sz2 = std::sin(z2);
    double cz2 = std::cos(z2);
    q[0] = sx2 * cy2 * cz2 - cx2 * sy2 * sz2;
    q[1] = cx2 * sy2 * cz2 + sx2 * cy2 * sz2;
    q[2] = cx2 * cy2 * sz2 - sx2 * sy2 * cz2;
    q[3] = cx2 * cy2 * cz2 + sx2 * sy2 * sz2;
}

bool WebcamHeadTracker::computeHeadPose()
{
    if (!_faceCascade)
        return false;

    timer t0, t1, t2, t3, t4;

    /* Face detection */
    t0.setNow();
    const int minFaceSize = 80;
    std::vector<cv::Rect> faces;
    _faceCascade->detectMultiScale(*_frame, faces, 1.1, 2,
            cv::CASCADE_SCALE_IMAGE | cv::CASCADE_FIND_BIGGEST_OBJECT,
            cv::Size(minFaceSize, minFaceSize));
    if (faces.size() < 1)
        return false;
    cv::Rect faceRect = faces[0];
    t1.setNow();

    /* Face landmark detection */
#if 1
    // A temporary workaround for a Dlib/OpenCV incompatibility:
    IplImage iplImg = cvIplImage(*_frame);
    dlib::cv_image<dlib::bgr_pixel> dlibFrame(&iplImg);
#else
    dlib::cv_image<dlib::bgr_pixel> dlibFrame(*_frame); // does not copy data
#endif
    dlib::rectangle dlibRect(faceRect.x, faceRect.y,
            faceRect.x + faceRect.width - 1, faceRect.y + faceRect.height - 1);
    dlib::full_object_detection shape = (*_faceModel)(dlibFrame, dlibRect);
    if (shape.num_parts() != 68)
        return false;
    std::vector<cv::Point2f> landmarks;
    landmarks.resize(68);
    for (int i = 0; i < 68; i++) {
        dlib::point p = shape.part(i);
        landmarks[i] = cv::Point(p.x(), p.y());
    }
    t2.setNow();

    /* Extract a subset of landmarks for which we have good guesses for
     * average positions from <https://en.wikipedia.org/wiki/Human_head>.
     * Everything must be in mm to be consistent with OpenCV assumptions.
     * We use landmarks that tend not to change too much with varying
     * facial expressions. */
    const cv::Point3f landmarkLeftEctocanthi  (-60.0f,   0.0f,   0.0f);
    const cv::Point3f landmarkRightEctocanthi (+60.0f,   0.0f,   0.0f);
    const cv::Point3f landmarkSellion         (  0.0f,   5.0f, -20.0f);
    const cv::Point3f landmarkSubnasale       (  0.0f, -42.0f, -30.0f);
    const cv::Point3f landmarkStomion         (  0.0f, -67.0f, -32.0f);
    const cv::Point3f landmarkLeftTragion     (-70.0f,   0.0f,  99.9f);
    const cv::Point3f landmarkRightTragion    (+70.0f,   0.0f,  99.9f);
    const std::vector<cv::Point3f> modelLandmarks( {
            landmarkLeftEctocanthi,
            landmarkLeftEctocanthi,
            landmarkRightEctocanthi,
            landmarkRightEctocanthi,
            landmarkSellion,
            landmarkSubnasale,
            landmarkStomion,
            landmarkLeftTragion,
            landmarkRightTragion
            });
    const int landmarkLeftEctocanthiIndex = 36;
    const int landmarkRightEctocanthiIndex = 45;
    const int landmarkSellionIndex = 27;
    const int landmarkSubnasaleIndex = 33;
    const int landmarkStomionIndex = 51;
    const int landmarkLeftTragionIndex = 0;
    const int landmarkRightTragionIndex = 16;
    const std::vector<cv::Point2f> imageLandmarks( {
            landmarks[landmarkLeftEctocanthiIndex],
            landmarks[landmarkLeftEctocanthiIndex],
            landmarks[landmarkRightEctocanthiIndex],
            landmarks[landmarkRightEctocanthiIndex],
            landmarks[landmarkSellionIndex],
            landmarks[landmarkSubnasaleIndex],
            landmarks[landmarkStomionIndex],
            landmarks[landmarkLeftTragionIndex],
            landmarks[landmarkRightTragionIndex]
            });
    cv::Matx33f cameraMatrix;
    cameraMatrix(0, 0) = _fx;
    cameraMatrix(0, 1) = 0.0f;
    cameraMatrix(0, 2) = _cx;
    cameraMatrix(1, 0) = 0.0f;
    cameraMatrix(1, 1) = _fy;
    cameraMatrix(1, 2) = _cy;
    cameraMatrix(2, 0) = 0.0f;
    cameraMatrix(2, 1) = 0.0f;
    cameraMatrix(2, 2) = 1.0f;
    cv::Mat distCoeffs(1, 5, CV_32F);
    distCoeffs.at<float>(0) = _k1;
    distCoeffs.at<float>(1) = _k2;
    distCoeffs.at<float>(2) = _p1;
    distCoeffs.at<float>(3) = _p2;
    distCoeffs.at<float>(4) = _k3;
    cv::Mat rvec(1, 3, CV_64F);
    rvec.at<double>(0) = M_PI; // 180 deg around x axis: null rotation in OpenCV orientation
    rvec.at<double>(1) = 0.0f;
    rvec.at<double>(2) = 0.0f;
    cv::Mat tvec(1, 3, CV_64F);
    tvec.at<double>(0) = 0.0f;
    tvec.at<double>(1) = 0.0f;
    tvec.at<double>(2) = 500.0f;
    // in my tests, using the CV_P3P solver with 4 points was less stable than using the iterative solver with 7
    //cv::solvePnP(modelLandmarks, imageLandmarks, cameraMatrix, distCoeffs, rvec, tvec, false, CV_P3P);
    cv::solvePnP(modelLandmarks, imageLandmarks, cameraMatrix, distCoeffs, rvec, tvec, true, cv::SOLVEPNP_ITERATIVE);
    double observedVec[3] = { tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2) };
    double observedQuat[4];
    rodriguesToQuaternion(&(rvec.at<double>(0)), observedQuat);
    t3.setNow();

    /* Feed the new measurement to the filter and save result */
    double estimatedVec[3] = { 0, 0, 0 };
    double estimatedQuat[4] = { 0, 0, 0, 0 };
    switch (_filter) {
    case Filter_None:
        {
            estimatedVec[0] = observedVec[0];
            estimatedVec[1] = observedVec[1];
            estimatedVec[2] = observedVec[2];
            estimatedQuat[0] = observedQuat[0];
            estimatedQuat[1] = observedQuat[1];
            estimatedQuat[2] = observedQuat[2];
            estimatedQuat[3] = observedQuat[3];
        }
        break;
    case Filter_Kalman:
        {
            double observedEulerAngles[3];
            quaternionToEuler(observedQuat, observedEulerAngles);
            // See http://docs.opencv.org/trunk/dc/d2c/tutorial_real_time_pose.html
            // for information on this!
            cv::Mat measurement(6, 1, CV_64F);
            measurement.at<double>(0) = observedVec[0];
            measurement.at<double>(1) = observedVec[1];
            measurement.at<double>(2) = observedVec[2];
            measurement.at<double>(3) = observedEulerAngles[0];
            measurement.at<double>(4) = observedEulerAngles[1];
            measurement.at<double>(5) = observedEulerAngles[2];
            cv::Mat prediction = _kalmanFilter->predict();
            cv::Mat estimation = _kalmanFilter->correct(measurement);
            estimatedVec[0] = estimation.at<double>(0);
            estimatedVec[1] = estimation.at<double>(1);
            estimatedVec[2] = estimation.at<double>(2);
            eulerToQuaternion(&(estimation.at<double>(9)), estimatedQuat);
        }
        break;
    case Filter_Double_Exponential:
        {
            _despFilter->step(observedVec, observedQuat,
                    0.2, 0.7,
                    estimatedVec, estimatedQuat);
        }
        break;
    }
    t4.setNow();

    /* Convert the internal representation to the external representation */
    // convert position
    _headPosition[0] = -estimatedVec[0] / 1000.0;
    _headPosition[1] = -estimatedVec[1] / 1000.0;
    _headPosition[2] =  estimatedVec[2] / 1000.0;
    // convert orientation (rotate 180 deg around x)
    _headOrientation[0] =  estimatedQuat[3];
    _headOrientation[1] = -estimatedQuat[2];
    _headOrientation[2] =  estimatedQuat[1];
    _headOrientation[3] = -estimatedQuat[0];

    /* Debug output */
    if (_debugOptions & Debug_Timing) {
        fprintf(stderr, "WHT: face detection:          %4.1f ms\n", duration(t0, t1));
        fprintf(stderr, "WHT: face landmark detection: %4.1f ms\n", duration(t1, t2));
        fprintf(stderr, "WHT: face model matching:     %4.1f ms\n", duration(t2, t3));
        fprintf(stderr, "WHT: filtering:               %4.1f ms\n", duration(t4, t3));
    }
    if (_debugOptions & Debug_Window) {
        // render face rectangle
        cv::rectangle(*_frame, faceRect, cv::Scalar(0, 0, 255));
        // render face model
        for (int i = 1; i <= 16; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 18; i <= 21; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 23; i <= 26; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 28; i <= 30; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 31; i <= 35; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        cv::line(*_frame, landmarks[30], landmarks[35], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 37; i <= 41; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        cv::line(*_frame, landmarks[36], landmarks[41], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 43; i <= 47; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        cv::line(*_frame, landmarks[42], landmarks[47], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 49; i <= 59; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        cv::line(*_frame, landmarks[48], landmarks[49], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 61; i <= 67; i++)
            cv::line(*_frame, landmarks[i - 1], landmarks[i], cv::Scalar(0, 255, 0), 1, 1, 0);
        cv::line(*_frame, landmarks[60], landmarks[67], cv::Scalar(0, 255, 0), 1, 1, 0);
        for (int i = 0; i < 68; i++)
            cv::circle(*_frame, landmarks[i], 2.5f, cv::Scalar(0, 0, 255), 1, 1, 0);
        cv::circle(*_frame, landmarks[landmarkLeftEctocanthiIndex],  3.0f, cv::Scalar(255, 255, 255), 1, 1, 0);
        cv::circle(*_frame, landmarks[landmarkRightEctocanthiIndex], 3.0f, cv::Scalar(255, 255, 255), 1, 1, 0);
        cv::circle(*_frame, landmarks[landmarkSellionIndex],         3.0f, cv::Scalar(255, 255, 255), 1, 1, 0);
        cv::circle(*_frame, landmarks[landmarkSubnasaleIndex],       3.0f, cv::Scalar(255, 255, 255), 1, 1, 0);
        cv::circle(*_frame, landmarks[landmarkStomionIndex],         3.0f, cv::Scalar(255, 255, 255), 1, 1, 0);
        cv::circle(*_frame, landmarks[landmarkLeftTragionIndex],     3.0f, cv::Scalar(255, 255, 255), 1, 1, 0);
        cv::circle(*_frame, landmarks[landmarkRightTragionIndex],    3.0f, cv::Scalar(255, 255, 255), 1, 1, 0);
        // render projected face model landmarks
        std::vector<cv::Point2f> projectedModelLandmarks;
        cv::projectPoints(modelLandmarks, rvec, tvec, cameraMatrix, distCoeffs, projectedModelLandmarks);
        cv::line(*_frame, projectedModelLandmarks[7], projectedModelLandmarks[0], cv::Scalar(255, 0, 0));
        cv::line(*_frame, projectedModelLandmarks[0], projectedModelLandmarks[4], cv::Scalar(255, 0, 0));
        cv::line(*_frame, projectedModelLandmarks[4], projectedModelLandmarks[2], cv::Scalar(255, 0, 0));
        cv::line(*_frame, projectedModelLandmarks[2], projectedModelLandmarks[8], cv::Scalar(255, 0, 0));
        cv::line(*_frame, projectedModelLandmarks[4], projectedModelLandmarks[5], cv::Scalar(255, 0, 0));
        cv::line(*_frame, projectedModelLandmarks[5], projectedModelLandmarks[6], cv::Scalar(255, 0, 0));
        cv::circle(*_frame, projectedModelLandmarks[0], 3.0f, cv::Scalar(255, 0, 0));
        cv::circle(*_frame, projectedModelLandmarks[2], 3.0f, cv::Scalar(255, 0, 0));
        cv::circle(*_frame, projectedModelLandmarks[4], 3.0f, cv::Scalar(255, 0, 0));
        cv::circle(*_frame, projectedModelLandmarks[5], 3.0f, cv::Scalar(255, 0, 0));
        cv::circle(*_frame, projectedModelLandmarks[6], 3.0f, cv::Scalar(255, 0, 0));
        cv::circle(*_frame, projectedModelLandmarks[7], 3.0f, cv::Scalar(255, 0, 0));
        cv::circle(*_frame, projectedModelLandmarks[8], 3.0f, cv::Scalar(255, 0, 0));
        // render projected filtered model
        std::vector<cv::Point2f> projectedFilteredModelLandmarks;
        tvec.at<double>(0) = estimatedVec[0];
        tvec.at<double>(1) = estimatedVec[1];
        tvec.at<double>(2) = estimatedVec[2];
        quaternionToRodrigues(estimatedQuat, &(rvec.at<double>(0)));
        cv::projectPoints(modelLandmarks, rvec, tvec, cameraMatrix, distCoeffs, projectedFilteredModelLandmarks);
        cv::line(*_frame, projectedFilteredModelLandmarks[7], projectedFilteredModelLandmarks[0], cv::Scalar(255, 255, 0));
        cv::line(*_frame, projectedFilteredModelLandmarks[0], projectedFilteredModelLandmarks[4], cv::Scalar(255, 255, 0));
        cv::line(*_frame, projectedFilteredModelLandmarks[4], projectedFilteredModelLandmarks[2], cv::Scalar(255, 255, 0));
        cv::line(*_frame, projectedFilteredModelLandmarks[2], projectedFilteredModelLandmarks[8], cv::Scalar(255, 255, 0));
        cv::line(*_frame, projectedFilteredModelLandmarks[4], projectedFilteredModelLandmarks[5], cv::Scalar(255, 255, 0));
        cv::line(*_frame, projectedFilteredModelLandmarks[5], projectedFilteredModelLandmarks[6], cv::Scalar(255, 255, 0));
        cv::circle(*_frame, projectedFilteredModelLandmarks[0], 3.0f, cv::Scalar(255, 255, 0));
        cv::circle(*_frame, projectedFilteredModelLandmarks[2], 3.0f, cv::Scalar(255, 255, 0));
        cv::circle(*_frame, projectedFilteredModelLandmarks[4], 3.0f, cv::Scalar(255, 255, 0));
        cv::circle(*_frame, projectedFilteredModelLandmarks[5], 3.0f, cv::Scalar(255, 255, 0));
        cv::circle(*_frame, projectedFilteredModelLandmarks[6], 3.0f, cv::Scalar(255, 255, 0));
        cv::circle(*_frame, projectedFilteredModelLandmarks[7], 3.0f, cv::Scalar(255, 255, 0));
        cv::circle(*_frame, projectedFilteredModelLandmarks[8], 3.0f, cv::Scalar(255, 255, 0));
        // show
        cv::imshow("webcam head tracker", *_frame);
        int key = cv::waitKey(1);
        if (key == 27 || key == 'q')
            _isReady = false;
        if (key == 'f')
            _filter = (_filter == Filter_None ? Filter_Kalman
                    : _filter == Filter_Kalman ? Filter_Double_Exponential
                    : Filter_None);
    }
    return true;
}

void WebcamHeadTracker::getHeadPosition(float* headPosition) const
{
    headPosition[0] = _headPosition[0];
    headPosition[1] = _headPosition[1];
    headPosition[2] = _headPosition[2];
}

void WebcamHeadTracker::getHeadOrientation(float* headOrientation) const
{
    headOrientation[0] = _headOrientation[0];
    headOrientation[1] = _headOrientation[1];
    headOrientation[2] = _headOrientation[2];
    headOrientation[3] = _headOrientation[3];
}