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

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

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

#include <boost/fusion/include/std_pair.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( sinhcosh_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0 & a2) const BOOST_NOEXCEPT
    {
      A0 a1;
      sinhcosh(a0, a1, a2);
      return a1;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sinhcosh_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE  std::pair<A0, A0> operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      A0 first, second;
      sinhcosh(a0, first, second);
      return {first, second};
    }
  };
} } }

#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @brief wraps Cal3_S2 class to python
 * @author Ellon Paiva Mendes (LAAS-CNRS)
 **/

#include <boost/python.hpp>

#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>

#include <gtsam/geometry/Cal3_S2.h>

using namespace boost::python;
using namespace gtsam;

BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Cal3_S2::print, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Cal3_S2::equals, 1, 2)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(uncalibrate_overloads, Cal3_S2::uncalibrate, 1, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(calibrate_overloads, Cal3_S2::calibrate, 1, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Cal3_S2::between, 1, 3)

// Function pointers to desambiguate Cal3_S2::calibrate calls
Point2 (Cal3_S2::*calibrate1)(const Point2 &, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp) const = &Cal3_S2::calibrate;
Vector3 (Cal3_S2::*calibrate2)(const Vector3 &) const = &Cal3_S2::calibrate;

void exportCal3_S2(){

class_<Cal3_S2, boost::shared_ptr<Cal3_S2> >("Cal3_S2", init<>())
  .def(init<double,double,double,double,double>())
  .def(init<const Vector &>())
  .def(init<double,int,int>(args("fov","w","h")))
  .def(init<std::string>())
  .def(repr(self))
  .def("print", &Cal3_S2::print, print_overloads(args("s")))
  .def("equals", &Cal3_S2::equals, equals_overloads(args("q","tol")))
  .def("fx",&Cal3_S2::fx)
  .def("fy",&Cal3_S2::fy)
  .def("skew",&Cal3_S2::skew)
  .def("px",&Cal3_S2::px)
  .def("py",&Cal3_S2::py)
  .def("principal_point",&Cal3_S2::principalPoint)
  .def("vector",&Cal3_S2::vector)
  .def("k",&Cal3_S2::K)
  .def("matrix",&Cal3_S2::matrix)
  .def("matrix_inverse",&Cal3_S2::matrix_inverse)
  .def("uncalibrate",&Cal3_S2::uncalibrate, uncalibrate_overloads())
  .def("calibrate",calibrate1, calibrate_overloads())
  .def("calibrate",calibrate2)
  .def("between",&Cal3_S2::between, between_overloads())
;
register_ptr_to_python< boost::shared_ptr<Cal3_S2> >();

}


//
// Copyright (c) 2009 Rutger ter Borg
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_NUMERIC_BINDINGS_TRANS_HPP
#define BOOST_NUMERIC_BINDINGS_TRANS_HPP

#include <boost/mpl/fold.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/max.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/end.hpp>
#include <boost/numeric/bindings/is_column_major.hpp>
#include <boost/numeric/bindings/rank.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/bandwidth.hpp>
#include <boost/numeric/bindings/tag.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/numeric/bindings/has_band_array.hpp>
#include <boost/numeric/bindings/has_linear_array.hpp>
#include <boost/ref.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace detail {

template< typename T, typename Conj >
struct trans_wrapper: reference_wrapper<T> {
    trans_wrapper( T& t ): reference_wrapper<T>( t ) {}
};

//
// In case of linear storage
//
template< typename T, typename Conj, typename Id, typename Enable >
struct adaptor< trans_wrapper<T, Conj>, Id, Enable > {

    typedef typename property_map_of< T >::type prop_of_T;
    typedef typename property_insert< T,

        // upgrade to at least a matrix
        mpl::pair<
            tag::entity,
            tag::tensor< mpl::max< tag::matrix, rank< T > >::type::value >
        >,

        // size1 <-> size2
        mpl::pair< tag::size_type<1>, typename result_of::size2< T >::type >,
        mpl::pair< tag::size_type<2>, typename result_of::size1< T >::type >,

        // row_major <-> column_major
        mpl::pair<
            tag::data_order,
            typename mpl::if_<
                is_column_major< T >,
                tag::row_major,
                tag::column_major >::type
        >,

        // Conjugate transform (if passed by template argument)
        Conj,

        // If T has a linear array, or has a band array
        // flip strides, stride1 <-> stride2
        typename mpl::if_< mpl::or_< has_linear_array< T >, has_band_array< T > >,
            mpl::pair< tag::stride_type<1>, typename result_of::stride2< T >::type >,
            mpl::void_
        >::type,
        typename mpl::if_< mpl::or_< has_linear_array< T >, has_band_array< T > >,
            mpl::pair< tag::stride_type<2>, typename result_of::stride1< T >::type >,
            mpl::void_
        >::type,

        // If T has a band array
        // flip bandwidths, bandwidth1 <-> bandwidth2
        typename mpl::if_< has_band_array< T >,
            mpl::pair< tag::bandwidth_type<1>, typename result_of::bandwidth2< T >::type >,
            mpl::void_
        >::type,
        typename mpl::if_< has_band_array< T >,
            mpl::pair< tag::bandwidth_type<2>, typename result_of::bandwidth1< T >::type >,
            mpl::void_
        >::type,

        // If a data_side tag is present:
        // upper <-> lower
        typename mpl::if_<
            mpl::has_key< prop_of_T, tag::data_side >,
            typename mpl::if_<
                is_same<
                    typename mpl::at< prop_of_T, tag::data_side >::type,
                    tag::upper
                >,
                mpl::pair< tag::data_side, tag::lower >,
                mpl::pair< tag::data_side, tag::upper >
            >::type,
            mpl::void_
        >::type

    >::type property_map;

    // Flip size1/size2
    static typename result_of::size2< T >::type size1( const Id& id ) {
        return bindings::size2( id.get() );
    }

    static typename result_of::size1< T >::type size2( const Id& id ) {
        return bindings::size1( id.get() );
    }

    // Value array access
    static typename result_of::begin_value< T >::type begin_value( Id& id ) {
        return bindings::begin_value( id.get() );
    }

    static typename result_of::end_value< T >::type end_value( Id& id ) {
        return bindings::end_value( id.get() );
    }

    // Linear array storage transpose
    // Flip stride1/stride2
    static typename result_of::stride2< T >::type stride1( const Id& id ) {
        return bindings::stride2( id.get() );
    }

    static typename result_of::stride1< T >::type stride2( const Id& id ) {
        return bindings::stride1( id.get() );
    }

    // Banded matrix transpose
    // Flip bandwidth1/bandwidth2
    static typename result_of::bandwidth2< T >::type bandwidth1( const Id& id ) {
        return bindings::bandwidth2( id.get() );
    }

    static typename result_of::bandwidth1< T >::type bandwidth2( const Id& id ) {
        return bindings::bandwidth1( id.get() );
    }

};

} // namespace detail

namespace result_of {

template< typename T >
struct trans {
    typedef detail::trans_wrapper<T, mpl::void_> type;
};

}

template< typename T >
const typename result_of::trans<T>::type trans( T& underlying ) {
    return detail::trans_wrapper<T, mpl::void_>( underlying );
}

template< typename T >
const typename result_of::trans<const T>::type trans( const T& underlying ) {
    return detail::trans_wrapper<const T, mpl::void_>( underlying );
}

namespace blas {

  using boost::numeric::bindings::trans;

} // namespace blas

} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


#include <vector>
#include <iostream>
#include <Eigen/Geometry>

int
main ()
{
  auto a = std::vector<char>(1024, 42);
  const auto* f1 = new (a.data()) Eigen::Vector3d{};
  std::cout << *f1 << "\n";
}


/** \file
 * cog.cpp
 *
 *  Created on: Mar 25, 2018
 *      Author: itto
 */

#include "Eigen/Dense"
#include "cog.h"
#include <boost/test/unit_test.hpp>


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file SteamTrajVar.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_TRAJECTORY_VAR_HPP
#define STEAM_TRAJECTORY_VAR_HPP

#include <Eigen/Core>

#include <steam/common/Time.hpp>
#include <steam/evaluator/blockauto/transform/TransformEvaluator.hpp>
#include <steam/state/VectorSpaceStateVar.hpp>

namespace steam {
namespace se3 {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief This class wraps a pose and velocity evaluator to act as a discrete-time trajectory
///        state variable for continuous-time trajectory estimation.
//////////////////////////////////////////////////////////////////////////////////////////////
class SteamTrajVar
{
 public:

  /// Shared pointer typedefs for readability
  typedef boost::shared_ptr<SteamTrajVar> Ptr;
  typedef boost::shared_ptr<const SteamTrajVar> ConstPtr;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  SteamTrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0,
               const VectorSpaceStateVar::Ptr& velocity);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get pose evaluator
  //////////////////////////////////////////////////////////////////////////////////////////////
  const se3::TransformEvaluator::Ptr& getPose() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get velocity state variable
  //////////////////////////////////////////////////////////////////////////////////////////////
  const VectorSpaceStateVar::Ptr& getVelocity() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Get timestamp
  //////////////////////////////////////////////////////////////////////////////////////////////
  const steam::Time& getTime() const;

 private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Timestamp of trajectory variable
  //////////////////////////////////////////////////////////////////////////////////////////////
  steam::Time time_;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Pose evaluator
  //////////////////////////////////////////////////////////////////////////////////////////////
  se3::TransformEvaluator::Ptr T_k0_;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Generalized 6D velocity state variable
  //////////////////////////////////////////////////////////////////////////////////////////////
  VectorSpaceStateVar::Ptr velocity_;
};

} // se3
} // steam

#endif // STEAM_TRAJECTORY_VAR_HPP


// Copyright (C) 2011-2020 Roki. Distributed under the MIT License
#define  BOOST_TEST_MAIN
#include <boost/test/included/unit_test.hpp>
#include <srook/optional.hpp>
#include <srook/type_traits/is_same.hpp>

BOOST_AUTO_TEST_SUITE(sroptional_test)

using srook::is_same;

BOOST_AUTO_TEST_CASE(optional_applicative1)
{
    auto result =
        srook::applicative(
            [](int x, int y, int z) SROOK_CXX17_CONSTEXPR noexcept -> int { return x + y + z; },
            srook::make_optionals(1, 2, 3)
        );
    SROOK_ST_ASSERT((is_same<SROOK_DECLTYPE(result), srook::optional<int>>::value));
    BOOST_TEST(*(result >>= [](int x) noexcept -> srook::optional<bool> { return x == 6; }));
}

BOOST_AUTO_TEST_CASE(optional_applicative2)
{
    auto result =
        srook::applicative(
            [](int x, int y, int z) SROOK_CXX17_CONSTEXPR noexcept -> int { return x * y * z; },
            srook::make_optionals(1, 2, 3)
        ) >>= [](int x) noexcept -> srook::optional<int> { return { x / 2 }; };
    SROOK_ST_ASSERT((is_same<SROOK_DECLTYPE(result), srook::optional<int>>::value));
    BOOST_TEST(*(result >>= [](int x) noexcept -> srook::optional<bool> { return x == 3; }));
}

BOOST_AUTO_TEST_CASE(optional_applicative3)
{
    auto result =
        srook::applicative(
            [](int x) SROOK_CXX17_CONSTEXPR noexcept -> int { return x * 2; },
            srook::applicative(
                [](int x, int y) SROOK_CXX17_CONSTEXPR noexcept -> int { return x * y; },
                srook::make_optional(2), srook::make_optional(3)
            )
        );
    SROOK_ST_ASSERT((is_same<SROOK_DECLTYPE(result), srook::optional<int>>::value));
    BOOST_TEST(*(result >>= [](int x) noexcept -> srook::optional<bool> { return x == 12; }));
}

BOOST_AUTO_TEST_CASE(optional_applicative4)
{
    using namespace srook::optionally::applicative_operators;

    {
        auto result = [](int x) SROOK_CXX17_CONSTEXPR noexcept -> int { return x * 2; } & srook::make_optional(2);
        SROOK_ST_ASSERT((is_same<SROOK_DECLTYPE(result), srook::optional<int>>::value));
        BOOST_TEST(*(result >>= [](int x) noexcept -> srook::optional<bool> { return x == 4; }));
    }

    const auto l = [](int x, int y, int z) SROOK_CXX17_CONSTEXPR noexcept -> int { return x * y * z; };
    {
        auto result = l & srook::make_optional(2) * srook::make_optional(3) * srook::make_optional(4);
        SROOK_ST_ASSERT((is_same<SROOK_DECLTYPE(result), srook::optional<int>>::value));
        BOOST_TEST(*(result >>= [](int x) noexcept -> srook::optional<bool> { return x == 24; }));
    }
    {
        auto result = l & srook::make_optionals(2, 3, 4);
        SROOK_ST_ASSERT((is_same<SROOK_DECLTYPE(result), srook::optional<int>>::value));
        BOOST_TEST(*(result >>= [](int x) noexcept -> srook::optional<bool> { return x == 24; }));

        result = l & srook::make_optionals(2, 3) * srook::make_optional(4);
        BOOST_TEST(*(result >>= [](int x) noexcept -> srook::optional<bool> { return x == 24; }));
    }
}

BOOST_AUTO_TEST_SUITE_END()


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

#include <iostream>
#include <boost/tuple/tuple.hpp>

#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/glas_tag.hpp>
#include <boost/numeric/mtl/operation/raw_copy.hpp>
#include <boost/numeric/mtl/operation/sub_matrix.hpp>
#include <boost/numeric/mtl/operation/print_matrix.hpp>
#include <boost/numeric/mtl/matrix/inserter.hpp>
#include <boost/numeric/mtl/matrix/transposed_view.hpp>

// This test is old and ugly, consider replacing it without decreasing coverage

using namespace std;

struct test_dense2D_exception {};

template <typename T1, typename T2>
void check_same_type(T1, T2)
{
    throw test_dense2D_exception();
}

// If same type we're fine
template <typename T1>
void check_same_type(T1, T1) {}

template <typename Parameters, typename ExpRowComplexity, typename ExpColComplexity>
struct test_dense2D
{
    template <typename Matrix, typename Tag, typename ExpComplexity>
    void two_d_iteration(char const* outer, Matrix & matrix, Tag, ExpComplexity)
    {
	namespace traits = mtl::traits;
	typename traits::row<Matrix>::type                                 row(matrix);
	typename traits::col<Matrix>::type                                 col(matrix);
	typename traits::const_value<Matrix>::type                         value(matrix);
	typedef typename traits::range_generator<Tag, Matrix>::type        cursor_type;
	typedef typename traits::range_generator<Tag, Matrix>::complexity  complexity;

	cout << outer << complexity() << '\n';
	check_same_type(complexity(), ExpComplexity());
	for (cursor_type cursor = mtl::begin<Tag>(matrix), cend = mtl::end<Tag>(matrix); cursor != cend; ++cursor) {
	    typedef mtl::tag::all     inner_tag;
	    typedef typename traits::range_generator<inner_tag, cursor_type>::type icursor_type;
	    for (icursor_type icursor = mtl::begin<inner_tag>(cursor), icend = mtl::end<inner_tag>(cursor); icursor != icend; ++icursor)
		cout << "matrix[" << row(*icursor) << ", " << col(*icursor) << "] = " << value(*icursor) << '\n';
	}
    }

    template <typename Matrix, typename Tag, typename ExpComplexity>
    void two_d_iterator_iteration(char const* outer, Matrix & matrix, Tag, ExpComplexity)
    {
	namespace traits = mtl::traits;
	typename traits::row<Matrix>::type                                 row(matrix);
	typename traits::col<Matrix>::type                                 col(matrix);
	typename traits::const_value<Matrix>::type                         value(matrix);
	typedef typename traits::range_generator<Tag, Matrix>::type        cursor_type;
	typedef typename traits::range_generator<Tag, Matrix>::complexity  complexity;

	cout // << "Matrix traversal with iterators"
	     << outer << complexity() << '\n';
	check_same_type(complexity(), ExpComplexity());
	for (cursor_type cursor = mtl::begin<Tag>(matrix), cend = mtl::end<Tag>(matrix); cursor != cend; ++cursor) {
	    typedef mtl::tag::iter::all     inner_tag;
	    typedef typename traits::range_generator<inner_tag, cursor_type>::type iter_type;
	    for (iter_type iter = mtl::begin<inner_tag>(cursor), i_end = mtl::end<inner_tag>(cursor); iter != i_end; ++iter)
		cout << *iter << '\n';
	}
    }

    template <typename Matrix>
    void one_d_iteration(char const* name, Matrix & matrix, size_t check_row, size_t check_col, double check)
    {
	namespace traits = mtl::traits;
	typename traits::row<Matrix>::type                                 row(matrix);
	typename traits::col<Matrix>::type                                 col(matrix);
	typename traits::value<Matrix>::type                               value(matrix);
	typedef  mtl::tag::nz                                          tag;
	typedef typename traits::range_generator<tag, Matrix>::type        cursor_type;
	typedef typename traits::range_generator<tag, Matrix>::complexity  complexity;

	cout << name << "\nElements: " << complexity() << '\n';
	for (cursor_type cursor = mtl::begin<tag>(matrix), cend = mtl::end<tag>(matrix); cursor != cend; ++cursor) {
	    cout << "matrix[" << row(*cursor) << ", " << col(*cursor) << "] = " << value(*cursor) << '\n';
	    if (row(*cursor) == check_row && col(*cursor) == check_col && value(*cursor) != check) throw test_dense2D_exception();
	}
    }

    void operator() (double element_1_2)
    {
	typedef mtl::dense2D<double, Parameters> matrix_type;
	matrix_type   matrix;
	matrix= 1., 2., 3.,
	        4., 5., 6.;
	// double        val[] = {1., 2., 3., 4., 5., 6.};
	// mtl::raw_copy(val, val+6, matrix);

	one_d_iteration("\nMatrix", matrix, 1, 2, element_1_2);
	two_d_iteration("\nRows: ", matrix, mtl::tag::row(), ExpRowComplexity());
	two_d_iteration("\nColumns: ", matrix, mtl::tag::col(), ExpColComplexity());
	two_d_iterator_iteration("\nRows (iterator): ", matrix, mtl::tag::row(), ExpRowComplexity());
	two_d_iterator_iteration("\nColumns (iterator): ", matrix, mtl::tag::col(), ExpColComplexity());

	mtl::transposed_view<matrix_type> trans_matrix(matrix);
	one_d_iteration("\nTransposed matrix", trans_matrix, 2, 1, element_1_2);
	two_d_iteration("\nRows: ", trans_matrix, mtl::tag::row(), ExpColComplexity());
	two_d_iteration("\nColumns: ", trans_matrix, mtl::tag::col(), ExpRowComplexity());
	two_d_iterator_iteration("\nRows (iterator): ", trans_matrix, mtl::tag::row(), ExpColComplexity());
	two_d_iterator_iteration("\nColumns (iterator): ", trans_matrix, mtl::tag::col(), ExpRowComplexity());

	cout << "\nmatrix[1][2] = " << matrix[1][2] << "\n";
	matrix[1][2]= 18.0;
	cout << "matrix[1][2] = " << matrix[1][2] << "\n";
	cout << "trans_matrix[2][1] = " << trans_matrix[2][1] << "\n";

	mtl::mat::inserter<matrix_type>  i(matrix);
	i(1, 2) << 17.0;
	cout << "matrix[1, 2] = " << matrix(1, 2) << "\n";
    }
};

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

    typedef mat::parameters<row_major, mtl::index::c_index, fixed::dimensions<2, 3> > parameters1;
    test_dense2D<parameters1, complexity_classes::linear_cached, complexity_classes::linear>()(6.0);

    // Don't use this Fortran indexing, it is absolutely incomplete
    // typedef mat::parameters<row_major, mtl::index::f_index, fixed::dimensions<2, 3> > parameters2;
    // test_dense2D<parameters2, complexity_classes::linear_cached, complexity_classes::linear>()(2.0);

    typedef mat::parameters<col_major, mtl::index::c_index, fixed::dimensions<2, 3> > parameters3;
    test_dense2D<parameters3, complexity_classes::linear, complexity_classes::linear_cached>()(6.0);

    // typedef mat::parameters<col_major, mtl::index::f_index, fixed::dimensions<2, 3> > parameters4;
    // test_dense2D<parameters4, complexity_classes::linear, complexity_classes::linear_cached>()(3.0);

#ifdef MTL_WITH_INITLIST
    dense2D<int> A = {{1, 2}, {3, 4}};
    std::stringstream ss;
    ss << A;

    std::cout << ss.str() << std::endl;
    std::string refsting = "[1 2]\n[3 4]\n";

    std::cout << "Equal to refstring: " << std::boolalpha << (refsting == ss.str()) << std::endl;
#endif

    return 0;
}


/**
 * \file dcs/testbed/anglano2014_fc2q_application_manager.hpp
 *
 * \brief Application manager based on the work by (Anglano et al., 2014)
 *
 * This class implements the FC2Q fuzzy controller proposed in [1].
 *
 * References:
 * -# C. Anglano, M. Canonico and M. Guazzone,
 *    "FC2Q: Exploiting Fuzzy Control in Server Consolidation for Cloud Applications with SLA Constraints,"
 *    Concurrency and Computation: Practice and Experience, 27:4491–4514, 2015.
 *    doi:10.1002/cpe.3410
 * .
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2014   Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_TESTBED_ANGLANO2014_FC2Q_APPLICATION_MANAGER_HPP
#define DCS_TESTBED_ANGLANO2014_FC2Q_APPLICATION_MANAGER_HPP

#include <boost/smart_ptr.hpp>
#include <boost/timer/timer.hpp>
#include <cmath>
#include <cstddef>
#include <dcs/assert.hpp>
#include <dcs/debug.hpp>
#include <dcs/exception.hpp>
#include <dcs/logging.hpp>
#include <dcs/math/function/clamp.hpp>
#include <dcs/math/traits/float.hpp>
#include <dcs/testbed/application_performance_category.hpp>
#include <dcs/testbed/base_application_manager.hpp>
//#include <dcs/testbed/data_estimators.hpp>
#include <dcs/testbed/data_smoothers.hpp>
#include <dcs/testbed/virtual_machine_performance_category.hpp>
#include <fl/Headers.h>
#include <fstream>
#include <limits>
#include <map>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>

namespace dcs { namespace testbed {

/**
 * \brief Application manager based on the work by (Anglano et al., 2014)
 *
 * This class implements the FC2Q fuzzy controller proposed in [1].
 *
 * References:
 * -# C. Anglano, M. Canonico and M. Guazzone,
 *    "FC2Q: Exploiting Fuzzy Control in Server Consolidation for Cloud Applications with SLA Constraints,"
 *    Concurrency and Computation: Practice and Experience, 27:4491–4514, 2015.
 *    doi:10.1002/cpe.3410
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename TraitsT>
class anglano2014_fc2q_application_manager: public base_application_manager<TraitsT>
{
	private: typedef base_application_manager<TraitsT> base_type;
	public: typedef typename base_type::traits_type traits_type;
	public: typedef typename traits_type::real_type real_type;
	private: typedef typename base_type::app_type app_type;
	private: typedef typename base_type::app_pointer app_pointer;
	private: typedef typename base_type::vm_identifier_type vm_identifier_type;
	private: typedef typename app_type::sensor_type sensor_type;
	private: typedef typename app_type::sensor_pointer sensor_pointer;
	private: typedef ::std::map<application_performance_category,sensor_pointer> out_sensor_map;
	private: typedef ::std::map<virtual_machine_performance_category,::std::map<vm_identifier_type,sensor_pointer> > in_sensor_map;

	private: static const std::size_t control_warmup_size;
	private: static const float resource_share_lb_scale_factor;

	private: static const ::std::string rgain_fuzzy_var_name;
	private: static const ::std::string cres_fuzzy_var_name;
	private: static const ::std::string deltac_fuzzy_var_name;

	public: anglano2014_fc2q_application_manager()
	: beta_(0.9),
	  p_fuzzy_eng_(new fl::Engine()),
	  ctl_count_(0),
	  ctl_skip_count_(0),
	  ctl_fail_count_(0)
	{
		init();
	}

	public: void smoothing_factor(real_type value)
	{
		beta_ = value;
	}

	public: real_type smoothing_factor() const
	{
		return beta_;
	}

	public: void export_data_to(::std::string const& fname)
	{
		dat_fname_ = fname;
	}

	private: void init()
	{
		DCS_DEBUG_ASSERT( p_fuzzy_eng_ );

		fl::InputVariable* p_iv = 0;

		p_iv = new fl::InputVariable();
		p_iv->setEnabled(true);
		p_iv->setName(cres_fuzzy_var_name);
		p_iv->setRange(0.0, 1.0);
		p_iv->addTerm(new fl::Ramp("LOW", 0.30, 0.00));
		p_iv->addTerm(new fl::Triangle("FINE", 0.10, 0.25, 0.40));
		p_iv->addTerm(new fl::Ramp("HIGH", 0.30, 1.00));
		p_fuzzy_eng_->addInputVariable(p_iv);

		p_iv = new fl::InputVariable();
		p_iv->setEnabled(true);
		p_iv->setName(rgain_fuzzy_var_name);
		p_iv->setRange(-1, 1);
		p_iv->addTerm(new fl::Ramp("LOW", 0.20, -0.40));
		p_iv->addTerm(new fl::Triangle("FINE", 0.10, 0.20, 0.30));
		p_iv->addTerm(new fl::Ramp("HIGH", 0.30, 1.00));
		p_fuzzy_eng_->addInputVariable(p_iv);

		fl::OutputVariable* p_ov = 0;
		p_ov = new fl::OutputVariable();
		p_ov->setEnabled(true);
		p_ov->setName(deltac_fuzzy_var_name);
		p_ov->setRange(-1, 1);
		//p_ov->setLockValueInRange(true);
		p_ov->fuzzyOutput()->setAccumulation(new fl::AlgebraicSum());
		p_ov->setDefuzzifier(new fl::Centroid());
		p_ov->setDefaultValue(fl::nan);
		p_ov->setPreviousValue(false);
		p_ov->addTerm(new fl::Triangle("BDW", -1.00, -0.55, -0.10));
		p_ov->addTerm(new fl::Triangle("DWN", -0.20, -0.125, -0.05));
		p_ov->addTerm(new fl::Triangle("STY", -0.10, 0.0, 0.10));
		p_ov->addTerm(new fl::Triangle("UP", 0.05, 0.125, 0.20));
		p_ov->addTerm(new fl::Triangle("BUP", 0.10, 0.55, 1.00));
		p_fuzzy_eng_->addOutputVariable(p_ov);

		fl::RuleBlock* p_rules = new fl::RuleBlock();
		p_rules->setEnabled(true);
		p_rules->setConjunction(new fl::Minimum());
		p_rules->setDisjunction(new fl::Maximum());
		p_rules->setImplication(new fl::AlgebraicProduct());
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is LOW and " + rgain_fuzzy_var_name + " is LOW then " + deltac_fuzzy_var_name + " is BUP", p_fuzzy_eng_.get()));
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is LOW and " + rgain_fuzzy_var_name + " is FINE then " + deltac_fuzzy_var_name + " is UP", p_fuzzy_eng_.get()));
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is LOW and " + rgain_fuzzy_var_name + " is HIGH then " + deltac_fuzzy_var_name + " is UP", p_fuzzy_eng_.get()));
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is FINE and " + rgain_fuzzy_var_name + " is LOW then " + deltac_fuzzy_var_name + " is UP", p_fuzzy_eng_.get()));
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is FINE and " + rgain_fuzzy_var_name + " is FINE then " + deltac_fuzzy_var_name + " is STY", p_fuzzy_eng_.get()));
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is FINE and " + rgain_fuzzy_var_name + " is HIGH then " + deltac_fuzzy_var_name + " is DWN", p_fuzzy_eng_.get()));
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is HIGH and " + rgain_fuzzy_var_name + " is LOW then " + deltac_fuzzy_var_name + " is STY", p_fuzzy_eng_.get()));
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is HIGH and " + rgain_fuzzy_var_name + " is FINE then " + deltac_fuzzy_var_name + " is DWN", p_fuzzy_eng_.get()));
		p_rules->addRule(fl::Rule::parse("if " + cres_fuzzy_var_name + " is HIGH and " + rgain_fuzzy_var_name + " is HIGH then " + deltac_fuzzy_var_name + " is BDW", p_fuzzy_eng_.get()));
		p_fuzzy_eng_->addRuleBlock(p_rules);
	}

	private: void do_reset()
	{
		typedef typename base_type::target_value_map::const_iterator target_iterator;
		typedef typename app_type::vm_pointer vm_pointer;

		const ::std::vector<vm_pointer> vms = this->app().vms();
		const ::std::size_t nvms = this->app().num_vms();

		// Reset output sensors
		out_sensors_.clear();
		for (target_iterator tgt_it = this->target_values().begin(),
							 tgt_end_it = this->target_values().end();
			 tgt_it != tgt_end_it;
			 ++tgt_it)
		{
			const application_performance_category cat = tgt_it->first;

			out_sensors_[cat] = this->app().sensor(cat);
		}

		// Reset input sensors
		in_sensors_.clear();
		for (::std::size_t i = 0; i < nvms; ++i)
		{
			vm_pointer p_vm = vms[i];

			in_sensors_[cpu_util_virtual_machine_performance][p_vm->id()] = p_vm->sensor(cpu_util_virtual_machine_performance);
			in_sensors_[memory_util_virtual_machine_performance][p_vm->id()] = p_vm->sensor(memory_util_virtual_machine_performance);
		}

		// Reset counters
		ctl_count_ = ctl_skip_count_
				   = ctl_fail_count_
				   = 0;

		// Reset fuzzy controller
		p_fuzzy_eng_->restart();

		// Reset resource utilization smoothers
		for (::std::size_t i = 0; i < nvms; ++i)
		{
			this->data_smoother(cpu_util_virtual_machine_performance, vms[i]->id(), ::boost::make_shared< testbed::brown_single_exponential_smoother<real_type> >(beta_));
			this->data_smoother(memory_util_virtual_machine_performance, vms[i]->id(), ::boost::make_shared< testbed::brown_single_exponential_smoother<real_type> >(beta_));
		}

		// Reset output data file
		if (p_dat_ofs_ && p_dat_ofs_->is_open())
		{
			p_dat_ofs_->close();
		}
		p_dat_ofs_.reset();
		if (!dat_fname_.empty())
		{
			p_dat_ofs_ = ::boost::make_shared< ::std::ofstream >(dat_fname_.c_str());
			if (!p_dat_ofs_->good())
			{
				::std::ostringstream oss;
				oss << "Cannot open output data file '" << dat_fname_ << "'";

				DCS_EXCEPTION_THROW(::std::runtime_error, oss.str());
			}

			*p_dat_ofs_ << "\"ts\"";

			for (::std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << ",\"CPUCap_{" << vms[i]->id() << "}(k)\",\"CPUShare_{" << vms[i]->id() << "}(k)\"";
			}
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << ",\"CPUShare_{" << vms[i]->id() << "}(k-1)\"";
			}
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << ",\"CPUUtil_{" << vms[i]->id() << "}(k-1)\",\"MemUtil_{" << vms[i]->id() << "}(k-1)\"";
			}
			for (target_iterator tgt_it = this->target_values().begin(),
							 	 tgt_end_it = this->target_values().end();
				 tgt_it != tgt_end_it;
				 ++tgt_it)
			{
				const application_performance_category cat = tgt_it->first;

				*p_dat_ofs_ << ",\"ReferenceOutput_{" << cat << "}(k-1)\",\"MeasuredOutput_{" << cat << "}(k-1)\",\"RelativeOutputError_{" << cat << "}(k-1)\"";
			}
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << ",\"Cres_{" << vms[i]->id() << "}(k-1)\"";
			}
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << ",\"DeltaC_{" << vms[i]->id() << "}(k)\"";
			}
			//NOTE: C(k) may differ from CPUShare(k) for several reasons:
			// - There is a latency in setting the new share (e.g., this is usually not the case of CPU but of other resources like the memory, whereby the new share is not immediately set but the memory is (de)allocated incrementally)
			// - There is another component between this controller and physical resources that may change the wanted share (e.g., if a physical resource is shared among different VMs, there can be a component that try to allocate the contented physical resource fairly).
			for (std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << ",\"C_{" << vms[i]->id() << "}(k)\"";
			}
			*p_dat_ofs_ << ",\"# Controls\",\"# Skip Controls\",\"# Fail Controls\"";
            *p_dat_ofs_ << ",\"Elapsed Time\"";
			*p_dat_ofs_ << ::std::endl;
		}
	}

	private: void do_sample()
	{
		typedef typename in_sensor_map::const_iterator in_sensor_iterator;
		typedef typename out_sensor_map::const_iterator out_sensor_iterator;
		typedef ::std::vector<typename sensor_type::observation_type> obs_container;
		typedef typename obs_container::const_iterator obs_iterator;

		DCS_DEBUG_TRACE("(" << this << ") BEGIN Do SAMPLE - Count: " << ctl_count_ << "/" << ctl_skip_count_ << "/" << ctl_fail_count_);

		// Collect input values
		for (in_sensor_iterator in_sens_it = in_sensors_.begin(),
								in_sens_end_it = in_sensors_.end();
			 in_sens_it != in_sens_end_it;
			 ++in_sens_it)
		{
			const virtual_machine_performance_category cat = in_sens_it->first;

			for (typename in_sensor_map::mapped_type::const_iterator vm_it = in_sens_it->second.begin(),
																	 vm_end_it = in_sens_it->second.end();
				 vm_it != vm_end_it;
				 ++vm_it)
			{
				const vm_identifier_type vm_id = vm_it->first;
				sensor_pointer p_sens = vm_it->second;

				// check: p_sens != null
				DCS_DEBUG_ASSERT( p_sens );

				p_sens->sense();
				if (p_sens->has_observations())
				{
					const obs_container obs = p_sens->observations();
					const obs_iterator end_it = obs.end();
					for (obs_iterator it = obs.begin();
						 it != end_it;
						 ++it)
					{
						//this->data_estimator(cat, vm_id).collect(it->value());
						this->data_smoother(cat, vm_id).smooth(it->value());
					}
				}
			}
		}

		// Collect output values
		for (out_sensor_iterator out_sens_it = out_sensors_.begin(),
								 out_sens_end_it = out_sensors_.end();
			 out_sens_it != out_sens_end_it;
			 ++out_sens_it)
		{
			const application_performance_category cat = out_sens_it->first;

			sensor_pointer p_sens = out_sens_it->second;

			// check: p_sens != null
			DCS_DEBUG_ASSERT( p_sens );

			p_sens->sense();
			if (p_sens->has_observations())
			{
				const obs_container obs = p_sens->observations();
				const obs_iterator end_it = obs.end();
				for (obs_iterator it = obs.begin();
					 it != end_it;
					 ++it)
				{
					this->data_estimator(cat).collect(it->value());
				}
			}
		}

		DCS_DEBUG_TRACE("(" << this << ") END Do SAMPLE - Count: " << ctl_count_ << "/" << ctl_skip_count_ << "/" << ctl_fail_count_);
	}

	private: void do_control()
	{
		typedef typename base_type::target_value_map::const_iterator target_iterator;
		typedef typename app_type::vm_pointer vm_pointer;

		DCS_DEBUG_TRACE("(" << this << ") BEGIN Do CONTROL - Count: " << ctl_count_ << "/" << ctl_skip_count_ << "/" << ctl_fail_count_);

        boost::timer::cpu_timer cpu_timer;

		++ctl_count_;

		bool skip_ctl = false;

		std::vector<real_type> old_shares;
		std::vector<real_type> new_shares;
		std::vector<real_type> deltacs;
		std::vector<real_type> cress;
		std::vector<real_type> cutils;
		std::map<application_performance_category,real_type> rgains;

		::std::vector<vm_pointer> vms = this->app().vms();
		const ::std::size_t nvms = vms.size();

		for (::std::size_t i = 0; i < nvms; ++i)
		{
			const virtual_machine_performance_category cat = cpu_util_virtual_machine_performance;
			const vm_pointer p_vm = vms[i];

//			if (this->data_estimator(cat, vms[i]->id()).count() > 0)
//			{
//				//const real_type uh = this->data_estimator(cat, p_vm->id()).estimate();
//				const real_type uh = this->data_smoother(cat, p_vm->id()).forecast(0);
//				const real_type c = p_vm->cpu_share();
//
//				cress[cat].push_back(c-uh);
//			}
//			else
//			{
//				// No observation collected during the last control interval
//				DCS_DEBUG_TRACE("No input observation collected during the last control interval -> Skip control");
//				skip_ctl = true;
//				break;
//			}
			const real_type uh = this->data_smoother(cat, p_vm->id()).forecast(0);
			const real_type c = p_vm->cpu_share();
			const real_type cres = c-uh;

			cress.push_back(cres);
			old_shares.push_back(c);
			cutils.push_back(uh);
DCS_DEBUG_TRACE("VM " << p_vm->id() << " - Performance Category: " << cat << " - Uhat(k): " << uh << " - C(k): " << c << " -> Cres(k+1): " << cres << " (Relative Cres(k+1): " << cres/c << ")");//XXX
		}

		if (!skip_ctl)
		{
			for (target_iterator tgt_it = this->target_values().begin(),
							 	 tgt_end_it = this->target_values().end();
				 tgt_it != tgt_end_it;
				 ++tgt_it)
			{
				const application_performance_category cat(tgt_it->first);

				// Compute a summary statistics of collected observation
				if (this->data_estimator(cat).count() > 0)
				{
					const real_type yh = this->data_estimator(cat).estimate();
					const real_type yr = this->target_value(cat);

					switch (cat)
					{
						case response_time_application_performance:
							rgains[cat] = (yr-yh)/yr;
							break;
						case throughput_application_performance:
							rgains[cat] = (yh-yr)/yr;
							break;
					}
DCS_DEBUG_TRACE("APP Performance Category: " << cat << " - Yhat(k): " << yh << " - R: " << yr << " -> Rgain(k+1): " << rgains.at(cat));//XXX
				}
				else
				{
					// No observation collected during the last control interval
					DCS_DEBUG_TRACE("No output observation collected during the last control interval -> Skip control");
					skip_ctl = true;
					break;
				}
#ifdef DCSXX_TESTBED_EXP_APP_MGR_RESET_ESTIMATION_EVERY_INTERVAL
				this->data_estimator(cat).reset();
#endif // DCSXX_TESTBED_EXP_APP_MGR_RESET_ESTIMATION_EVERY_INTERVAL
			}
		}

		// Skip control until we see enough observations.
		// This should give enough time to let the estimated performance metric
		// (e.g., 95th percentile of response time) stabilize
		if (ctl_count_ <= control_warmup_size)
		{
			skip_ctl = true;
		}

        if (!skip_ctl)
        {
			//FIXME: actually we only handle SISO systems
			DCS_ASSERT(rgains.size() == 1,
					   DCS_EXCEPTION_THROW(::std::runtime_error,
					   "Only SISO system are currently managed"));

			// Perform fuzzy control
			bool ok = false;
			//::std::vector<real_type> deltacs(nvms);
			try
			{
				for (::std::size_t i = 0; i < nvms; ++i)
				{
					const real_type cres = cress[i];
					const real_type cutil = cutils[i];
					const real_type rgain = rgains.begin()->second;
					const real_type old_share = old_shares[i];
					const real_type deltac_lb = std::min(1.0, cutil*resource_share_lb_scale_factor)-old_share;
					const real_type deltac_ub = std::max(0.0, 1-old_share);

					p_fuzzy_eng_->setInputValue(cres_fuzzy_var_name, cres/old_share);
					p_fuzzy_eng_->setInputValue(rgain_fuzzy_var_name, rgain);
					//p_fuzzy_eng_->getOutputVariable(deltac_fuzzy_var_name)->setMinimum(-cres);
					//p_fuzzy_eng_->getOutputVariable(deltac_fuzzy_var_name)->setMaximum(1-old_shares[i]);

					p_fuzzy_eng_->process();

					const real_type fuzzy_deltac = p_fuzzy_eng_->getOutputValue(deltac_fuzzy_var_name);

					real_type deltac = fuzzy_deltac;
					deltac = dcs::math::clamp(deltac, deltac_lb, deltac_ub);

					deltacs.push_back(deltac);
DCS_DEBUG_TRACE("VM " << vms[i]->id() << " -> DeltaC(k+1): " << deltacs.at(i) << " (computed: " << fuzzy_deltac << ", lb: " << deltac_lb << ", ub: " << deltac_ub << ")");//XXX
				}

				ok = true;
			}
			catch (fl::Exception const& fe)
			{
				DCS_DEBUG_TRACE( "Caught exception: " << fe.what() );

				::std::ostringstream oss;
				oss << "Unable to compute optimal control: " << fe.what();
				::dcs::log_warn(DCS_LOGGING_AT, oss.str());
			}
			catch (::std::exception const& e)
			{
				DCS_DEBUG_TRACE( "Caught exception: " << e.what() );

				::std::ostringstream oss;
				oss << "Unable to compute optimal control: " << e.what();
				::dcs::log_warn(DCS_LOGGING_AT, oss.str());
			}

			// Apply fuzzy control results
			if (ok)
			{
				for (::std::size_t i = 0; i < nvms; ++i)
				{
					vm_pointer p_vm = vms[i];

					const real_type old_share = p_vm->cpu_share();
					const real_type new_share = ::std::max(::std::min(old_share+deltacs[i], 1.0), 0.0);

					DCS_DEBUG_TRACE("VM '" << p_vm->id() << "' - old-share: " << old_share << " - new-share: " << new_share);

					if (::std::isfinite(new_share) && !::dcs::math::float_traits<real_type>::essentially_equal(old_share, new_share))
					{
						p_vm->cpu_share(new_share);
DCS_DEBUG_TRACE("VM " << vms[i]->id() << " -> C(k+1): " << new_share);//XXX

						new_shares.push_back(new_share);
					}
					else
					{
DCS_DEBUG_TRACE("VM " << vms[i]->id() << " -> C(k+1) not set!");//XXX

						new_shares.push_back(old_share);
					}
				}
DCS_DEBUG_TRACE("Control applied");//XXX
			}
			else
			{
				++ctl_fail_count_;

				::std::ostringstream oss;
				oss << "Control not applied: failed to solve the control problem";
				::dcs::log_warn(DCS_LOGGING_AT, oss.str());
			}
		}
		else
		{
			++ctl_skip_count_;
		}

        cpu_timer.stop();

		// Export to file
		if (p_dat_ofs_)
		{
			if (new_shares.size() == 0)
			{
				for (::std::size_t i = 0; i < nvms; ++i)
				{
					const vm_pointer p_vm = vms[i];

					// check: p_vm != null
					DCS_DEBUG_ASSERT( p_vm );

					new_shares.push_back(p_vm->cpu_share());
				}
			}
			if (old_shares.size() == 0)
			{
				for (::std::size_t i = 0; i < nvms; ++i)
				{
					const vm_pointer p_vm = vms[i];

					// check: p_vm != null
					DCS_DEBUG_ASSERT( p_vm );

					old_shares.push_back(p_vm->cpu_share());
				}
			}
			if (deltacs.size() == 0)
			{
				deltacs.assign(nvms, std::numeric_limits<real_type>::quiet_NaN());
			}
			if (cress.size() == 0)
			{
				cress.assign(nvms, std::numeric_limits<real_type>::quiet_NaN());
			}
			if (rgains.size() == 0)
			{
				for (target_iterator tgt_it = this->target_values().begin(),
									 tgt_end_it = this->target_values().end();
				tgt_it != tgt_end_it;
				++tgt_it)
				{
					const application_performance_category cat = tgt_it->first;

					rgains[cat] = std::numeric_limits<real_type>::quiet_NaN();
				}
			}

			*p_dat_ofs_ << ::std::time(0) << ",";
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				const vm_pointer p_vm = vms[i];

				// check: p_vm != null
				DCS_DEBUG_ASSERT( p_vm );

				if (i != 0)
				{
					*p_dat_ofs_ << ",";
				}
				*p_dat_ofs_ << p_vm->cpu_cap() << "," << p_vm->cpu_share();
			}
			*p_dat_ofs_ << ",";
            for (std::size_t i = 0; i < nvms; ++i)
            {
                if (i != 0)
                {
                    *p_dat_ofs_ << ",";
                }
                *p_dat_ofs_ << old_shares[i];
            }
			*p_dat_ofs_ << ",";
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				const vm_pointer p_vm = vms[i];

				// check: p_vm != null
				DCS_DEBUG_ASSERT( p_vm );

				if (i != 0)
				{
					*p_dat_ofs_ << ",";
				}
				//*p_dat_ofs_ << this->data_smoother(cpu_util_virtual_machine_performance, p_vm->id()).forecast(0)
				//			<< "," << this->data_smoother(memory_util_virtual_machine_performance, p_vm->id()).forecast(0);
				*p_dat_ofs_ << cutils[i]
							<< "," << this->data_smoother(memory_util_virtual_machine_performance, p_vm->id()).forecast(0);
			}
			*p_dat_ofs_ << ",";
			for (target_iterator tgt_it = this->target_values().begin(),
								 tgt_end_it = this->target_values().end();
				 tgt_it != tgt_end_it;
				 ++tgt_it)
			{
				const application_performance_category cat = tgt_it->first;

				if (tgt_it != this->target_values().begin())
				{
					*p_dat_ofs_ << ",";
				}
				const real_type yh = this->data_estimator(cat).estimate();
				const real_type yr = tgt_it->second;
				const real_type rgain = rgains.at(cat);
				*p_dat_ofs_ << yr << "," << yh << "," << rgain;
			}
			for (::std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << "," << cress[i];
			}
			for (std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << "," << deltacs[i];
			}
			for (std::size_t i = 0; i < nvms; ++i)
			{
				*p_dat_ofs_ << "," << new_shares[i];
			}
			*p_dat_ofs_ << "," << ctl_count_ << "," << ctl_skip_count_ << "," << ctl_fail_count_;
            *p_dat_ofs_ << "," << (cpu_timer.elapsed().user+cpu_timer.elapsed().system);
			*p_dat_ofs_ << ::std::endl;
		}

		DCS_DEBUG_TRACE("(" << this << ") END Do CONTROL - Count: " << ctl_count_ << "/" << ctl_skip_count_ << "/" << ctl_fail_count_);
	}

	private: real_type beta_; ///< The EWMA smoothing factor for resource utilizations
	private: ::boost::shared_ptr<fl::Engine> p_fuzzy_eng_; ///< The fuzzy control engine
	private: ::std::size_t ctl_count_; ///< Number of times control function has been invoked
	private: ::std::size_t ctl_skip_count_; ///< Number of times control has been skipped
	private: ::std::size_t ctl_fail_count_; ///< Number of times control has failed
	private: in_sensor_map in_sensors_;
	private: out_sensor_map out_sensors_;
	private: ::std::string dat_fname_;
	private: ::boost::shared_ptr< ::std::ofstream > p_dat_ofs_;
}; // anglano2014_fc2q_application_manager

template <typename T>
const std::size_t anglano2014_fc2q_application_manager<T>::control_warmup_size = 5;

template <typename T>
const float anglano2014_fc2q_application_manager<T>::resource_share_lb_scale_factor = 1.1;

template <typename T>
const ::std::string anglano2014_fc2q_application_manager<T>::rgain_fuzzy_var_name = "Rgain";

template <typename T>
const ::std::string anglano2014_fc2q_application_manager<T>::cres_fuzzy_var_name = "Cres";

template <typename T>
const ::std::string anglano2014_fc2q_application_manager<T>::deltac_fuzzy_var_name = "DeltaC";

}} // Namespace dcs::testbed

#endif // DCS_TESTBED_ANGLANO2014_FC2Q_APPLICATION_MANAGER_HPP


#include <boost/lambda/lambda.hpp>
#include <iostream>
#include <iterator>
#include <algorithm>

using namespace boost;
using namespace lambda;
using namespace std;

int main()
{
    typedef istream_iterator<int> in;

    for_each(
        in(cin), in(), cout << (_1 * 3) << " " );
    cout << endl;
}


#ifdef STAN_OPENCL
#include <stan/math/prim.hpp>
#include <stan/math/opencl/opencl.hpp>
#include <test/unit/math/expect_near_rel.hpp>
#include <test/unit/util.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <gtest/gtest.h>
#include <algorithm>

TEST(MathMatrixCL, mdivide_right_tri_low_cl_exception) {
  stan::math::matrix_d m1(2, 3);
  m1 << 1, 2, 3, 4, 5, 6;
  stan::math::matrix_cl<double> m2(m1, stan::math::matrix_cl_view::Lower);
  stan::math::matrix_cl<double> m3(2, 3);
  EXPECT_THROW(stan::math::mdivide_right_tri_low(m2, m2),
               std::invalid_argument);
  m2.view(stan::math::matrix_cl_view::Upper);
  EXPECT_THROW(stan::math::mdivide_right_tri_low(m2, m2),
               std::invalid_argument);

  stan::math::matrix_d m4(3, 3);
  m4 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  stan::math::matrix_cl<double> m5(m1, stan::math::matrix_cl_view::Entire);
  EXPECT_THROW(stan::math::mdivide_right_tri_low(m5, m5),
               std::invalid_argument);
  m5.view(stan::math::matrix_cl_view::Diagonal);
  EXPECT_THROW(stan::math::mdivide_right_tri_low(m5, m5),
               std::invalid_argument);
}

void mdivide_right_tri_low_Ab_test(int size) {
  boost::random::mt19937 rng;
  auto m1 = stan::math::matrix_d(size, size);
  for (int i = 0; i < size; i++) {
    for (int j = 0; j < i; j++) {
      m1(i, j) = stan::math::uniform_rng(-5, 5, rng);
    }
    m1(i, i) = 10000.0;
    for (int j = i + 1; j < size; j++) {
      m1(i, j) = 0.0;
    }
  }
  // force the CPU version for comparison
  stan::math::opencl_context.tuning_opts().tri_inverse_size_worth_transfer
      = size * 2;
  stan::math::matrix_d m1_cpu = stan::math::mdivide_right_tri_low(m1, m1);

  stan::math::matrix_cl<double> m1_A_cl(m1, stan::math::matrix_cl_view::Lower);
  stan::math::matrix_d m1_cl = stan::math::from_matrix_cl(
      stan::math::mdivide_right_tri_low(m1_A_cl, m1_A_cl));
  double max_error = 0;
  for (int i = 0; i < m1_cpu.size(); i++) {
    stan::test::expect_near_rel("mdivide_right_tri_low (OpenCL)", m1_cpu(i),
                                m1_cl(i));
  }
}

TEST(MathMatrixCL, mdivide_right_tri_low_test_cl_small) {
  mdivide_right_tri_low_Ab_test(3);
}

TEST(MathMatrixCL, mdivide_right_tri_low_test_cl_1under_block_size) {
  mdivide_right_tri_low_Ab_test(31);
}

TEST(MathMatrixCL, mdivide_right_tri_low_test_cl_1over_block_size) {
  mdivide_right_tri_low_Ab_test(33);
}

TEST(MathMatrixCL, mdivide_right_tri_low_test_cl_big_power_of_2) {
  mdivide_right_tri_low_Ab_test(512);
}

TEST(MathMatrixCL, mdivide_right_tri_low_test_cl_big_non_power_of_2) {
  mdivide_right_tri_low_Ab_test(700);
}

TEST(MathMatrixCL, mdivide_right_tri_low_test_cl_very_big_non_power_of_2) {
  mdivide_right_tri_low_Ab_test(1500);
}
#endif


/* Copyright (c) 2016, the Cap authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#define BOOST_TEST_MODULE SuperCapacitor:

#include "main.cc"

#include <cap/energy_storage_device.h>
#include <boost/test/unit_test.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/info_parser.hpp>
#include <boost/format.hpp>
#include <memory>
#include <iostream>
#include <fstream>

namespace cap
{

void check_sanity(std::shared_ptr<cap::EnergyStorageDevice> dev)
{
  for (auto imposed_current : {10e-3, 5e-3, 2e-3})
  {
    dev->evolve_one_time_step_constant_current(2.0, imposed_current);
    double measured_current;
    dev->get_current(measured_current);
    BOOST_TEST(imposed_current == measured_current);
  }

  for (auto imposed_voltage : {1.4, 1.6, 1.8, 2.0, 2.2})
  {
    dev->evolve_one_time_step_constant_voltage(2.0, imposed_voltage);
    double measured_voltage;
    dev->get_voltage(measured_voltage);
    BOOST_TEST(imposed_voltage == measured_voltage);
  }

  for (auto imposed_power : {
           1e-3, 2e-3,
       })
  {
    dev->evolve_one_time_step_constant_power(2.0, imposed_power);
    double measured_voltage;
    dev->get_voltage(measured_voltage);
    double measured_current;
    dev->get_current(measured_current);
    BOOST_TEST(imposed_power == measured_voltage * measured_current);
  }

  // TODO constant load is not implemented yet
  // for (auto imposed_load : { 100.0, 33.0, })
  // {
  //     dev->evolve_one_time_step_constant_load(2.0, imposed_load);
  //     double measured_voltage;
  //     dev->get_voltage(measured_voltage);
  //     double measured_current;
  //     dev->get_current(measured_current);
  //     BOOST_TEST(imposed_load == - measured_voltage / measured_current);
  // }
}

} // end namespace cap

double constexpr relative_tolerance = 1.0e-2;

BOOST_AUTO_TEST_CASE(test_supercapacitor,
                     *boost::unit_test::tolerance(relative_tolerance))
{
  // build an energy storage device
  boost::property_tree::ptree ptree;
  boost::property_tree::info_parser::read_info("super_capacitor.info", ptree);
  boost::mpi::communicator world;
  std::shared_ptr<cap::EnergyStorageDevice> supercap =
      cap::EnergyStorageDevice::build(ptree, world);

  // check sanity
  cap::check_sanity(supercap);
}


//
// For licensing see accompanying LICENSE.txt file.
// Copyright (C) 2020 Apple Inc. All Rights Reserved.
//

#include <cassert>
#include <iostream>
#include <string>

#include <ApproxMVBB/ComputeApproxMVBB.hpp>

#include <args.hxx>

#include <armadillo>

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

    //
    // parse input arguments
    //

    args::ArgumentParser parser("generate_oriented_bounding_box", "");

    args::HelpFlag               help                                     (parser, "__DUMMY__",                            "Display this help menu",               {'h', "help"});
    args::ValueFlag<std::string> points_file_arg                          (parser, "POINTS_FILE",                          "points_file",                          {"points_file"},                          args::Options::Required);
    args::ValueFlag<double>      n_epsilon_arg                            (parser, "N_EPSILON",                            "n_epsilon",                            {"n_epsilon"},                            args::Options::Required);
    args::ValueFlag<int>         n_point_samples_arg                      (parser, "N_POINT_SAMPLES",                      "n_point_samples",                      {"n_point_samples"},                      args::Options::Required);
    args::ValueFlag<int>         n_grid_size_arg                          (parser, "N_GRID_SIZE",                          "n_grid_size",                          {"n_grid_size"},                          args::Options::Required);
    args::ValueFlag<int>         n_diam_opt_loops_arg                     (parser, "N_DIAM_OPT_LOOPS",                     "n_diam_opt_loops",                     {"n_diam_opt_loops"},                     args::Options::Required);
    args::ValueFlag<int>         n_grid_search_opt_loops_arg              (parser, "N_GRID_SEARCH_OPT_LOOPS",              "n_grid_search_opt_loops",              {"n_grid_search_opt_loops"},              args::Options::Required);
    args::ValueFlag<std::string> output_bounding_box_center_file_arg      (parser, "OUTPUT_BOUNDING_BOX_CENTER_FILE",      "output_bounding_box_center_file",      {"output_bounding_box_center_file"},      args::Options::Required);
    args::ValueFlag<std::string> output_bounding_box_extent_file_arg      (parser, "OUTPUT_BOUNDING_BOX_EXTENT_FILE",      "output_bounding_box_extent_file",      {"output_bounding_box_extent_file"},      args::Options::Required);
    args::ValueFlag<std::string> output_bounding_box_orientation_file_arg (parser, "OUTPUT_BOUNDING_BOX_ORIENTATION_FILE", "output_bounding_box_orientation_file", {"output_bounding_box_orientation_file"}, args::Options::Required);
    args::Flag                   silent_arg                               (parser, "__DUMMY__",                            "silent",                               {"silent"});

    try {
        parser.ParseCLI(argc, argv);
    } catch (args::Completion e) {
        std::cout << parser;
        return 1;
    } catch (args::Help e) {
        std::cout << parser;
        return 1;
    } catch (args::ParseError e) {
        std::cout << parser;
        std::cout << std::endl << std::endl << std::endl << e.what() << std::endl << std::endl << std::endl;
        return 1;
    } catch (args::RequiredError e) {
        std::cout << parser;
        std::cout << std::endl << std::endl << std::endl << e.what() << std::endl << std::endl << std::endl;
        return 1;
    }

    auto points_file                          = args::get(points_file_arg);
    auto n_epsilon                            = args::get(n_epsilon_arg);
    auto n_point_samples                      = args::get(n_point_samples_arg);
    auto n_grid_size                          = args::get(n_grid_size_arg);
    auto n_diam_opt_loops                     = args::get(n_diam_opt_loops_arg);
    auto n_grid_search_opt_loops              = args::get(n_grid_search_opt_loops_arg);
    auto output_bounding_box_center_file      = args::get(output_bounding_box_center_file_arg);
    auto output_bounding_box_extent_file      = args::get(output_bounding_box_extent_file_arg);
    auto output_bounding_box_orientation_file = args::get(output_bounding_box_orientation_file_arg);
    auto silent                               = args::get(silent_arg);

    arma::mat points;

    if (!silent) {
        std::cout << "[HYPERSIM: GENERATE_ORIENTED_BOUNDING_BOXES] Begin..." << std::endl;
    }

    //
    // load input data
    //

    points.load(points_file, arma::hdf5_binary_trans);

    assert(points.n_cols == 2 || points.n_cols == 3);

    //
    // compute bounding box
    //

    arma::vec bounding_box_center_world, bounding_box_extent_world;
    arma::mat bounding_box_R_world_from_obj;

    if (points.n_cols == 2) {

        // Note that ApproxMVBB assumes that each point is a column, which is different to our convention in Hypersim, so we transpose here.
        ApproxMVBB::Matrix2Dyn points_amvbb(2, points.n_rows);
        for (int i = 0; i < points.n_rows; i++) {
            points_amvbb.col(i) << points(i,0), points(i,1);
        }

        // see https://github.com/gabyx/ApproxMVBB for details
        ApproxMVBB::MinAreaRectangle amvbb_min_area_rectangle(points_amvbb);
        amvbb_min_area_rectangle.compute();
        auto rect = amvbb_min_area_rectangle.getMinRectangle();

        auto rect_center = rect.m_p + 0.5*rect.m_u*rect.m_uL + 0.5*rect.m_v*rect.m_vL;

        arma::vec bounding_box_extent_obj_({rect.m_uL, rect.m_vL});
        arma::mat bounding_box_R_world_from_obj_({
            {rect.m_u(0), rect.m_v(0)},
            {rect.m_u(1), rect.m_v(1)}});

        bounding_box_center_world = {rect_center(0), rect_center(1)};

        // Our convention is as follows: We set the x-axis to be the longest axis. We set the positive direction
        // of the x-axis to be the direction that points roughly towards the center of mass of the points. We set
        // the y-axis to be the positive x-axis rotated by positive 90 degrees.

        arma::vec points_mean_world = arma::mean(points, 0);
        arma::vec points_bbcenter_to_mean = points_mean_world - bounding_box_center_world;

        arma::uvec sorted_axes = sort_index(bounding_box_extent_obj_, "descend");
        arma::vec axis_0 = bounding_box_R_world_from_obj_.col(sorted_axes(0));
        auto axis_0_length = bounding_box_extent_obj_(sorted_axes(0));
        if (arma::dot(axis_0, points_bbcenter_to_mean) < 0) {
            axis_0 = -axis_0;
        }

        arma::mat R({
            {0, -1},
            {1,  0}});
        arma::vec axis_1 = R*axis_0; // positive rotation by 90 degrees
        auto axis_1_length = bounding_box_extent_obj_(sorted_axes(1));

        bounding_box_extent_world = {axis_0_length, axis_1_length};
        bounding_box_R_world_from_obj = {
            {axis_0(0), axis_1(0)},
            {axis_0(1), axis_1(1)}};
    }

    if (points.n_cols == 3) {

        // Note that ApproxMVBB assumes that each point is a column, which is different to our convention in Hypersim, so we transpose here.
        ApproxMVBB::Matrix3Dyn points_amvbb(3, points.n_rows);
        for (int i = 0; i < points.n_rows; i++) {
            points_amvbb.col(i) << points(i,0), points(i,1), points(i,2);
        }

        // suppress overly verbose output
        auto std_cout_rdbuf = std::cout.rdbuf();
        std::ostringstream std_ostringstream;
        if (silent) {
            std::cout.rdbuf(std_ostringstream.rdbuf());
        }

        // see https://github.com/gabyx/ApproxMVBB for details
        ApproxMVBB::OOBB oobb = ApproxMVBB::approximateMVBB(points_amvbb, n_epsilon, n_point_samples, n_grid_size, n_diam_opt_loops, n_grid_search_opt_loops);

        if (silent) {
            std::cout.rdbuf(std_cout_rdbuf);
        }

        // make all points inside the OOBB, see https://github.com/gabyx/ApproxMVBB for details
        ApproxMVBB::Matrix33 A_KI = oobb.m_q_KI.matrix().transpose();
        for(int i = 0; i < points_amvbb.cols(); i++) {
            oobb.unite(A_KI*points_amvbb.col(i));
        }

        arma::vec bounding_box_center_obj_({oobb.center()(0), oobb.center()(1), oobb.center()(2)});
        arma::vec bounding_box_extent_obj_({oobb.extent()(0), oobb.extent()(1), oobb.extent()(2)});
        arma::mat bounding_box_R_world_from_obj_({
            {oobb.m_q_KI.matrix()(0,0), oobb.m_q_KI.matrix()(0,1), oobb.m_q_KI.matrix()(0,2)},
            {oobb.m_q_KI.matrix()(1,0), oobb.m_q_KI.matrix()(1,1), oobb.m_q_KI.matrix()(1,2)},
            {oobb.m_q_KI.matrix()(2,0), oobb.m_q_KI.matrix()(2,1), oobb.m_q_KI.matrix()(2,2)}});

        bounding_box_center_world = bounding_box_R_world_from_obj_*bounding_box_center_obj_;

        // Our convention is as follows: We set the x-axis to be the longest axis. We set the positive direction
        // of the x-axis to be the direction that points roughly towards the center of mass of the points. We set
        // the y-axis to be the 2nd longest axis, and we set the positive direction of the y-axis to be the
        // direction that points roughly towards the center of mass of the points. We set the positive z-axis to
        // be x cross y.

        arma::vec points_mean_world = arma::mean(points, 0);
        arma::vec points_bbcenter_to_mean = points_mean_world - bounding_box_center_world;

        arma::uvec sorted_axes = sort_index(bounding_box_extent_obj_, "descend");
        arma::vec axis_0 = bounding_box_R_world_from_obj_.col(sorted_axes(0));
        auto axis_0_length = bounding_box_extent_obj_(sorted_axes(0));
        if (arma::dot(axis_0, points_bbcenter_to_mean) < 0) {
            axis_0 = -axis_0;
        }
        arma::vec axis_1 = bounding_box_R_world_from_obj_.col(sorted_axes(1));
        auto axis_1_length = bounding_box_extent_obj_(sorted_axes(1));
        if (arma::dot(axis_1, points_bbcenter_to_mean) < 0) {
            axis_1 = -axis_1;
        }

        arma::vec axis_2 = arma::cross(axis_0, axis_1);
        auto axis_2_length = bounding_box_extent_obj_(sorted_axes(2));

        bounding_box_extent_world = {axis_0_length, axis_1_length, axis_2_length};
        bounding_box_R_world_from_obj = {
            {axis_0(0), axis_1(0), axis_2(0)},
            {axis_0(1), axis_1(1), axis_2(1)},
            {axis_0(2), axis_1(2), axis_2(2)}};
    }

    //
    // save bounding box
    //

    if (!silent) {
        std::cout << "[HYPERSIM: GENERATE_ORIENTED_BOUNDING_BOXES] Saving bounding box results..." << std::endl;
    }

    bounding_box_center_world.save(output_bounding_box_center_file, arma::hdf5_binary_trans);
    bounding_box_extent_world.save(output_bounding_box_extent_file, arma::hdf5_binary_trans);
    bounding_box_R_world_from_obj.save(output_bounding_box_orientation_file, arma::hdf5_binary_trans);

    if (!silent) {
        std::cout << "[HYPERSIM: GENERATE_ORIENTED_BOUNDING_BOXES] Finished." << std::endl;
    }

    return 0;
}


/*
 *
 * Copyright (c) Toon Knapen & Kresimir Fresl 2003
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_HESV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_HESV_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/ilaenv.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    /////////////////////////////////////////////////////////////////////
    //
    // system of linear equations A * X = B
    // with A Hermitian indefinite matrix
    //
    /////////////////////////////////////////////////////////////////////

    namespace detail {

      inline
      integer_t hetrf_block (traits::complex_f,
                       integer_t const ispec, char const ul, integer_t const n)
      {
        char ul2[2] = "x"; ul2[0] = ul;
        return ilaenv (ispec, "CHETRF", ul2, n);
      }
      inline
      integer_t hetrf_block (traits::complex_d,
                       integer_t const ispec, char const ul, integer_t const n)
      {
        char ul2[2] = "x"; ul2[0] = ul;
        return ilaenv (ispec, "ZHETRF", ul2, n);
      }

    }

    template <typename HermA>
    integer_t hetrf_block (char const q, char const ul, HermA const& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::general_t
      >::value));
#endif
      assert (q == 'O' || q == 'M');
      assert (ul == 'U' || ul == 'L');

      integer_t n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermA>::value_type val_t;
#else
      typedef typename HermA::value_type val_t;
#endif
      integer_t ispec = (q == 'O' ? 1 : 2);
      return detail::hetrf_block (val_t(), ispec, ul, n);
    }

    template <typename HermA>
    integer_t hetrf_block (char const q, HermA const& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_t
      >::value));
#endif
      assert (q == 'O' || q == 'M');

      char ul = traits::matrix_uplo_tag (a);
      integer_t n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermA>::value_type val_t;
#else
      typedef typename HermA::value_type val_t;
#endif
      integer_t ispec = (q == 'O' ? 1 : 2);
      return detail::hetrf_block (val_t(), ispec, ul, n);
    }

    template <typename HermA>
    integer_t hetrf_work (char const q, char const ul, HermA const& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::general_t
      >::value));
#endif
      assert (q == 'O' || q == 'M');
      assert (ul == 'U' || ul == 'L');

      integer_t n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermA>::value_type val_t;
#else
      typedef typename HermA::value_type val_t;
#endif
      integer_t lw = -13;
      if (q == 'M')
        lw = 1;
      if (q == 'O')
        lw = n * detail::hetrf_block (val_t(), 1, ul, n);
      return lw;
    }

    template <typename HermA>
    integer_t hetrf_work (char const q, HermA const& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_t
      >::value));
#endif
      assert (q == 'O' || q == 'M');

      char ul = traits::matrix_uplo_tag (a);
      integer_t n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermA>::value_type val_t;
#else
      typedef typename HermA::value_type val_t;
#endif
      integer_t lw = -13;
      if (q == 'M')
        lw = 1;
      if (q == 'O')
        lw = n * detail::hetrf_block (val_t(), 1, ul, n);
      return lw;
    }

    template <typename HermA>
    inline
    integer_t hesv_work (char const q, char const ul, HermA const& a) {
      return hetrf_work (q, ul, a);
    }

    template <typename HermA>
    inline
    integer_t hesv_work (char const q, HermA const& a) { return hetrf_work (q, a); }

    /*
     * hesv() computes the solution to a system of linear equations
     * A * X = B, where A is an N-by-N Hermitian matrix and X and B
     * are N-by-NRHS matrices.
     *
     * The diagonal pivoting method is used to factor A as
     *   A = U * D * U^H,  if UPLO = 'U',
     *   A = L * D * L^H,  if UPLO = 'L',
     * where  U (or L) is a product of permutation and unit upper
     * (lower) triangular matrices, and D is Hermitian and block
     * diagonal with 1-by-1 and 2-by-2 diagonal blocks. The factored
     * form of A is then used to solve the system of equations A * X = B.
     */

    namespace detail {

      inline
      void hesv (char const uplo, integer_t const n, integer_t const nrhs,
                 traits::complex_f* a, integer_t const lda, integer_t* ipiv,
                 traits::complex_f* b, integer_t const ldb,
                 traits::complex_f* w, integer_t const lw, integer_t* info)
      {
        LAPACK_CHESV (&uplo, &n, &nrhs,
                      traits::complex_ptr (a), &lda, ipiv,
                      traits::complex_ptr (b), &ldb,
                      traits::complex_ptr (w), &lw, info);
      }

      inline
      void hesv (char const uplo, integer_t const n, integer_t const nrhs,
                 traits::complex_d* a, integer_t const lda, integer_t* ipiv,
                 traits::complex_d* b, integer_t const ldb,
                 traits::complex_d* w, integer_t const lw, integer_t* info)
      {
        LAPACK_ZHESV (&uplo, &n, &nrhs,
                      traits::complex_ptr (a), &lda, ipiv,
                      traits::complex_ptr (b), &ldb,
                      traits::complex_ptr (w), &lw, info);
      }

      template <typename HermA, typename MatrB, typename IVec, typename Work>
      int hesv (char const ul, HermA& a, IVec& i, MatrB& b, Work& w) {

        integer_t const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::matrix_size1 (b));
        assert (n == traits::vector_size (i));

        integer_t info;
        hesv (ul, n, traits::matrix_size2 (b),
              traits::matrix_storage (a),
              traits::leading_dimension (a),
              traits::vector_storage (i),
              traits::matrix_storage (b),
              traits::leading_dimension (b),
              traits::vector_storage (w),
              traits::vector_size (w),
              &info);
        return info;
      }

    }

    template <typename HermA, typename MatrB, typename IVec, typename Work>
    int hesv (char const ul, HermA& a, IVec& i, MatrB& b, Work& w) {

      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t const lw = traits::vector_size (w);
      assert (lw >= 1);
      return detail::hesv (ul, a, i, b, w);
    }

    template <typename HermA, typename MatrB, typename IVec, typename Work>
    int hesv (HermA& a, IVec& i, MatrB& b, Work& w) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t const lw = traits::vector_size (w);
      assert (lw >= 1);
      char uplo = traits::matrix_uplo_tag (a);
      return detail::hesv (uplo, a, i, b, w);
    }

    template <typename HermA, typename MatrB>
    int hesv (char const ul, HermA& a, MatrB& b) {
      // with 'internal' pivot and work vectors

      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t const n = traits::matrix_size1 (a);
      integer_t info = -101;
      traits::detail::array<integer_t> i (n);

      if (i.valid()) {
        info = -102;
        integer_t lw = hetrf_work ('O', ul, a);
        assert (lw >= 1); // paranoia ?
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
        typedef typename traits::matrix_traits<HermA>::value_type val_t;
#else
        typedef typename HermA::value_type val_t;
#endif
        traits::detail::array<val_t> w (lw);
        if (w.valid())
          info = detail::hesv (ul, a, i, b, w);
      }
      return info;
    }

    template <typename HermA, typename MatrB>
    int hesv (HermA& a, MatrB& b) {
      // with 'internal' pivot and work vectors

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t const n = traits::matrix_size1 (a);
      char uplo = traits::matrix_uplo_tag (a);
      integer_t info = -101;
      traits::detail::array<integer_t> i (n);

      if (i.valid()) {
        info = -102;
        integer_t lw = hetrf_work ('O', a);
        assert (lw >= 1); // paranoia ?
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
        typedef typename traits::matrix_traits<HermA>::value_type val_t;
#else
        typedef typename HermA::value_type val_t;
#endif
        traits::detail::array<val_t> w (lw);
        w.resize (lw);
        if (w.valid())
          info = detail::hesv (uplo, a, i, b, w);
      }
      return info;
    }

    /*
     * hetrf() computes the factorization of a Hermitian matrix A using
     * the  Bunch-Kaufman diagonal pivoting method. The form of the
     * factorization is
     *    A = U * D * U^H  or  A = L * D * L^H
     * where U (or L) is a product of permutation and unit upper (lower)
     * triangular matrices, and D is Hermitian and block diagonal with
     * 1-by-1 and 2-by-2 diagonal blocks.
     */

    namespace detail {

      inline
      void hetrf (char const uplo, integer_t const n,
                  traits::complex_f* a, integer_t const lda, integer_t* ipiv,
                  traits::complex_f* w, integer_t const lw, integer_t* info)
      {
        LAPACK_CHETRF (&uplo, &n,
                       traits::complex_ptr (a), &lda, ipiv,
                       traits::complex_ptr (w), &lw, info);
      }

      inline
      void hetrf (char const uplo, integer_t const n,
                  traits::complex_d* a, integer_t const lda, integer_t* ipiv,
                  traits::complex_d* w, integer_t const lw, integer_t* info)
      {
        LAPACK_ZHETRF (&uplo, &n,
                       traits::complex_ptr (a), &lda, ipiv,
                       traits::complex_ptr (w), &lw, info);
      }

      template <typename HermA, typename IVec, typename Work>
      int hetrf (char const ul, HermA& a, IVec& i, Work& w) {

        integer_t const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::vector_size (i));

        integer_t info;
        hetrf (ul, n, traits::matrix_storage (a),
               traits::leading_dimension (a),
               traits::vector_storage (i),
               traits::vector_storage (w),
               traits::vector_size (w),
               &info);
        return info;
      }

    }

    template <typename HermA, typename IVec, typename Work>
    int hetrf (char const ul, HermA& a, IVec& i, Work& w) {

      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      assert (traits::vector_size (w) >= 1);
      return detail::hetrf (ul, a, i, w);
    }

    template <typename HermA, typename IVec, typename Work>
    int hetrf (HermA& a, IVec& i, Work& w) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_t
      >::value));
#endif

      std::ptrdiff_t const lw = traits::vector_size (w);
      assert (lw >= 1);
      char uplo = traits::matrix_uplo_tag (a);
      return detail::hetrf (uplo, a, i, w);
    }

    template <typename HermA, typename Ivec>
    int hetrf (char const ul, HermA& a, Ivec& i) {
      // with 'internal' work vector

      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t info = -101;
      integer_t lw = hetrf_work ('O', ul, a);
      assert (lw >= 1); // paranoia ?
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermA>::value_type val_t;
#else
      typedef typename HermA::value_type val_t;
#endif
      traits::detail::array<val_t> w (lw);
      if (w.valid())
        info = detail::hetrf (ul, a, i, w);
      return info;
    }

    template <typename HermA, typename Ivec>
    int hetrf (HermA& a, Ivec& i) {
      // with 'internal' work vector

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_t
      >::value));
#endif

      char uplo = traits::matrix_uplo_tag (a);
      integer_t info = -101;
      integer_t lw = hetrf_work ('O', a);
      assert (lw >= 1); // paranoia ?
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<HermA>::value_type val_t;
#else
      typedef typename HermA::value_type val_t;
#endif
      traits::detail::array<val_t> w (lw);
      if (w.valid())
        info = detail::hetrf (uplo, a, i, w);
      return info;
    }

    /*
     * hetrs() solves a system of linear equations A*X = B with
     * a Hermitian matrix A using the factorization
     *    A = U * D * U^H   or  A = L * D * L^H
     * computed by hetrf().
     */

    namespace detail {

      inline
      void hetrs (char const uplo, integer_t const n, integer_t const nrhs,
                  traits::complex_f const* a, integer_t const lda,
                  integer_t const* ipiv,
                  traits::complex_f* b, integer_t const ldb, integer_t* info)
      {
        LAPACK_CHETRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (a), &lda, ipiv,
                       traits::complex_ptr (b), &ldb, info);
      }

      inline
      void hetrs (char const uplo, integer_t const n, integer_t const nrhs,
                  traits::complex_d const* a, integer_t const lda,
                  integer_t const* ipiv,
                  traits::complex_d* b, integer_t const ldb, integer_t* info)
      {
        LAPACK_ZHETRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (a), &lda, ipiv,
                       traits::complex_ptr (b), &ldb, info);
      }

      template <typename HermA, typename MatrB, typename IVec>
      int hetrs (char const ul, HermA const& a, IVec const& i, MatrB& b) {

        integer_t const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::matrix_size1 (b));
        assert (n == traits::vector_size (i));

        integer_t info;
        hetrs (ul, n, traits::matrix_size2 (b),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
               traits::matrix_storage (a),
#else
               traits::matrix_storage_const (a),
#endif
               traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
               traits::vector_storage (i),
#else
               traits::vector_storage_const (i),
#endif
               traits::matrix_storage (b),
               traits::leading_dimension (b), &info);
        return info;
      }

    }

    template <typename HermA, typename MatrB, typename IVec>
    inline
    int hetrs (char const ul, HermA const& a, IVec const& i, MatrB& b) {

      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      return detail::hetrs (ul, a, i, b);
    }

    template <typename HermA, typename MatrB, typename IVec>
    inline
    int hetrs (HermA const& a, IVec const& i, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<HermA>::matrix_structure,
        traits::hermitian_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      char uplo = traits::matrix_uplo_tag (a);
      return detail::hetrs (uplo, a, i, b);
    }

    // TO DO: hetri

  }

}}}

#endif


// Copyright (C) 2016-2018 T. Zachary Laine
//
// 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 <boost/yap/expression.hpp>

#include <chrono>
#include <iostream>

#include <benchmark/benchmark.h>

template<typename T>
using term = boost::yap::terminal<boost::yap::expression, T>;

namespace yap = boost::yap;
namespace bh = boost::hana;

//[ arithmetic_perf_decls
namespace user {

    struct number
    {
        double value;

        friend number operator+(number lhs, number rhs)
        {
            return number{lhs.value + rhs.value};
        }

        friend number operator*(number lhs, number rhs)
        {
            return number{lhs.value * rhs.value};
        }
    };
}
//]

double get_noise()
{
    auto const start_time = std::chrono::high_resolution_clock::now();
    auto const start_time_ns =
        std::chrono::time_point_cast<std::chrono::nanoseconds>(start_time);
    return 1.0 * start_time_ns.time_since_epoch().count();
}

user::number g_a{get_noise()};
user::number g_x{get_noise()};
user::number g_y{get_noise()};

//[ arithmetic_perf_eval_as_yap_expr
user::number eval_as_yap_expr(user::number a_, user::number x_, user::number y_)
{
    term<user::number> a{{a_}};
    term<user::number> x{{x_}};
    term<user::number> y{{y_}};
    auto expr = (a * x + y) * (a * x + y) + (a * x + y);
    return yap::evaluate(expr);
}
//]

void BM_eval_as_yap_expr(benchmark::State & state)
{
    double d = 0;
    while (state.KeepRunning()) {
        user::number const n = eval_as_yap_expr(g_a, g_x, g_y);
        d += n.value;
    }
    std::cout << "Sum of doubles=" << d << "\n";
}

//[ arithmetic_perf_eval_as_yap_expr_4x
user::number
eval_as_yap_expr_4x(user::number a_, user::number x_, user::number y_)
{
    term<user::number> a{{a_}};
    term<user::number> x{{x_}};
    term<user::number> y{{y_}};
    auto expr = (a * x + y) * (a * x + y) + (a * x + y) +
                (a * x + y) * (a * x + y) + (a * x + y) +
                (a * x + y) * (a * x + y) + (a * x + y) +
                (a * x + y) * (a * x + y) + (a * x + y);
    return yap::evaluate(expr);
}
//]

void BM_eval_as_yap_expr_4x(benchmark::State & state)
{
    double d = 0;
    while (state.KeepRunning()) {
        user::number const n = eval_as_yap_expr_4x(g_a, g_x, g_y);
        d += n.value;
    }
    std::cout << "Sum of doubles=" << d << "\n";
}

//[ arithmetic_perf_eval_as_cpp_expr
user::number eval_as_cpp_expr(user::number a, user::number x, user::number y)
{
    return (a * x + y) * (a * x + y) + (a * x + y);
}
//]

void BM_eval_as_cpp_expr(benchmark::State & state)
{
    double d = 0;
    while (state.KeepRunning()) {
        user::number const n = eval_as_cpp_expr(g_a, g_x, g_y);
        d += n.value;
    }
    std::cout << "Sum of doubles=" << d << "\n";
}

//[ arithmetic_perf_eval_as_cpp_expr_4x
user::number eval_as_cpp_expr_4x(user::number a, user::number x, user::number y)
{
    return (a * x + y) * (a * x + y) + (a * x + y) + (a * x + y) * (a * x + y) +
           (a * x + y) + (a * x + y) * (a * x + y) + (a * x + y) +
           (a * x + y) * (a * x + y) + (a * x + y);
}
//]

void BM_eval_as_cpp_expr_4x(benchmark::State & state)
{
    double d = 0;
    while (state.KeepRunning()) {
        user::number const n = eval_as_cpp_expr_4x(g_a, g_x, g_y);
        d += n.value;
    }
    std::cout << "Sum of doubles=" << d << "\n";
}

BENCHMARK(BM_eval_as_yap_expr);
BENCHMARK(BM_eval_as_yap_expr_4x);
BENCHMARK(BM_eval_as_cpp_expr);
BENCHMARK(BM_eval_as_cpp_expr_4x);

BENCHMARK_MAIN();


/* \author Bastian Steder */
#include <iostream>
#include <boost/thread/thread.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/console/parse.h>
typedef pcl::PointXYZ PointType;

float angular_resolution = 0.5f;
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
bool setUnseenToMaxRange = false;

void printUsage (const char* progName)
{
	std::cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
		<< "Options:\n"
		<< "-------------------------------------------\n"
		<< "-r <float>   angular resolution in degrees (default "<<angular_resolution<<")\n"
		<< "-c <int>     coordinate frame (default "<< (int)coordinate_frame<<")\n"
		<< "-m           Treat all unseen points to max range\n"
		<< "-h           this help\n"
		<< "\n\n";
}

int main (int argc, char** argv)
{
  if (pcl::console::find_argument (argc, argv, "-h") >= 0)
  {
    printUsage (argv[0]);
    return 0;
  }
  if (pcl::console::find_argument (argc, argv, "-m") >= 0)
  {
    setUnseenToMaxRange = true;
    cout << "Setting unseen values in range image to maximum range readings.\n";
  }
  int tmp_coordinate_frame;
  if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
  {
    coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
    cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
  }
  if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
    cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
  angular_resolution = pcl::deg2rad (angular_resolution);

  pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
  pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
  pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
  Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
  std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");
  if (!pcd_filename_indices.empty ())
  {
    std::string filename = argv[pcd_filename_indices[0]];
    if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
    {
      cout << "Was not able to open file \""<<filename<<"\".\n";
      printUsage (argv[0]);
      return 0;
    }
    scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0], point_cloud.sensor_origin_[1],
				point_cloud.sensor_origin_[2])) *Eigen::Affine3f (point_cloud.sensor_orientation_);
      std::string far_ranges_filename = pcl::getFilenameWithoutExtension (filename)+"_far_ranges.pcd";
    if (pcl::io::loadPCDFile(far_ranges_filename.c_str(), far_ranges) == -1)
      std::cout << "Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
  }
  else
  {
    cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
    for (float x=-0.5f; x<=0.5f; x+=0.01f)
    {
      for (float y=-0.5f; y<=0.5f; y+=0.01f)
      {
        PointType point;  point.x = x;  point.y = y;  point.z = 2.0f - y;
        point_cloud.points.push_back (point);
      }
    }
    point_cloud.width = (int) point_cloud.points.size ();  point_cloud.height = 1;
  }

  float noise_level = 0.0;
  float min_range = 0.0f;
  int border_size = 1;
  boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage);
  pcl::RangeImage& range_image = *range_image_ptr;
  range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f),
		pcl::deg2rad (180.0f), scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
  range_image.integrateFarRanges (far_ranges);
  if (setUnseenToMaxRange)
    range_image.setUnseenToMaxRange ();

  pcl::visualization::PCLVisualizer viewer ("3D Viewer");
  viewer.setBackgroundColor (1, 1, 1);
  viewer.addCoordinateSystem (1.0f);
  pcl::visualization::PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 0, 0, 0);
  viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
  //PointCloudColorHandlerCustom<pcl::PointWithRange>   range_image_color_handler (range_image_ptr, 150, 150, 150);
  //viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
  //viewer.setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 2, "range image");

  pcl::RangeImageBorderExtractor border_extractor (&range_image);
  pcl::PointCloud<pcl::BorderDescription> border_descriptions;
  border_extractor.compute (border_descriptions);

  pcl::PointCloud<pcl::PointWithRange>::Ptr border_points_ptr(new pcl::PointCloud<pcl::PointWithRange>),
		veil_points_ptr(new pcl::PointCloud<pcl::PointWithRange>), shadow_points_ptr(new pcl::PointCloud<pcl::PointWithRange>);
  pcl::PointCloud<pcl::PointWithRange>& border_points = *border_points_ptr, & veil_points = * veil_points_ptr, & shadow_points = *shadow_points_ptr;
  for (int y=0; y< (int)range_image.height; ++y)
  {
    for (int x=0; x< (int)range_image.width; ++x)
    {
      if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__OBSTACLE_BORDER])
        border_points.points.push_back (range_image.points[y*range_image.width + x]);
      if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__VEIL_POINT])
        veil_points.points.push_back (range_image.points[y*range_image.width + x]);
      if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__SHADOW_BORDER])
        shadow_points.points.push_back (range_image.points[y*range_image.width + x]);
    }
  }
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> border_points_color_handler (border_points_ptr, 0, 255, 0);
  viewer.addPointCloud<pcl::PointWithRange> (border_points_ptr, border_points_color_handler, "border points");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "border points");
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> veil_points_color_handler (veil_points_ptr, 255, 0, 0);
  viewer.addPointCloud<pcl::PointWithRange> (veil_points_ptr, veil_points_color_handler, "veil points");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "veil points");
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> shadow_points_color_handler (shadow_points_ptr, 0, 255, 255);
  viewer.addPointCloud<pcl::PointWithRange> (shadow_points_ptr, shadow_points_color_handler, "shadow points");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "shadow points");

  pcl::visualization::RangeImageVisualizer* range_image_borders_widget = NULL;
  range_image_borders_widget =
    pcl::visualization::RangeImageVisualizer::getRangeImageBordersWidget (range_image, -std::numeric_limits<float>::infinity (),
		std::numeric_limits<float>::infinity (), false, border_descriptions, "Range image with borders" );

  while (!viewer.wasStopped ())
  {
    range_image_borders_widget->spinOnce ();
    viewer.spinOnce ();
    pcl_sleep(0.01);
  }
}


#pragma once

#include <stdint.h>

#include <chrono>
#include <string>
#include <sstream>
#include <iomanip>
#include <vector>

#include <boost/algorithm/string.hpp>
#include <boost/date_time.hpp>

namespace mio
{
    namespace chrono
    {
        inline std::chrono::nanoseconds now()
        {
            using namespace std::chrono;
            return duration_cast<nanoseconds>(system_clock::now().time_since_epoch());
        }

        inline std::chrono::nanoseconds to_time(const std::string &time)
        {
            using namespace std::chrono_literals;
            std::vector<std::string> strs[4];
            boost::split(strs[0], time, boost::is_any_of(" "));

            boost::split(strs[1], strs[0][0], boost::is_any_of("-"));
            boost::posix_time::ptime end{boost::gregorian::date(atoll(strs[1].at(0).c_str()), atoll(strs[1].at(1).c_str()), atoll(strs[1].at(2).c_str()))};
            boost::posix_time::time_duration diff = end - boost::posix_time::ptime(boost::gregorian::date(1970, 1, 1));
            auto ret = std::chrono::nanoseconds(diff.total_nanoseconds()) - 8h;

            if (strs[0].size() >= 2)
            {
                boost::split(strs[2], strs[0][1], boost::is_any_of(":"));
                ret += std::stoull(strs[2][0]) * 1h;
                ret += std::stoull(strs[2][1]) * 1min;
                ret += std::stoull(strs[2][2]) * 1s;
            }

            if (strs[0].size() >= 3)
            {
                boost::split(strs[3], strs[0][2], boost::is_any_of(":"));
                ret += std::stoull(strs[3][0]) * 1ms;
                ret += std::stoull(strs[3][1]) * 1us;
                ret += std::stoull(strs[3][2]) * 1ns;
            }

            return ret;
        }

        class stopwatch
        {
        private:
            std::chrono::nanoseconds start_ = mio::chrono::now();

        public:
            std::chrono::nanoseconds operator()() const
            {
                return mio::chrono::now() - this->start_;
            }
        };
    } // namespace chrono

    template <typename Rep, typename Period>
    inline std::string to_string(const std::chrono::duration<Rep, Period> &rtime)
    {

        using namespace std::chrono;
        using namespace std::chrono_literals;

        unsigned long long ms = (unsigned long long)(rtime % 1s / 1ms);
        unsigned long long us = (unsigned long long)(rtime % 1ms / 1us);
        unsigned long long ns = (unsigned long long)(rtime % 1us / 1ns);

        std::stringstream ss;
        std::chrono::time_point<std::chrono::system_clock> tt(std::chrono::duration_cast<std::chrono::milliseconds>(rtime));
        auto t = std::chrono::system_clock::to_time_t(tt);
        ss << std::put_time(std::localtime(&t), "%F %T ");

        char buf[128];
        sprintf(buf, "%03llu:%03llu:%03llu", ms, us, ns);

        ss << buf;
        return ss.str();
    }
} // namespace mio