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

// Copyright (c) 2020 Tinko Bartels, Berlin, Germany.

// Contributed and/or modified by Tinko Bartels,
//   as part of Google Summer of Code 2020 program.

// 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_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_COEFFICIENT_LIST_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_COEFFICIENT_LIST_HPP

#include <cstddef>

#include <boost/mp11/integral.hpp>
#include <boost/mp11/list.hpp>

#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/static_util.hpp>

namespace boost { namespace geometry
{

namespace detail { namespace generic_robust_predicates
{

//only meant to be used in assertions
template <typename T> using is_mp_int =
    boost::mp11::mp_same<T, boost::mp11::mp_int<T::value>>;

template <typename L> using is_coeff_list = boost::mp11::mp_and
    <
        boost::mp11::mp_all_of<L, is_mp_int>,
        boost::mp11::mp_similar<L, boost::mp11::mp_list<>>
    >;

template <typename L>
struct coeff_truncate_impl
{
private:
    static_assert(is_coeff_list<L>::value, "L must be a coefficient list");
    using first_nz = boost::mp11::mp_find_if<L, is_not_zero>;
    using after_second_nz =
        boost::mp11::mp_min<
            boost::mp11::mp_size_t<first_nz::value + 2>,
            boost::mp11::mp_size<L>
        >;
    using tail = boost::mp11::mp_erase_c<L, 0, after_second_nz::value>;
    using head = boost::mp11::mp_erase
            <
                L,
                after_second_nz,
                boost::mp11::mp_size<L>
            >;
    using round_up = boost::mp11::mp_less
            <
                boost::mp11::mp_find_if<tail, is_not_zero>,
                boost::mp11::mp_size<tail>
            >;
public:
    using type = boost::mp11::mp_if
            <
                round_up,
                boost::mp11::mp_push_back<head, boost::mp11::mp_int<1>>,
                head
            >;
    static_assert(is_coeff_list<L>::value, "type should be a coefficient list");
};

template <typename L> using coeff_truncate = typename coeff_truncate_impl<L>::type;

template <typename L> using app_zero_b =
    boost::mp11::mp_push_front<L, boost::mp11::mp_int<0>>;
template <typename L> using app_zero_f =
    boost::mp11::mp_push_back<L, boost::mp11::mp_int<0>>;
template <typename L> using mult_by_1_p_eps = coeff_truncate
        <
            boost::mp11::mp_transform
                <
                    boost::mp11::mp_plus,
                    app_zero_b<L>,
                    app_zero_f<L>
                >
        >;

template
<
    typename L,
    typename N,
    typename done = boost::mp11::mp_bool<N::value == 0>
>
struct mult_by_1_p_eps_pow_impl
{
private:
    using next = mult_by_1_p_eps<L>;
public:
    using type =
        typename mult_by_1_p_eps_pow_impl
            <
                next,
                boost::mp11::mp_int<N::value - 1>
            >::type;
};

template <typename L, typename N>
struct mult_by_1_p_eps_pow_impl<L, N, boost::mp11::mp_true>
{
public:
    using type = L;
};

template <typename L, typename N> using mult_by_1_p_eps_pow = coeff_truncate
        <
            typename mult_by_1_p_eps_pow_impl<L, N>::type
        >;

template <typename L> using div_by_1_m_eps_helper =
    boost::mp11::mp_partial_sum
        <
            L,
            boost::mp11::mp_int<0>,
            boost::mp11::mp_plus
        >;

template <typename L> using div_by_1_m_eps = coeff_truncate
    <
        boost::mp11::mp_push_back
            <
                boost::mp11::mp_pop_back<div_by_1_m_eps_helper<L>>,
                inc<boost::mp11::mp_back<div_by_1_m_eps_helper<L>>>
            >
    >;

template
<
    typename L1,
    typename L2,
    typename L,
    typename L1_Empty = typename empty_or_void<L1>::type,
    typename L2_Empty = typename empty_or_void<L2>::type
>
struct coeff_merge_impl {};

template
<
    typename L1,
    typename L2,
    typename L
>
struct coeff_merge_impl
<
    L1,
    L2,
    L,
    boost::mp11::mp_false,
    boost::mp11::mp_false
>
{
    using type = typename coeff_merge_impl
        <
            boost::mp11::mp_pop_front<L1>,
            boost::mp11::mp_pop_front<L2>,
            boost::mp11::mp_push_back
                <
                    L,
                    boost::mp11::mp_plus
                        <
                            boost::mp11::mp_front<L1>,
                            boost::mp11::mp_front<L2>
                        >
                >
        >::type;
};

template
<
    typename L1,
    typename L2,
    typename L
>
struct coeff_merge_impl
    <
        L1,
        L2,
        L,
        boost::mp11::mp_true,
        boost::mp11::mp_true
    >
{
    using type = L;
};

template
<
    typename L1,
    typename L2,
    typename L
>
struct coeff_merge_impl
    <
        L1,
        L2,
        L,
        boost::mp11::mp_true,
        boost::mp11::mp_false
    >
{
    using type = boost::mp11::mp_append<L, L2>;
};

template
<
    typename L1,
    typename L2,
    typename L
>
struct coeff_merge_impl
    <
        L1,
        L2,
        L,
        boost::mp11::mp_false,
        boost::mp11::mp_true
    >
{
    using type = boost::mp11::mp_append<L, L1>;
};

template <typename L1, typename L2> using coeff_merge =
    coeff_truncate
        <
            typename coeff_merge_impl
                <
                    L1,
                    L2,
                    boost::mp11::mp_list<>
                >::type
        >;

template
<
    typename L1,
    typename L2,
    typename L,
    typename L1_Empty = typename empty_or_void<L1>::type,
    typename L2_Empty = typename empty_or_void<L2>::type
>
struct coeff_max_impl {};

template
<
    typename L1,
    typename L2,
    typename L
>
struct coeff_max_impl<L1, L2, L, boost::mp11::mp_false, boost::mp11::mp_false>
{
    using type = typename coeff_max_impl
        <
            boost::mp11::mp_if
                <
                    boost::mp11::mp_less
                        <
                            boost::mp11::mp_front<L1>,
                            boost::mp11::mp_front<L2>
                        >,
                    boost::mp11::mp_list<>,
                    boost::mp11::mp_pop_front<L1>
                >,
            boost::mp11::mp_if
                <
                    boost::mp11::mp_less
                        <
                            boost::mp11::mp_front<L2>,
                            boost::mp11::mp_front<L1>
                        >,
                    boost::mp11::mp_list<>,
                    boost::mp11::mp_pop_front<L2>
                >,
            boost::mp11::mp_push_back
                <
                    L,
                    boost::mp11::mp_max
                        <
                            boost::mp11::mp_front<L1>,
                            boost::mp11::mp_front<L2>
                        >
                >
        >::type;
};

template
<
    typename L1,
    typename L2,
    typename L
>
struct coeff_max_impl
    <
        L1,
        L2,
        L,
        boost::mp11::mp_true,
        boost::mp11::mp_true
    >
{
    using type = L;
};

template
<
    typename L1,
    typename L2,
    typename L
>
struct coeff_max_impl
    <
        L1,
        L2,
        L,
        boost::mp11::mp_true,
        boost::mp11::mp_false
    >
{
    using type = boost::mp11::mp_append<L, L2>;
};

template
<
    typename L1,
    typename L2,
    typename L
>
struct coeff_max_impl
    <
        L1,
        L2,
        L,
        boost::mp11::mp_false,
        boost::mp11::mp_true
    >
{
    using type = boost::mp11::mp_append<L, L1>;
};

template <typename L1, typename L2> using coeff_max =
    coeff_truncate
        <
            typename coeff_max_impl
                <
                    L1,
                    L2,
                    boost::mp11::mp_list<>
                >::type
        >;

template
<
    typename L,
    std::size_t Tail_Size =
          boost::mp11::mp_size<L>::value
        - boost::mp11::mp_find_if<L, is_not_zero>::value
>
struct coeff_round_impl
{
private:
    using first_nz = boost::mp11::mp_find_if<L, is_not_zero>;
    using tail = boost::mp11::mp_erase_c<L, 0, first_nz::value + 2>;
    using zero_tail = boost::mp11::mp_same
        <
            boost::mp11::mp_find_if<tail, is_not_zero>,
            boost::mp11::mp_size<tail>
        >;
    using head = boost::mp11::mp_erase_c
        <
            L,
            first_nz::value + 2,
            boost::mp11::mp_size<L>::value
        >;
    using minor = boost::mp11::mp_if
        <
            zero_tail,
            boost::mp11::mp_back<head>,
            inc<boost::mp11::mp_back<head>>
        >;
    using major = boost::mp11::mp_at<head, first_nz>;
    using major_rounded = boost::mp11::mp_int< 1 << log_2_ceil<major>::value >;
    using minor_rounded = boost::mp11::mp_int
        <
            (minor::value / major_rounded::value) * major_rounded::value < minor::value ?
                  (minor::value / major_rounded::value + 1) * major_rounded::value
                : (minor::value / major_rounded::value) * major_rounded::value
        >;
public:
    using type = boost::mp11::mp_push_back
        <
            boost::mp11::mp_pop_back<head>,
            minor_rounded
        >;
};

template <typename L>
struct coeff_round_impl<L, 0> { using type = L; };

template <typename L>
struct coeff_round_impl<L, 1> { using type = L; };

template <typename L> using coeff_round = typename coeff_round_impl<L>::type;

template <typename IV1, typename IV2> using indexed_value_product =
    boost::mp11::mp_list
        <
            boost::mp11::mp_plus
                <
                    boost::mp11::mp_first<IV1>,
                    boost::mp11::mp_first<IV2>
                >,
            boost::mp11::mp_int
                <
                      boost::mp11::mp_second<IV1>::value
                    * boost::mp11::mp_second<IV2>::value
                >
        >;

template <typename V1> struct add_nested
{
    template <typename K, typename V2> using fn =
        boost::mp11::mp_plus<V1, V2>;
};

template <typename M, typename IV> using list_product_fold =
    boost::mp11::mp_map_update_q
        <
            M,
            IV,
            add_nested< boost::mp11::mp_second<IV> >
        >;

template<typename IV1, typename IV2> using index_less =
    boost::mp11::mp_less
        <
            boost::mp11::mp_first<IV1>,
            boost::mp11::mp_first<IV2>
        >;

template <typename L1, typename L2> using list_product =
    strip_index
        <
            boost::mp11::mp_sort
                <
                    boost::mp11::mp_fold
                        <
                            boost::mp11::mp_product
                                <
                                    indexed_value_product,
                                    indexed<L1>,
                                    indexed<L2>
                                >,
                            boost::mp11::mp_list<>,
                            list_product_fold
                        >,
                    index_less
                >
        >;

}} // namespace detail::generic_robust_predicates

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_COEFFICIENT_LIST_HPP


#include <gtest/gtest.h>
#include <stan/math/prim/mat.hpp>
#include <boost/math/distributions.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <test/unit/math/prim/scal/prob/util.hpp>
#include <exception>
#include <limits>
#include <vector>

using Eigen::MatrixXd;
using Eigen::VectorXd;
using stan::math::gaussian_dlm_obs_rng;

class ProbDistributionsGaussianDLMInputsRng : public ::testing::Test {
 protected:
  virtual void SetUp() {
    FF = MatrixXd::Random(2, 3);
    GG = MatrixXd::Random(2, 2);
    V = MatrixXd::Identity(3, 3);
    V_vec = VectorXd::Constant(3, 1.0);
    W = MatrixXd::Identity(2, 2);
    y = MatrixXd::Random(3, 5);
    m0 = VectorXd::Random(2);
    C0 = MatrixXd::Identity(2, 2);
    T = 5;
  }

  MatrixXd FF;
  MatrixXd GG;
  MatrixXd V;
  VectorXd V_vec;
  MatrixXd W;
  MatrixXd y;
  VectorXd m0;
  MatrixXd C0;
  int T;
};

TEST_F(ProbDistributionsGaussianDLMInputsRng, PoliciesF) {
  boost::random::mt19937 rng;
  MatrixXd FF_sz1 = MatrixXd::Random(4, 3);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF_sz1, GG, V, W, m0, C0, T, rng),
               std::invalid_argument);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF_sz1, GG, V_vec, W, m0, C0, T, rng),
               std::invalid_argument);
  MatrixXd FF_sz2 = MatrixXd::Random(2, 4);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF_sz2, GG, V, W, m0, C0, T, rng),
               std::invalid_argument);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF_sz2, GG, V_vec, W, m0, C0, T, rng),
               std::invalid_argument);
  // finite and NaN
  MatrixXd FF_inf = FF;
  FF_inf(0, 0) = std::numeric_limits<double>::infinity();
  EXPECT_THROW(gaussian_dlm_obs_rng(FF_inf, GG, V, W, m0, C0, T, rng),
               std::domain_error);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF_inf, GG, V_vec, W, m0, C0, T, rng),
               std::domain_error);
  MatrixXd FF_nan = FF;
  FF_nan(0, 0) = std::numeric_limits<double>::quiet_NaN();
  EXPECT_THROW(gaussian_dlm_obs_rng(FF_nan, GG, V, W, m0, C0, T, rng),
               std::domain_error);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF_nan, GG, V_vec, W, m0, C0, T, rng),
               std::domain_error);
}

TEST_F(ProbDistributionsGaussianDLMInputsRng, PoliciesG) {
  boost::random::mt19937 rng;
  // size
  MatrixXd GG_sz1 = MatrixXd::Random(3, 3);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG_sz1, V, W, m0, C0, T, rng),
               std::invalid_argument);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG_sz1, V_vec, W, m0, C0, T, rng),
               std::invalid_argument);
  MatrixXd GG_sz2 = MatrixXd::Random(2, 3);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG_sz2, V, W, m0, C0, T, rng),
               std::invalid_argument);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG_sz2, V_vec, W, m0, C0, T, rng),
               std::invalid_argument);
  // finite and NaN
  MatrixXd GG_inf = GG;
  GG_inf(0, 0) = std::numeric_limits<double>::infinity();
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG_inf, V, W, m0, C0, T, rng),
               std::domain_error);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG_inf, V_vec, W, m0, C0, T, rng),
               std::domain_error);
  MatrixXd GG_nan = GG;
  GG_nan(0, 0) = std::numeric_limits<double>::quiet_NaN();
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG_nan, V, W, m0, C0, T, rng),
               std::domain_error);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG_nan, V_vec, W, m0, C0, T, rng),
               std::domain_error);
}

TEST_F(ProbDistributionsGaussianDLMInputsRng, PoliciesW) {
  boost::random::mt19937 rng;
  // Not symmetric
  MatrixXd W_asym = W;
  W_asym(0, 1) = 1;
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W_asym, m0, C0, T, rng),
               std::domain_error);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_vec, W_asym, m0, C0, T, rng),
               std::domain_error);
  // negative
  MatrixXd W_neg = W;
  W_neg(0, 0) = -1;
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W_neg, m0, C0, T, rng),
               std::domain_error);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_vec, W_neg, m0, C0, T, rng),
               std::domain_error);
  // finite and NaN
  MatrixXd W_infinite = W;
  W_infinite(0, 0) = std::numeric_limits<double>::infinity();
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W_infinite, m0, C0, T, rng),
               std::domain_error);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_vec, W_infinite, m0, C0, T, rng),
               std::domain_error);
  MatrixXd W_nan = W;
  W_nan(0, 0) = std::numeric_limits<double>::quiet_NaN();
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W_nan, m0, C0, T, rng),
               std::domain_error);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_vec, W_nan, m0, C0, T, rng),
               std::domain_error);
  // wrong size
  MatrixXd W_sz = MatrixXd::Identity(4, 4);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W_sz, m0, C0, T, rng),
               std::invalid_argument);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_vec, W_sz, m0, C0, T, rng),
               std::invalid_argument);
  // not square
  MatrixXd W_notsq = MatrixXd::Identity(2, 3);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W_notsq, m0, C0, T, rng),
               std::invalid_argument);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_vec, W_notsq, m0, C0, T, rng),
               std::invalid_argument);
  // positive semi-definite is okay
  MatrixXd W_psd = MatrixXd::Zero(2, 2);
  W_psd(0, 0) = 1.0;
  EXPECT_NO_THROW(gaussian_dlm_obs_rng(FF, GG, V, W_psd, m0, C0, T, rng));
  EXPECT_NO_THROW(gaussian_dlm_obs_rng(FF, GG, V_vec, W_psd, m0, C0, T, rng));
  // Non positive semi-definite is not.
  MatrixXd W_bad = MatrixXd::Zero(2, 2);
  W_bad << 1, 2, 2, 1;
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W_bad, m0, C0, T, rng),
               std::domain_error);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_vec, W_bad, m0, C0, T, rng),
               std::domain_error);
}

TEST_F(ProbDistributionsGaussianDLMInputsRng, PoliciesVMatrix) {
  boost::random::mt19937 rng;
  // Not symmetric
  MatrixXd V_asym = V;
  V_asym(0, 2) = 1;
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_asym, W, m0, C0, T, rng),
               std::domain_error);
  // negative
  MatrixXd V_neg = V;
  V_neg(0, 2) = -1;
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_neg, W, m0, C0, T, rng),
               std::domain_error);
  // finite and NaN
  MatrixXd V_infinite = V;
  V_infinite(0, 0) = std::numeric_limits<double>::infinity();
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_infinite, W, m0, C0, T, rng),
               std::domain_error);
  MatrixXd V_nan = V;
  V_nan(0, 0) = std::numeric_limits<double>::quiet_NaN();
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_nan, W, m0, C0, T, rng),
               std::domain_error);
  // wrong size
  MatrixXd V2 = MatrixXd::Identity(2, 2);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V2, W, m0, C0, T, rng),
               std::invalid_argument);
  // not square
  MatrixXd V3 = MatrixXd::Identity(2, 3);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V3, W, m0, C0, T, rng),
               std::invalid_argument);
  // positive semi-definite is okay
  MatrixXd V_psd = MatrixXd::Zero(3, 3);
  V_psd(0, 0) = 1.0;
  EXPECT_NO_THROW(gaussian_dlm_obs_rng(FF, GG, V_psd, W, m0, C0, T, rng));
  // Non positive semi-definite is not.
  MatrixXd V_bad = MatrixXd::Zero(3, 3);
  V_bad << 1, 2, 2, 2, 1, 2, 2, 2, 1;
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_bad, W, m0, C0, T, rng),
               std::domain_error);
}

TEST_F(ProbDistributionsGaussianDLMInputsRng, PoliciesVVector) {
  boost::random::mt19937 rng;
  // negative
  MatrixXd V_neg = V_vec;
  V_neg(0) = -1;
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_neg, W, m0, C0, T, rng),
               std::invalid_argument);
  // finite and NaN
  MatrixXd V_infinite = V_vec;
  V_infinite(0) = std::numeric_limits<double>::infinity();
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_infinite, W, m0, C0, T, rng),
               std::domain_error);
  MatrixXd V_nan = V_vec;
  V_nan(0) = std::numeric_limits<double>::quiet_NaN();
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_nan, W, m0, C0, T, rng),
               std::domain_error);
  // wrong size
  VectorXd V_badsz = VectorXd::Constant(2, 1.0);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_badsz, W, m0, C0, T, rng),
               std::invalid_argument);
  // positive semi-definite is okay
  VectorXd V_psd = VectorXd::Zero(3);
  V_psd(0) = 1.0;
  EXPECT_NO_THROW(gaussian_dlm_obs_rng(FF, GG, V_psd, W, m0, C0, T, rng));
  // Non positive semi-definite is not, but with vector V, V is
  // diagonal so non psd means negative entries, and we already tested
  // that those give an error.
}

TEST_F(ProbDistributionsGaussianDLMInputsRng, Policiesm0) {
  boost::random::mt19937 rng;
  // size
  VectorXd m0_sz = VectorXd::Zero(4, 1);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W, m0_sz, C0, T, rng),
               std::invalid_argument);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_vec, W, m0_sz, C0, T, rng),
               std::invalid_argument);
  // finite and NaN
  VectorXd m0_inf = m0;
  m0_inf(0) = std::numeric_limits<double>::infinity();
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W, m0_inf, C0, T, rng),
               std::domain_error);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_vec, W, m0_inf, C0, T, rng),
               std::domain_error);
  VectorXd m0_nan = m0;
  m0_nan(0) = std::numeric_limits<double>::quiet_NaN();
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W, m0_nan, C0, T, rng),
               std::domain_error);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_vec, W, m0_nan, C0, T, rng),
               std::domain_error);
}

TEST_F(ProbDistributionsGaussianDLMInputsRng, PoliciesC0) {
  boost::random::mt19937 rng;
  // size
  MatrixXd C0_sz = MatrixXd::Identity(3, 3);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W, m0, C0_sz, T, rng),
               std::invalid_argument);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_vec, W, m0, C0_sz, T, rng),
               std::invalid_argument);
  // negative
  MatrixXd C0_neg = C0;
  C0_neg(0, 0) = -1;
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W, m0, C0_neg, T, rng),
               std::domain_error);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_vec, W, m0, C0_neg, T, rng),
               std::domain_error);
  // asymmetric
  MatrixXd C0_asym = C0;
  C0_asym(0, 1) = 1;
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W, m0, C0_asym, T, rng),
               std::domain_error);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_vec, W, m0, C0_neg, T, rng),
               std::domain_error);
  // not square
  MatrixXd C0_notsq = MatrixXd::Identity(3, 2);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W, m0, C0_notsq, T, rng),
               std::invalid_argument);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V_vec, W, m0, C0_notsq, T, rng),
               std::invalid_argument);
  // not positive definite.
  MatrixXd C0_psd = MatrixXd::Zero(2, 2);
  C0_psd(0, 0) = 1.0;
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W, m0, C0_psd, T, rng),
               std::domain_error);
}

TEST_F(ProbDistributionsGaussianDLMInputsRng, PoliciesT) {
  boost::random::mt19937 rng;
  // Must be positive.
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W, m0, C0, 0, rng),
               std::domain_error);
  EXPECT_THROW(gaussian_dlm_obs_rng(FF, GG, V, W, m0, C0, -1, rng),
               std::domain_error);
}

TEST_F(ProbDistributionsGaussianDLMInputsRng, chiSquaredGoodnessOfFit) {
  boost::random::mt19937 rng;

  // With identity state transition and initial mean 0 and identity
  // covariance matrices:
  // state at 0 = initial noise
  // state at t+1 = state at t + noise[t+1]
  // observed at T =
  //     initial noise
  //     + noise[1] + ... + noise[T]
  //     + observation noise.
  //   = sum of (T+2) variance 1 gaussians
  //   = variance T+2 gaussian noise
  // (Mean 0.)

  int n = 3;
  int r = 1;

  VectorXd m0_(n, 1);
  m0_ << 0, 0, 0;

  MatrixXd C0_ = MatrixXd::Identity(n, n);

  MatrixXd FF_(n, r);
  FF_ << 1, 0, 0;

  MatrixXd V_ = MatrixXd::Identity(r, r);

  MatrixXd GG_ = MatrixXd::Identity(n, n);

  MatrixXd W_ = MatrixXd::Identity(n, n);

  // Small enough that variance T_ + 2 and variance T_ + 1 can be
  // correctly distinguished. Just to keep the off-by-one errors under
  // control.
  int T_ = 3;

  int N = 10000;
  int K = boost::math::round(2 * std::pow(N, 0.4));
  boost::math::normal_distribution<> dist(0, sqrt(T_ + 2));

  std::vector<double> quantiles;
  for (int i = 1; i < K; i++)
    quantiles.push_back(quantile(dist, i * std::pow(K, -1.0)));
  quantiles.push_back(std::numeric_limits<double>::max());

  std::vector<double> samples;
  Eigen::MatrixXd y_;
  for (int n = 0; n < N; ++n) {
    y_ = gaussian_dlm_obs_rng(FF_, GG_, V_, W_, m0_, C0_, T_, rng);
    ASSERT_TRUE(y_.rows() == r && y_.cols() == T_);
    samples.push_back(y_(0, T_ - 1));
  }

  assert_matches_quantiles(samples, quantiles, 1e-6);
}


/**************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                            *
 * All rights reserved.                                                   *
 *                                                                        *
 * This file is part of the mfmg library. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the  *
 * top-level directory                                                    *
 *                                                                        *
 * SPDX-License-Identifier: BSD-3-Clause                                  *
 *************************************************************************/

#ifndef UTILS_H
#define UTILS_H

#include <mfmg/common/exceptions.hpp>

#include <deal.II/lac/la_parallel_vector.h>
#include <deal.II/lac/trilinos_sparse_matrix.h>
#include <deal.II/lac/trilinos_sparsity_pattern.h>

#include <Teuchos_ParameterList.hpp>

#include <boost/property_tree/ptree.hpp>

#include <algorithm>
#include <numeric>
#include <vector>

namespace mfmg
{
void ptree2plist(boost::property_tree::ptree const &ptree,
                 Teuchos::ParameterList &plist);

template <typename T>
std::vector<unsigned int> sort_permutation(std::vector<T> const &vec_1,
                                           std::vector<T> const &vec_2)
{
  ASSERT(vec_1.size() == vec_2.size(), "The vectors need to have the same size "
                                       "but vector 1 has a size " +
                                           std::to_string(vec_1.size()) +
                                           " and vector 2 has a size " +
                                           std::to_string(vec_2.size()));

  std::vector<unsigned int> permutation(vec_1.size());
  std::iota(permutation.begin(), permutation.end(), 0);
  std::sort(permutation.begin(), permutation.end(),
            [&](unsigned int i, unsigned int j) {
              if (vec_1[i] != vec_1[j])
                return (vec_1[i] < vec_1[j]);
              else
                return (vec_2[i] < vec_2[j]);
            });

  return permutation;
}

template <typename T>
void apply_permutation_in_place(std::vector<unsigned int> const &permutation,
                                std::vector<T> &vec)
{
  unsigned int const n_elem = vec.size();
  std::vector<bool> done(n_elem, false);
  for (unsigned int i = 0; i < n_elem; ++i)
  {
    if (done[i] == false)
    {
      done[i] = true;
      unsigned int prev_j = i;
      unsigned int j = permutation[i];
      while (i != j)
      {
        std::swap(vec[prev_j], vec[j]);
        done[j] = true;
        prev_j = j;
        j = permutation[j];
      }
    }
  }
}

template <typename ScalarType>
void check_restriction_matrix(
    MPI_Comm comm, std::vector<dealii::Vector<ScalarType>> const &eigenvectors,
    std::vector<std::vector<dealii::types::global_dof_index>> const
        &dof_indices_maps,
    dealii::LinearAlgebra::distributed::Vector<ScalarType> const
        &locally_relevant_global_diag,
    std::vector<std::vector<ScalarType>> const &diag_elements,
    std::vector<unsigned int> const &n_local_eigenvectors)
{
#if MFMG_DEBUG
  // Check that the locally_relevant_global_diag is the sum of the agglomerates
  // diagonal
  // TODO do not ask user for the locally_relevant_global_diag
  dealii::LinearAlgebra::distributed::Vector<ScalarType> new_global_diag(
      locally_relevant_global_diag.get_partitioner());
  unsigned int const n_agglomerates = n_local_eigenvectors.size();
  for (unsigned int i = 0; i < n_agglomerates; ++i)
  {
    // Get the size of the eigenvectors in agglomerate i
    unsigned int offset = std::accumulate(n_local_eigenvectors.begin(),
                                          n_local_eigenvectors.begin() + i, 0);
    unsigned int const n_elem = eigenvectors[offset].size();

    for (unsigned int j = 0; j < n_elem; ++j)
    {
      dealii::types::global_dof_index const global_pos = dof_indices_maps[i][j];
      new_global_diag[global_pos] += diag_elements[i][j];
    }
  }
  new_global_diag.compress(dealii::VectorOperation::add);
  new_global_diag -= locally_relevant_global_diag;
  ASSERT((new_global_diag.linfty_norm() /
          locally_relevant_global_diag.linfty_norm()) < 1e-14,
         "Sum of agglomerate diagonals is not equal to the global diagonal");

  // Check that the sum of the weight matrices is the identity
  auto locally_owned_dofs =
      locally_relevant_global_diag.locally_owned_elements();
  dealii::TrilinosWrappers::SparsityPattern sp(locally_owned_dofs,
                                               locally_owned_dofs, comm);
  for (auto local_index : locally_owned_dofs)
    sp.add(local_index, local_index);
  sp.compress();

  dealii::TrilinosWrappers::SparseMatrix weight_matrix(sp);
  unsigned int pos = 0;
  for (unsigned int i = 0; i < n_agglomerates; ++i)
  {
    unsigned int const n_elem = eigenvectors[pos].size();
    for (unsigned int j = 0; j < n_elem; ++j)
    {
      dealii::types::global_dof_index const global_pos = dof_indices_maps[i][j];
      double const value =
          diag_elements[i][j] / locally_relevant_global_diag[global_pos];
      weight_matrix.add(global_pos, global_pos, value);
    }
    pos += n_local_eigenvectors[i];
  }

  // Compress the matrix
  weight_matrix.compress(dealii::VectorOperation::add);

  for (auto index : locally_owned_dofs)
    ASSERT(std::abs(weight_matrix.diag_element(index) - 1.0) < 1e-14,
           "Sum of local weight matrices is not the identity");
#else
  std::ignore = comm;
  std::ignore = eigenvectors;
  std::ignore = dof_indices_maps;
  std::ignore = locally_relevant_global_diag;
  std::ignore = diag_elements;
  std::ignore = n_local_eigenvectors;
#endif
}
} // namespace mfmg

#endif


//==============================================================================
//
//   (c) Copyright, 2012 University Corporation for Atmospheric Research (UCAR).
//       All rights reserved.
//
//==============================================================================

/**
 *
 * @file apps/probe_message_dataset_manager/process.cc
 *
 */

// Include files
#include <log/log.hh>
#include <libconfig.h++>
#include <stdio.h>
#include <stdlib.h>
#include <boost/format.hpp>
#include <Eigen/Dense>
#include <unordered_map>

using namespace Eigen;

#include "process.hh"
#include "vdt/stat_tools.hh"
#include "vdt/vdt_probe_message_file_reader.hh"

using std::unordered_map;
using namespace std;
using namespace libconfig;
using boost::format;

extern Log *Logg;
extern int Debug_level;

const double FILL_VALUE = -9999.;

process::~process()
{
}

int print_header(FILE *fp_out)
{
  return(fprintf(fp_out, "num_invalid_location,num_valid_location,num_cells,area_cells,percent_cell_coverage,latitude_min,latitude_iqr_25,latitude_median,latitude_iqr_75,latitude_max,longitude_min,longitude_iqr_25,longitude_median,longitude_iqr_75,longitude_max,air_temp2_min,air_temp2_iqr_25,air_temp2_median,air_temp2_iqr_75,air_temp2_max,surface_temp_min,surface_temp_iqr_25,surface_temp_median,surface_temp_iqr_75,surface_temp_max\n"));
}

int print_field(FILE *fp_out, const vector<double> &field_values, const vector<size_t> &quartile_indices)
{
  if (field_values.size() == 0)
    {
      return(fprintf(fp_out, ",-9999.,-9999.,-9999.,-9999.,-9999."));
    }
  else
    {
      return(fprintf(fp_out, ",%lf,%lf,%lf,%lf,%lf", field_values[0],field_values[quartile_indices[0]],field_values[quartile_indices[1]],field_values[quartile_indices[2]],field_values[field_values.size()-1]));
    }
}

int print_field(FILE *fp_out, const vector<float> &field_values, const vector<size_t> &quartile_indices)
{
  if (field_values.size() == 0)
    {
      return(fprintf(fp_out, ",-9999.,-9999.,-9999.,-9999.,-9999."));
    }
  else
    {
      return(fprintf(fp_out, ",%f,%f,%f,%f,%f", field_values[0],field_values[quartile_indices[0]],field_values[quartile_indices[1]],field_values[quartile_indices[2]],field_values[field_values.size()-1]));
    }
}

int msg_stats(time_t nearest_delta_time, vector<vdt_probe_message> &msgs, FILE *fp_out)
{
  // Get rid of messages with 0 or missing latitude or longitude
  vector<double> obs_time;
  vector<double> latitude;
  vector<double> longitude;
  vector<float> air_temp2;
  vector<float> surface_temp;

  MatrixXi conus_lat_long = MatrixXi::Zero(90, 180);

  size_t invalid_location = 0;
  for (size_t i=0; i<msgs.size(); i++)
    {
      double lat = msgs[i].get_latitude();
      double lon = msgs[i].get_longitude();
      float air_temp2_value = msgs[i].get_air_temp2();
      float surface_temp_value = msgs[i].get_surface_temp();
      if (lat == 0.0 or lat == FILL_VALUE or lon == 0.0 or lon == FILL_VALUE)
	{
	  invalid_location++;
	}
      else
	{
	  int int_lat = static_cast<int> (fabs(floor(lat)));
	  int int_lon = static_cast<int> (fabs(floor(lon)));
	  conus_lat_long(int_lat, int_lon) = 1;
	  latitude.push_back(lat);
	  longitude.push_back(lon);

	  if (air_temp2_value != FILL_VALUE)
	    {
	      air_temp2.push_back(air_temp2_value);
	    }

	  if (surface_temp_value != FILL_VALUE)
	    {
	      surface_temp.push_back(surface_temp_value);
	    }
	}
    }

  if (latitude.size() == 0)
    {
      Logg->write_time_info("Error: no valid lat-long locations\n");
      return 1;
    }

  int number_cells = conus_lat_long.sum();
  Logg->write_time_info("number cells %d, number 0 messages = %ld, number messages with valid location %ld\n", number_cells, invalid_location, latitude.size());

  vector<size_t> lat_quartiles;
  vector<size_t> lon_quartiles;
  vector<size_t> air_temp2_quartiles;
  vector<size_t> surface_temp_quartiles;

  ST_quartiles(latitude, lat_quartiles);
  ST_quartiles(longitude, lon_quartiles);
  ST_quartiles(air_temp2, air_temp2_quartiles);
  ST_quartiles(surface_temp, surface_temp_quartiles);

  int area_cells = fabs((floor(latitude[latitude.size()-1]) - floor(latitude[0]) + 1) * (floor(longitude[longitude.size()-1]) - floor(longitude[0]) + 1));
  int percent_cell_coverage = static_cast<int>(100*static_cast<float>(number_cells)/area_cells);

  Logg->write_time_info("range cells %d, percent cells covered %f\n", area_cells, static_cast<float>(number_cells)/area_cells);

  // Write output statistics
  Logg->write_time_info("writing output statistics for probe message file[s]\n");

  char date_string[64];
  strftime(date_string, 64, "%Y%m%d:%H%M", gmtime(&nearest_delta_time));

  fprintf(fp_out, "%s,%ld,%ld,%d,%d,%d", date_string, invalid_location, longitude.size(), number_cells, area_cells, percent_cell_coverage);

  if (print_field(fp_out, latitude, lat_quartiles) < 0)
    {
      Logg->write_time_error("failed writing latitude\n");
      return -1;
    }

  if (print_field(fp_out, longitude, lon_quartiles) < 0)
    {
      Logg->write_time_error("failed writing longitude\n");
      return -1;
    }

  if (print_field(fp_out, air_temp2, air_temp2_quartiles) < 0)
    {
      Logg->write_time_error("failed writing air_temp2\n");
      return -1;
    }

  if (print_field(fp_out, surface_temp, surface_temp_quartiles) < 0)
    {
      Logg->write_time_error("failed writing surface_temp\n");
      return -1;
    }
  fprintf(fp_out, "\n");
  return 0;
}

int process::run()
{
  // Read configuration file
  Logg->write_time("Info: reading config file %s\n", args.cfg_file.c_str());

  config_reader cfg_reader(args.cfg_file);
  if (cfg_reader.error != string(""))
    {
      Logg->write_time("Error: configuration file error %s\n", cfg_reader.error.c_str());
      return -1;
    }

  // Read probe messages from each input file into vector
  vector<vdt_probe_message> msgs;

  // NOTE: the reader should be updated to include file list reading capability
  unordered_map<time_t, vector<vdt_probe_message> > time_vector_map;
  unordered_map<time_t, vector<vdt_probe_message> >::iterator itr;
  vector<time_t> delta_times;
  vector<vdt_probe_message> file_msgs;

  for (size_t i=0; i<args.probe_message_file_list.size(); i++)
    {
      file_msgs.clear();

      vdt_probe_message_file_reader probe_rdr(args.probe_message_file_list[0].c_str());
      probe_rdr.get_probe_messages(file_msgs);

      Logg->write_time("Info: processing file %s having %ld messages\n", args.probe_message_file_list[i].c_str(), file_msgs.size());

      // append file_msgs to msgs
      for (size_t j=0; j<file_msgs.size(); j++)
	{
	  double msg_time = file_msgs[j].get_obs_time();
	  time_t nearest_delta_time = floor(msg_time/args.time_delta) * args.time_delta;
	  printf("nearest delta time %ld, time_delta %d\n", nearest_delta_time, args.time_delta);

	  itr = time_vector_map.find(nearest_delta_time);
	  if (itr == time_vector_map.end())
	    {
	      delta_times.push_back(nearest_delta_time);
	      time_vector_map[nearest_delta_time].push_back(file_msgs[j]);
	    }
	  else
	    {
	      itr->second.push_back(file_msgs[j]);
	    }
	  //time_vector_map[nearest_delta_time].push_back(file_msgs[j]);
	}
    }

  // Sort delta_times into ascending order
  sort(delta_times.begin(), delta_times.end());
  Logg->write_time("Info: number of delta times %ld\n", delta_times.size());

  FILE *fp_out = fopen(args.out_file.c_str(), "w");

  if (print_header(fp_out) < 0)
    {
      Logg->write_time_info("failed in print_header to %s\n");
      return -1;
    }

  for (size_t i=0; i<delta_times.size(); i++)
    {
      vector<vdt_probe_message> &msgs = time_vector_map[delta_times[i]];
      Logg->write_time("Info: delta_time, number of messages: %ld, %ld\n", delta_times[i], msgs.size());

      int ret = msg_stats(delta_times[i], msgs, fp_out);
      if (ret < 0)
	{
	  Logg->write_time("Error: msgs_stats failure\n");
	  return -1;
	}
    }

  fclose(fp_out);

  return 0;
}


#include <iostream>                    // std::cout
#include <fstream>                     // std::ifstream, std::ofstream
#include <filesystem>                  // std::filesystem
#include <string>                      // std::string
#include <chrono>                      // std::chrono
#include <utility>                     // std::pair
// For External Library
#include <torch/torch.h>               // torch
#include <boost/program_options.hpp>   // boost::program_options
// For Original Header
#include "loss.hpp"                    // Loss
#include "networks.hpp"                // VariationalAutoEncoder
#include "transforms.hpp"              // transforms_Compose
#include "datasets.hpp"                // datasets::ImageFolderPairWithPaths
#include "dataloader.hpp"              // DataLoader::ImageFolderPairWithPaths
#include "visualizer.hpp"              // visualizer

// Define Namespace
namespace fs = std::filesystem;
namespace po = boost::program_options;

// ---------------
// Test Function
// ---------------
void test(po::variables_map &vm, torch::Device &device, VariationalAutoEncoder &model, std::vector<transforms_Compose> &transform){

    constexpr std::pair<float, float> output_range = {-1.0, 1.0};  // range of the value in output images

    // (0) Initialization and Declaration
    float ave_rec_loss, ave_kld_loss, ave_GT_loss;
    double seconds, ave_time;
    std::string path, result_dir, fname;
    std::string input_dir, output_dir;
    std::ofstream ofs;
    std::chrono::system_clock::time_point start, end;
    std::tuple<torch::Tensor, torch::Tensor, std::vector<std::string>, std::vector<std::string>> data;
    torch::Tensor imageI, imageO, output;
    torch::Tensor rec, kld, GT_loss;
    datasets::ImageFolderPairWithPaths dataset;
    DataLoader::ImageFolderPairWithPaths dataloader;

    // (1) Get Test Dataset
    input_dir = "datasets/" + vm["dataset"].as<std::string>() + '/' + vm["test_in_dir"].as<std::string>();
    output_dir = "datasets/" + vm["dataset"].as<std::string>() + '/' + vm["test_out_dir"].as<std::string>();
    dataset = datasets::ImageFolderPairWithPaths(input_dir, output_dir, transform, transform);
    dataloader = DataLoader::ImageFolderPairWithPaths(dataset, /*batch_size_=*/1, /*shuffle_=*/false, /*num_workers_=*/0);
    std::cout << "total test images : " << dataset.size() << std::endl << std::endl;

    // (2) Get Model
    path = "checkpoints/" + vm["dataset"].as<std::string>() + "/models/epoch_" + vm["test_load_epoch"].as<std::string>() + ".pth";
    torch::load(model, path);

    // (3) Set Loss Function
    auto criterion = Loss(vm["loss"].as<std::string>());

    // (4) Initialization of Value
    ave_rec_loss = 0.0;
    ave_kld_loss = 0.0;
    ave_GT_loss = 0.0;
    ave_time = 0.0;

    // (5) Tensor Forward
    model->eval();
    result_dir = vm["test_result_dir"].as<std::string>();  fs::create_directories(result_dir);
    ofs.open(result_dir + "/loss.txt", std::ios::out);
    while (dataloader(data)){

        imageI = std::get<0>(data).to(device);
        imageO = std::get<1>(data).to(device);

        start = std::chrono::system_clock::now();

        output = model->forward(imageI);

        end = std::chrono::system_clock::now();
        seconds = (double)std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() * 0.001 * 0.001;

        rec = criterion(output, imageI);
        kld = vm["Lambda"].as<float>() * model->kld_just_before();
        GT_loss = criterion(output, imageO);

        ave_rec_loss += rec.item<float>();
        ave_kld_loss += kld.item<float>();
        ave_GT_loss += GT_loss.item<float>();
        ave_time += seconds;

        std::cout << '<' << std::get<2>(data).at(0) << "> " << vm["loss"].as<std::string>() << ':' << rec.item<float>() << " kld:" << kld.item<float>() << " GT_" << vm["loss"].as<std::string>() << ':' << GT_loss.item<float>() << std::endl;
        ofs << '<' << std::get<2>(data).at(0) << "> " << vm["loss"].as<std::string>() << ':' << rec.item<float>() << " kld:" << kld.item<float>() << " GT_" << vm["loss"].as<std::string>() << ':' << GT_loss.item<float>() << std::endl;

        fname = result_dir + '/' + std::get<3>(data).at(0);
        visualizer::save_image(output.detach(), fname, /*range=*/output_range, /*cols=*/1, /*padding=*/0);

    }

    // (6) Calculate Average
    ave_rec_loss = ave_rec_loss / (float)dataset.size();
    ave_kld_loss = ave_kld_loss / (float)dataset.size();
    ave_GT_loss = ave_GT_loss / (float)dataset.size();
    ave_time = ave_time / (double)dataset.size();

    // (7) Average Output
    std::cout << "<All> " << vm["loss"].as<std::string>() << ':' << ave_rec_loss << " kld:" << ave_kld_loss << " GT_" << vm["loss"].as<std::string>() << ':' << ave_GT_loss << " (time:" << ave_time << ')' << std::endl;
    ofs << "<All> " << vm["loss"].as<std::string>() << ':' << ave_rec_loss << " kld:" << ave_kld_loss << " GT_" << vm["loss"].as<std::string>() << ':' << ave_GT_loss << " (time:" << ave_time << ')' << std::endl;

    // Post Processing
    ofs.close();

    // End Processing
    return;

}


//          Copyright Carl Philipp Reh 2009 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/literal.hpp>
#include <fcppt/make_literal_boost_units.hpp>
#include <fcppt/catch/begin.hpp>
#include <fcppt/catch/end.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/length.hpp>
#include <catch2/catch.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_CATCH_BEGIN

TEST_CASE("literal boost_units", "[literal]")
{
  using length = boost::units::quantity<boost::units::si::length, int>;

  CHECK(fcppt::literal<length>(2).value() == 2);
}

FCPPT_CATCH_END


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#ifndef __multicontact_api_math_search_hpp__
#define __multicontact_api_math_search_hpp__

#include <Eigen/Dense>

namespace multicontact_api
{
  namespace math
  {

    template<class DenseDerived>
    Eigen::DenseIndex binarySearch(const Eigen::DenseBase<DenseDerived> & vector,
                                   const typename DenseDerived::Scalar & value)
    {
      EIGEN_STATIC_ASSERT_VECTOR_ONLY(DenseDerived);

      int left_id = 0;
      int right_id = vector.ssize()-1;

      while(left_id <= right_id)
      {
        int middle_id = left_id + (right_id - left_id)/2;
        if(vector[middle_id] < value)
          left_id = middle_id+1;
        else if(vector[middle_id] > value)
          right_id = middle_id-1;
        else
          return middle_id;
      }

      return left_id-1;
    }
  }
}

#endif // ifndef __multicontact_api_math_search_hpp__


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

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

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

#ifndef BOOST_GEOMETRY_GEOMETRIES_ADAPTED_BOOST_POLYGON_RING_HPP
#define BOOST_GEOMETRY_GEOMETRIES_ADAPTED_BOOST_POLYGON_RING_HPP

// Adapts Geometries from Boost.Polygon for usage in Boost.Geometry
// boost::polygon::polygon_data -> boost::geometry::ring

#include <cstddef>
#include <boost/polygon/polygon.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/mutable_range.hpp>
#include <boost/geometry/core/tags.hpp>

#ifndef DOXYGEN_NO_TRAITS_SPECIALIZATIONS

namespace boost { namespace geometry
{

namespace traits
{

template <typename CoordinateType>
struct tag<boost::polygon::polygon_data<CoordinateType> >
{
    typedef ring_tag type;
};

template <typename CoordinateType>
struct clear<boost::polygon::polygon_data<CoordinateType> >
{
    static inline void apply(boost::polygon::polygon_data<CoordinateType>& data)
    {
        // There is no "clear" function but we can assign
        // a newly (and therefore empty) constructed polygon
        boost::polygon::assign(data, boost::polygon::polygon_data<CoordinateType>());
    }
};

template <typename CoordinateType>
struct push_back<boost::polygon::polygon_data<CoordinateType> >
{
    typedef boost::polygon::point_data<CoordinateType> point_type;

    static inline void apply(boost::polygon::polygon_data<CoordinateType>& data,
         point_type const& point)
    {
        // Boost.Polygon's polygons are not appendable. So create a temporary vector,
        // add a record and set it to the original. Of course: this is not efficient.
        // But there seems no other way (without using a wrapper)
        std::vector<point_type> temporary_vector
            (
                boost::polygon::begin_points(data),
                boost::polygon::end_points(data)
            );
        temporary_vector.push_back(point);
        data.set(temporary_vector.begin(), temporary_vector.end());
    }
};

} // namespace traits

}} // namespace boost::geometry

// Adapt Boost.Polygon's polygon_data to Boost.Range
// This just translates to
// polygon_data.begin() and polygon_data.end()
namespace boost
{
    template<typename CoordinateType>
    struct range_mutable_iterator<boost::polygon::polygon_data<CoordinateType> >
    {
        typedef typename boost::polygon::polygon_traits
            <
                boost::polygon::polygon_data<CoordinateType>
            >::iterator_type type;
    };

    template<typename CoordinateType>
    struct range_const_iterator<boost::polygon::polygon_data<CoordinateType> >
    {
        typedef typename boost::polygon::polygon_traits
            <
                boost::polygon::polygon_data<CoordinateType>
            >::iterator_type type;
    };

    template<typename CoordinateType>
    struct range_size<boost::polygon::polygon_data<CoordinateType> >
    {
        typedef std::size_t type;
    };

} // namespace boost

// Support Boost.Polygon's polygon_data for Boost.Range ADP
namespace boost { namespace polygon
{

template<typename CoordinateType>
inline typename polygon_traits
        <
            polygon_data<CoordinateType>
        >::iterator_type range_begin(polygon_data<CoordinateType>& polygon)
{
    return polygon.begin();
}

template<typename CoordinateType>
inline typename polygon_traits
        <
            polygon_data<CoordinateType>
        >::iterator_type range_begin(polygon_data<CoordinateType> const& polygon)
{
    return polygon.begin();
}

template<typename CoordinateType>
inline typename polygon_traits
        <
            polygon_data<CoordinateType>
        >::iterator_type range_end(polygon_data<CoordinateType>& polygon)
{
    return polygon.end();
}

template<typename CoordinateType>
inline typename polygon_traits
        <
            polygon_data<CoordinateType>
        >::iterator_type range_end(polygon_data<CoordinateType> const& polygon)
{
    return polygon.end();
}

template<typename CoordinateType>
inline std::size_t range_calculate_size(polygon_data<CoordinateType> const& polygon)
{
    return polygon.size();
}

}}

#endif // DOXYGEN_NO_TRAITS_SPECIALIZATIONS

#endif // BOOST_GEOMETRY_GEOMETRIES_ADAPTED_BOOST_POLYGON_RING_HPP


#pragma once

#include <iostream>
#include <stdexcept>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/StdVector>

#ifdef __SRRG_PARALLEL_KDTREE__
#include <omp.h>
#endif

namespace srrg2_core {

  /**
     KDTree: implements a KDTree, it requires a type T
     and a dimension D
  */
  template <class T, size_t D>
  class KDTree {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    // typedef for defining a vector variable sized points
    using VectorTD       = Eigen::Matrix<T, D, 1>;
    using VectorTDVector = std::vector<VectorTD, Eigen::aligned_allocator<VectorTD>>;
    enum NodeType { Leaf = 0x0, Middle = 0x1 };

    /**
       TreeNode class. Represents a base class for a node in the search tree.

       if the type is a Middle node
       it represents a splitting plane, and has 2 child nodes
       that refer to the set of points to the two sides of the splitting plane.
       A splitting plane is parameterized as a point on a plane and as a normal
       to the plane.

       If the type is a leaf
       it represents a bucket containing points in a neighborhood
       To avoid copies, the bucket is kept as a range
       _min_index.._max_indexindices and the points
       in the _points and _indices
    */
    class TreeNode {
    public:
      friend class KDTree;
      //! ctor
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      TreeNode(KDTree<T, D>* tree_, int node_num = 0) :
        _tree(tree_),
        _node_num(node_num),
        _node_type(KDTree<T, D>::Leaf) {
        _mean.setZero();
        _normal.setZero();
        _left_child  = 0;
        _right_child = 0;
        _min_index   = -1;
        _max_index   = -1;
        _num_points  = 0;
      }

      TreeNode(KDTree<T, D>* tree_,
               int node_num,
               const VectorTD& mean_,
               const VectorTD& normal_,
               TreeNode* left_child  = 0,
               TreeNode* right_child = 0) :
        _tree(tree_),
        _node_num(node_num),
        _node_type(KDTree<T, D>::Middle) {
        _min_index = _max_index = -1;
        assert(normal_.rows() == D);
        assert(mean_.rows() == D);
        _normal      = normal_;
        _mean        = mean_;
        _num_points  = 0;
        _left_child  = left_child;
        _right_child = right_child;
        if (_left_child) {
          _num_points += _left_child->numPoints();
        }
        if (_right_child) {
          _num_points += _right_child->numPoints();
        }
      }

      //! dtor
      ~TreeNode() {
        if (_left_child) {
          delete _left_child;
          _left_child = 0;
        }
        if (_right_child) {
          delete _right_child;
          _right_child = 0;
        }
      }

      //! function to search for the neighbor
      //! @param answer: the neighbor found
      //! @param query: the point to search
      //! @param maximum distance allowed for a point
      //! @returns the distance of the closest point. -1 if no point found
      //! within range
      T findNeighbor(VectorTD& answer,
                     int& index,
                     const VectorTD& query,
                     const T max_distance) const {
        switch (_node_type) {
          case KDTree<T, D>::Leaf: {
            T d_max                         = std::numeric_limits<T>::max();
            const VectorTDVector& points    = this->_tree->_points;
            const std::vector<int>& indices = this->_tree->_indices;
            for (size_t i = _min_index; i < _max_index; ++i) {
              T d = (points[i] - query).squaredNorm();
              if (d < d_max) {
                answer = points[i];
                index  = indices[i];
                d_max  = d;
              }
            }

            if (d_max > max_distance * max_distance) {
              index = -1;
              return -1;
            }
            return d_max;
          }

          case KDTree<T, D>::Middle: {
            bool is_left    = side(query);
            TreeNode* child = is_left ? _left_child : _right_child;
            if (child) {
              if (child->_node_type != KDTree<T, D>::Middle &&
                  child->_node_type != KDTree<T, D>::Leaf) {
                std::cerr << "KDTree::findNeighbor|ERROR, calling sanity check (" << _tree << ")\n";
                std::cerr << "KDTree::findNeighbor|good queries" << _tree->good_queries
                          << std::endl;
                std::cerr << "KDTree::findNeighbor|result: " << _tree->sanityCheck() << std::endl;
              }
              return child->findNeighbor(answer, index, query, max_distance);
            }
          }
            return -1;
          default:
            std::cerr << "KDTree::findNeighbor|ERROR, sanity check (" << _tree
                      << ") : " << _tree->sanityCheck() << std::endl;
            throw std::runtime_error("KDTree::findNeighbor|ERROR, unknown node type");
        }
      }

      //! function to search for all the points near to the query point
      //! @param answer: the neighbors found
      //! @param query: the point to search
      //! @param maximum distance allowed for a point
      //! @returns void
      void findNeighbors(VectorTDVector& answers,
                         std::vector<int>& indices,
                         const VectorTD& query,
                         const T maximum_squared_distance) const {
        assert(maximum_squared_distance >= 0);
        switch (_node_type) {
          case KDTree<T, D>::Leaf: {
            // ds return all points in this leaf that satisfy the distance
            assert(_max_index >= _min_index);
            const size_t number_of_points = _max_index - _min_index;
            answers.clear();
            answers.reserve(number_of_points);
            indices.clear();
            indices.reserve(number_of_points);
            for (size_t i = _min_index; i < _max_index; ++i) {
              assert(i < _tree->_points.size());
              const T distance = (_tree->_points[i] - query).squaredNorm();
              if (distance < maximum_squared_distance) {
                answers.emplace_back(_tree->_points[i]);
                indices.emplace_back(_tree->_indices[i]);
              }
            }
            return;
          }

          case KDTree<T, D>::Middle: {
            bool is_left    = side(query);
            TreeNode* child = is_left ? _left_child : _right_child;
            if (child) {
              if (child->_node_type != KDTree<T, D>::Middle &&
                  child->_node_type != KDTree<T, D>::Leaf) {
                std::cerr << "KDTree::findNeighbors|ERROR, calling sanity check (" << _tree << ")"
                          << std::endl;
                std::cerr << "KDTree::findNeighbors|good queries" << _tree->good_queries
                          << std::endl;
                std::cerr << "KDTree::findNeighbors|result: " << _tree->sanityCheck() << std::endl;
              }
              child->findNeighbors(answers, indices, query, maximum_squared_distance);
              return;
            }
          }
            return;

          default:
            std::cerr << "KDTree::findNeighbors|ERROR, sanity check (" << _tree
                      << ") : " << _tree->sanityCheck() << std::endl;
            throw std::runtime_error("KDTree::findNeighbors|ERROR, unknown node type");
        }
      }

      size_t numPoints() const {
        return _num_points;
      }

      size_t minIndex() const {
        return _min_index;
      }

      size_t maxIndex() const {
        return _max_index;
      }

      inline void setMinIndex(size_t min_index) {
        _min_index = min_index;
      }

      inline void setMaxIndex(size_t max_index) {
        _max_index = max_index;
      }

      //! mean const accessor
      inline const VectorTD& mean() const {
        return _mean;
      }

      //! normal const accessor
      inline const VectorTD& normal() const {
        return _normal;
      }

      //! mean left accessor
      inline TreeNode* leftChild() const {
        return _left_child;
      }

      //! mean right accessor
      inline TreeNode* rightChild() const {
        return _right_child;
      }

      inline bool side(const VectorTD& query_point) const {
        return _normal.dot(query_point - _mean) < 0;
      }

    protected:
      KDTree<T, D>* _tree;
      int _node_num;
      const KDTree<T, D>::NodeType _node_type;
      size_t _min_index;
      size_t _max_index;
      VectorTD _normal;
      VectorTD _mean;
      size_t _num_points;
      TreeNode* _left_child;
      TreeNode* _right_child;
    };

    //! ctor
    KDTree(const VectorTDVector& points_, T max_leaf_range, size_t min_leaf_points = 20) {
      good_queries = 0;
      _num_nodes   = 0;
      _points      = points_;
      _aux_points  = points_;
      _indices.resize(points_.size());
      _aux_indices.resize(points_.size());
      _min_leaf_points = min_leaf_points;
      for (size_t i = 0; i < _indices.size(); ++i) {
        _indices[i] = i;
      }
      _root = _buildTree(0, points_.size(), max_leaf_range, 0);
    }

    //! dtor
    ~KDTree() {
      if (_root) {
        delete _root;
      }
      _root = 0;
    }

    bool sanityCheck() const {
      std::vector<int> checked_indices(_points.size());
      std::fill(checked_indices.begin(), checked_indices.end(), -1);
      const size_t k = sanityCheck(checked_indices, 0, _root);
      if (k != _points.size()) {
        throw std::runtime_error("KDTree::sanityCheck(void)|ERROR, illegal size reported");
      }
      std::sort(checked_indices.begin(), checked_indices.end(), std::less<int>());
      for (size_t i = 0; i < checked_indices.size(); i++) {
        if (i != static_cast<size_t>(checked_indices[i])) {
          throw std::runtime_error("KDTree::sanityCheck(void)|ERROR, missing indices");
        }
      }
      return true;
    }

    int sanityCheck(std::vector<int>& checked_indices, int k, const TreeNode* node) const {
      if (!node) {
        return k;
      }
      switch (node->_node_type) {
        case KDTree<T, D>::Leaf:
          for (size_t i = node->_min_index; i < node->_max_index; i++) {
            int idx = _indices[i];
            if (checked_indices[k] != -1) {
              throw std::runtime_error("KDTree::sanityCheck|ERROR, writing on an occupied index");
            }
            checked_indices[k] = idx;
            k++;
          }
          return k;
        case KDTree<T, D>::Middle:
          k = sanityCheck(checked_indices, k, node->_left_child);
          k = sanityCheck(checked_indices, k, node->_right_child);
          return k;
        default:
          throw std::runtime_error("KDTree::sanityCheck|ERROR, illegal type index");
      }
    }

    //! num_nodes accessor
    inline size_t numNodes() const {
      return _num_nodes;
    }

    inline size_t numPoints() const {
      return _points.size();
    }

    //! function to search for the neighbor
    //! @param answer: the neighbor found
    //! @param query: the point to search
    //! @param maximum distance allowed for a point
    //! @returns the distance of the closest point. -1 if no point found within
    //! range
    inline T
    findNeighbor(VectorTD& answer, int& index, const VectorTD& query, const T max_distance) const {
      if (!_root) {
        throw std::runtime_error("KDTree::findNeighbor|ERROR, no root node");
      }
      good_queries++;
      return _root->findNeighbor(answer, index, query, max_distance);
    }

    //! function to search for all the points in the same leaf of the query
    //! point
    //! @param answers: the neighbors found
    //! @param indices: the indices of the neighbors found
    //! @param query: the query point
    //! @param max_distance: maximum distance allowed for a point
    //! @returns void

    inline void findNeighbors(VectorTDVector& answers,
                              std::vector<int>& indices,
                              const VectorTD& query,
                              const T max_distance) const {
      if (!_root) {
        throw std::runtime_error("KDTree::findNeighbors|ERROR, no root node");
      }

      good_queries++;
      _root->findNeighbors(answers, indices, query, max_distance);
    }

    inline void printKDTree() {
      _printKDTree(_root);
    }

    mutable int good_queries;

  protected:
    /**
       Partitions a point vector in two vectors, computing the splitting plane
       as the largest eigenvalue of the point covariance
       @param mean: the returned mean of the splitting plane
       @param normal: the normal of the splitting plane
       @param left: the returned left vector of points
       @param right: the returned right vector of points
       @param points: the array of points
       @returns the distance of the farthest point from the plane
    */
    T _splitPoints(VectorTD& mean,
                   VectorTD& normal,
                   size_t& num_left_points,
                   const size_t min_index,
                   const size_t max_index) {
      // if points empty, nothing to do
      if (min_index == max_index) {
        return 0;
      }

      const size_t num_points            = max_index - min_index;
      const T inverse_num_points         = 1.0 / num_points;
      VectorTD sum                       = VectorTD::Zero();
      Eigen::Matrix<T, D, D> squared_sum = Eigen::Matrix<T, D, D>::Zero();
      Eigen::Matrix<T, D, D> covariance  = Eigen::Matrix<T, D, D>::Zero();
      for (size_t i = min_index; i < max_index; ++i) {
        sum += _points[i];
        squared_sum += _points[i] * _points[i].transpose();
      }
      mean       = sum * inverse_num_points;
      covariance = squared_sum * inverse_num_points - mean * mean.transpose();

      // eigenvalue decomposition
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, D, D>> solver;
      solver.compute(covariance, Eigen::ComputeEigenvectors);
      normal = solver.eigenvectors().col(D - 1).normalized();

      // the following var will contain the range of points along the normal
      // vector
      T max_distance_from_plane = 0;

      // run through the points and split them in the left or the right set
      size_t left_index  = min_index;
      size_t right_index = max_index;

      size_t num_left  = 0;
      size_t num_right = 0;
      for (size_t i = min_index; i < max_index; ++i) {
        T distance_from_plane = normal.dot(_points[i] - mean);
        if (fabs(distance_from_plane) > max_distance_from_plane) {
          max_distance_from_plane = fabs(distance_from_plane);
        }

        bool side = distance_from_plane < 0;
        if (side) {
          _aux_points[left_index]  = _points[i];
          _aux_indices[left_index] = _indices[i];
          left_index++;
          num_left++;
        } else {
          right_index--;
          _aux_points[right_index]  = _points[i];
          _aux_indices[right_index] = _indices[i];
          num_right++;
        }
      }
      assert(max_index - min_index == num_right + num_left);
      for (size_t i = min_index; i < max_index; ++i) {
        _points[i]  = _aux_points[i];
        _indices[i] = _aux_indices[i];
      }

      num_left_points = num_left;
      return max_distance_from_plane;
    }

    //! function to build the tree
    //! @param points: the points
    //! @param max_leaf_range: specify the size of the "box" below which a leaf
    //! node is generated returns the root of the search tree
    TreeNode*
    _buildTree(const size_t min_index, const size_t max_index, const T max_leaf_range, int level) {
      const size_t num_points = max_index - min_index;
      if (!num_points) {
        return 0;
      }

      VectorTD mean;
      VectorTD normal;
      size_t num_left_points = 0;

      const T range = _splitPoints(mean, normal, num_left_points, min_index, max_index);
      assert(range >= 0);

      TreeNode* node = 0;
      if (range < max_leaf_range || num_points < _min_leaf_points) {
        node = new TreeNode(this, _num_nodes);
        node->setMinIndex(min_index);
        node->setMaxIndex(max_index);
        node->_num_points = num_points;
      } else {
        TreeNode *left_tree, *right_tree;

#ifdef __SRRG_PARALLEL_KDTREE__
        int num_threads = omp_get_max_threads();
        int split_level = -1;
        if (level > 0)
          split_level = floor(log(num_threads) / log(2));

        if (split_level == level) {
#pragma omp parallel sections
          {
#pragma omp section
            {
              left_tree =
                _buildTree(min_index, min_index + num_left_points, max_leaf_range, level + 1);
            }
#pragma omp section
            {
              right_tree =
                _buildTree(min_index + num_left_points, max_index, max_leaf_range, level + 1);
            }
          }
        } else {
#endif //__SRRG_PARALLEL_KDTREE__

          left_tree = _buildTree(min_index, min_index + num_left_points, max_leaf_range, level + 1);
          right_tree =
            _buildTree(min_index + num_left_points, max_index, max_leaf_range, level + 1);
#ifdef __SRRG_PARALLEL_KDTREE__
        }
#endif
        node = new TreeNode(this, _num_nodes, mean, normal, left_tree, right_tree);
      }
      _num_nodes++;
      return node;
    }

    void _printKDTree(TreeNode* node) {
      switch (node->_node_type) {
        case KDTree<T, D>::Leaf:
          std::cerr << "Leaf: " << std::endl;
          for (size_t i = node->_min_index; i < node->_max_index; ++i) {
            std::cerr << _points[i].transpose() << std::endl;
          }
          std::cerr << std::endl;
          break;
        case KDTree<T, D>::Middle:
          _printKDTree(node->leftChild());
          _printKDTree(node->rightChild());
          break;
      }
    }

    size_t _num_nodes;
    size_t _min_leaf_points;
    TreeNode* _root;
    VectorTDVector _aux_points;
    VectorTDVector _points;
    std::vector<int> _indices;
    std::vector<int> _aux_indices;
  };

} // namespace srrg2_core


#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/return_internal_reference.hpp>

#include <scitbx/vec3.h>

#include <mmtbx/geometry/sphere_surface_sampling.hpp>
#include <boost_adaptbx/boost_range_python.hpp>

namespace mmtbx
{
namespace geometry
{
namespace sphere_surface_sampling
{
namespace
{

template < typename Vector >
struct sphere_surface_wrappers
{
  static void wrap()
  {
    using namespace boost::python;
    boost_adaptbx::python::generic_range_wrapper<
      typename GoldenSpiral< Vector >::storage_type
      >::wrap( "points_range" );

    class_< GoldenSpiral< Vector > >( "golden_spiral", no_init )
      .def(
        init< size_t >( ( arg( "count" ) ) )
          )
      .add_property(
        "unit_area",
        make_function(
          &GoldenSpiral< Vector >::unit_area,
          return_value_policy< copy_const_reference >()
          )
        )
      .add_property(
        "count",
        make_function(
          &GoldenSpiral< Vector >::count,
          return_value_policy< copy_const_reference >()
          )
        )
      .add_property(
        "points",
        make_function(
          &GoldenSpiral< Vector >::points,
          return_internal_reference<>()
          )
        )
      ;
  }
};

void init_module()
{
  sphere_surface_wrappers< scitbx::vec3< double > >::wrap();
}

} // namespace <anonymous>
} // namespace sphere_surface_sampling
} // namespace geometry
} // namespace mmtbx

BOOST_PYTHON_MODULE(mmtbx_geometry_sphere_surface_sampling_ext)
{
  mmtbx::geometry::sphere_surface_sampling::init_module();
}


#include <ros/ros.h>
#include <vector>
#include <math.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <stdio.h>
#include <boost/foreach.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/features/normal_3d.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <std_msgs/String.h>
#include <sstream>

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

boost::shared_ptr<pcl::visualization::PCLVisualizer>
simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer("3D Viewer"));
  viewer->setBackgroundColor(0, 0, 0);
  viewer->addPointCloud<pcl::PointXYZ> (cloud,"sample cloud1");
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud1");
  viewer->initCameraParameters ();
  return (viewer);
}

void callback(const PointCloud::ConstPtr& msg)
{
  ros::NodeHandle n;
  ros::Publisher pub = n.advertise<std_msgs::String>("/kinect_data",100);
  ROS_INFO("I heard: [%d] [%d]",msg->width, msg->height);
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pass (new pcl::PointCloud<pcl::PointXYZ>());
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pass1 (new pcl::PointCloud<pcl::PointXYZ>());
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_vox (new pcl::PointCloud<pcl::PointXYZ>());
  pcl::PassThrough<pcl::PointXYZ> pass;
  pcl::PassThrough<pcl::PointXYZ> pass1;
  pass.setInputCloud (msg);
  pass.setFilterFieldName ("y");
  pass.setFilterLimits (0.0, 8.0);
  pass.setFilterLimitsNegative (true);
  pass.filter (*cloud_pass);
  ROS_INFO("I heard: [%d] [%d]",cloud_pass->width, cloud_pass->height);
  //pass1.setInputCloud (cloud_pass);
  //pass1.setFilterFieldName ("y");
  //pass1.setFilterLimits (100.0, 200.0);
  //pass1.setFilterLimitsNegative (true);
  //pass1.filter (*cloud_pass1);
  ROS_INFO("I heard: [%d] [%d]",cloud_pass1->width, cloud_pass1->height);
  std::cout<<"pass done\n";
  pcl::VoxelGrid<pcl::PointXYZ> vox;
  vox.setInputCloud (cloud_pass);
  vox.setLeafSize (0.04f, 0.04f, 0.04f);
  vox.filter (*cloud_vox);
  std::cout<<"vox done\n";
  ROS_INFO("I heard: [%d] [%d]",cloud_vox->width, cloud_vox->height);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud_vox);
  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (0.05); // 2cm
  ec.setMinClusterSize (50);
  ec.setMaxClusterSize (10000);
  ec.setSearchMethod (tree);
  ec.setInputCloud (cloud_vox);
  ec.extract (cluster_indices);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
  int j = 0;
  std::vector<std::vector<float> > moi, xcor;
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
    j++;
    pcl::PointCloud<pcl::PointXYZ>::Ptr clusters (new pcl::PointCloud<pcl::PointXYZ>);
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
    {
      clusters->points.push_back (cloud_vox->points[*pit]); //*
      cloud_cluster->points.push_back (cloud_vox->points[*pit]); //*
      //cloud_cluster->width = 0;
      //cloud_cluster->height = 0;
    }
    pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
    feature_extractor.setInputCloud (clusters);
    feature_extractor.compute ();
    std::vector <float> moment_of_inertia;
    std::vector <float> eccentricity;
    pcl::PointXYZ min_point_AABB;
    pcl::PointXYZ max_point_AABB;
    pcl::PointXYZ min_point_OBB;
    pcl::PointXYZ max_point_OBB;
    pcl::PointXYZ position_OBB;
    Eigen::Matrix3f rotational_matrix_OBB;
    float major_value, middle_value, minor_value;
    Eigen::Vector3f major_vector, middle_vector, minor_vector;
    Eigen::Vector3f mass_center;
    feature_extractor.getMomentOfInertia (moment_of_inertia);
    feature_extractor.getEccentricity (eccentricity);
    feature_extractor.getAABB (min_point_AABB, max_point_AABB);
    feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
    feature_extractor.getEigenValues (major_value, middle_value, minor_value);
    feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
    feature_extractor.getMassCenter (mass_center);
    //for(int i=j-1; i<=j; j++)
    //{
    std::vector<float> row1;
    for(int k=0;k<2;k++)
      row1.push_back(mass_center[k]);
    moi.push_back(row1);
    //}
     //for(int i=j-1; i<=j; j++)
    //{
    std::vector<float> row2;
    row2.push_back(min_point_AABB.x);
    row2.push_back(max_point_AABB.x);
    row2.push_back(min_point_AABB.z);
    row2.push_back(max_point_AABB.z);
    row2.push_back(min_point_AABB.y);
    row2.push_back(max_point_AABB.y);
    xcor.push_back(row2);
    //}
  }
  std::cout<<endl<<j<<endl;
  std::stringstream ss;
  std_msgs::String kin_val;
  //for(int i=0;i<j;i++)
  //std::cout<<moi[i][0]<<" "<<moi[i][1]<<endl;
  for(int i=0; i<j; i++)
      {
        if(xcor[i][2]<2.0 && abs(xcor[i][5]-xcor[i][4]>0.15))
        {
          //std::cout<<moi[i][1]<<" dist = "<<xcor[i][2]<<endl;
          if(moi[i][0]>=0)
	  {
            std::cout<<"left\n";
	    ss << "left";
            kin_val.data = ss.str();
	    goto label;
	  }
          else //if(fabs(xcor[i][0])>fabs(xcor[i][1]))
	  {
            std::cout<<"right\n";
    	    ss << "right";
            kin_val.data = ss.str();
	    goto label;
	  }
        }
	//else
       }
	std::cout<<"straight\n";
	ss << "straight";
  	kin_val.data = ss.str();
      label:
	pub.publish(kin_val);
  /*viewer = simpleVis(cloud_cluster);
  //viewer = simpleVis(cloud_vox);
  while(!viewer->wasStopped())
  {
     viewer->spinOnce(100);
     boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  }*/
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "intel");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("/camera/depth/color/points", 1, callback);
  ros::spin();
  return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_SIMD_COMMON_SPLATTED_MAXIMUM_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_COMMON_SPLATTED_MAXIMUM_HPP_INCLUDED
#include <boost/simd/swar/functions/splatted_maximum.hpp>
#include <boost/dispatch/meta/strip.hpp>

#include <boost/simd/include/functions/simd/splat.hpp>
#include <boost/simd/include/functions/simd/maximum.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::splatted_maximum_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                            )
  {

    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef A0 type;
      type that = boost::simd::splat<type>(boost::simd::maximum(a0));
      return that;
    }
  };
} } }

#endif


/**
 * @file navigator.cpp
 * @brief Accepts navigation commands and publishes commands to UAV
 */

#include <flightros/navigator.hpp>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <algorithm>
#include <string>
#include <cv_bridge/cv_bridge.h>
#include "opencv2/opencv.hpp"
#include <vector>
#include <cmath>
#include <ctgmath>
#include <random>
#include <iostream>
#include <std_srvs/Empty.h>

/* Resets for Forest */
/*
#define RESET_POS_X 62.0
#define RESET_POS_Y 90.0
#define RESET_POS_Z 33.0
*/

/* Resets for Warehouse box  */
#define RESET_POS_X -8.0
#define RESET_POS_Y 15.0
#define RESET_POS_Z 2.0

#define RESET_REQ_DURATION 2.0

#define PI 3.14159
#define X_POS_STD 0.5
#define Y_POS_STD 1.0
#define YAW_STD PI/24.0
#define USE_RANDOM true

#define TIME_HORIZON 0.5
#define GOAL_POSITION_Y 20.0
#define X_THRESHOLD 20.0
#define Z_THRESHOLD 1.5
#define MAX_HEADING PI/4.0 // +/- PI/4 = +/- 45 deg
#define MAX_VELOCITY 3.0
#define RUNNING_Z_PGAIN 0.5
#define YAW_PGAIN 1.0
#define YAW_PGAIN_RESET 1.0

Navigator::Navigator(ros::NodeHandle* nh, double freq) : _nh(*nh),
                                                         _rate(freq){

    initializePublishers();
    initializeSubscribers();
    initializeServices();

    // command variables
    _cmd_heading = 0.0;
    _cmd_velocity = 0.0;

    _last_cmd = ros::Time::now();
    _not_reset = ros::Time::now();

    _goal_pos.x = RESET_POS_X;
    _goal_pos.y = RESET_POS_Y + GOAL_POSITION_Y;
    _goal_pos.z = RESET_POS_Z;

    _in_collision.data = false;
}

/* ---------------------------------- */
/*      INITIALIZATION FUNCTIONS      */
/* ---------------------------------- */

void Navigator::initializeSubscribers(){
    _odom_sub = _nh.subscribe
        ("hummingbird/ground_truth/odometry", 10, &Navigator::_odom_cb, this);
    _camera_sub = _nh.subscribe
        ("hummingbird/camera/image", 10, &Navigator::_camera_cb, this);
    _depth_sub = _nh.subscribe
        ("hummingbird/camera/noisy_depth", 10, &Navigator::_depth_cb, this);
    _uncertainty_sub = _nh.subscribe
        ("hummingbird/camera/uncertainty", 10, &Navigator::_uncertainty_cb, this);
    _collision_sub = _nh.subscribe
        ("hummingbird/collision", 10, &Navigator::_collision_cb, this);
}

void Navigator::initializePublishers(){
    _cmd_pub = _nh.advertise<geometry_msgs::TwistStamped>
        ("hummingbird/autopilot/velocity_command", 10);
}

void Navigator::initializeServices(){
    _quadstate_server = _nh.advertiseService("navigator/get_state", &Navigator::_quadstate_cb, this);
    _octomap_reset_client = _nh.serviceClient<std_srvs::Empty>("/octomap_server/reset");
}

/* ---------------------------------- */
/*         CALLBACK FUNCTIONS         */
/* ---------------------------------- */

bool Navigator::_quadstate_cb(flightros::QuadState::Request &req,
                   flightros::QuadState::Response &res){

    std::string::size_type sz;
    _cmd_heading = std::stof(req.in.substr(2),&sz) * MAX_HEADING;
    _cmd_velocity = (std::stof(req.in.substr(2).substr(sz)) + 1.)/2. * MAX_VELOCITY;
    _last_cmd = ros::Time::now();

    // ROS_INFO_STREAM(_cmd_heading);
    // ROS_INFO_STREAM(_cmd_velocity);
    unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
    std::default_random_engine generator (seed);
    std::normal_distribution<float> x_distribution(0.0, X_POS_STD);
    std::normal_distribution<float> y_distribution(0.0, Y_POS_STD);
    std::normal_distribution<float> yaw_distribution(0.0, YAW_STD);

    if (req.in[0] == '0'){
        ROS_WARN("QuadState service: Resetting simulation");
        ROS_INFO("QuadState service: Resetting simulation");
        float yaw_mod;

        /*  RANDOMIZED STARTING POSITIOn */
        if (USE_RANDOM) {
            ROS_INFO("Resetting to random position...");
            x_mod = x_distribution(generator);
            y_mod = y_distribution(generator);
            yaw_mod = yaw_distribution(generator);
        } else {
            x_mod = 0.0;
            y_mod = 0.0;
            yaw_mod = 0.0;
        }

        _goal_pos.y = RESET_POS_Y + y_mod + GOAL_POSITION_Y;

        // blocking operation: move drone to reset position
        while ( true ){

            // check is ROS is shutting down since this callback is blocking
            if (ros::isShuttingDown()){
                ros::shutdown();
            }

            geometry_msgs::TwistStamped vel_msg;
            vel_msg.header.stamp = ros::Time::now();
            // Reset to a random position

            vel_msg.twist.linear.x = std::clamp( 1.0 * (RESET_POS_X - _curr_pos.x - x_mod), -10.0, 10.0 );
            vel_msg.twist.linear.y = std::clamp( 1.0 * (RESET_POS_Y - _curr_pos.y - y_mod), -10.0, 10.0 );
            vel_msg.twist.linear.z = std::clamp( 1.0 * (RESET_POS_Z - _curr_pos.z), -10.0, 10.0 );
            vel_msg.twist.angular.x = 0;
            vel_msg.twist.angular.y = 0;
            vel_msg.twist.angular.z = YAW_PGAIN_RESET * (yaw_mod - _yaw);
            _cmd_pub.publish(vel_msg);
            // ROS_INFO_STREAM("Resetting drone position");

            if ((std::pow(_curr_pos.x + x_mod -RESET_POS_X, 2) + std::pow(_curr_pos.y + y_mod -RESET_POS_Y, 2) + std::pow(_curr_pos.z-RESET_POS_Z, 2)) > 1.0){
                _not_reset = ros::Time::now();
            }
            else if (ros::Time::now() - _not_reset > ros::Duration(RESET_REQ_DURATION)){
                ROS_INFO_STREAM("Drone is reset");
                break;
            }

            // spin
            ros::spinOnce();
            _rate.sleep();

        }

        // request octomap reset
        std_srvs::Empty empty_srv;
        if (_octomap_reset_client.call(empty_srv))
            ROS_INFO_STREAM("Successfully called octomap reset");
        else
            ROS_ERROR("Failed to call service /octomap_server/reset");

        // fill relevant fields
        res.header = _rgb.header;

        std_msgs::Bool done;
        done.data = false;
        res.done = done;
        std_msgs::Bool crash_msg;
        crash_msg.data = false;
        res.crash = crash_msg;

        cv_bridge::CvImagePtr cv_ptr_rgb, cv_ptr_depth;
        try{
            cv_ptr_rgb = cv_bridge::toCvCopy(_rgb, sensor_msgs::image_encodings::BGR8);
            cv_ptr_depth = cv_bridge::toCvCopy(_depth, sensor_msgs::image_encodings::MONO8);

            // split & merge attempt
            cv::Mat rgb_channels[3];
            cv::split(cv_ptr_rgb->image, rgb_channels);
            cv::Mat rgbd_channels[4] = {rgb_channels[0],
                                        rgb_channels[1],
                                        rgb_channels[2],
                                        cv_ptr_depth->image};
            std::vector<int> sizes{cv_ptr_rgb->image.rows, cv_ptr_rgb->image.cols};
            cv::Mat outMat(sizes, CV_32FC4);
            cv::merge(rgbd_channels, 4, outMat);
            // outMat = outMat * (1.0f/255.0f);
            sensor_msgs::ImagePtr rgbd_msg = cv_bridge::CvImage(_rgb.header, "8UC4", outMat).toImageMsg();
            res.image = *rgbd_msg;

            // Also set the uncertainty here
            res.prob_image = _uncertainty;

            // mixChannels attempt
            // std::vector<int> sizes{cv_ptr_rgb->image.rows, cv_ptr_rgb->image.cols};
            // cv::Mat outMat(sizes, CV_32FC4);
            // const cv::Mat inMat[] = {cv_ptr_rgb->image, cv_ptr_depth->image};
            // const int from_to[] = {0,0, 1,1, 2,2, 3,3};
            // cv::mixChannels(inMat, 2, &outMat, 1, from_to, 4);
            // sensor_msgs::ImagePtr rgbd_msg = cv_bridge::CvImage(_rgb.header, "32FC4", outMat).toImageMsg();
            // res.image = *rgbd_msg;
        }
        catch (cv_bridge::Exception& e){
            ROS_ERROR("cv_bridge exception: %s", e.what());
            // return;
        }

        res.current_position = _curr_pos;
        res.goal_position = _goal_pos;

        _in_collision.data = false;

    }

    else if (req.in[0] == '1'){
        res.header = _rgb.header;
        std_msgs::Bool done;
        done.data = (_curr_pos.y >= _goal_pos.y);
        res.done = done;
        // collision || backwards || x too much || z too much
        std_msgs::Bool crash_msg;
        crash_msg.data = ( _in_collision.data ||
                           _curr_pos.y < (RESET_POS_Y + y_mod - 5.0) ||
                           ( _curr_pos.x > (RESET_POS_X + x_mod + X_THRESHOLD) || _curr_pos.x < (RESET_POS_X + x_mod - X_THRESHOLD)) ||
                           ( _curr_pos.z < RESET_POS_Z - Z_THRESHOLD || _curr_pos.z > RESET_POS_Z + Z_THRESHOLD) ||
                           // manually checking for approximate collision with box in WAREHOUSE
                           (_curr_pos.x < -7.2 && _curr_pos.x > -10.5 && _curr_pos.y > 19.0 && _curr_pos.y < 23.0));
        if (_in_collision.data) {
            ROS_INFO("Collision with environment!");
        } else if (crash_msg.data) {
            if (_curr_pos.y < (RESET_POS_Y + y_mod - 5.0)) {
                ROS_INFO("out of bounds in y!");
            }
            if (( _curr_pos.x > (RESET_POS_X + x_mod + X_THRESHOLD) || _curr_pos.x < (RESET_POS_X + x_mod - X_THRESHOLD))) {
                ROS_INFO("out of bounds in x!");
            } else {
                ROS_INFO("out of bounds in z!");
            }
        }
        res.crash = crash_msg;

        cv_bridge::CvImagePtr cv_ptr_rgb, cv_ptr_depth;
        try{
            cv_ptr_rgb = cv_bridge::toCvCopy(_rgb, sensor_msgs::image_encodings::BGR8);
            cv_ptr_depth = cv_bridge::toCvCopy(_depth, sensor_msgs::image_encodings::MONO8);

            // split & merge attempt
            cv::Mat rgb_channels[3];
            cv::split(cv_ptr_rgb->image, rgb_channels);
            cv::Mat rgbd_channels[4] = {rgb_channels[0],
                                        rgb_channels[1],
                                        rgb_channels[2],
                                        cv_ptr_depth->image};

            std::vector<int> sizes{cv_ptr_rgb->image.rows, cv_ptr_rgb->image.cols};
            cv::Mat outMat(sizes, CV_32FC4);
            cv::merge(rgbd_channels, 4, outMat);
            sensor_msgs::ImagePtr rgbd_msg = cv_bridge::CvImage(_rgb.header, "8UC4", outMat).toImageMsg();
            res.image = *rgbd_msg;
            res.prob_image = _uncertainty;

            // mixChannels attempt
            // std::vector<int> sizes{cv_ptr_rgb->image.rows, cv_ptr_rgb->image.cols};
            // cv::Mat outMat(sizes, CV_32FC4);
            // const cv::Mat inMat[] = {cv_ptr_rgb->image, cv_ptr_depth->image};
            // const int from_to[] = {0,0, 1,1, 2,2, 3,3};
            // cv::mixChannels(inMat, 2, &outMat, 1, from_to, 4);
            // sensor_msgs::ImagePtr rgbd_msg = cv_bridge::CvImage(_rgb.header, "32FC4", outMat).toImageMsg();
            // res.image = *rgbd_msg;
        }
        catch (cv_bridge::Exception& e){
            ROS_ERROR("cv_bridge exception: %s", e.what());
            // return;
        }

        res.current_position = _curr_pos;
        res.goal_position = _goal_pos;

    }

    else {
        ROS_ERROR("Get State service request value not recognized (0 or 1 supported)!");
    }

    return true;
}

void Navigator::_odom_cb(const nav_msgs::Odometry::ConstPtr& msg){
    _odom = *msg;
    _curr_pos = _odom.pose.pose.position;
    _curr_orient = _odom.pose.pose.orientation;
    // ROS_INFO("curr_orient.wxyz = (%f, %f, %f, %f)", _curr_orient.w, _curr_orient.x, _curr_orient.y, _curr_orient.z);
    _yaw = asin(2*_curr_orient.x*_curr_orient.y + 2*_curr_orient.z*_curr_orient.w);
    /*
    if (_curr_orient.w < 0) {
        Eigen::Quaterniond q(-_curr_orient.w, -_curr_orient.x, -_curr_orient.y, -_curr_orient.z);
        auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);
        _yaw = euler[2]; // map frame yaw angle
    } else {
        Eigen::Quaterniond q(_curr_orient.w, _curr_orient.x, _curr_orient.y, _curr_orient.z);
        auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);
        _yaw = euler[2]; // map frame yaw angle
    }
    */
//    ROS_INFO("Yaw est is %f\n", _yaw);
}

void Navigator::_uncertainty_cb(const sensor_msgs::Image::ConstPtr& msg){
    _uncertainty = *msg;
    _uncertainty.header.stamp = ros::Time::now();
}

void Navigator::_camera_cb(const sensor_msgs::Image::ConstPtr& msg){
    _rgb = *msg;
    _rgb.header.stamp = ros::Time::now();
}

void Navigator::_depth_cb(const sensor_msgs::Image::ConstPtr& msg){
    _depth = *msg;
    _depth.header.stamp = ros::Time::now();
}

void Navigator::_collision_cb(const std_msgs::Bool::ConstPtr& msg){
    if (!(_in_collision.data))
        _in_collision = *msg;
}

/* ---------------------------------- */
/*            RUN FUNCTION            */
/* ---------------------------------- */

void Navigator::run(){

    while (ros::ok()){

        // if last message was recent enough
        if (ros::Time::now() - _last_cmd < ros::Duration(TIME_HORIZON)){
            geometry_msgs::TwistStamped vel_msg;
            vel_msg.header.stamp = ros::Time::now();

            // ROS_INFO("cmd_velocity is %f\n", _cmd_velocity);
            // ROS_INFO("cur_heading is %f\n", _yaw);
            // ROS_INFO("-------------------");
            vel_msg.twist.linear.x = _cmd_velocity * (-sin(_yaw));
            vel_msg.twist.linear.y = _cmd_velocity * cos(_yaw);
            vel_msg.twist.linear.z = RUNNING_Z_PGAIN * (RESET_POS_Z - _curr_pos.z);

            // assume incoming heading double is from [-PI/4, PI/4]
            double yaw_change = std::clamp(_cmd_heading, -MAX_HEADING, MAX_HEADING);
            vel_msg.twist.angular.x = 0;
            vel_msg.twist.angular.y = 0;
            if ((_yaw < -MAX_HEADING && yaw_change < 0) || (_yaw > MAX_HEADING && yaw_change > 0))
                vel_msg.twist.angular.z = 0;
            else
                vel_msg.twist.angular.z = YAW_PGAIN * yaw_change;
            _cmd_pub.publish(vel_msg);
        }

        // if not in reset mode and don't have a recent heading/vel command
        else {
            // command zero velocity
            geometry_msgs::TwistStamped vel_msg;
            vel_msg.header.stamp = ros::Time::now();
            vel_msg.twist.linear.x = 0;
            vel_msg.twist.linear.y = 0;
            vel_msg.twist.linear.z = RUNNING_Z_PGAIN * (RESET_POS_Z - _curr_pos.z);
            vel_msg.twist.angular.x = 0;
            vel_msg.twist.angular.y = 0;
            vel_msg.twist.angular.z = 0;
            _cmd_pub.publish(vel_msg);
        }

        // spin
        ros::spinOnce();
        _rate.sleep();
    }

}

/* ---------------------------------- */
/*          INSTANTIATE NODE          */
/* ---------------------------------- */

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

    ros::init(argc, argv, "navigator");
    ros::NodeHandle nh;
    Navigator Navigator_Node(&nh, 30.0);
    ROS_INFO("Navigator: running");
    Navigator_Node.run();

    return 0;
}


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define PRECISION(x) std::fixed << std::setprecision(x)

using namespace std;

using ll = long long;
using VI = std::vector<int>;
using VI2D = std::vector<vector<int>>;
using VLL = std::vector<long long>;
using VLL2D = std::vector<vector<long long>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T, std::size_t N>
struct make_vector_type {
	using type =
		typename std::vector<typename make_vector_type<T, (N - 1)>::type>;
};

template <typename T>
struct make_vector_type<T, 0> {
	using type = typename std::vector<T>;
};

template <typename T, size_t N>
auto make_vector_impl(const std::vector<std::size_t>& ls, T init_value) {
	if constexpr(N == 0) {
		return std::vector<T>(ls[N], init_value);
	} else {
		return typename make_vector_type<T, N>::type(
			ls[N], make_vector_impl<T, (N - 1)>(ls, init_value));
	}
}

template <typename T, std::size_t N>
auto make_vector(const std::size_t (&ls)[N], T init_value) {
	std::vector<std::size_t> dimensions(N);
	for(int i = 0; i < N; i++) {
		dimensions[N - i - 1] = ls[i];
	}
	return make_vector_impl<T, N - 1>(dimensions, init_value);
}

template <typename T>
std::vector<T> make_vector(std::size_t size, T init_value) {
	return std::vector<T>(size, init_value);
}

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max<T>(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min<T>(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

struct Transform {
	ll e00;
	ll e01;
	ll e10;
	ll e11;
};

Transform dot(Transform a, Transform b) {
	Transform ret = {a.e00 * b.e00 + a.e01 * b.e10,
					 a.e00 * b.e01 + a.e01 * b.e11,
					 a.e10 * b.e00 + a.e11 * b.e10,
					 a.e10 * b.e01 + a.e11 * b.e11};
	return ret;
}

pair<ll, ll> dot(Transform a, pair<ll, ll> b) {
	pair<ll, ll> ret = {b.first * a.e00 + b.second * a.e01,
						b.first * a.e10 + b.second * a.e11};
	return ret;
}

pair<ll, ll> sum(pair<ll, ll> a, pair<ll, ll> b) {
	return make_pair(a.first + b.first, a.second + b.second);
}

int main() {
	int n;
	cin >> n;
	VLL x(n), y(n);
	REP(i, n) { cin >> x[i] >> y[i]; }
	int m;
	cin >> m;
	VI op(m), p(m);
	REP(i, m) {
		cin >> op[i];
		if(op[i] > 2) {
			cin >> p[i];
		}
	}
	int q;
	cin >> q;
	VI a(q), b(q);
	REP(i, q) { cin >> a[i] >> b[i]; }

	vector<Transform> transforms(m + 1, {1, 0, 0, 1});
	vector<pair<ll, ll>> linear(m + 1, {0, 0});
	Transform clockwise_rotate = {0, 1, -1, 0};
	Transform counter_clockwise_rotate = {0, -1, 1, 0};
	REP(i, m) {
		if(op[i] == 1) {
			transforms[i + 1] = dot(clockwise_rotate, transforms[i]);
			linear[i + 1] = dot(clockwise_rotate, linear[i]);
		} else if(op[i] == 2) {
			transforms[i + 1] = dot(counter_clockwise_rotate, transforms[i]);
			linear[i + 1] = dot(counter_clockwise_rotate, linear[i]);
		} else if(op[i] == 3) {
			transforms[i + 1] = dot(Transform({-1, 0, 0, 1}), transforms[i]);
			linear[i + 1] = sum(dot(Transform({-1, 0, 0, 1}), linear[i]),
								make_pair(p[i] * 2, 0));
		} else {
			transforms[i + 1] = dot(Transform({1, 0, 0, -1}), transforms[i]);
			linear[i + 1] = sum(dot(Transform({1, 0, 0, -1}), linear[i]),
								make_pair(0, p[i] * 2));
		}
	}

	REP(i, q) {
		pair<ll, ll> point = make_pair(x[b[i] - 1], y[b[i] - 1]);
		auto result = sum(dot(transforms[a[i]], point), linear[a[i]]);
		cout << result.first << " " << result.second << endl;
	}
	return 0;
}


#include <iostream>

#include <Eigen/Dense>

#include "crocoddyl/core/utils/timer.hpp"

#include "multicopter_mpc/path.h"
#include "multicopter_mpc/mpc-main.hpp"
#include "utils.hpp"

int main(int argc, char* argv[]) {
  crocoddyl::Timer timer;

  std::string mission_type = MULTICOPTER_MPC_MISSION_DIR "/passthrough.yaml";
  std::string mpc_main_yaml_path = MULTICOPTER_MPC_OCP_DIR "/mpc-main.yaml";

  multicopter_mpc::MpcMain mpc_main(multicopter_mpc::MultiCopterTypes::Iris, mission_type, mpc_main_yaml_path);
  Simulator simulator(mpc_main.getMpcController()->getTimeStep());

  Eigen::VectorXd state = mpc_main.getMpcController()->getStateMultibody()->zero();
  Eigen::VectorXd control = Eigen::VectorXd::Zero(mpc_main.getMpcController()->getMcParams()->n_rotors_);

  std::size_t T = mpc_main.getMpcController()->getTrajectoryGenerator()->getKnots();
  Eigen::ArrayXd duration(T);
  Eigen::ArrayXd iterations(T);
  Eigen::ArrayXd duration_iteration(T);
  duration.setZero();
  iterations.setZero();
  for (std::size_t i = 0; i < T; ++i) {
    timer.reset();
    mpc_main.setCurrentState(state);
    mpc_main.runMpcStep();
    control = mpc_main.getMotorsThrust();
    duration[i] = timer.get_us_duration();
    iterations[i] = mpc_main.getMpcController()->getSolver()->get_iter();
    duration_iteration[i] = duration[i] / iterations[i];
    state = simulator.simulateStep(state, control);
    if (i % 20 == 0) {
      std::cout << "Step number: " << i << std::endl;
    }
  }
  std::cout << "Function call: \t\t\t\t"
            << "AVG(in us)\t"
            << "STDDEV(in us)\t"
            << "MAX(in us)\t"
            << "MIN(in us)" << std::endl;

  std::cout << "runMpcStep: \t\t\t\t" << AVG(duration) << " us\t" << STDDEV(duration) << " us\t" << duration.maxCoeff()
            << " us\t" << duration.minCoeff() << " us" << std::endl;

  std::cout << "Avg.time per iteration: \t\t" << AVG(duration_iteration) << " us\t" << STDDEV(duration_iteration) << " us\t"
            << duration_iteration.maxCoeff() << " us\t" << duration_iteration.minCoeff() << " us" << std::endl;

  std::cout << "Avg. number of iterations: \t\t" << AVG(iterations) << " \t" << STDDEV(iterations) << " \t"
            << iterations.maxCoeff() << " \t" << iterations.minCoeff() << " " << std::endl;
  std::cout << "This is the final state: \n" << state << std::endl;
}