#define DEBUG 1
/**
 * File    : C.cpp
 * Author  : Kazune Takahashi
 * Created : 6/16/2020, 4:51:02 PM
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <climits>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::multiprecision::cpp_int;
using ll = long long;
using ld = long double;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1'000'000'007LL};
// constexpr ll MOD{998'244'353LL}; // be careful
constexpr ll MAX_SIZE{3'000'010LL};
// constexpr ll MAX_SIZE{30'000'010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
bool ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
    return true;
  }
  return false;
}
template <typename T>
bool ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
    return true;
  }
  return false;
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(Mint const &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(Mint const &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(Mint const &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(Mint const &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(Mint const &a) const { return Mint(*this) += a; }
  Mint operator-(Mint const &a) const { return Mint(*this) -= a; }
  Mint operator*(Mint const &a) const { return Mint(*this) *= a; }
  Mint operator/(Mint const &a) const { return Mint(*this) /= a; }
  bool operator<(Mint const &a) const { return x < a.x; }
  bool operator<=(Mint const &a) const { return x <= a.x; }
  bool operator>(Mint const &a) const { return x > a.x; }
  bool operator>=(Mint const &a) const { return x >= a.x; }
  bool operator==(Mint const &a) const { return x == a.x; }
  bool operator!=(Mint const &a) const { return !(*this == a); }
  Mint power(ll N) const
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, Mint<MOD> const &rhs) { return rhs + lhs; }
template <ll MOD>
Mint<MOD> operator-(ll lhs, Mint<MOD> const &rhs) { return -rhs + lhs; }
template <ll MOD>
Mint<MOD> operator*(ll lhs, Mint<MOD> const &rhs) { return rhs * lhs; }
template <ll MOD>
Mint<MOD> operator/(ll lhs, Mint<MOD> const &rhs) { return Mint<MOD>{lhs} / rhs; }
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a) { return stream >> a.x; }
template <ll MOD>
ostream &operator<<(ostream &stream, Mint<MOD> const &a) { return stream << a.x; }
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i{2LL}; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i{1LL}; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
template <typename T>
T gcd(T x, T y) { return y ? gcd(y, x % y) : x; }
template <typename T>
T lcm(T x, T y) { return x / gcd(x, y) * y; }
// ----- for C++17 -----
template <typename T>
int popcount(T x) // C++20
{
  int ans{0};
  while (x != 0)
  {
    ans += x & 1;
    x >>= 1;
  }
  return ans;
}
// ----- Infty -----
template <typename T>
constexpr T Infty() { return numeric_limits<T>::max(); }
template <typename T>
constexpr T mInfty() { return numeric_limits<T>::min(); }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1'000'000'000'000'010LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

// ----- Solve -----

class Solve
{

public:
  Solve()
  {
  }

  void flush()
  {
  }

private:
};

// ----- SegTree -----
// Referring to the following great materials.
//  - tsutaj-san's article: https://tsutaj.hatenablog.com/entry/2017/03/30/224339
//  - drken-san's article: https://drken1215.hatenablog.com/entry/2019/02/19/110200
//  - tsutaj-san's libary: https://tsutaj.github.io/cpp_library/library/structure/strc_021_dynamic_lazy_segtree.cpp.html
// Many thanks to them.

template <typename Monoid, typename Action>
class SegTree
{
  struct SegNode
  {
    bool need_update;
    unique_ptr<SegNode> left, right;
    Monoid value;
    Action lazy_value;

    SegNode() {}
    SegNode(Monoid value, Action lazy_value) : need_update{false}, left{nullptr}, right{nullptr}, value{value}, lazy_value{lazy_value} {}
  };

  using FuncAction = function<void(Monoid &, Action)>;
  using FuncMonoid = function<Monoid(Monoid, Monoid)>;
  using FuncLazy = function<void(Action &, Action)>;
  using FuncIndex = function<Action(Action, int)>;

  // fields
  int N;
  unique_ptr<SegNode> root;
  // unities
  Monoid unity_monoid;
  Action unity_action;
  // functions
  FuncAction func_update;
  FuncMonoid func_combine;
  FuncLazy func_lazy;
  FuncIndex func_accumulate;

public:
  SegTree() {}
  SegTree(
      int n, Monoid unity_monoid, Action unity_action,
      FuncAction func_update,
      FuncMonoid func_combine,
      FuncLazy func_lazy,
      FuncIndex func_accumulate)
      : N{1}, root{make_unique<SegNode>(unity_monoid, unity_action)},
        unity_monoid(unity_monoid), unity_action(unity_action),
        func_update(func_update),
        func_combine(func_combine),
        func_lazy(func_lazy),
        func_accumulate(func_accumulate)
  {
    while (N < n)
    {
      N <<= 1;
    }
  }

  void update(int a, int b, Action const &x) { update(root.get(), a, b, x, 0, N); }
  void update(int a, Action const &x) { update(a, a + 1, x); }
  Monoid query(int a, int b) { return query(root.get(), a, b, 0, N); }
  Monoid query(int a) { return query(a, a + 1); }
  Monoid operator[](size_t i) { return query(static_cast<int>(i)); }

private:
  void node_maker(unique_ptr<SegNode> &pt) const
  {
    if (!pt)
    {
      pt = make_unique<SegNode>(unity_monoid, unity_action);
    }
  }

  void evaluate(SegNode *node, int l, int r)
  {
    if (!node->need_update)
    {
      return;
    }
    func_update(node->value, func_accumulate(node->lazy_value, r - l));
    if (r - l > 1)
    {
      node_maker(node->left);
      func_lazy(node->left->lazy_value, node->lazy_value);
      node->left->need_update = true;
      node_maker(node->right);
      func_lazy(node->right->lazy_value, node->lazy_value);
      node->right->need_update = true;
    }
    node->lazy_value = unity_action;
    node->need_update = false;
  }

  void update(SegNode *node, int a, int b, Action const &x, int l, int r)
  {
    evaluate(node, l, r);
    if (b <= l || r <= a)
    {
      return;
    }
    if (a <= l && r <= b)
    {
      func_lazy(node->lazy_value, x);
      node->need_update = true;
      evaluate(node, l, r);
    }
    else
    {
      auto mid{(l + r) >> 1};
      node_maker(node->left);
      update(node->left.get(), a, b, x, l, mid);
      node_maker(node->right);
      update(node->right.get(), a, b, x, mid, r);
      node->value = func_combine(node->left->value, node->right->value);
    }
  }

  Monoid query(SegNode *node, int a, int b, int l, int r)
  {
    if (b <= l || r <= a)
    {
      return unity_monoid;
    }
    evaluate(node, l, r);
    if (a <= l && r <= b)
    {
      return node->value;
    }
    auto mid{(l + r) >> 1};
    auto vl{(node->left ? query(node->left.get(), a, b, l, mid) : unity_monoid)};
    auto vr{(node->right ? query(node->right.get(), a, b, mid, r) : unity_monoid)};
    return func_combine(vl, vr);
  }
};

// ----- RangePlusQuery -----
//  - update(i, x) : a[i] += x;,
//  - update(s, t, x) : a[i] += x; for all i \in [s, t),
//  - query(i) : return a[i];,
//  - query(s, t) : return the sum a[i] where i runs on [s, t).

// ----- RangeSumQuery -----
// http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=DSL_2_B&lang=ja
//  - update(i, x) : a[i] += x;,
//  - query(s, t) : return the sum a[i] where i runs on [s, t).

// ----- RangeAddQuery -----
// http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=DSL_2_E&lang=ja
//  - update(s, t, x) : a[i] += x; for all i \in [s, t),
//  - query(i) : return a[i].

// ----- RSU_RAU -----
// http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=DSL_2_G&lang=ja
//  - update(s, t, x) : a[i] += x; for all i \in [s, t),
//  - query(s, t) : return the sum a[i] where i runs on [s, t).

template <typename Monoid>
SegTree<Monoid, Monoid> RangePlusQuery(int N, Monoid const &monoid_zero)
{
  using Action = Monoid;
  return SegTree<Monoid, Action>{
      N, monoid_zero, monoid_zero,
      [](Monoid &x, Action y) { x += y; },
      [](Monoid x, Monoid y) { return x + y; },
      [](Action &x, Action y) { return x += y; },
      [](Action x, int y) { return x * static_cast<Action>(y); }};
}

template <typename Monoid>
SegTree<Monoid, Monoid> RangePlusQuery(int N)
{
  return RangePlusQuery<Monoid>(N, 0);
}

// ----- RangeMinQuery -----
//  - update(i, x) : a[i] = x;,
//  - update(s, t, x) : a[i] = x; for all i \in [s, t),
//  - query(i) : return a[i];,
//  - query(s, t) : return the minimum of a[i] where i runs on [s, t).

// ----- RangeMinimumQuery -----
// http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=DSL_2_A&lang=ja
//  - update(i, x) : a[i] = x;,
//  - query(s, t) : return the minimum of a[i] where i runs on [s, t).

// ----- RangeUpdateQuery -----
// http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=DSL_2_D&lang=ja
//  - update(s, t, x) : a[i] = x; for all i \in [s, t),
//  - query(i) : return a[i].

// ----- RMQ_RUQ -----
// http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=DSL_2_F&lang=ja
//  - update(s, t, x) : a[i] = x; for all i \in [s, t),
//  - query(s, t) : return the minimum of a[i] where i runs on [s, t).

template <typename Monoid>
SegTree<Monoid, Monoid> RangeMinQuery(int N, Monoid const &monoid_infty)
{
  using Action = Monoid;
  return SegTree<Monoid, Action>{
      N, monoid_infty, monoid_infty,
      [](Monoid &x, Action y) { x = y; },
      [](Monoid x, Monoid y) { return min(x, y); },
      [](Action &x, Action y) { return x = y; },
      [](Action x, int) { return x; }};
}

template <typename Monoid>
SegTree<Monoid, Monoid> RangeMinQuery(int N)
{
  return RangeMinQuery<Monoid>(N, numeric_limits<Monoid>::max());
}

// ----- RMQ_RAQ -----
// http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=DSL_2_H&lang=ja
//  - update(s, t, x) : a[i] += x; for all i \in [s, t),
//  - query(s, t) : return the minimum of a[i] where i runs on [s, t).
// update should be called as follows.
// tree.update(s, t, make_tuple(x, true));

template <typename Monoid>
SegTree<Monoid, tuple<Monoid, bool>> RMQ_RAQ(int N, Monoid const &monoid_zero, Monoid const &monoid_infty)
{
  using Action = tuple<Monoid, bool>;
  auto tree{SegTree<Monoid, Action>{
      N, monoid_infty, Action{monoid_zero, true},
      [](Monoid &x, Action y) {
        if (get<1>(y))
        {
          x += get<0>(y);
        }
        else
        {
          x = get<0>(y);
        }
      },
      [](Monoid x, Monoid y) { return min(x, y); },
      [](Action &x, Action y) {
        if (get<1>(y))
        {
          get<0>(x) += get<0>(y);
        }
        else
        {
          x = y;
        }
      },
      [](Action x, int) { return x; }}};
  tree.update(0, N, Action{monoid_zero, false});
  return tree;
}

template <typename Monoid>
SegTree<Monoid, tuple<Monoid, bool>> RMQ_RAQ(int N)
{
  return RMQ_RAQ<Monoid>(N, 0, numeric_limits<Monoid>::max());
}

// ----- RSQ_RUQ -----
// http://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=DSL_2_I&lang=ja
//  - update(s, t, x) : a[i] = x; for all i \in [s, t),
//  - query(s, t) : return the sum of a[i] where i runs on [s, t).

template <typename Monoid>
SegTree<Monoid, Monoid> RSQ_RUQ(int N, Monoid const &monoid_zero)
{
  using Action = Monoid;
  return SegTree<Monoid, Action>{
      N, monoid_zero, monoid_zero,
      [](Monoid &x, Action y) { x = y; },
      [](Monoid x, Monoid y) { return x + y; },
      [](Action &x, Action y) { return x = y; },
      [](Action x, int y) { return x * static_cast<Action>(y); }};
}

template <typename Monoid>
SegTree<Monoid, Monoid> RSQ_RUQ(int N)
{
  return RSQ_RUQ<Monoid>(N, 0);
}

// ----- main() -----

/*
int main()
{
  Solve solve;
  solve.flush();
}
*/

int main()
{
  int N;
  cin >> N;
  vector<int> A(N);
  for (auto i{0}; i < N; ++i)
  {
    cin >> A[i];
    A[i]--;
  }
  auto tree{RangePlusQuery<int>(N)};
  int ans{0};
  for (auto i{0}; i < N; ++i)
  {
    if (tree.query(0, A[i]) == 0)
    {
      ++ans;
    }
    tree.update(A[i], 1);
  }
  cout << ans << endl;
}


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP

#include <algorithm>

#include <boost/geometry/core/exception.hpp>

#include <boost/geometry/geometries/concepts/point_concept.hpp>
#include <boost/geometry/geometries/concepts/segment_concept.hpp>

#include <boost/geometry/arithmetic/determinant.hpp>
#include <boost/geometry/algorithms/detail/assign_values.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

// Temporary / will be Strategy as template parameter
#include <boost/geometry/strategies/side.hpp>
#include <boost/geometry/strategies/cartesian/side_by_triangle.hpp>

#include <boost/geometry/strategies/side_info.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace intersection
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <std::size_t Dimension, typename Segment, typename T>
static inline void segment_arrange(Segment const& s, T& s_1, T& s_2, bool& swapped)
{
    s_1 = get<0, Dimension>(s);
    s_2 = get<1, Dimension>(s);
    if (s_1 > s_2)
    {
        std::swap(s_1, s_2);
        swapped = true;
    }
}

template <std::size_t Index, typename Segment>
inline typename geometry::point_type<Segment>::type get_from_index(
            Segment const& segment)
{
    typedef typename geometry::point_type<Segment>::type point_type;
    point_type point;
    geometry::detail::assign::assign_point_from_index
        <
            Segment, point_type, Index, 0, dimension<Segment>::type::value
        >::apply(segment, point);
    return point;
}

}
#endif

/***
template <typename T>
inline std::string rdebug(T const& value)
{
    if (math::equals(value, 0)) return "'0'";
    if (math::equals(value, 1)) return "'1'";
    if (value < 0) return "<0";
    if (value > 1) return ">1";
    return "<0..1>";
}
***/

/*!
    \see http://mathworld.wolfram.com/Line-LineIntersection.html
 */
template <typename Policy, typename CalculationType = void>
struct relate_cartesian_segments
{
    typedef typename Policy::return_type return_type;
    typedef typename Policy::segment_type1 segment_type1;
    typedef typename Policy::segment_type2 segment_type2;

    //typedef typename point_type<segment_type1>::type point_type;
    //BOOST_CONCEPT_ASSERT( (concept::Point<point_type>) );

    BOOST_CONCEPT_ASSERT( (concept::ConstSegment<segment_type1>) );
    BOOST_CONCEPT_ASSERT( (concept::ConstSegment<segment_type2>) );

    typedef typename select_calculation_type
        <segment_type1, segment_type2, CalculationType>::type coordinate_type;

    /// Relate segments a and b
    static inline return_type apply(segment_type1 const& a, segment_type2 const& b)
    {
        coordinate_type const dx_a = get<1, 0>(a) - get<0, 0>(a); // distance in x-dir
        coordinate_type const dx_b = get<1, 0>(b) - get<0, 0>(b);
        coordinate_type const dy_a = get<1, 1>(a) - get<0, 1>(a); // distance in y-dir
        coordinate_type const dy_b = get<1, 1>(b) - get<0, 1>(b);
        return apply(a, b, dx_a, dy_a, dx_b, dy_b);
    }

    // Relate segments a and b using precalculated differences.
    // This can save two or four subtractions in many cases
    static inline return_type apply(segment_type1 const& a, segment_type2 const& b,
            coordinate_type const& dx_a, coordinate_type const& dy_a,
            coordinate_type const& dx_b, coordinate_type const& dy_b)
    {
        typedef side::side_by_triangle<coordinate_type> side;
        side_info sides;

        coordinate_type const zero = 0;
        bool const a_is_point = math::equals(dx_a, zero) && math::equals(dy_a, zero);
        bool const b_is_point = math::equals(dx_b, zero) && math::equals(dy_b, zero);

        if(a_is_point && b_is_point)
        {
            if(math::equals(get<1,0>(a), get<1,0>(b)) && math::equals(get<1,1>(a), get<1,1>(b)))
            {
                 Policy::degenerate(a, true);
            }
            else
            {
                return Policy::disjoint();
            }
        }

        bool collinear_use_first = math::abs(dx_a) + math::abs(dx_b) >= math::abs(dy_a) + math::abs(dy_b);

        sides.set<0>
            (
                side::apply(detail::get_from_index<0>(b)
                    , detail::get_from_index<1>(b)
                    , detail::get_from_index<0>(a)),
                side::apply(detail::get_from_index<0>(b)
                    , detail::get_from_index<1>(b)
                    , detail::get_from_index<1>(a))
            );
        sides.set<1>
            (
                side::apply(detail::get_from_index<0>(a)
                    , detail::get_from_index<1>(a)
                    , detail::get_from_index<0>(b)),
                side::apply(detail::get_from_index<0>(a)
                    , detail::get_from_index<1>(a)
                    , detail::get_from_index<1>(b))
            );

        bool collinear = sides.collinear();

        robustness_verify_collinear(a, b, a_is_point, b_is_point, sides, collinear);
        robustness_verify_meeting(a, b, sides, collinear, collinear_use_first);

        if (sides.same<0>() || sides.same<1>())
        {
            // Both points are at same side of other segment, we can leave
            if (robustness_verify_same_side(a, b, sides))
            {
                return Policy::disjoint();
            }
        }

        // Degenerate cases: segments of single point, lying on other segment, non disjoint
        if (a_is_point)
        {
            return Policy::degenerate(a, true);
        }
        if (b_is_point)
        {
            return Policy::degenerate(b, false);
        }

        typedef typename select_most_precise
            <
                coordinate_type, double
            >::type promoted_type;

        // r: ratio 0-1 where intersection divides A/B
        // (only calculated for non-collinear segments)
        promoted_type r;
        if (! collinear)
        {
            // Calculate determinants - Cramers rule
            coordinate_type const wx = get<0, 0>(a) - get<0, 0>(b);
            coordinate_type const wy = get<0, 1>(a) - get<0, 1>(b);
            coordinate_type const d = geometry::detail::determinant<coordinate_type>(dx_a, dy_a, dx_b, dy_b);
            coordinate_type const da = geometry::detail::determinant<coordinate_type>(dx_b, dy_b, wx, wy);

            coordinate_type const zero = coordinate_type();
            if (math::equals(d, zero))
            {
                // This is still a collinear case (because of FP imprecision this can occur here)
                // sides.debug();
                sides.set<0>(0,0);
                sides.set<1>(0,0);
                collinear = true;
            }
            else
            {
                r = promoted_type(da) / promoted_type(d);

                if (! robustness_verify_r(a, b, r))
                {
                    return Policy::disjoint();
                }

                robustness_handle_meeting(a, b, sides, dx_a, dy_a, wx, wy, d, r);

                if (robustness_verify_disjoint_at_one_collinear(a, b, sides))
                {
                    return Policy::disjoint();
                }

            }
        }

        if(collinear)
        {
            if (collinear_use_first)
            {
                return relate_collinear<0>(a, b);
            }
            else
            {
                // Y direction contains larger segments (maybe dx is zero)
                return relate_collinear<1>(a, b);
            }
        }

        return Policy::segments_intersect(sides, r,
            dx_a, dy_a, dx_b, dy_b,
            a, b);
    }

private :

    // Ratio should lie between 0 and 1
    // Also these three conditions might be of FP imprecision, the segments were actually (nearly) collinear
    template <typename T>
    static inline bool robustness_verify_r(
                segment_type1 const& a, segment_type2 const& b,
                T& r)
    {
        T const zero = 0;
        T const one = 1;
        if (r < zero || r > one)
        {
            if (verify_disjoint<0>(a, b) || verify_disjoint<1>(a, b))
            {
                // Can still be disjoint (even if not one is left or right from another)
                // This is e.g. in case #snake4 of buffer test.
                return false;
            }

            //std::cout << "ROBUSTNESS: correction of r " << r << std::endl;
            // sides.debug();

            // ROBUSTNESS: the r value can in epsilon-cases much larger than 1, while (with perfect arithmetic)
            // it should be one. It can be 1.14 or even 1.98049 or 2 (while still intersecting)

            // If segments are crossing (we can see that with the sides)
            // and one is inside the other, there must be an intersection point.
            // We correct for that.
            // This is (only) in case #ggl_list_20110820_christophe in unit tests

            // If segments are touching (two sides zero), of course they should intersect
            // This is (only) in case #buffer_rt_i in the unit tests)

            // If one touches in the middle, they also should intersect (#buffer_rt_j)

            // Note that even for ttmath r is occasionally > 1, e.g. 1.0000000000000000000000036191231203575

            if (r > one)
            {
                r = one;
            }
            else if (r < zero)
            {
                r = zero;
            }
        }
        return true;
    }

    static inline void robustness_verify_collinear(
                segment_type1 const& a, segment_type2 const& b,
                bool a_is_point, bool b_is_point,
                side_info& sides,
                bool& collinear)
    {
        if ((sides.zero<0>() && ! b_is_point && ! sides.zero<1>()) || (sides.zero<1>() && ! a_is_point && ! sides.zero<0>()))
        {
            // If one of the segments is collinear, the other must be as well.
            // So handle it as collinear.
            // (In float/double epsilon margins it can easily occur that one or two of them are -1/1)
            // sides.debug();
            sides.set<0>(0,0);
            sides.set<1>(0,0);
            collinear = true;
        }
    }

    static inline void robustness_verify_meeting(
                segment_type1 const& a, segment_type2 const& b,
                side_info& sides,
                bool& collinear, bool& collinear_use_first)
    {
        if (sides.meeting())
        {
            // If two segments meet each other at their segment-points, two sides are zero,
            // the other two are not (unless collinear but we don't mean those here).
            // However, in near-epsilon ranges it can happen that two sides are zero
            // but they do not meet at their segment-points.
            // In that case they are nearly collinear and handled as such.
            if (! point_equals
                    (
                        select(sides.zero_index<0>(), a),
                        select(sides.zero_index<1>(), b)
                    )
                )
            {
                sides.set<0>(0,0);
                sides.set<1>(0,0);
                collinear = true;

                if (collinear_use_first && analyse_equal<0>(a, b))
                {
                    collinear_use_first = false;
                }
                else if (! collinear_use_first && analyse_equal<1>(a, b))
                {
                    collinear_use_first = true;
                }

            }
        }
    }

    // Verifies and if necessary correct missed touch because of robustness
    // This is the case at multi_polygon_buffer unittest #rt_m
    static inline bool robustness_verify_same_side(
                segment_type1 const& a, segment_type2 const& b,
                side_info& sides)
    {
        int corrected = 0;
        if (sides.one_touching<0>())
        {
            if (point_equals(
                        select(sides.zero_index<0>(), a),
                        select(0, b)
                    ))
            {
                sides.correct_to_zero<1, 0>();
                corrected = 1;
            }
            if (point_equals
                    (
                        select(sides.zero_index<0>(), a),
                        select(1, b)
                    ))
            {
                sides.correct_to_zero<1, 1>();
                corrected = 2;
            }
        }
        else if (sides.one_touching<1>())
        {
            if (point_equals(
                        select(sides.zero_index<1>(), b),
                        select(0, a)
                    ))
            {
                sides.correct_to_zero<0, 0>();
                corrected = 3;
            }
            if (point_equals
                    (
                        select(sides.zero_index<1>(), b),
                        select(1, a)
                    ))
            {
                sides.correct_to_zero<0, 1>();
                corrected = 4;
            }
        }

        return corrected == 0;
    }

    static inline bool robustness_verify_disjoint_at_one_collinear(
                segment_type1 const& a, segment_type2 const& b,
                side_info const& sides)
    {
        if (sides.one_of_all_zero())
        {
            if (verify_disjoint<0>(a, b) || verify_disjoint<1>(a, b))
            {
                return true;
            }
        }
        return false;
    }

    // If r is one, or zero, segments should meet and their endpoints.
    // Robustness issue: check if this is really the case.
    // It turns out to be no problem, see buffer test #rt_s1 (and there are many cases generated)
    // It generates an "ends in the middle" situation which is correct.
    template <typename T, typename R>
    static inline void robustness_handle_meeting(segment_type1 const& a, segment_type2 const& b,
                side_info& sides,
                T const& dx_a, T const& dy_a, T const& wx, T const& wy,
                T const& d, R const& r)
    {
        return;

        T const db = geometry::detail::determinant<T>(dx_a, dy_a, wx, wy);

        R const zero = 0;
        R const one = 1;
        if (math::equals(r, zero) || math::equals(r, one))
        {
            R rb = db / d;
            if (rb <= 0 || rb >= 1 || math::equals(rb, 0) || math::equals(rb, 1))
            {
                if (sides.one_zero<0>() && ! sides.one_zero<1>()) // or vice versa
                {
#if defined(BOOST_GEOMETRY_COUNT_INTERSECTION_EQUAL)
                    extern int g_count_intersection_equal;
                    g_count_intersection_equal++;
#endif
                    sides.debug();
                    std::cout << "E r=" << r << " r.b=" << rb << " ";
                }
            }
        }
    }

    template <std::size_t Dimension>
    static inline bool verify_disjoint(segment_type1 const& a,
                    segment_type2 const& b)
    {
        coordinate_type a_1, a_2, b_1, b_2;
        bool a_swapped = false, b_swapped = false;
        detail::segment_arrange<Dimension>(a, a_1, a_2, a_swapped);
        detail::segment_arrange<Dimension>(b, b_1, b_2, b_swapped);
        return math::smaller(a_2, b_1) || math::larger(a_1, b_2);
    }

    template <typename Segment>
    static inline typename point_type<Segment>::type select(int index, Segment const& segment)
    {
        return index == 0
            ? detail::get_from_index<0>(segment)
            : detail::get_from_index<1>(segment)
            ;
    }

    // We cannot use geometry::equals here. Besides that this will be changed
    // to compare segment-coordinate-values directly (not necessary to retrieve point first)
    template <typename Point1, typename Point2>
    static inline bool point_equals(Point1 const& point1, Point2 const& point2)
    {
        return math::equals(get<0>(point1), get<0>(point2))
            && math::equals(get<1>(point1), get<1>(point2))
            ;
    }

    // We cannot use geometry::equals here. Besides that this will be changed
    // to compare segment-coordinate-values directly (not necessary to retrieve point first)
    template <typename Point1, typename Point2>
    static inline bool point_equality(Point1 const& point1, Point2 const& point2,
                    bool& equals_0, bool& equals_1)
    {
        equals_0 = math::equals(get<0>(point1), get<0>(point2));
        equals_1 = math::equals(get<1>(point1), get<1>(point2));
        return equals_0 && equals_1;
    }

    template <std::size_t Dimension>
    static inline bool analyse_equal(segment_type1 const& a, segment_type2 const& b)
    {
        coordinate_type const a_1 = geometry::get<0, Dimension>(a);
        coordinate_type const a_2 = geometry::get<1, Dimension>(a);
        coordinate_type const b_1 = geometry::get<0, Dimension>(b);
        coordinate_type const b_2 = geometry::get<1, Dimension>(b);
        return math::equals(a_1, b_1)
            || math::equals(a_2, b_1)
            || math::equals(a_1, b_2)
            || math::equals(a_2, b_2)
            ;
    }

    template <std::size_t Dimension>
    static inline return_type relate_collinear(segment_type1 const& a,
                                               segment_type2 const& b)
    {
        coordinate_type a_1, a_2, b_1, b_2;
        bool a_swapped = false, b_swapped = false;
        detail::segment_arrange<Dimension>(a, a_1, a_2, a_swapped);
        detail::segment_arrange<Dimension>(b, b_1, b_2, b_swapped);
        if (math::smaller(a_2, b_1) || math::larger(a_1, b_2))
        //if (a_2 < b_1 || a_1 > b_2)
        {
            return Policy::disjoint();
        }
        return relate_collinear(a, b, a_1, a_2, b_1, b_2, a_swapped, b_swapped);
    }

    /// Relate segments known collinear
    static inline return_type relate_collinear(segment_type1 const& a
            , segment_type2 const& b
            , coordinate_type a_1, coordinate_type a_2
            , coordinate_type b_1, coordinate_type b_2
            , bool a_swapped, bool b_swapped)
    {
        // All ca. 150 lines are about collinear rays
        // The intersections, if any, are always boundary points of the segments. No need to calculate anything.
        // However we want to find out HOW they intersect, there are many cases.
        // Most sources only provide the intersection (above) or that there is a collinearity (but not the points)
        // or some spare sources give the intersection points (calculated) but not how they align.
        // This source tries to give everything and still be efficient.
        // It is therefore (and because of the extensive clarification comments) rather long...

        // \see http://mpa.itc.it/radim/g50history/CMP/4.2.1-CERL-beta-libes/file475.txt
        // \see http://docs.codehaus.org/display/GEOTDOC/Point+Set+Theory+and+the+DE-9IM+Matrix
        // \see http://mathworld.wolfram.com/Line-LineIntersection.html

        // Because of collinearity the case is now one-dimensional and can be checked using intervals
        // This function is called either horizontally or vertically
        // We get then two intervals:
        // a_1-------------a_2 where a_1 < a_2
        // b_1-------------b_2 where b_1 < b_2
        // In all figures below a_1/a_2 denotes arranged intervals, a1-a2 or a2-a1 are still unarranged

        // Handle "equal", in polygon neighbourhood comparisons a common case

        bool const opposite = a_swapped ^ b_swapped;
        bool const both_swapped = a_swapped && b_swapped;

        // Check if segments are equal or opposite equal...
        bool const swapped_a1_eq_b1 = math::equals(a_1, b_1);
        bool const swapped_a2_eq_b2 = math::equals(a_2, b_2);

        if (swapped_a1_eq_b1 && swapped_a2_eq_b2)
        {
            return Policy::segment_equal(a, opposite);
        }

        bool const swapped_a2_eq_b1 = math::equals(a_2, b_1);
        bool const swapped_a1_eq_b2 = math::equals(a_1, b_2);

        bool const a1_eq_b1 = both_swapped ? swapped_a2_eq_b2 : a_swapped ? swapped_a2_eq_b1 : b_swapped ? swapped_a1_eq_b2 : swapped_a1_eq_b1;
        bool const a2_eq_b2 = both_swapped ? swapped_a1_eq_b1 : a_swapped ? swapped_a1_eq_b2 : b_swapped ? swapped_a2_eq_b1 : swapped_a2_eq_b2;

        bool const a1_eq_b2 = both_swapped ? swapped_a2_eq_b1 : a_swapped ? swapped_a2_eq_b2 : b_swapped ? swapped_a1_eq_b1 : swapped_a1_eq_b2;
        bool const a2_eq_b1 = both_swapped ? swapped_a1_eq_b2 : a_swapped ? swapped_a1_eq_b1 : b_swapped ? swapped_a2_eq_b2 : swapped_a2_eq_b1;

        // The rest below will return one or two intersections.
        // The delegated class can decide which is the intersection point, or two, build the Intersection Matrix (IM)
        // For IM it is important to know which relates to which. So this information is given,
        // without performance penalties to intersection calculation

        bool const has_common_points = swapped_a1_eq_b1 || swapped_a1_eq_b2 || swapped_a2_eq_b1 || swapped_a2_eq_b2;

        // "Touch" -> one intersection point -> one but not two common points
        // -------->             A (or B)
        //         <----------   B (or A)
        //        a_2==b_1         (b_2==a_1 or a_2==b1)

        // The check a_2/b_1 is necessary because it excludes cases like
        // ------->
        //     --->
        // ... which are handled lateron

        // Corresponds to 4 cases, of which the equal points are determined above
        // #1: a1---->a2 b1--->b2   (a arrives at b's border)
        // #2: a2<----a1 b2<---b1   (b arrives at a's border)
        // #3: a1---->a2 b2<---b1   (both arrive at each others border)
        // #4: a2<----a1 b1--->b2   (no arrival at all)
        // Where the arranged forms have two forms:
        //    a_1-----a_2/b_1-------b_2 or reverse (B left of A)
        if ((swapped_a2_eq_b1 || swapped_a1_eq_b2) && ! swapped_a1_eq_b1 && ! swapped_a2_eq_b2)
        {
            if (a2_eq_b1) return Policy::collinear_touch(get<1, 0>(a), get<1, 1>(a), 0, -1);
            if (a1_eq_b2) return Policy::collinear_touch(get<0, 0>(a), get<0, 1>(a), -1, 0);
            if (a2_eq_b2) return Policy::collinear_touch(get<1, 0>(a), get<1, 1>(a), 0, 0);
            if (a1_eq_b1) return Policy::collinear_touch(get<0, 0>(a), get<0, 1>(a), -1, -1);
        }

        // "Touch/within" -> there are common points and also an intersection of interiors:
        // Corresponds to many cases:
        // #1a: a1------->a2  #1b:        a1-->a2
        //          b1--->b2         b1------->b2
        // #2a: a2<-------a1  #2b:        a2<--a1
        //          b1--->b2         b1------->b2
        // #3a: a1------->a2  #3b:        a1-->a2
        //          b2<---b1         b2<-------b1
        // #4a: a2<-------a1  #4b:        a2<--a1
        //          b2<---b1         b2<-------b1

        // Note: next cases are similar and handled by the code
        // #4c: a1--->a2
        //      b1-------->b2
        // #4d: a1-------->a2
        //      b1-->b2

        // For case 1-4: a_1 < (b_1 or b_2) < a_2, two intersections are equal to segment B
        // For case 5-8: b_1 < (a_1 or a_2) < b_2, two intersections are equal to segment A
        if (has_common_points)
        {
            // Either A is in B, or B is in A, or (in case of robustness/equals)
            // both are true, see below
            bool a_in_b = (b_1 < a_1 && a_1 < b_2) || (b_1 < a_2 && a_2 < b_2);
            bool b_in_a = (a_1 < b_1 && b_1 < a_2) || (a_1 < b_2 && b_2 < a_2);

            if (a_in_b && b_in_a)
            {
                // testcase "ggl_list_20110306_javier"
                // In robustness it can occur that a point of A is inside B AND a point of B is inside A,
                // still while has_common_points is true (so one point equals the other).
                // If that is the case we select on length.
                coordinate_type const length_a = geometry::math::abs(a_1 - a_2);
                coordinate_type const length_b = geometry::math::abs(b_1 - b_2);
                if (length_a > length_b)
                {
                    a_in_b = false;
                }
                else
                {
                    b_in_a = false;
                }
            }

            int const arrival_a = a_in_b ? 1 : -1;
            if (a2_eq_b2) return Policy::collinear_interior_boundary_intersect(a_in_b ? a : b, a_in_b, 0, 0, false);
            if (a1_eq_b2) return Policy::collinear_interior_boundary_intersect(a_in_b ? a : b, a_in_b, arrival_a, 0, true);
            if (a2_eq_b1) return Policy::collinear_interior_boundary_intersect(a_in_b ? a : b, a_in_b, 0, -arrival_a, true);
            if (a1_eq_b1) return Policy::collinear_interior_boundary_intersect(a_in_b ? a : b, a_in_b, arrival_a, -arrival_a, false);
        }

        // "Inside", a completely within b or b completely within a
        // 2 cases:
        // case 1:
        //        a_1---a_2        -> take A's points as intersection points
        //   b_1------------b_2
        // case 2:
        //   a_1------------a_2
        //       b_1---b_2         -> take B's points
        if (a_1 > b_1 && a_2 < b_2)
        {
            // A within B
            return Policy::collinear_a_in_b(a, opposite);
        }
        if (b_1 > a_1 && b_2 < a_2)
        {
            // B within A
            return Policy::collinear_b_in_a(b, opposite);
        }

        /*

        Now that all cases with equal,touch,inside,disjoint,
        degenerate are handled the only thing left is an overlap

        Either a1 is between b1,b2
        or a2 is between b1,b2 (a2 arrives)

        Next table gives an overview.
        The IP's are ordered following the line A1->A2

             |                                 |
             |          a_2 in between         |       a_1 in between
             |                                 |
        -----+---------------------------------+--------------------------
             |   a1--------->a2                |       a1--------->a2
             |          b1----->b2             |   b1----->b2
             |   (b1,a2), a arrives            |   (a1,b2), b arrives
             |                                 |
        -----+---------------------------------+--------------------------
        a sw.|   a2<---------a1*               |       a2<---------a1*
             |           b1----->b2            |   b1----->b2
             |   (a1,b1), no arrival           |   (b2,a2), a and b arrive
             |                                 |
        -----+---------------------------------+--------------------------
             |   a1--------->a2                |       a1--------->a2
        b sw.|           b2<-----b1            |   b2<-----b1
             |   (b2,a2), a and b arrive       |   (a1,b1), no arrival
             |                                 |
        -----+---------------------------------+--------------------------
        a sw.|    a2<---------a1*              |       a2<---------a1*
        b sw.|            b2<-----b1           |   b2<-----b1
             |   (a1,b2), b arrives            |   (b1,a2), a arrives
             |                                 |
        -----+---------------------------------+--------------------------
        * Note that a_1 < a_2, and a1 <> a_1; if a is swapped,
          the picture might seem wrong but it (supposed to be) is right.
        */

        if (b_1 < a_2 && a_2 < b_2)
        {
            // Left column, from bottom to top
            return
                both_swapped ? Policy::collinear_overlaps(get<0, 0>(a), get<0, 1>(a), get<1, 0>(b), get<1, 1>(b), -1,  1, opposite)
                : b_swapped  ? Policy::collinear_overlaps(get<1, 0>(b), get<1, 1>(b), get<1, 0>(a), get<1, 1>(a),  1,  1, opposite)
                : a_swapped  ? Policy::collinear_overlaps(get<0, 0>(a), get<0, 1>(a), get<0, 0>(b), get<0, 1>(b), -1, -1, opposite)
                :              Policy::collinear_overlaps(get<0, 0>(b), get<0, 1>(b), get<1, 0>(a), get<1, 1>(a),  1, -1, opposite)
                ;
        }
        if (b_1 < a_1 && a_1 < b_2)
        {
            // Right column, from bottom to top
            return
                both_swapped ? Policy::collinear_overlaps(get<0, 0>(b), get<0, 1>(b), get<1, 0>(a), get<1, 1>(a),  1, -1, opposite)
                : b_swapped  ? Policy::collinear_overlaps(get<0, 0>(a), get<0, 1>(a), get<0, 0>(b), get<0, 1>(b), -1, -1, opposite)
                : a_swapped  ? Policy::collinear_overlaps(get<1, 0>(b), get<1, 1>(b), get<1, 0>(a), get<1, 1>(a),  1,  1, opposite)
                :              Policy::collinear_overlaps(get<0, 0>(a), get<0, 1>(a), get<1, 0>(b), get<1, 1>(b), -1,  1, opposite)
                ;
        }
        // Nothing should goes through. If any we have made an error
        // std::cout << "Robustness issue, non-logical behaviour" << std::endl;
        return Policy::error("Robustness issue, non-logical behaviour");
    }
};

}} // namespace strategy::intersection

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INTERSECTION_HPP


#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <bench/BenchTimer.h>

using namespace Eigen;

template<typename Quat>
EIGEN_DONT_INLINE void quatmul_default(const Quat& a, const Quat& b, Quat& c)
{
  c = a * b;
}

template<typename Quat>
EIGEN_DONT_INLINE void quatmul_novec(const Quat& a, const Quat& b, Quat& c)
{
  c = internal::quat_product<0, Quat, Quat, typename Quat::Scalar, Aligned>::run(a,b);
}

template<typename Quat> void bench(const std::string& label)
{
  int tries = 10;
  int rep = 1000000;
  BenchTimer t;

  Quat a(4, 1, 2, 3);
  Quat b(2, 3, 4, 5);
  Quat c;

  std::cout.precision(3);

  BENCH(t, tries, rep, quatmul_default(a,b,c));
  std::cout << label << " default " << 1e3*t.best(CPU_TIMER) << "ms  \t" << 1e-6*double(rep)/(t.best(CPU_TIMER)) << " M mul/s\n";

  BENCH(t, tries, rep, quatmul_novec(a,b,c));
  std::cout << label << " novec   " << 1e3*t.best(CPU_TIMER) << "ms  \t" << 1e-6*double(rep)/(t.best(CPU_TIMER)) << " M mul/s\n";
}

int main()
{
  bench<Quaternionf>("float ");
  bench<Quaterniond>("double");

  return 0;

}


////////////////////////////////////////////////////////////////////////
// exhaustive test in simd mode for functor nt2::arg
//        versus a0>=0 ? 0 : nt2::Pi<r_t>() with float elements
////////////////////////////////////////////////////////////////////////
#include <nt2/include/functions/arg.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/include/functions/successor.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/include/constants/real.hpp>
#include <nt2/include/functions/iround.hpp>
#include <cmath>

#include <nt2/sdk/meta/cardinal_of.hpp>
#include <nt2/include/functions/load.hpp>
#include <nt2/include/functions/min.hpp>
#include <iostream>
typedef BOOST_SIMD_DEFAULT_EXTENSION               ext_t;
typedef boost::simd::native<float,ext_t>  n_t;
typedef typename nt2::meta::as_integer<n_t>::type   in_t;
typedef typename nt2::meta::call<arg_(n_t)>::type r_t;
static inline r_t repfunc(const float & a0){
   return a0>=0 ? 0 : nt2::Pi<r_t>()(a0);
}
int main(){
  float mini = nt2::Valmin<float>();
  float maxi = nt2::Valmax<float>();
  const nt2::uint32_t N = nt2::meta::cardinal_of<n_t>::value;
  const in_t vN = nt2::splat<in_t>(N);
  const nt2::uint32_t M =  10;
  nt2::uint32_t histo[M+1];
  for(nt2::uint32_t i = 0; i < M; i++) histo[i] = 0;
  float a[N];
  a[0] = mini;
  for(nt2::uint32_t i = 1; i < N; i++)
    a[i] = nt2::successor(a[i-1], 1);
  n_t a0 = nt2::load<n_t>(&a[0],0);
  nt2::uint32_t k = 0;
  std::cout << "a line of points to wait for... be patient!" << std::endl;
  for(; a0[N-1] < maxi; a0 = nt2::successor(a0, vN))
    {
      n_t z =  nt2::arg(a0);
      for(nt2::uint32_t i = 0; i < N; i++)
        {
           float v = repfunc(a0[i]);
           float sz = z[i];
           ++histo[nt2::min(M, nt2::iround(2*nt2::ulpdist(v, sz)))];
           ++k;
           if (k%100000000 == 0){
             std::cout << "." << std::flush; ++j;
             if (j == 80){std::cout << std::endl; j = 0;}
           }
        }
      }
    std::cout << "exhaustive test for " << std::endl;
    std::cout << " nt2::arg versus a0>=0 ? 0 : nt2::Pi<r_t>() " << std::endl;
    std::cout << " in simd mode and float type" << std::endl;
    for(nt2::uint32_t i = 0; i < M; i++)
      std::cout << i/2.0 << " -> " << histo[i] << std::endl;
    std::cout << k << " values computed" << std::endl;
    std::cout << std::endl;
    std::cout << std::endl;
    for(nt2::uint32_t i = 0; i < M; i++)
      std::cout << i/2.0 << " -> "
                << (histo[i]*100.0/k) << "%" << std::endl;
    return 0;
}


/**
 * \copyright
 * Copyright (c) 2012-2019, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "EigenLinearSolver.h"

#include <logog/include/logog.hpp>

#ifdef USE_MKL
#include <Eigen/PardisoSupport>
#endif

#ifdef USE_EIGEN_UNSUPPORTED
#include <Eigen/Sparse>
#include <unsupported/Eigen/src/IterativeSolvers/GMRES.h>
#include <unsupported/Eigen/src/IterativeSolvers/Scaling.h>
#endif

#include "BaseLib/ConfigTree.h"
#include "EigenVector.h"
#include "EigenMatrix.h"
#include "EigenTools.h"

#include "MathLib/LinAlg/LinearSolverOptions.h"

namespace MathLib
{

// TODO change to LinearSolver
class EigenLinearSolverBase
{
public:
    using Vector = EigenVector::RawVectorType;
    using Matrix = EigenMatrix::RawMatrixType;

    virtual ~EigenLinearSolverBase() = default;

    //! Solves the linear equation system \f$ A x = b \f$ for \f$ x \f$.
    virtual bool solve(Matrix &A, Vector const& b, Vector &x, EigenOption &opt) = 0;
};

namespace details
{

/// Template class for Eigen direct linear solvers
template <class T_SOLVER>
class EigenDirectLinearSolver final : public EigenLinearSolverBase
{
public:
    bool solve(Matrix& A, Vector const& b, Vector& x, EigenOption& opt) override
    {
        INFO("-> solve with %s",
             EigenOption::getSolverName(opt.solver_type).c_str());
        if (!A.isCompressed())
        {
            A.makeCompressed();
        }

        _solver.compute(A);
        if(_solver.info()!=Eigen::Success) {
            ERR("Failed during Eigen linear solver initialization");
            return false;
        }

        x = _solver.solve(b);
        if(_solver.info()!=Eigen::Success) {
            ERR("Failed during Eigen linear solve");
            return false;
        }

        return true;
    }

private:
    T_SOLVER _solver;
};

/// Template class for Eigen iterative linear solvers
template <class T_SOLVER>
class EigenIterativeLinearSolver final : public EigenLinearSolverBase
{
public:
    bool solve(Matrix& A, Vector const& b, Vector& x, EigenOption& opt) override
    {
        INFO("-> solve with %s (precon %s)",
             EigenOption::getSolverName(opt.solver_type).c_str(),
             EigenOption::getPreconName(opt.precon_type).c_str());
        _solver.setTolerance(opt.error_tolerance);
        _solver.setMaxIterations(opt.max_iterations);

        if (!A.isCompressed())
        {
            A.makeCompressed();
        }

        _solver.compute(A);
        if(_solver.info()!=Eigen::Success) {
            ERR("Failed during Eigen linear solver initialization");
            return false;
        }

        x = _solver.solveWithGuess(b, x);
        INFO("\t iteration: %d/%ld", _solver.iterations(), opt.max_iterations);
        INFO("\t residual: %e\n", _solver.error());

        if(_solver.info()!=Eigen::Success) {
            ERR("Failed during Eigen linear solve");
            return false;
        }

        return true;
    }

private:
    T_SOLVER _solver;
};

template <template <typename, typename> class Solver, typename Precon>
std::unique_ptr<EigenLinearSolverBase> createIterativeSolver()
{
    using Slv = EigenIterativeLinearSolver<
        Solver<EigenMatrix::RawMatrixType, Precon>>;
    return std::make_unique<Slv>();
}

template <template <typename, typename> class Solver>
std::unique_ptr<EigenLinearSolverBase> createIterativeSolver(
    EigenOption::PreconType precon_type)
{
    switch (precon_type) {
        case EigenOption::PreconType::NONE:
            return createIterativeSolver<Solver,
                                         Eigen::IdentityPreconditioner>();
        case EigenOption::PreconType::DIAGONAL:
            return createIterativeSolver<
                Solver, Eigen::DiagonalPreconditioner<double>>();
        case EigenOption::PreconType::ILUT:
            // TODO for this preconditioner further options can be passed.
            // see https://eigen.tuxfamily.org/dox/classEigen_1_1IncompleteLUT.html
            return createIterativeSolver<
                Solver, Eigen::IncompleteLUT<double>>();
        default:
            OGS_FATAL("Invalid Eigen preconditioner type.");
    }
}

template <typename Mat, typename Precon>
using EigenCGSolver = Eigen::ConjugateGradient<Mat, Eigen::Lower, Precon>;

std::unique_ptr<EigenLinearSolverBase> createIterativeSolver(
    EigenOption::SolverType solver_type, EigenOption::PreconType precon_type)
{
    switch (solver_type) {
        case EigenOption::SolverType::BiCGSTAB: {
            return createIterativeSolver<Eigen::BiCGSTAB>(precon_type);
        }
        case EigenOption::SolverType::CG: {
            return createIterativeSolver<EigenCGSolver>(precon_type);
        }
        case EigenOption::SolverType::GMRES: {
#ifdef USE_EIGEN_UNSUPPORTED
            return createIterativeSolver<Eigen::GMRES>(precon_type);
#else
            OGS_FATAL(
                "The code is not compiled with the Eigen unsupported modules. "
                "Linear solver type GMRES is not available.");
#endif
        }
        default:
            OGS_FATAL("Invalid Eigen iterative linear solver type. Aborting.");
    }
}

}  // namespace details

EigenLinearSolver::EigenLinearSolver(
                            const std::string& /*solver_name*/,
                            const BaseLib::ConfigTree* const option)
{
    using Matrix = EigenMatrix::RawMatrixType;

    if (option)
    {
        setOption(*option);
    }

    // TODO for my taste it is much too unobvious that the default solver type
    //      currently is SparseLU.
    switch (_option.solver_type) {
        case EigenOption::SolverType::SparseLU: {
            using SolverType =
                Eigen::SparseLU<Matrix, Eigen::COLAMDOrdering<int>>;
            _solver = std::make_unique<
                details::EigenDirectLinearSolver<SolverType>>();
            return;
        }
        case EigenOption::SolverType::BiCGSTAB:
        case EigenOption::SolverType::CG:
        case EigenOption::SolverType::GMRES:
            _solver = details::createIterativeSolver(_option.solver_type,
                                                     _option.precon_type);
            return;
        case EigenOption::SolverType::PardisoLU: {
#ifdef USE_MKL
            using SolverType = Eigen::PardisoLU<EigenMatrix::RawMatrixType>;
            _solver.reset(new details::EigenDirectLinearSolver<SolverType>);
            return;
#else
            OGS_FATAL(
                "The code is not compiled with Intel MKL. Linear solver type "
                "PardisoLU is not available.");
#endif
        }
    }

    OGS_FATAL("Invalid Eigen linear solver type. Aborting.");
}

EigenLinearSolver::~EigenLinearSolver() = default;

void EigenLinearSolver::setOption(BaseLib::ConfigTree const& option)
{
    ignoreOtherLinearSolvers(option, "eigen");
    //! \ogs_file_param{prj__linear_solvers__linear_solver__eigen}
    auto const ptSolver = option.getConfigSubtreeOptional("eigen");
    if (!ptSolver)
    {
        return;
    }

    if (auto solver_type =
            //! \ogs_file_param{prj__linear_solvers__linear_solver__eigen__solver_type}
            ptSolver->getConfigParameterOptional<std::string>("solver_type")) {
        _option.solver_type = MathLib::EigenOption::getSolverType(*solver_type);
    }
    if (auto precon_type =
            //! \ogs_file_param{prj__linear_solvers__linear_solver__eigen__precon_type}
            ptSolver->getConfigParameterOptional<std::string>("precon_type")) {
        _option.precon_type = MathLib::EigenOption::getPreconType(*precon_type);
    }
    if (auto error_tolerance =
            //! \ogs_file_param{prj__linear_solvers__linear_solver__eigen__error_tolerance}
            ptSolver->getConfigParameterOptional<double>("error_tolerance")) {
        _option.error_tolerance = *error_tolerance;
    }
    if (auto max_iteration_step =
            //! \ogs_file_param{prj__linear_solvers__linear_solver__eigen__max_iteration_step}
            ptSolver->getConfigParameterOptional<int>("max_iteration_step")) {
        _option.max_iterations = *max_iteration_step;
    }
    if (auto scaling =
            //! \ogs_file_param{prj__linear_solvers__linear_solver__eigen__scaling}
            ptSolver->getConfigParameterOptional<bool>("scaling")) {
#ifdef USE_EIGEN_UNSUPPORTED
        _option.scaling = *scaling;
#else
        OGS_FATAL(
            "The code is not compiled with the Eigen unsupported modules. "
            "scaling is not available.");
#endif
    }
}

bool EigenLinearSolver::solve(EigenMatrix &A, EigenVector& b, EigenVector &x)
{
    INFO("------------------------------------------------------------------");
    INFO("*** Eigen solver computation");

#ifdef USE_EIGEN_UNSUPPORTED
    std::unique_ptr<Eigen::IterScaling<EigenMatrix::RawMatrixType>> scal;
    if (_option.scaling)
    {
        INFO("-> scale");
        scal =
            std::make_unique<Eigen::IterScaling<EigenMatrix::RawMatrixType>>();
        scal->computeRef(A.getRawMatrix());
        b.getRawVector() = scal->LeftScaling().cwiseProduct(b.getRawVector());
    }
#endif
    auto const success = _solver->solve(A.getRawMatrix(), b.getRawVector(),
                                        x.getRawVector(), _option);
#ifdef USE_EIGEN_UNSUPPORTED
    if (scal)
    {
        x.getRawVector() = scal->RightScaling().cwiseProduct(x.getRawVector());
    }
#endif

    INFO("------------------------------------------------------------------");

    return success;
}

}  // namespace MathLib


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

#include <boost/hana/assert.hpp>
#include <boost/hana/integral_constant.hpp>
using namespace boost::hana;

int main() {

{

//! [mult]
BOOST_HANA_CONSTANT_CHECK(mult(int_<3>, int_<5>) == int_<15>);
BOOST_HANA_CONSTEXPR_CHECK(mult(4, 2) == 8);

//! [mult]

}{

//! [one]
BOOST_HANA_CONSTANT_CHECK(one<IntegralConstant<int>>() == int_<1>);
BOOST_HANA_CONSTEXPR_CHECK(one<long>() == 1l);
//! [one]

}{

//! [power]
BOOST_HANA_CONSTANT_CHECK(power(int_<3>, int_<2>) == int_<3 * 3>);
BOOST_HANA_CONSTEXPR_CHECK(power(2, int_<4>) == 16);
//! [power]

}

}


#pragma once

#include <Eigen/Geometry>
#include "autd3.hpp"

namespace dynaman {

	std::vector<std::vector<size_t>> combination(size_t max, size_t num);
}


//  (C) Copyright Matt Borland 2021.
//  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)

#include <cmath>
#include <cfloat>
#include <cstdint>
#include <limits>
#include <type_traits>
#include <boost/math/ccmath/isfinite.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

template <typename T>
void test()
{
    if constexpr (std::numeric_limits<T>::has_quiet_NaN)
    {
        static_assert(!boost::math::ccmath::isfinite(std::numeric_limits<T>::quiet_NaN()), "Wrong response to NAN");
    }

    static_assert(boost::math::ccmath::isfinite(T(0)), "Wrong response to 0");

    if constexpr (!std::is_integral_v<T>)
    {
        static_assert(boost::math::ccmath::isfinite((std::numeric_limits<T>::min)()/2), "Wrong response to subnormal");
        static_assert(!boost::math::ccmath::isfinite(std::numeric_limits<T>::infinity()), "Wrong response to infinity");
    }
    else
    {
        // Integer types define infinity as 0
        // https://en.cppreference.com/w/cpp/types/numeric_limits/infinity
        static_assert(boost::math::ccmath::isfinite(std::numeric_limits<T>::infinity()), "Wrong response to infinity");
    }
}

#ifndef BOOST_MATH_NO_CONSTEXPR_DETECTION
int main()
{
    test<float>();
    test<double>();

    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test<long double>();
    #endif

    #if defined(BOOST_HAS_FLOAT128) && !defined(BOOST_MATH_USING_BUILTIN_CONSTANT_P)
    test<boost::multiprecision::float128>();
    #endif

    test<int>();
    test<unsigned>();
    test<long>();
    test<std::int32_t>();
    test<std::int64_t>();
    test<std::uint32_t>();

    return 0;
}
#else
int main()
{
    return 0;
}
#endif


#include <iostream>
#include <mtl/dense1D.h>
#include <mtl/mtl.h>

/*
  example output:

  60

  */

int
main()
{
  using namespace mtl;
  //begin
  dense1D<double> x(10, 2), y(10, 3);
  double s = dot(x, y);
  //end
  std::cout << s << std::endl;
  return 0;
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "math-precomp.h"  // Precompiled headers
//
#include <mrpt/math/CQuaternion.h>
#include <mrpt/math/TPoint2D.h>
#include <mrpt/math/TPoint3D.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/TPose3D.h>
#include <mrpt/math/homog_matrices.h>  // homogeneousMatrixInverse()

#include <Eigen/Dense>

using namespace mrpt::math;

static_assert(std::is_trivially_copyable_v<TPose3D>);

TPose3D::TPose3D(const TPoint2D& p)
	: x(p.x), y(p.y), z(0.0), yaw(0.0), pitch(0.0), roll(0.0)
{
}
TPose3D::TPose3D(const TPose2D& p)
	: x(p.x), y(p.y), z(0.0), yaw(p.phi), pitch(0.0), roll(0.0)
{
}
TPose3D::TPose3D(const TPoint3D& p)
	: x(p.x), y(p.y), z(p.z), yaw(0.0), pitch(0.0), roll(0.0)
{
}
void TPose3D::asString(std::string& s) const
{
	s = mrpt::format(
		"[%f %f %f %f %f %f]", x, y, z, RAD2DEG(yaw), RAD2DEG(pitch),
		RAD2DEG(roll));
}
void TPose3D::getAsQuaternion(
	mrpt::math::CQuaternion<double>& q,
	mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 4, 3>> out_dq_dr) const
{
	// See:
	// http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
	const double cy = cos(yaw * 0.5), sy = sin(yaw * 0.5);
	const double cp = cos(pitch * 0.5), sp = sin(pitch * 0.5);
	const double cr = cos(roll * 0.5), sr = sin(roll * 0.5);

	const double ccc = cr * cp * cy;
	const double ccs = cr * cp * sy;
	const double css = cr * sp * sy;
	const double sss = sr * sp * sy;
	const double scc = sr * cp * cy;
	const double ssc = sr * sp * cy;
	const double csc = cr * sp * cy;
	const double scs = sr * cp * sy;

	q.w(ccc + sss);
	q.x(scc - css);
	q.y(csc + scs);
	q.z(ccs - ssc);

	// Compute 4x3 Jacobian: for details, see technical report:
	//   Parameterizations of SE(3) transformations: equivalences, compositions
	//   and uncertainty, J.L. Blanco (2010).
	//   https://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
	if (out_dq_dr)
	{
		alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[4 * 3] = {
			-0.5 * q[3], 0.5 * (-csc + scs), -0.5 * q[1],
			-0.5 * q[2], 0.5 * (-ssc - ccs), 0.5 * q[0],
			0.5 * q[1],	 0.5 * (ccc - sss),	 0.5 * q[3],
			0.5 * q[0],	 0.5 * (-css - scc), -0.5 * q[2]};
		out_dq_dr.value().get().loadFromArray(nums);
	}
}
void TPose3D::composePoint(const TPoint3D& l, TPoint3D& g) const
{
	CMatrixDouble33 R;
	this->getRotationMatrix(R);
	TPoint3D res;
	res.x = R(0, 0) * l.x + R(0, 1) * l.y + R(0, 2) * l.z + this->x;
	res.y = R(1, 0) * l.x + R(1, 1) * l.y + R(1, 2) * l.z + this->y;
	res.z = R(2, 0) * l.x + R(2, 1) * l.y + R(2, 2) * l.z + this->z;

	g = res;
}
TPoint3D TPose3D::composePoint(const TPoint3D& l) const
{
	TPoint3D g;
	composePoint(l, g);
	return g;
}

void TPose3D::inverseComposePoint(const TPoint3D& g, TPoint3D& l) const
{
	CMatrixDouble44 H;
	this->getInverseHomogeneousMatrix(H);
	TPoint3D res;
	res.x = H(0, 0) * g.x + H(0, 1) * g.y + H(0, 2) * g.z + H(0, 3);
	res.y = H(1, 0) * g.x + H(1, 1) * g.y + H(1, 2) * g.z + H(1, 3);
	res.z = H(2, 0) * g.x + H(2, 1) * g.y + H(2, 2) * g.z + H(2, 3);

	l = res;
}
TPoint3D TPose3D::inverseComposePoint(const TPoint3D& g) const
{
	TPoint3D l;
	inverseComposePoint(g, l);
	return l;
}

void TPose3D::getRotationMatrix(mrpt::math::CMatrixDouble33& R) const
{
	const double cy = cos(yaw);
	const double sy = sin(yaw);
	const double cp = cos(pitch);
	const double sp = sin(pitch);
	const double cr = cos(roll);
	const double sr = sin(roll);

	alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double rot_vals[] = {
		cy * cp,
		cy * sp * sr - sy * cr,
		cy * sp * cr + sy * sr,
		sy * cp,
		sy * sp * sr + cy * cr,
		sy * sp * cr - cy * sr,
		-sp,
		cp * sr,
		cp * cr};
	R.loadFromArray(rot_vals);
}
void TPose3D::SO3_to_yaw_pitch_roll(
	const mrpt::math::CMatrixDouble33& R, double& yaw, double& pitch,
	double& roll)
{
	ASSERTDEBMSG_(
		std::abs(
			sqrt(square(R(0, 0)) + square(R(1, 0)) + square(R(2, 0))) - 1) <
			3e-3,
		"Homogeneous matrix is not orthogonal & normalized!: " +
			R.inMatlabFormat());
	ASSERTDEBMSG_(
		std::abs(
			sqrt(square(R(0, 1)) + square(R(1, 1)) + square(R(2, 1))) - 1) <
			3e-3,
		"Homogeneous matrix is not orthogonal & normalized!: " +
			R.inMatlabFormat());
	ASSERTDEBMSG_(
		std::abs(
			sqrt(square(R(0, 2)) + square(R(1, 2)) + square(R(2, 2))) - 1) <
			3e-3,
		"Homogeneous matrix is not orthogonal & normalized!: " +
			R.inMatlabFormat());

	// Pitch is in the range [-pi/2, pi/2 ], so this calculation is enough:
	pitch = atan2(-R(2, 0), hypot(R(0, 0), R(1, 0)));

	// Roll:
	if ((fabs(R(2, 1)) + fabs(R(2, 2))) <
		10 * std::numeric_limits<double>::epsilon())
	{
		// Gimbal lock between yaw and roll. This one is arbitrarily forced to
		// be zero.
		// Check
		// https://reference.mrpt.org/devel/classmrpt_1_1poses_1_1_c_pose3_d.html.
		// If cos(pitch)==0, the homogeneous matrix is:
		// When sin(pitch)==1:
		//  /0  cysr-sycr cycr+sysr x\   /0  sin(r-y) cos(r-y)  x\.
		//  |0  sysr+cycr sycr-cysr y| = |0  cos(r-y) -sin(r-y) y|
		//  |-1     0         0     z|   |-1    0         0     z|
		//  \0      0         0     1/   \0     0         0     1/
		//
		// And when sin(pitch)=-1:
		//  /0 -cysr-sycr -cycr+sysr x\   /0 -sin(r+y) -cos(r+y) x\.
		//  |0 -sysr+cycr -sycr-cysr y| = |0 cos(r+y)  -sin(r+y) y|
		//  |1      0          0     z|   |1    0          0     z|
		//  \0      0          0     1/   \0    0          0     1/
		//
		// Both cases are in a "gimbal lock" status. This happens because pitch
		// is vertical.

		roll = 0.0;
		if (pitch > 0) yaw = atan2(R(1, 2), R(0, 2));
		else
			yaw = atan2(-R(1, 2), -R(0, 2));
	}
	else
	{
		roll = atan2(R(2, 1), R(2, 2));
		// Yaw:
		yaw = atan2(R(1, 0), R(0, 0));
	}
}

void TPose3D::fromHomogeneousMatrix(const mrpt::math::CMatrixDouble44& HG)
{
	SO3_to_yaw_pitch_roll(
		CMatrixDouble33(HG.blockCopy<3, 3>(0, 0)), yaw, pitch, roll);
	x = HG(0, 3);
	y = HG(1, 3);
	z = HG(2, 3);
}
void TPose3D::composePose(const TPose3D other, TPose3D& result) const
{
	CMatrixDouble44 me_H, o_H;
	this->getHomogeneousMatrix(me_H);
	other.getHomogeneousMatrix(o_H);
	result.fromHomogeneousMatrix(
		CMatrixDouble44(me_H.asEigen() * o_H.asEigen()));
}
void TPose3D::getHomogeneousMatrix(mrpt::math::CMatrixDouble44& HG) const
{
	CMatrixDouble33 R;
	getRotationMatrix(R);
	HG.block<3, 3>(0, 0) = R.asEigen();
	HG(0, 3) = x;
	HG(1, 3) = y;
	HG(2, 3) = z;
	HG(3, 0) = HG(3, 1) = HG(3, 2) = 0.;
	HG(3, 3) = 1.;
}
void TPose3D::getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44& HG) const
{  // Get current HM & inverse in-place:
	this->getHomogeneousMatrix(HG);
	mrpt::math::homogeneousMatrixInverse(HG);
}

void TPose3D::fromString(const std::string& s)
{
	CMatrixDouble m;
	if (!m.fromMatlabStringFormat(s))
		THROW_EXCEPTION_FMT(
			"Malformed expression in ::fromString, s=\"%s\"", s.c_str());
	ASSERTMSG_(
		m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
	x = m(0, 0);
	y = m(0, 1);
	z = m(0, 2);
	yaw = DEG2RAD(m(0, 3));
	pitch = DEG2RAD(m(0, 4));
	roll = DEG2RAD(m(0, 5));
}

TPose3D mrpt::math::operator-(const TPose3D& p)
{
	CMatrixDouble44 H;
	p.getInverseHomogeneousMatrix(H);
	TPose3D ret;
	ret.fromHomogeneousMatrix(H);
	return ret;
}
TPose3D mrpt::math::operator-(const TPose3D& b, const TPose3D& a)
{
	// b - a = A^{-1} * B
	CMatrixDouble44 Hainv, Hb;
	a.getInverseHomogeneousMatrix(Hainv);
	b.getHomogeneousMatrix(Hb);
	TPose3D ret;
	ret.fromHomogeneousMatrix(CMatrixDouble44(Hainv.asEigen() * Hb.asEigen()));
	return ret;
}


#include <gtest/gtest.h>
#include "helpers.hh"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "../CameraModel/cameramodel.hh"
#include "../geometry/Bounds.hh"
#include "../geometry/LineSegment/LineSegment2/LineSegment2i/linesegment2i.hh"

using namespace bold;
using namespace Eigen;

TEST (CameraModelTests, directionForPixel)
{
  auto imageWidth = 11;
  auto imageHeight = 11;
  auto rangeVerticalDegs = 90;
  auto rangeHorizontalDegs = 90;

  CameraModel cameraModel1(imageWidth, imageHeight,
                           rangeVerticalDegs, rangeHorizontalDegs);

  EXPECT_TRUE ( VectorsEqual(Vector3d(0, 1, 0).normalized(),
                             cameraModel1.directionForPixel(Vector2d(5.5, 5.5)) ) );

  EXPECT_TRUE ( VectorsEqual(Vector3d(1, 1, 0).normalized(),
                             cameraModel1.directionForPixel(Vector2d( 0, 5.5)) ) );
  EXPECT_TRUE ( VectorsEqual(Vector3d(-1, 1, 0).normalized(),
                             cameraModel1.directionForPixel(Vector2d(11, 5.5)) ) );

  EXPECT_TRUE ( VectorsEqual(Vector3d(0, 1, -1).normalized(),
                             cameraModel1.directionForPixel(Vector2d(5.5,  0)) ) );
  EXPECT_TRUE ( VectorsEqual(Vector3d(0, 1,  1).normalized(),
                             cameraModel1.directionForPixel(Vector2d(5.5, 11)) ) );

  auto v = Vector3d( -1, 1, 0).normalized();
  v.x() *= 2.0/5.5;
  EXPECT_TRUE ( VectorsEqual(v.normalized(),
                             cameraModel1.directionForPixel(Vector2d(7.5,  5.5)) ) );

  rangeVerticalDegs = 45;
  rangeHorizontalDegs = 60;

  double th = tan(.5 * 60.0 / 180.0 * M_PI);
  double tv = tan(.5 * 45.0 / 180.0 * M_PI);

  CameraModel cameraModel2(imageWidth, imageHeight,
                           rangeVerticalDegs, rangeHorizontalDegs);

  EXPECT_TRUE ( VectorsEqual(Vector3d(0, 1, 0).normalized(),
                             cameraModel2.directionForPixel(Vector2d(5.5, 5.5)) ) );

  EXPECT_TRUE ( VectorsEqual(Vector3d(th, 1, 0).normalized(),
                             cameraModel2.directionForPixel(Vector2d( 0, 5.5)) ) );
  EXPECT_TRUE ( VectorsEqual(Vector3d(-th, 1, 0).normalized(),
                             cameraModel2.directionForPixel(Vector2d(11, 5.5)) ) );

  EXPECT_TRUE ( VectorsEqual(Vector3d(0, 1, -tv).normalized(),
                             cameraModel2.directionForPixel(Vector2d(5.5,  0)) ) );
  EXPECT_TRUE ( VectorsEqual(Vector3d(0, 1,  tv).normalized(),
                             cameraModel2.directionForPixel(Vector2d(5.5, 11)) ) );

}

TEST (CameraModelTests, pixelForDirection)
{
  auto imageWidth = 11;
  auto imageHeight = 11;
  auto rangeVerticalDegs = 90;
  auto rangeHorizontalDegs = 90;

  CameraModel cameraModel1(imageWidth, imageHeight, rangeVerticalDegs, rangeHorizontalDegs);

  EXPECT_EQ ( Maybe<Vector2d>::empty(), cameraModel1.pixelForDirection(Vector3d(0,  0, 0)) );
  EXPECT_EQ ( Maybe<Vector2d>::empty(), cameraModel1.pixelForDirection(Vector3d(1,  0, 0)) );
  EXPECT_EQ ( Maybe<Vector2d>::empty(), cameraModel1.pixelForDirection(Vector3d(0,  0, 1)) );
  EXPECT_EQ ( Maybe<Vector2d>::empty(), cameraModel1.pixelForDirection(Vector3d(1, -1, 1)) );

  EXPECT_TRUE ( VectorsEqual( Vector2d(5.5, 5.5), *cameraModel1.pixelForDirection(Vector3d(0, 1, 0)) ) );

  EXPECT_TRUE ( VectorsEqual( Vector2d( 0, 5.5), *cameraModel1.pixelForDirection(Vector3d( 1, 1, 0)) ) );
  EXPECT_TRUE ( VectorsEqual( Vector2d(11, 5.5), *cameraModel1.pixelForDirection(Vector3d(-1, 1, 0)) ) );

  EXPECT_TRUE ( VectorsEqual( Vector2d(5.5,  0), *cameraModel1.pixelForDirection(Vector3d(0, 1, -1)) ) );
  EXPECT_TRUE ( VectorsEqual( Vector2d(5.5, 11), *cameraModel1.pixelForDirection(Vector3d(0, 2,  2)) ) );

  auto v = Vector3d(-1, 1, 0).normalized();
  v.x() *= 2.0/5.5;
  EXPECT_TRUE ( VectorsEqual( Vector2d(7.5, 5.5), *cameraModel1.pixelForDirection(v) ) );

  // 2 time range
  rangeVerticalDegs = 45;
  rangeHorizontalDegs = 60;

  CameraModel cameraModel2(imageWidth, imageHeight, rangeVerticalDegs, rangeHorizontalDegs);

  double th = tan(.5 * 60.0 / 180.0 * M_PI);
  double tv = tan(.5 * 45.0 / 180.0 * M_PI);

  EXPECT_TRUE ( VectorsEqual( Vector2d(5.5, 5.5), *cameraModel2.pixelForDirection(Vector3d(0, 1, 0)) ) );

  EXPECT_TRUE ( VectorsEqual( Vector2d( 0, 5.5), *cameraModel2.pixelForDirection(Vector3d(th, 1, 0)) ) );
  EXPECT_TRUE ( VectorsEqual( Vector2d(11, 5.5), *cameraModel2.pixelForDirection(Vector3d(-th, 1, 0)) ) );

  EXPECT_TRUE ( VectorsEqual( Vector2d(5.5,  0), *cameraModel2.pixelForDirection(Vector3d(0, 1, -tv)) ) );
  EXPECT_TRUE ( VectorsEqual( Vector2d(5.5, 11), *cameraModel2.pixelForDirection(Vector3d(0, 1, tv)) ) );
}


#include <iomanip>
#include <cstdlib>
#include <math.h>
#include <cmath>
#include <random>
#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "gtest/gtest.h"
#include "../include/unscented_kalman_filter.h"

namespace rviz {
  cv::Point2i cv_offset(
      Eigen::Vector2d e_p, int image_width=2000, int image_height=2000){
    cv::Point2i output;
    output.x = int(e_p(0) * 100) + image_width/2;
    output.y = image_height - int(e_p(1) * 100) - image_height/3;
    return output;
  };

  void ellipse_drawing(
    cv::Mat bg_img, Eigen::Matrix2d pest, Eigen::Vector2d center,
    cv::Scalar ellipse_color=cv::Scalar(0, 0, 255)
  ){
    Eigen::EigenSolver<Eigen::Matrix2d> ces(pest);
    Eigen::Matrix2d e_value = ces.pseudoEigenvalueMatrix();
    Eigen::Matrix2d e_vector = ces.pseudoEigenvectors();

    double angle = std::atan2(e_vector(0, 1), e_vector(0, 0));
    cv::ellipse(
      bg_img,
      cv_offset(center, bg_img.cols, bg_img.rows),
      cv::Size(e_value(0,0)*100, e_value(1,1)*100),
      angle / M_PI * 180,
      0,
      360,
      ellipse_color,
      2,
      4);
  };
}

namespace ukf {
// Constan velocity model
auto motion_model_cv = [](Eigen::Vector4d x, Eigen::Vector2d u, double dt)-> Eigen::Vector4d {
  Eigen::Matrix4d F;
  F <<1.0,  dt,   0.,   0.,
          0., 1.0,   0.,   0.,
          0.,   0., 1.0,   dt,
          0.,   0.,   0.,  1.0;

  return F * x;
};

auto observation_model_cv = [](Eigen::Vector4d x) -> Eigen::Vector2d {
  Eigen::Matrix<double, 2, 4> H;
  H << 1., 0., 0., 0.,
       0., 0., 1., 0.;
  return H * x;
};

auto wrap_angles_default = [](Eigen::VectorXd x) -> Eigen::VectorXd {
  return x;
};

auto linear_motion_model = [](Eigen::Vector2d x, Eigen::Vector2d u, double dt)-> Eigen::Vector2d {
  Eigen::Matrix2d F;
  F <<  1.0, dt,
        0, 1.0;

  return F * x;
};

auto observation_model = [](Eigen::Vector2d x) -> Eigen::Vector2d {
  Eigen::Matrix<double, 2, 2> H;
  H << 1., 0.,
       0., 1.;
  return H * x;
};

auto observation_model_2x4 = [](Eigen::Vector4d x) -> Eigen::Vector2d {
  // State to measurement function H, is with dimensions dim_z x dim_x with ones  in the locations to be updated.
  Eigen::Matrix<double, 2, 4> H;
  H << 1., 0., 0., 0.,
       0., 1., 0., 0.;
  return H * x;
};

auto motion_model_4x4 = [](Eigen::Vector4d x, Eigen::Vector2d u, double dt) -> Eigen::Vector4d {
  Eigen::Matrix4d F;
  F <<1.0,   0.,   0.,   0.,
        0., 1.0,   0.,   0.,
        0.,   0., 1.0,   0.,
        0.,   0.,   0.,  0.; // TODO: Should this be a 1.0? PythonRobotics has 0.0

  // control input v (m/s), yaw_rate (rad/s)
  Eigen::Matrix<double, 4, 2> B;
  B << dt * std::cos(x(2)),  0.,
       dt * std::sin(x(2)),  0.,
                        0.0,  dt,
                        1.0,  0.0;
  return F * x + B * u;
};

auto wrap_angles = [](Eigen::Vector4d x) -> Eigen::Vector4d{
  // When state is represented by [x y yaw v].
  Eigen::Vector4d angles_wrapped = x;

  while (angles_wrapped(2) < -M_PI) {
    angles_wrapped(2) += (2*M_PI);
  }

  while (angles_wrapped(2) > M_PI) {
    angles_wrapped(2) -= (2*M_PI);
  }

  return angles_wrapped;
};

Eigen::VectorXd residuals(Eigen::VectorXd const& sigma, Eigen::VectorXd const& x)
{
  return sigma - x;
}

Eigen::VectorXd residuals_x(Eigen::VectorXd const& sigma, Eigen::VectorXd const& x)
{
  // Residuals when state is represented by [x y yaw v].
  Eigen::VectorXd residuals(x.rows());
  residuals(0) = sigma(0) - x(0);
  residuals(1) = sigma(1) - x(1);
  residuals(3) = sigma(3) - x(3);

  auto yaw_residual = sigma(2) - x(2);

  while (yaw_residual < -M_PI) {
    yaw_residual += (2*M_PI);
  }

  while (yaw_residual > M_PI) {
    yaw_residual -= (2*M_PI);
  }
  //assert((yaw_residual < M_PI) && (yaw_residual > -M_PI));
  residuals(2) = yaw_residual;
  return residuals;
}

Eigen::VectorXd mean_fn_x(Eigen::MatrixXd const& sigmas, Eigen::VectorXd const& Wm)
{
  // State mean when state is represented by [x y yaw v].
  Eigen::VectorXd mean(sigmas.cols());
  mean.setZero();
  double ss = 0.0;
  double sc = 0.0;

  for (auto i=0; i< sigmas.rows(); ++i) {
    Eigen::VectorXd s = sigmas.row(i);

    mean(0) += s(0) * Wm(i);
    mean(1) += s(1) * Wm(i);
    mean(3) += s(3) * Wm(i);
    ss += std::sin(s(2)) * Wm(i);
    sc += std::cos(s(2)) * Wm(i);
  }
  mean(2) = std::atan2(ss,sc);
  return mean;
}

Eigen::VectorXd mean_fn(Eigen::MatrixXd const& sigmas, Eigen::VectorXd const& Wm)
{
  return sigmas.transpose() * Wm;
}

auto subtract_fn = residuals;
auto subtract_fn_x = residuals_x;
}

TEST(UKFTest, NumSigmaTest) {
  auto points = ukf::MerweScaledSigmaPoints(2, 0.1,0.1,0.1, ukf::subtract_fn);
  auto n_sigmas = points.num_sigmas();
  EXPECT_EQ(n_sigmas, 5);
}

TEST(UKFTest, SigmaPoints2DTest) {
  auto n = 2;
  auto alpha = 1e-3;
  auto beta = 2.0;
  auto kappa = 0.0;
  auto points = ukf::MerweScaledSigmaPoints(n, alpha, beta, kappa, ukf::subtract_fn);

  Eigen::Vector2d x;
  x << 1.,2.;

  Eigen::Matrix2d P;
  P << 2.,1.2,
       1.2,2.;

  auto sigmas = points.sigma_points(x, P);
  Eigen::Matrix<double, 5, 2> sigmas_expected;

  sigmas_expected << 1., 2.,
                    1.002, 2.0012,
                    1., 2.0016,
                    0.9980, 1.9988,
                    1., 1.9984;

  ASSERT_TRUE((sigmas - sigmas_expected).norm() < 1e-5);
}

TEST(UKFTest, SigmaPointsTest) {
  auto points = ukf::MerweScaledSigmaPoints(4, 1e-3,2.0,0.0, ukf::subtract_fn);
  Eigen::Vector4d x = Eigen::Vector4d::Zero();
  Eigen::Matrix4d P = Eigen::Matrix4d::Identity();
  auto sigmas = points.sigma_points(x, P);

  Eigen::Matrix<double, 9, 4> sigmas_expected;
  sigmas_expected << 0., 0., 0., 0.,
                    0.0020, 0., 0., 0.,
                    0., 0.0020, 0., 0.,
                    0., 0., 0.0020, 0.,
                    0., 0., 0., 0.0020,
                    -0.0020, 0., 0., 0.,
                    0., -0.0020, 0., 0.,
                    0., 0., -0.0020, 0.,
                    0., 0., 0., -0.0020;

  ASSERT_TRUE((sigmas - sigmas_expected).norm() < 1e-5);
  ASSERT_TRUE(std::abs(points.get_wm().sum()-1.) < 1e-5);
}

TEST(UKFTest, ComputeWeightsTest) {
  auto n = 2;
  auto alpha = 1e-3;
  auto beta = 2.0;
  auto kappa = 0.0;
  auto points = ukf::MerweScaledSigmaPoints(n, alpha, beta, kappa, ukf::subtract_fn);
  auto wm = points.get_wm();

  Eigen::Matrix<double,5,1> wm_expected;
  wm_expected << -999998.9999712,
                  249999.9999928,
                  249999.9999928,
                  249999.9999928,
                  249999.9999928;

  ASSERT_TRUE((wm - wm_expected).norm() < 1e-5);
}

TEST(UKFTest, UnscentedTransformTest) {
  auto n = 2;
  auto alpha = 1e-3;
  auto beta = 2.0;
  auto kappa = 0.0;
  auto points = ukf::MerweScaledSigmaPoints(n, alpha, beta, kappa, ukf::subtract_fn);

  Eigen::Vector2d x;
  x << 1.,2.;

  Eigen::Matrix2d P;
  P << 2.,1.2,
       1.2,2.;

  Eigen::Matrix2d noise_cov;
  noise_cov << 0.,0.,
               0.,0.;

  auto sigmas = points.sigma_points(x, P);

  auto pair = ukf::unscented_transform(sigmas, points.get_wm(), points.get_wc(), noise_cov, ukf::residuals, ukf::mean_fn);

  ASSERT_TRUE((std::get<0>(pair) - x).norm() < 1e-5);
  ASSERT_TRUE((std::get<1>(pair) - P).norm() < 1e-5);
}

TEST(UKFTest, ResidualsTest) {
  auto n = 2;
  auto alpha = 0.001;
  auto beta = 2.0;
  auto kappa = 0.0;
  auto points = ukf::MerweScaledSigmaPoints(n, alpha, beta, kappa, ukf::subtract_fn);

  Eigen::Vector2d x;
  x << 1.,2.;

  Eigen::Matrix2d P;
  P << 2.,1.2,
       1.2,2.;

  auto sigmas = points.sigma_points(x, P);

  Eigen::MatrixXd residuals = ukf::residuals(sigmas.row(1), x);
  Eigen::Matrix<double,2,1> residuals_expected;
  residuals_expected << 0.002,0.0012;
  ASSERT_TRUE((residuals - residuals_expected).norm() < 1e-5);
}

TEST(UKFTest, UKFPredictTest) {
  auto n = 2;
  auto alpha = 0.001;
  auto beta = 2.0;
  auto kappa = 0.0;
  auto points = ukf::MerweScaledSigmaPoints(n, alpha, beta, kappa, ukf::subtract_fn);

  Eigen::Vector2d x;
  x << 1.,2.;

  Eigen::Matrix2d P;
  P << 2.,1.2,
       1.2,2.;

  Eigen::Vector2d u;
  u << 1.0, 0.1;

  Eigen::VectorXd  z = Eigen::VectorXd::Zero(n);

  auto dim_z = n;
  auto dt = 0.1;
  auto ukf = ukf::UnscentedKalmanFilter(
                n,
                dim_z,
                dt,
                points,
                ukf::wrap_angles_default,
                ukf::linear_motion_model,
                ukf::observation_model,
                ukf::residuals,
                ukf::residuals,
                ukf::mean_fn,
                ukf::mean_fn);

  ukf.predict(u);
  ukf.update(z);
  //TODO: FINISH TESTING
}

TEST(UKFTest, CrossVarianceTest) {
  auto n = 2;
  auto dim_z = 2;
  auto dt = 0.1;
  auto alpha = 0.001;
  auto beta = 2.0;
  auto kappa = 0.0;
  auto points = ukf::MerweScaledSigmaPoints(n, alpha, beta, kappa, ukf::subtract_fn);

  Eigen::Vector2d x;
  x << 1.,2.;

  Eigen::Matrix2d P;
  P << 2.,1.2,
       1.2,2.;

  // control input
  Eigen::Vector2d u;
  u << 1.0, 0.1;

  Eigen::VectorXd  z = Eigen::VectorXd::Zero(n);

  // Get sigmas_f (process)
  Eigen::MatrixXd sigmas = points.sigma_points(x, P);
  auto n_sigmas = points.num_sigmas();
  Eigen::MatrixXd sigmas_f = Eigen::MatrixXd::Zero(n_sigmas, n);

  // Pass the sigmas through the process model.
  for (auto i=0; i<n_sigmas; i++) {
    sigmas_f.row(i) = ukf::linear_motion_model(sigmas.row(i), u, dt);
  }

  // Get sigmas_h (measurement)
  Eigen::MatrixXd sigmas_h = Eigen::MatrixXd::Zero(n_sigmas, dim_z);

  // Pass the sigmas processed by the process(motion) model through the observation model.
  for (auto i=0; i<n_sigmas; i++) {
    sigmas_h.row(i) = ukf::observation_model(sigmas_f.row(i));
  }

  auto ukf = ukf::UnscentedKalmanFilter(
                n,
                dim_z,
                dt,
                points,
                ukf::wrap_angles_default,
                ukf::linear_motion_model,
                ukf::observation_model,
                ukf::residuals,
                ukf::residuals,
                ukf::mean_fn,
                ukf::mean_fn);

  auto CV = ukf.cross_variance(x, z,sigmas_f, sigmas_h);
  // TODO: Finish test
}

TEST(UKFTest, SimulationSimpleCVTest) {
  // Constant Velocity model
  auto n = 4; // [x, x_dot, y, y_dot]
  auto dt = 0.1;
  auto alpha = 0.1;
  auto beta = 2.0;
  auto kappa = 1.0;
  auto points = ukf::MerweScaledSigmaPoints(n, alpha, beta, kappa, ukf::subtract_fn);

  // Dont really need, but current implementation requires a u to pass to motion_model.
  Eigen::Vector2d u;
  u << 0.0, 0.0;

  // noisy control input
  Eigen::Vector2d ud;

  // observation z
  Eigen::Vector2d z;

  // dead reckoning
  Eigen::Vector4d xDR;
  xDR << 0.1,1.0,0.1,1.0;

  // ground truth reading
  Eigen::Vector4d xTrue;
  xTrue << 0.1,1.0,0.1,1.0;

  Eigen::Vector4d xEst;
  xEst << 0.1,1.0,0.1,1.0;

  std::vector<Eigen::Vector4d> hxDR;
  std::vector<Eigen::Vector4d> hxTrue;
  std::vector<Eigen::Vector4d> hxEst;
  std::vector<Eigen::Vector2d> hz;

  Eigen::Matrix4d PEst = Eigen::Matrix4d::Identity();

  // Motion model covariance
  Eigen::Matrix4d Q = Eigen::Matrix4d::Identity();
  Q(0,0)=0.25 * (dt*dt*dt*dt) * 0.02;
  Q(0,1)=0.5 * (dt*dt*dt) * 0.02;
  Q(1,0)=0.5 * (dt*dt*dt) * 0.02;
  Q(1,1)=(dt*dt) * 0.02;
  Q(2,2)=0.25 * (dt*dt*dt*dt) * 0.02;
  Q(2,3)=0.5 * (dt*dt*dt) * 0.02;
  Q(3,2)=0.5 * (dt*dt*dt) * 0.02;
  Q(3,3)=(dt*dt) * 0.02;

  Eigen::Matrix2d R = Eigen::Matrix2d::Identity();
  R(0,0)=0.09;
  R(1,1)=0.09;

  // Motion model simulation error
  Eigen::Matrix2d Qsim = Eigen::Matrix2d::Identity();
  Qsim(0,0)=0.25 * (dt*dt*dt*dt) * 0.02;
  Qsim(1,1)=(dt*dt) * 0.02;

  // Observation model simulation error
  Eigen::Matrix2d Rsim = Eigen::Matrix2d::Identity();
  Rsim(0,0)=0.09;
  Rsim(1,1)=0.09;

  auto ukf = ukf::UnscentedKalmanFilter(
                n,
                z.rows(),
                dt,
                points,
                ukf::wrap_angles_default,
                ukf::motion_model_cv,
                ukf::observation_model_cv,
                ukf::residuals,
                ukf::residuals,
                ukf::mean_fn,
                ukf::mean_fn);
    ukf.Q = Q;
    ukf.R = R;
    auto time = 0.0;

    std::random_device rd{};
    std::mt19937 gen{rd()};
    std::normal_distribution<> gaussian_d{0,1};

    cv::namedWindow("ukf");
    while (time < 50.0) {
      time += dt;

      ud(0) = u(0) + gaussian_d(gen) * Qsim(0,0);
      ud(1) = u(1) + gaussian_d(gen) * Qsim(1,1);

      xTrue = ukf::motion_model_cv(xTrue, u, dt);
      //std::cout << "xTrue" << xTrue << std::endl;
      xDR = ukf::motion_model_cv(xDR, ud, dt);

      z(0) = time + gaussian_d(gen) * Rsim(0,0);
      z(1) = time + gaussian_d(gen) * Rsim(1,1);

      ukf.predict(ud);
      ukf.update(z);

      auto xEst = ukf.x_post;

      hxDR.push_back(xDR);
      hxTrue.push_back(xTrue);
      hxEst.push_back(xEst);
      hz.push_back(z);

      //visualization

      cv::Mat bg(700,1500, CV_8UC3, cv::Scalar(255,255,255));

      for(unsigned int j=0; j<hxDR.size(); j++){

        // Green groundtruth
        // State is found at index 0 and 2.
        Eigen::Vector2d x_true;
        x_true << hxTrue[j](0), hxTrue[j](2);
        cv::circle(bg, rviz::cv_offset(x_true, bg.cols, bg.rows),
                   7, cv::Scalar(0,255,0), 2);

        // blue estimation
        Eigen::Vector2d x_est;
        x_est << hxEst[j](0),hxEst[j](2);
        cv::circle(bg, rviz::cv_offset(x_est, bg.cols, bg.rows),
                   10, cv::Scalar(255,0,0), 5);

        // black dead reckoning
        Eigen::Vector2d x_dr;
        x_dr << hxDR[j](0),hxDR[j](2);
        cv::circle(bg, rviz::cv_offset(x_dr, bg.cols, bg.rows),
                   7, cv::Scalar(0, 0, 0), -1);
      }

      // red observation
      for(unsigned int i=0; i<hz.size(); i++){
        cv::circle(bg, rviz::cv_offset(hz[i], bg.cols, bg.rows),
                 7, cv::Scalar(0, 0, 255), -1);
      }

      Eigen::Vector2d ellipse_x_est;
      ellipse_x_est << xEst(0), xEst(2);
      rviz::ellipse_drawing(bg, PEst.block(0,0,2,2), ellipse_x_est);

      cv::imshow("ukf", bg);
      cv::waitKey(5);

    }
}

TEST(UKFTest, SimulationTest) {
  auto n = 4; // [x y yaw v]'
  auto dt = 0.1;
  auto alpha = 0.001;
  auto beta = 2.0;
  auto kappa = 0.0;
  auto points = ukf::MerweScaledSigmaPoints(n, alpha, beta, kappa, ukf::subtract_fn_x);

  // control input v (m/s), yaw_rate (rad/s)
  Eigen::Vector2d u;
  u << 1.0, 0.1;

  // noisy control input
  Eigen::Vector2d ud;

  // observation z
  Eigen::Vector2d z;

  // dead reckoning
  Eigen::Vector4d xDR;
  xDR << 0.1,0.1,0.0,1.0;

  // ground truth reading
  Eigen::Vector4d xTrue;
  xTrue << 0.1,0.1,0.0,1.0;;

  Eigen::Vector4d xEst;
  xEst << 0.1,0.1,0.0,1.0;;

  std::vector<Eigen::Vector4d> hxDR;
  std::vector<Eigen::Vector4d> hxTrue;
  std::vector<Eigen::Vector4d> hxEst;
  std::vector<Eigen::Vector2d> hz;

  Eigen::Matrix4d PEst = Eigen::Matrix4d::Identity();

  // Motional model covariance
  Eigen::Matrix4d Q = Eigen::Matrix4d::Identity();
  Q(0,0)=0.25 * (dt*dt*dt*dt) * 0.02;
  Q(0,1)=0.5 * (dt*dt*dt) * 0.02;
  Q(1,0)=0.5 * (dt*dt*dt) * 0.02;
  Q(1,1)=(dt*dt) * 0.02;
  Q(2,2)=0.25 * (dt*dt*dt*dt) * 0.02;
  Q(2,3)=0.5 * (dt*dt*dt) * 0.02;
  Q(3,2)=0.5 * (dt*dt*dt) * 0.02;
  Q(3,3)=(dt*dt) * 0.02;

  Eigen::Matrix2d R = Eigen::Matrix2d::Identity();
  R(0,0)=0.09;
  R(1,1)=0.09;

  // Motion model simulation error
  Eigen::Matrix2d Qsim = Eigen::Matrix2d::Identity();
  Qsim(0,0)=0.09;
  Qsim(1,1)=0.09;

  // Observation model simulation error
  Eigen::Matrix2d Rsim = Eigen::Matrix2d::Identity();
  Rsim(0,0)=0.09;
  Rsim(1,1)=0.09;

  auto ukf = ukf::UnscentedKalmanFilter(
                n,
                z.rows(),
                dt,
                points,
                ukf::wrap_angles,
                ukf::motion_model_4x4,
                ukf::observation_model_2x4,
                ukf::residuals_x,
                ukf::residuals,
                ukf::mean_fn_x,
                ukf::mean_fn);

  // Initial settings.
  ukf.x = xTrue;
  ukf.Q = Q;
  ukf.R = R;

  auto time = 0.0;

  std::random_device rd{};
  std::mt19937 gen{rd()};
  std::normal_distribution<> gaussian_d{0,1};

  cv::namedWindow("ukf");
  auto count = 0;
  while (time < 100.0) {
    time += dt;

    ud(0) = u(0) + gaussian_d(gen) * Qsim(0,0);
    ud(1) = u(1) + gaussian_d(gen) * Qsim(1,1);

    xTrue = ukf::motion_model_4x4(xTrue, u, dt);

    xDR = ukf::motion_model_4x4(xDR, ud, dt);

    z(0) = xTrue(0) + gaussian_d(gen) * Rsim(0,0);
    z(1) = xTrue(1) + gaussian_d(gen) * Rsim(1,1);

    ukf.predict(ud);
    ukf.update(z);

    auto xEst = ukf.x_post;

    hxDR.push_back(xDR);
    hxTrue.push_back(xTrue);
    hxEst.push_back(xEst);
    hz.push_back(z);

    // Visualization adapted from https://github.com/onlytailei/CppRobotics
    cv::Mat bg(700,1500, CV_8UC3, cv::Scalar(255,255,255));
    for(unsigned int j=0; j<hxDR.size(); j++){

      // green groundtruth
      cv::circle(bg, rviz::cv_offset(hxTrue[j].head(2), bg.cols, bg.rows),
                 7, cv::Scalar(0,255,0), -1);

      // blue estimation
      cv::circle(bg, rviz::cv_offset(hxEst[j].head(2), bg.cols, bg.rows),
                 10, cv::Scalar(255,0,0), 5);

      // black dead reckoning
      cv::circle(bg, rviz::cv_offset(hxDR[j].head(2), bg.cols, bg.rows),
                 7, cv::Scalar(0, 0, 0), -1);
    }

    // red observation
    for(unsigned int i=0; i<hz.size(); i++){
      cv::circle(bg, rviz::cv_offset(hz[i], bg.cols, bg.rows),
               7, cv::Scalar(0, 0, 255), -1);
    }

    rviz::ellipse_drawing(bg, PEst.block(0,0,2,2), xEst.head(2));
    cv::imshow("ukf", bg);
    cv::waitKey(5);

    std::string int_count = std::to_string(count++);
    cv::imwrite("/home/tasuku/ws/CppLocalization/src/video/ukf_"+int_count+".png", bg);
  }
}

TEST(UKFTest, InitTest) {
  auto dim_x = 4;
  auto dim_z = 2;
  auto dt = 1.0;
  auto alpha = 1e-3;
  auto beta = 2.0;
  auto kappa = 0.0;
  auto points = ukf::MerweScaledSigmaPoints(dim_x, alpha, beta, kappa, ukf::subtract_fn);

  ukf::UnscentedKalmanFilter ukf = ukf::UnscentedKalmanFilter(
                                                dim_x,
                                                dim_z,
                                                dt,
                                                points,
                                                ukf::wrap_angles_default,
                                                ukf::linear_motion_model,
                                                ukf::observation_model,
                                                ukf::residuals,
                                                ukf::residuals,
                                                ukf::mean_fn,
                                                ukf::mean_fn);
  uint dim = ukf.x.size();
  EXPECT_EQ(dim, 4);
}


//##########################################################################
//#                                                                        #
//#                       CLOUDCOMPARE PLUGIN: qPCL                        #
//#                                                                        #
//#  This program is free software; you can redistribute it and/or modify  #
//#  it under the terms of the GNU General Public License as published by  #
//#  the Free Software Foundation; version 2 or later of the License.      #
//#                                                                        #
//#  This program is distributed in the hope that it will be useful,       #
//#  but WITHOUT ANY WARRANTY; without even the implied warranty of        #
//#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the          #
//#  GNU General Public License for more details.                          #
//#                                                                        #
//#                         COPYRIGHT: Luca Penasa                         #
//#                                                                        #
//##########################################################################
//
#include "StatisticalOutliersRemover.h"

//Local
#include "dialogs/StatisticalOutliersRemoverDlg.h"
#include "../utils/cc2sm.h"
#include "../utils/sm2cc.h"

//PCL
#include <pcl/filters/statistical_outlier_removal.h>

//qCC_plugins
#include <ccMainAppInterface.h>

//qCC_db
#include <ccPointCloud.h>

//Qt
#include <QMainWindow>

//Boost
#include <boost/make_shared.hpp>

void removeOutliersStatistical(const PCLCloud::ConstPtr incloud, int knn, double nSigma, PCLCloud::Ptr outcloud)
{
	pcl::StatisticalOutlierRemoval<PCLCloud> remover;
	remover.setInputCloud(incloud);
	remover.setMeanK(knn);
	remover.setStddevMulThresh(nSigma);
	remover.filter(*outcloud);
}

StatisticalOutliersRemover::StatisticalOutliersRemover()
	: BaseFilter(FilterDescription("Statistical Outlier Removal",
									"Filter outlier data based on point neighborhood statistics",
									"Filter the points that are farther of their neighbors than the average (plus a number of times the standard deviation)",
									":/toolbar/PclUtils/icons/sor_outlier_remover.png"))
	, m_dialog(0)
	, m_k(0)
	, m_std(0.0f)
{
}

StatisticalOutliersRemover::~StatisticalOutliersRemover()
{
	//we must delete parent-less dialogs ourselves!
	if (m_dialog && m_dialog->parent() == 0)
		delete m_dialog;
}

int StatisticalOutliersRemover::compute()
{
	//get selected as pointcloud
	ccPointCloud* cloud = this->getSelectedEntityAsCCPointCloud();
	if (!cloud)
		return -1;

	//now as sensor message
	PCLCloud::Ptr tmp_cloud = cc2smReader(cloud).getAsSM();
	if (!tmp_cloud)
		return -1;

	PCLCloud::Ptr outcloud ( new PCLCloud);
	removeOutliersStatistical(tmp_cloud, m_k, m_std, outcloud);

	//get back outcloud as a ccPointCloud
	ccPointCloud* final_cloud = sm2ccConverter(outcloud).getCloud();
	if (!final_cloud)
		return -1;

	//create a suitable name for the entity
	final_cloud->setName(QString("%1_k%2_std%3").arg(cloud->getName()).arg(m_k).arg(m_std));
	final_cloud->setDisplay(cloud->getDisplay());
	//copy global shift & scale
	final_cloud->setGlobalScale(cloud->getGlobalScale());
	final_cloud->setGlobalShift(cloud->getGlobalShift());

	//disable original cloud
	cloud->setEnabled(false);
	if (cloud->getParent())
		cloud->getParent()->addChild(final_cloud);

	emit newEntity(cloud);

	return 1;
}

int StatisticalOutliersRemover::openInputDialog()
{
	if (!m_dialog)
	{
		m_dialog = new SORDialog(m_app ? m_app->getMainWindow() : 0);
	}

	return m_dialog->exec() ? 1 : 0;
}

void StatisticalOutliersRemover::getParametersFromDialog()
{
	//get values from dialog
	if (m_dialog)
	{
		m_k = m_dialog->spinK->value();
		m_std = static_cast<float>(m_dialog->spinStd->value());
	}
}


/* --------------------------------------------------------------------------
*
* (C) Copyright …
*
* ---------------------------------------------------------------------------
*/

/*!
 * @file Transform3DToEigenTransformConverter.cpp
 * @date 22/01/2018
 * @authors Alessandro Bianco
 */

/*!
 * @addtogroup Converters
 *
 * Implementation of Transform3DToEigenTransformConverter class.
 *
 *
 * @{
 */

/* --------------------------------------------------------------------------
 *
 * Includes
 *
 * --------------------------------------------------------------------------
 */

#include "Transform3DToEigenTransformConverter.hpp"
#include <Errors/Assert.hpp>
#include <Eigen/Geometry>

namespace Converters {

using namespace PoseWrapper;

/* --------------------------------------------------------------------------
 *
 * Public Member Functions
 *
 * --------------------------------------------------------------------------
 */
const Eigen::Matrix4f Transform3DToEigenTransformConverter::Convert(const PoseWrapper::Transform3DConstPtr& transform)
	{
	Eigen::Matrix4f conversion;

	Eigen::Quaternionf eigenRotation( GetWOrientation(*transform), GetXOrientation(*transform), GetYOrientation(*transform), GetZOrientation(*transform));
	Eigen::Matrix3f rotationMatrix = eigenRotation.toRotationMatrix();

	Eigen::Translation<float, 3> eigenTranslation( GetXPosition(*transform), GetYPosition(*transform), GetZPosition(*transform));

	conversion << 	rotationMatrix(0,0), rotationMatrix(0,1), rotationMatrix(0,2), eigenTranslation.x(),
			rotationMatrix(1,0), rotationMatrix(1,1), rotationMatrix(1,2), eigenTranslation.y(),
			rotationMatrix(2,0), rotationMatrix(2,1), rotationMatrix(2,2), eigenTranslation.z(),
			0, 0, 0, 1;

	return conversion;
	}

const Eigen::Matrix4f Transform3DToEigenTransformConverter::ConvertShared(const PoseWrapper::Transform3DSharedConstPtr& transform)
	{
	return Convert(transform.get());
	}

}

/** @} */