//
// hqn-cli.cpp
// ~~~~~~~~~~
//
// Copyright (c) 2003-2010 Christopher M. Kohlhoff (chris at kohlhoff dot com)
//
// 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 <cstdlib>
#include <iostream>
#include <boost/bind.hpp>
#include <boost/asio.hpp>
#include <boost/asio/ssl.hpp>
#include <boost/program_options.hpp>
#include <boost/program_options/options_description.hpp>

#include "../client/hqn-client.h"
#include "console.hpp"

unsigned info(const std::vector<std::string>&) {
	std::cout
			<< "Welcome to the example console. This command does not really\n"
			<< "do anything aside from printing this statement. Thus it does\n"
			<< "not need to look into the arguments that are passed to it.\n";
	return hqn::cli::console::Ok;
}

// In this command we implement a basic calculator
unsigned calc(const std::vector<std::string> &input) {
	if (input.size() != 4) {
		// The first element of the input array is always the name of the
		// command as registered in the console.
		std::cout << "Usage: " << input[0] << " num1 operator num2\n";
		// We can return an arbitrary error code, which we can catch later
		// as Console will return it.
		return hqn::cli::console::Error;
	}
	double num1 = std::stod(input[1]), num2 = std::stod(input[3]);

	char op = input[2][0];

	double result;
	switch (op) {
	case '*':
		result = num1 * num2;
		break;
	case '+':
		result = num1 + num2;
		break;
	case '/':
		result = num1 / num2;
		break;
	case '-':
		result = num1 - num2;
		break;
	default:
		std::cout << "The inserted operator is not supported\n";
		// Again, we can return an arbitrary error code to catch it later.
		return 2;
	}
	std::cout << "Result: " << result << '\n';
	return hqn::cli::console::Ok;
}

int main(int argc, char *argv[]) {
	try {

		boost::program_options::options_description desc("Options");
		desc.add_options()("help", "")("version", "")("host",
				boost::program_options::value<std::string>()->default_value(
						"localhost"))("port",
				boost::program_options::value<std::string>()->default_value(
						std::to_string(hqn::config::defaults::server_port)));

		boost::program_options::variables_map vm;
		boost::program_options::store(
				boost::program_options::parse_command_line(argc, argv, desc),
				vm);
		boost::program_options::notify(vm);

		if (vm.size() > 0) {
			if (vm.count("help") == 1) {
				hqn::config::show_help(false);

			} else if (vm.count("version") == 1) {
				hqn::config::show_version();

			} else if (vm.size() == 2) {
				hqn::wrap w(
						boost::any_cast<std::string>(vm["host"].value()).c_str(),
						boost::any_cast<std::string>(vm["port"].value()).c_str());
				hqn::config::show_version();
				hqn::cli::console c(">");
				c.registerCommand("info", info);
				c.registerCommand("calc", calc);
				c.executeCommand("help");
				c.executeFile("exampleScript");
				int retCode = 0;
				do {
					retCode = c.readLine();
					// We can also change the prompt based on last return value:
					if (retCode == hqn::cli::console::Ok)
						c.setGreeting(">");
					else
						c.setGreeting("!>");

					if (retCode == 1) {
						std::cout << "Received error code 1\n";
					} else if (retCode == 2) {
						std::cout << "Received error code 2\n";
					}
				} while (retCode != hqn::cli::console::Quit);
			} else {
				hqn::config::show_help(false);
			}
		}
	} catch (boost::system::error_code &e) {
		std::cerr << "Exception: " << e.message() << "\n";
	}

	return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 20012 - 2012   LRI    UMR 12623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_FACT_12_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_FACT_12_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/config.hpp>

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4310) // truncation of constant
#endif

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Fact_12 generic tag

     Represents the Fact_12 constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Fact_12,double
                                , 479001600,0x4de467e0, 0x41bc8cfc00000000ll
                                )
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::Fact_12, Site> dispatching_Fact_12(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::Fact_12, Site>();
   }
   template<class... Args>
   struct impl_Fact_12;
  }
  /*!
    Generates 12! that is 479001600

    @par Semantic:

    @code
    T r = Fact_12<T>();
    @endcode

    is similar to:

    @code
    T r = T(479001600);
    @endcode

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Fact_12, Fact_12)
} }

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#include <boost/simd/constant/common.hpp>

#endif


#include <boost/test/unit_test.hpp>
#include <boost/test/test_tools.hpp>
#include <boost/test/floating_point_comparison.hpp>
using namespace boost::unit_test;

#include <cairomm/matrix.h>

// this is necessary for BOOST_CHECK_EQUAL, but there's no equivalent in the C
// API, so I'm reluctant to include it in cairomm right now
bool operator==(const Cairo::Matrix& A, const Cairo::Matrix& B)
{
  return
    A.xx == B.xx &&
    A.yx == B.yx &&
    A.xy == B.xy &&
    A.yy == B.yy &&
    A.x0 == B.x0 &&
    A.y0 == B.y0;
}

// this is necessary for BOOST_CHECK_EQUAL to work but doesn't seem useful
// enough to put in the actual implementation
std::ostream& operator<<(std::ostream& out, const Cairo::Matrix& matrix)
{
  return out << "[ "
    << matrix.xx << ", "
    << matrix.yx << ", "
    << matrix.xy << ", "
    << matrix.yy << ", "
    << matrix.x0 << ", "
    << matrix.y0 << " ]";
}

void test_constructors()
{
  cairo_matrix_t c_identity;
  cairo_matrix_init_identity(&c_identity);
  BOOST_CHECK_EQUAL(c_identity.xx, Cairo::identity_matrix().xx);
  BOOST_CHECK_EQUAL(c_identity.xy, Cairo::identity_matrix().xy);
  BOOST_CHECK_EQUAL(c_identity.yx, Cairo::identity_matrix().yx);
  BOOST_CHECK_EQUAL(c_identity.yy, Cairo::identity_matrix().yy);
  BOOST_CHECK_EQUAL(c_identity.x0, Cairo::identity_matrix().x0);
  BOOST_CHECK_EQUAL(c_identity.y0, Cairo::identity_matrix().y0);

  // nonsense values, just for testing
  const double xx=1, yx=2, xy=3, yy=5, x0=6, y0=7;
  cairo_matrix_t c_matrix;
  cairo_matrix_init(&c_matrix, xx, yx, xy, yy, x0, y0);
  Cairo::Matrix cpp_matrix(xx, yx, xy, yy, x0, y0);

  BOOST_CHECK_EQUAL(c_matrix.xx, cpp_matrix.xx);
  BOOST_CHECK_EQUAL(c_matrix.xy, cpp_matrix.xy);
  BOOST_CHECK_EQUAL(c_matrix.yx, cpp_matrix.yx);
  BOOST_CHECK_EQUAL(c_matrix.yy, cpp_matrix.yy);
  BOOST_CHECK_EQUAL(c_matrix.x0, cpp_matrix.x0);
  BOOST_CHECK_EQUAL(c_matrix.y0, cpp_matrix.y0);
}

void test_invert()
{
  // test a valid matrix
  BOOST_CHECK_NO_THROW(Cairo::identity_matrix().invert());
  // check a degenerate matrix
  Cairo::Matrix degenerate(0,0,0,0,0,0);
  BOOST_CHECK_THROW(degenerate.invert(), std::logic_error);
}

cairo_matrix_t* test_matrix = nullptr;
static void foo(cairo_matrix_t* matrix)
{
  test_matrix = matrix;
}

void test_cast()
{
  // make sure that we can cast between C++ and C types without ill effect
  auto matrix = Cairo::identity_matrix();
  auto casted = (cairo_matrix_t) Cairo::identity_matrix();
  // check that it's equal to the identity matrix
  cairo_matrix_t identity;
  cairo_matrix_init_identity(&identity);

  BOOST_CHECK_EQUAL(casted.xx, identity.xx);
  BOOST_CHECK_EQUAL(casted.xy, identity.xy);
  BOOST_CHECK_EQUAL(casted.yx, identity.yx);
  BOOST_CHECK_EQUAL(casted.yy, identity.yy);
  BOOST_CHECK_EQUAL(casted.x0, identity.x0);
  BOOST_CHECK_EQUAL(casted.y0, identity.y0);

  // pass C++ type as an argument to C
  foo(&matrix);
  BOOST_CHECK_EQUAL(matrix.xx, test_matrix->xx);
  BOOST_CHECK_EQUAL(matrix.xy, test_matrix->xy);
  BOOST_CHECK_EQUAL(matrix.yx, test_matrix->yx);
  BOOST_CHECK_EQUAL(matrix.yy, test_matrix->yy);
  BOOST_CHECK_EQUAL(matrix.x0, test_matrix->x0);
  BOOST_CHECK_EQUAL(matrix.y0, test_matrix->y0);
}

void test_multiply()
{
  auto A = Cairo::scaling_matrix(2, 4);
  auto B = Cairo::translation_matrix(5.3, 1.2);
  auto C = A * B;
  Cairo::Matrix D;
  D.multiply(A, B);
  BOOST_CHECK_EQUAL(C, D);
}

test_suite*
init_unit_test_suite(int /*argc*/, char** /*argv*/)
{
  test_suite* test= BOOST_TEST_SUITE( "Cairo::Matrix Tests" );

  test->add (BOOST_TEST_CASE (&test_constructors));
  test->add (BOOST_TEST_CASE (&test_invert));
  test->add (BOOST_TEST_CASE (&test_cast));
  test->add (BOOST_TEST_CASE (&test_multiply));

  return test;
}


#pragma once

#include <Eigen/Dense>
#include <PnC/WBC/WBC.hpp>
#include <PnC/WBC/ContactSpec.hpp>
#include <Utils/IO/IOUtilities.hpp>

class Task;

class OSC: public WBC{
    public:
        OSC (const std::vector<bool> & act_list, const Eigen::MatrixXd* Jci = NULL);
        virtual ~OSC (){}
        virtual void updateSetting(const Eigen::MatrixXd & A,
                const Eigen::MatrixXd & Ainv,
                const Eigen::VectorXd & cori,
                const Eigen::VectorXd & grav,
                void* extra_setting = NULL);

        virtual void makeTorque(
                const std::vector<Task*> & task_list,
                const std::vector<ContactSpec*> & contact_list,
                Eigen::VectorXd & cmd,
                void* extra_input = NULL);

    private:
};


#define BOOST_TEST_MODULE pcraster geo raster_dim
#include <boost/test/unit_test.hpp>
#include "geo_rasterdim.h"
#include "geo_neighbour.h"

BOOST_AUTO_TEST_CASE(target)
{
  using namespace geo;

  RasterDim rd(3,3);

  // all NBS are inside grid
  LinearLoc f=rd.convert(CellLoc(1,1));
  BOOST_CHECK(f==4);

  BOOST_CHECK(rd.target<LDD>(f,7)==0);
  BOOST_CHECK(rd.target<LDD>(f,3)==8);
  BOOST_CHECK(rd.target<LDD>(f,5)==4);

  BOOST_CHECK(rd.target<NB>(f,2)==8);
/*
  std::cout << "val " << rd.target<LDD>(f,7) << " \n";
  BOOST_CHECK(rd.target<NB>(f,1, false));
  BOOST_CHECK(rd.target<NB>(f,2, false));
  BOOST_CHECK(rd.target<NB>(f,3, false));
  BOOST_CHECK(rd.target<NB>(f,4, false));
  BOOST_CHECK(rd.target<NB>(f,5, false));
  BOOST_CHECK(rd.target<NB>(f,6, false));
*/
  //BOOST_CHECK(nbTarget(f,rd,7, false));
/*
  // left NBs are outside
  f=rd.convert(CellLoc(1,0));

  BOOST_CHECK(lddTarget(f,rd,1, true ));
  BOOST_CHECK(lddTarget(f,rd,2, false));
  BOOST_CHECK(lddTarget(f,rd,3, false));
  BOOST_CHECK(lddTarget(f,rd,4, true ));
  BOOST_CHECK(lddTarget(f,rd,5, false));
  BOOST_CHECK(lddTarget(f,rd,6, false));
  BOOST_CHECK(lddTarget(f,rd,7, true ));
  BOOST_CHECK(lddTarget(f,rd,8, false));
  BOOST_CHECK(lddTarget(f,rd,9, false));

  // top/left NBs are outside
  f=rd.convert(CellLoc(1,0));

  BOOST_CHECK(lddTarget(f,rd,1, true ));
  BOOST_CHECK(lddTarget(f,rd,2, false));
  BOOST_CHECK(lddTarget(f,rd,3, false));
  BOOST_CHECK(lddTarget(f,rd,4, true ));
  BOOST_CHECK(lddTarget(f,rd,5, false));
  BOOST_CHECK(lddTarget(f,rd,6, false));
  BOOST_CHECK(lddTarget(f,rd,7, true ));
  BOOST_CHECK(lddTarget(f,rd,8, true ));
  BOOST_CHECK(lddTarget(f,rd,9, true ));

  // bottom/right NBs are outside
  f=rd.convert(CellLoc(rd.nrRows()-1,rd.nrCols()-1));

  BOOST_CHECK(lddTarget(f,rd,1, true ));
  BOOST_CHECK(lddTarget(f,rd,2, true ));
  BOOST_CHECK(lddTarget(f,rd,3, true ));
  BOOST_CHECK(lddTarget(f,rd,4, false));
  BOOST_CHECK(lddTarget(f,rd,5, false));
  BOOST_CHECK(lddTarget(f,rd,6, true ));
  BOOST_CHECK(lddTarget(f,rd,7, false));
  BOOST_CHECK(lddTarget(f,rd,8, false));
  BOOST_CHECK(lddTarget(f,rd,9, true ));
*/
}


// Copyright 2020 the Autoware Foundation
//
// 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#ifndef NDT__UTILS_HPP_
#define NDT__UTILS_HPP_

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <geometry_msgs/msg/transform.hpp>
#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
#include <helper_functions/float_comparisons.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <Eigen/Eigenvalues>
#include <algorithm>
#include <limits>
#include <tuple>

namespace autoware
{
namespace localization
{
namespace ndt
{

struct PointWithCovariances
{
  double x;
  double y;
  double z;
  double icov_xx;
  double icov_xy;
  double icov_xz;
  double icov_yy;
  double icov_yz;
  double icov_zz;
  friend bool operator==(const PointWithCovariances & p1, const PointWithCovariances & p2)
  {
    constexpr auto eps = std::numeric_limits<double>::epsilon();
    return common::helper_functions::comparisons::rel_eq(p1.x, p2.x, eps) &&
           common::helper_functions::comparisons::rel_eq(p1.y, p2.y, eps) &&
           common::helper_functions::comparisons::rel_eq(p1.z, p2.z, eps) &&
           common::helper_functions::comparisons::rel_eq(p1.icov_xx, p2.icov_xx, eps) &&
           common::helper_functions::comparisons::rel_eq(p1.icov_xy, p2.icov_xy, eps) &&
           common::helper_functions::comparisons::rel_eq(p1.icov_xz, p2.icov_xz, eps) &&
           common::helper_functions::comparisons::rel_eq(p1.icov_yy, p2.icov_yy, eps) &&
           common::helper_functions::comparisons::rel_eq(p1.icov_yz, p2.icov_yz, eps) &&
           common::helper_functions::comparisons::rel_eq(p1.icov_zz, p2.icov_zz, eps);
  }
};
LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_xx);
LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_xy);
LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_xz);
LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_yy);
LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_yz);
LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_zz);

using PointWithCovariancesFieldGenerators = std::tuple<
  point_cloud_msg_wrapper::field_x_generator,
  point_cloud_msg_wrapper::field_y_generator,
  point_cloud_msg_wrapper::field_z_generator,
  field_icov_xx_generator,
  field_icov_xy_generator,
  field_icov_xz_generator,
  field_icov_yy_generator,
  field_icov_yz_generator,
  field_icov_zz_generator>;

using NdtMapCloudModifier =
  point_cloud_msg_wrapper::PointCloud2Modifier<PointWithCovariances,
    PointWithCovariancesFieldGenerators>;

using NdtMapCloudView =
  point_cloud_msg_wrapper::PointCloud2View<PointWithCovariances,
    PointWithCovariancesFieldGenerators>;

/// This function will check if the covariance is valid based on its eigenvalues. If the covariance
/// is valid, eigen values smaller than a fraction of the biggest eigen value will be capped to the
/// threshold. Covariance then will be reconstructed from the modified set of eigen values and
/// vectors. This should result in increased numerical stability as stated in [Magnusson 2009].
/// If the covariance is invalid, it will not be updated and false will be returned.
/// \tparam Derived Deduced Eigen Matrix type.
/// \param covariance [in, out] Covariance matrix to get stabilized.
/// \param scaling_factor [in] The ratio between the max. eigen value and the minimum
/// allowed eigenvalue. Default value is 0.01 as suggested in [Magnusson 2009], page 60.
/// \return True if the covariance matrix is valid.
template<typename Derived>
bool try_stabilize_covariance(
  Eigen::MatrixBase<Derived> & covariance,
  typename Derived::PlainMatrix::Scalar scaling_factor = 0.1)
{
  using CovMatrixT = typename Derived::PlainMatrix;
  using ScalarT = typename CovMatrixT::Scalar;
  using IndexT = typename CovMatrixT::Index;
  using SolverT = Eigen::SelfAdjointEigenSolver<CovMatrixT>;
  using VectorT = typename SolverT::RealVectorType;

  Eigen::SelfAdjointEigenSolver<CovMatrixT> solver(covariance);
  if (solver.info() != Eigen::Success) {return false;}

  VectorT eigen_values = solver.eigenvalues();  // Sorted in increasing order.
  const auto min_eigen_value = eigen_values[0];
  const auto max_eigen_value = eigen_values[eigen_values.size() - IndexT{1U}];
  const auto stabilized_min_eigen_value = scaling_factor * max_eigen_value;
  if (stabilized_min_eigen_value < std::numeric_limits<ScalarT>::epsilon()) {return false;}
  if (min_eigen_value > scaling_factor * max_eigen_value) {return true;}  // Already stable.
  for (auto i = IndexT{0U}; i < eigen_values.size(); ++i) {
    eigen_values[i] = std::max(eigen_values[i], stabilized_min_eigen_value);
  }
  covariance =
    solver.eigenvectors() * eigen_values.asDiagonal() * solver.eigenvectors().transpose();
  return true;
}

template<typename T>
using EigenPose = Eigen::Matrix<T, 6U, 1U>;
template<typename T>
using EigenTransform = Eigen::Transform<T, 3, Eigen::Affine, Eigen::ColMajor>;
using RosTransform = geometry_msgs::msg::Transform;
using RosPose = geometry_msgs::msg::Pose;
namespace transform_adapters
{
/// Template function to convert a 6D pose to a transformation matrix.
/// This function should be specialized and implemented for the supported types.
/// \tparam PoseT Pose type.
/// \tparam TransformT Transform type.
/// \param[in] pose pose to convert
/// \param[out] transform resulting transform
template<typename PoseT, typename TransformT>
void pose_to_transform(const PoseT & pose, TransformT & transform);

/// Template function to convert a 6D pose to a transformation matrix.
/// This function should be specialized and implemented for the supported types.
/// \tparam PoseT Pose type.
/// \tparam TransformT Transform type.
/// \param[in] transform resulting transform
/// \param[out] pose pose to convert
template<typename PoseT, typename TransformT>
void transform_to_pose(const TransformT & transform, PoseT & pose);

template<typename T>
void pose_to_transform(
  const EigenPose<T> & pose,
  EigenTransform<T> & transform)
{
  static_assert(std::is_floating_point<T>::value, "Eigen transform should use floating points");
  transform.setIdentity();
  transform.translation() = pose.head(3);
  transform.rotate(Eigen::AngleAxis<T>(pose(3), Eigen::Vector3d::UnitX()));
  transform.rotate(Eigen::AngleAxis<T>(pose(4), Eigen::Vector3d::UnitY()));
  transform.rotate(Eigen::AngleAxis<T>(pose(5), Eigen::Vector3d::UnitZ()));
}

/// Specialization to convert from the eigen pose to the ros transform type.
/// \tparam T Eigen scalar type
template<typename T>
void pose_to_transform(
  const EigenPose<T> & pose,
  RosTransform & transform)
{
  static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
  Eigen::Quaternion<T> eig_rot{Eigen::Quaternion<T>{}.setIdentity()};
  eig_rot.setIdentity();
  eig_rot =
    Eigen::AngleAxis<T>(pose(3), Eigen::Matrix<T, 3, 1>::UnitX()) *
    Eigen::AngleAxis<T>(pose(4), Eigen::Matrix<T, 3, 1>::UnitY()) *
    Eigen::AngleAxis<T>(pose(5), Eigen::Matrix<T, 3, 1>::UnitZ());

  decltype(RosTransform::translation) trans;
  decltype(RosTransform::rotation) rot;

  trans.set__x(pose(0)).set__y(pose(1)).set__z(pose(2));
  transform.set__translation(trans);

  rot.set__x(eig_rot.x()).
  set__y(eig_rot.y()).
  set__z(eig_rot.z()).
  set__w(eig_rot.w());
  transform.set__rotation(rot);
}

/// Specialization to convert from the eigen pose to the ros pose type.
/// `pose_to_transform` template is used as conversion to `RosPose`
/// is identical to conversion to `RosTransform`
/// \tparam T Eigen scalar type
template<typename T>
void pose_to_transform(
  const EigenPose<T> & pose,
  RosPose & ros_pose)
{
  static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
  Eigen::Quaternion<T> eig_rot{Eigen::Quaternion<T>{}.setIdentity()};
  eig_rot =
    Eigen::AngleAxis<T>(pose(3), Eigen::Matrix<T, 3, 1>::UnitX()) *
    Eigen::AngleAxis<T>(pose(4), Eigen::Matrix<T, 3, 1>::UnitY()) *
    Eigen::AngleAxis<T>(pose(5), Eigen::Matrix<T, 3, 1>::UnitZ());

  decltype(RosPose::position) trans;
  decltype(RosTransform::rotation) rot;

  trans.set__x(pose(0)).set__y(pose(1)).set__z(pose(2));
  ros_pose.set__position(trans);

  rot.set__x(eig_rot.x()).
  set__y(eig_rot.y()).
  set__z(eig_rot.z()).
  set__w(eig_rot.w());
  ros_pose.set__orientation(rot);
}

/// Specialization to convert from the ros pose type to an eigen one.
/// \tparam T Eigen scalar type
template<typename T>
void transform_to_pose(const RosTransform & transform, EigenPose<T> & pose)
{
  static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
  const auto & ros_rot = transform.rotation;
  const auto & ros_trans = transform.translation;
  Eigen::Quaternion<T> eig_rot{ros_rot.w, ros_rot.x, ros_rot.y, ros_rot.z};
  pose(0) = ros_trans.x;
  pose(1) = ros_trans.y;
  pose(2) = ros_trans.z;

  const auto rot = eig_rot.matrix().eulerAngles(0, 1, 2);
  pose(3) = rot(0);
  pose(4) = rot(1);
  pose(5) = rot(2);
}

/// Specialization to convert from the ros pose type to an eigen one.
/// `transform_to_pose` template is used as conversion from `RosPose`
/// is identical to conversion from `RosTransform`
/// \tparam T Eigen scalar type
template<typename T>
void transform_to_pose(const RosPose & ros_pose, EigenPose<T> & pose)
{
  static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
  const auto & ros_rot = ros_pose.orientation;
  const auto & ros_trans = ros_pose.position;
  Eigen::Quaternion<T> eig_rot{ros_rot.w, ros_rot.x, ros_rot.y, ros_rot.z};
  pose(0) = ros_trans.x;
  pose(1) = ros_trans.y;
  pose(2) = ros_trans.z;

  const auto rot = eig_rot.matrix().eulerAngles(0, 1, 2);
  pose(3) = rot(0);
  pose(4) = rot(1);
  pose(5) = rot(2);
}

}  // namespace transform_adapters
}  // namespace ndt
}  // namespace localization
}  // namespace autoware

#endif  // NDT__UTILS_HPP_


#include <iostream>
#include <vector>
#include <cstring>
#include <string>
#include <boost/foreach.hpp>

// #include "opencv2/opencv.hpp"
// #include "opencv2/highgui/highgui.hpp"

#include "pgm_out.hpp"
#include "segments.hpp"

#include "opencv4/opencv2/opencv.hpp"
#include "opencv4/opencv2/highgui/highgui.hpp"

#define MAP_W 800
#define MAP_H 800

using namespace std;
//using namespace cv;

int main(int argc, char *argv[])
{
    Map map;
    vector<unsigned char> map_data(MAP_H*MAP_W);
    fill(map_data.begin(), map_data.end(), 255);

    map.data = map_data;
    map.height = MAP_H;
    map.width = MAP_W;
    map.cpm = 8.0;

    Point A, B, C, D, E;
    A << 400, 300;
    B << 500, 350;
    C << 500, 450;
    D << 300, 450;
    E << 300, 350;

    Segment sA, sB, sC, sD, sE;
    sA.A = A;
    sA.B = B;
    sB.A = B;
    sB.B = C;
    sC.A = C;
    sC.B = D;
    sD.A = D;
    sD.B = E;
    sE.A = E;
    sE.B = A;

    Polygon polygon;
    polygon.seg.push_back(sA);
    polygon.seg.push_back(sB);
    polygon.seg.push_back(sC);
    polygon.seg.push_back(sD);
    polygon.seg.push_back(sE);

    Point F, G, H, I, J;
    F << 600, 390;
    G << 500, 400;
    H << 430, 430;
    I << 400, 600;
    J << 350, 330;

    vector<Point> points;
    points.push_back(F);
    points.push_back(G);
    points.push_back(H);
    points.push_back(I);
    points.push_back(J);

    BOOST_FOREACH(Point p, points)
    {
        cout << p << endl;
        if(isInPolygon(polygon, p))
            cout << "is in polygon.\n\n";
        else
            cout << "is not polygon.\n\n";
    }

    cout << "error : " << write_pgm("./map.pgm", map) << endl;

    cv::namedWindow("Output",1);
    cv::Mat output = cv::Mat::zeros(MAP_H, MAP_W, CV_8UC3);

    cv::Point pts2draw[1][5];
    pts2draw[0][0] = cv::Point(400, 300);
    pts2draw[0][1] = cv::Point(500, 350);
    pts2draw[0][2] = cv::Point(500, 450);
    pts2draw[0][3] = cv::Point(300, 450);
    pts2draw[0][4] = cv::Point(300, 350);

    const cv::Point* ptset[1] = {pts2draw[0]};

    int npts[] = {5};
    cv::fillPoly(output, ptset, npts, 1, cv::Scalar(255, 255, 255), cv::LINE_8);

    cv::imshow("Output", output);
    cv::waitKey(0);

    return 0;
}


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

    @copyright 2016 NumScale SAS

  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_FUNCTION_IFNOT_MINUS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IFNOT_MINUS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-boolean
    This function object conditionally returns  @c x (respectively @c x-y)
    if @c c is @ref True (respectively  @ref False)

    @par Header <boost/simd/function/ifnot_minus.hpp>

    @par Example:

      @snippet ifnot_minus.cpp ifnot_minus

    @par Possible output:

      @snippet ifnot_minus.txt ifnot_minus

  **/
  Value1 ifnot_minus(Value0 const& c, Value1 const& x, Value1 const& y);
} }
#endif

#include <boost/simd/function/scalar/ifnot_minus.hpp>
#include <boost/simd/function/simd/ifnot_minus.hpp>

#endif


/*
 * test_export.cpp
 *
 *  Created on: 2013-4-30
 *      Author: fasiondog
 */

#ifdef TEST_ALL_IN_ONE
    #include <boost/test/unit_test.hpp>
#else
    #define BOOST_TEST_MODULE test_hikyuu_indicator_suite
    #include <boost/test/unit_test.hpp>
#endif

#include <hikyuu/config.h>

#if HKU_SUPPORT_SERIALIZATION

#include <fstream>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <hikyuu/StockManager.h>
#include <hikyuu/indicator/build_in.h>

using namespace hku;

/**
 * @defgroup test_indicator_serialization test_indicator_serialization
 * @ingroup test_hikyuu_indicator_suite
 * @{
 */

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_AMA_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/AMA.xml";

    Stock stock = sm.getStock("sh000001");
    KData kdata = stock.getKData(KQuery(-20));
    Indicator ma1 = AMA(CLOSE(kdata), 10);
    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(ma1);
    }

    Indicator ma2;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(ma2);
    }

    BOOST_CHECK(ma1.size() == ma2.size());
    BOOST_CHECK(ma1.discard() == ma2.discard());
    BOOST_CHECK(ma1.getResultNumber() == ma2.getResultNumber());
    for (size_t i = 0; i < ma1.size(); ++i) {
        BOOST_CHECK_CLOSE(ma1.get(i, 0), ma2.get(i, 0), 0.00001);
        BOOST_CHECK_CLOSE(ma1.get(i, 1), ma2.get(i, 1), 0.00001);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_DIFF_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/DIFF.xml";

    Stock stock = sm.getStock("sh000001");
    KData kdata = stock.getKData(KQuery(-20));
    Indicator ma1 = DIFF(CLOSE(kdata));
    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(ma1);
    }

    Indicator ma2;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(ma2);
    }

    BOOST_CHECK(ma1.size() == ma2.size());
    BOOST_CHECK(ma1.discard() == ma2.discard());
    BOOST_CHECK(ma1.getResultNumber() == ma2.getResultNumber());
    for (size_t i = 0; i < ma1.size(); ++i) {
        BOOST_CHECK_CLOSE(ma1[i], ma2[i], 0.00001);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_MA_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/MA.xml";

    Stock stock = sm.getStock("sh000001");
    KData kdata = stock.getKData(KQuery(-20));
    Indicator ma1 = MA(CLOSE(kdata), 10);
    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(ma1);
    }

    Indicator ma2;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(ma2);
    }

    BOOST_CHECK(ma1.size() == ma2.size());
    BOOST_CHECK(ma1.discard() == ma2.discard());
    BOOST_CHECK(ma1.getResultNumber() == ma2.getResultNumber());
    for (size_t i = 0; i < ma1.size(); ++i) {
        BOOST_CHECK_CLOSE(ma1[i], ma2[i], 0.00001);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_EMA_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/EMA.xml";

    Stock stock = sm.getStock("sh000001");
    KData kdata = stock.getKData(KQuery(-20));
    Indicator ma1 = EMA(CLOSE(kdata), 10);
    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(ma1);
    }

    Indicator ma2;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(ma2);
    }

    BOOST_CHECK(ma1.size() == ma2.size());
    BOOST_CHECK(ma1.discard() == ma2.discard());
    BOOST_CHECK(ma1.getResultNumber() == ma2.getResultNumber());
    for (size_t i = 0; i < ma1.size(); ++i) {
        BOOST_CHECK_CLOSE(ma1[i], ma2[i], 0.00001);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_KDATA_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/IKDATA.xml";

    Stock stock = sm.getStock("sh000001");
    KData kdata = stock.getKData(KQuery(-20));
    Indicator ma1 = KDATA(kdata);
    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(ma1);
    }

    Indicator ma2;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(ma2);
    }

    BOOST_CHECK(ma1.size() == ma2.size());
    BOOST_CHECK(ma1.discard() == ma2.discard());
    BOOST_CHECK(ma1.getResultNumber() == ma2.getResultNumber());
    for (size_t i = 0; i < ma1.size(); ++i) {
        BOOST_CHECK_CLOSE(ma1.get(i,0), ma2.get(i,0), 0.00001);
        BOOST_CHECK_CLOSE(ma1.get(i,1), ma2.get(i,1), 0.00001);
        BOOST_CHECK_CLOSE(ma1.get(i,2), ma2.get(i,2), 0.00001);
        BOOST_CHECK_CLOSE(ma1.get(i,3), ma2.get(i,3), 0.00001);
        BOOST_CHECK_CLOSE(ma1.get(i,4), ma2.get(i,4), 0.00001);
        BOOST_CHECK_CLOSE(ma1.get(i,5), ma2.get(i,5), 0.00001);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_PRICELIST_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/PRICELIST.xml";

    PriceList d;
    for (size_t i = 0; i < 20; ++i) {
        d.push_back(i);
    }

    Indicator ma1 = PRICELIST(d);
    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(ma1);
    }

    Indicator ma2;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(ma2);
    }

    BOOST_CHECK(ma1.size() == ma2.size());
    BOOST_CHECK(ma1.discard() == ma2.discard());
    BOOST_CHECK(ma1.getResultNumber() == ma2.getResultNumber());
    for (size_t i = 0; i < ma1.size(); ++i) {
        BOOST_CHECK_CLOSE(ma1[i], ma2[i], 0.00001);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_MACD_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/MACD.xml";

    Stock stock = sm.getStock("sh000001");
    KData kdata = stock.getKData(KQuery(-20));
    Indicator ma1 = MACD(CLOSE(kdata));
    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(ma1);
    }

    Indicator ma2;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(ma2);
    }

    BOOST_CHECK(ma1.size() == ma2.size());
    BOOST_CHECK(ma1.discard() == ma2.discard());
    BOOST_CHECK(ma1.getResultNumber() == ma2.getResultNumber());
    for (size_t i = 0; i < ma1.size(); ++i) {
        BOOST_CHECK_CLOSE(ma1.get(i,0), ma2.get(i,0), 0.00001);
        BOOST_CHECK_CLOSE(ma1.get(i,1), ma2.get(i,1), 0.00001);
        BOOST_CHECK_CLOSE(ma1.get(i,2), ma2.get(i,2), 0.00001);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_SAFTYLOSS_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/SAFTYLOSS.xml";

    Stock stock = sm.getStock("sh000001");
    KData kdata = stock.getKData(KQuery(-20));
    Indicator ma1 = SAFTYLOSS(CLOSE(kdata));
    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(ma1);
    }

    Indicator ma2;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(ma2);
    }

    BOOST_CHECK(ma1.size() == ma2.size());
    BOOST_CHECK(ma1.discard() == ma2.discard());
    BOOST_CHECK(ma1.getResultNumber() == ma2.getResultNumber());
    for (size_t i = 0; i < ma1.size(); ++i) {
        BOOST_CHECK_CLOSE(ma1[i], ma2[i], 0.00001);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_STDEV_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/STDEV.xml";

    Stock stock = sm.getStock("sh000001");
    KData kdata = stock.getKData(KQuery(-20));
    Indicator ma1 = STDEV(CLOSE(kdata), 10);
    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(ma1);
    }

    Indicator ma2;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(ma2);
    }

    BOOST_CHECK(ma1.size() == ma2.size());
    BOOST_CHECK(ma1.discard() == ma2.discard());
    BOOST_CHECK(ma1.getResultNumber() == ma2.getResultNumber());
    for (size_t i = 0; i < ma1.size(); ++i) {
        BOOST_CHECK_CLOSE(ma1[i], ma2[i], 0.00001);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_VIGOR_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/VIGOR.xml";

    Stock stock = sm.getStock("sh000001");
    KData kdata = stock.getKData(KQuery(-20));
    Indicator ma1 = VIGOR(kdata, 10);
    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(ma1);
    }

    Indicator ma2;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(ma2);
    }

    BOOST_CHECK(ma1.size() == ma2.size());
    BOOST_CHECK(ma1.discard() == ma2.discard());
    BOOST_CHECK(ma1.getResultNumber() == ma2.getResultNumber());
    for (size_t i = 0; i < ma1.size(); ++i) {
        BOOST_CHECK_CLOSE(ma1[i], ma2[i], 0.00001);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_WAEVE_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/WEAVE.xml";

    Stock stock = sm.getStock("sh000001");
    KData kdata = stock.getKData(KQuery(-20));
    Indicator c = CLOSE(kdata);
    Indicator w1 = WEAVE(c);
    Indicator w2 = w1(c);
    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(w2);
    }

    Indicator w3;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(w3);
    }

    BOOST_CHECK(w2.size() == w3.size());
    BOOST_CHECK(w2.discard() == w3.discard());
    BOOST_CHECK(w1.getResultNumber() == 1);
    BOOST_CHECK(w2.getResultNumber() == 2);
    BOOST_CHECK(w2.getResultNumber() == w3.getResultNumber());
    for (size_t i = 0; i < w2.size(); ++i) {
        BOOST_CHECK_CLOSE(w2[i], w3[i], 0.00001);
    }
}

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_CVAL_export ) {
    StockManager& sm = StockManager::instance();
    string filename(sm.tmpdir());
    filename += "/CVAL.xml";

    Stock stock = sm.getStock("sh000001");
    KData kdata = stock.getKData(KQuery(-20));
    Indicator ma1 = CVAL(CLOSE(kdata), 100.0);
    {
        std::ofstream ofs(filename);
        boost::archive::xml_oarchive oa(ofs);
        oa << BOOST_SERIALIZATION_NVP(ma1);
    }

    Indicator ma2;
    {
        std::ifstream ifs(filename);
        boost::archive::xml_iarchive ia(ifs);
        ia >> BOOST_SERIALIZATION_NVP(ma2);
    }

    BOOST_CHECK(ma1.size() == ma2.size());
    BOOST_CHECK(ma1.discard() == ma2.discard());
    BOOST_CHECK(ma1.getResultNumber() == ma2.getResultNumber());
    for (size_t i = 0; i < ma1.size(); ++i) {
        BOOST_CHECK_CLOSE(ma1[i], ma2[i], 0.00001);
    }
}

/** @} */

#endif /* HKU_SUPPORT_SERIALIZATION */


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

typedef tiny::MathTypes<float> MT;
typedef MT::vector3_type       V;
typedef MT::real_type          T;

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(sphere_test)
{

  {
    geometry::Sphere<V> A;
    BOOST_CHECK( geometry::is_valid(A) == true );

    BOOST_CHECK_CLOSE(A.center()(0), 0.0, 0.01 );
    BOOST_CHECK_CLOSE(A.center()(1), 0.0, 0.01 );
    BOOST_CHECK_CLOSE(A.center()(2), 0.0, 0.01 );
    BOOST_CHECK_CLOSE(A.radius(),    1.0, 0.01 );
  }

  {
    V const center = V::make(1.0, 2.0, 3.0);
    T const radius = 4.0;

    geometry::Sphere<V> A = geometry::make_sphere(center, radius);
    BOOST_CHECK( geometry::is_valid(A) == true );

    BOOST_CHECK_CLOSE(A.center()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.center()(1), 2.0, 0.01 );
    BOOST_CHECK_CLOSE(A.center()(2), 3.0, 0.01 );
    BOOST_CHECK_CLOSE(A.radius(),    4.0, 0.01 );
  }

  {
    V const center = V::make(1.0, 2.0, 3.0);
    T const radius = 4.0;

    geometry::Sphere<V> A;

    A.center() = center;
    A.radius() = radius;

    BOOST_CHECK( geometry::is_valid(A) == true );

    BOOST_CHECK_CLOSE(A.center()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(A.center()(1), 2.0, 0.01 );
    BOOST_CHECK_CLOSE(A.center()(2), 3.0, 0.01 );
    BOOST_CHECK_CLOSE(A.radius(),    4.0, 0.01 );
  }

  {
    V const center = V::make(1.0, 2.0, 3.0);
    T const radius = 4.0;

    geometry::Sphere<V> A = geometry::make_sphere(center, radius);

    geometry::Sphere<V> B = A;
    geometry::Sphere<V> C(A);

    BOOST_CHECK( geometry::is_valid(B) == true );
    BOOST_CHECK( geometry::is_valid(C) == true );

    BOOST_CHECK_CLOSE(B.center()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(B.center()(1), 2.0, 0.01 );
    BOOST_CHECK_CLOSE(B.center()(2), 3.0, 0.01 );
    BOOST_CHECK_CLOSE(B.radius(),    4.0, 0.01 );

    BOOST_CHECK_CLOSE(C.center()(0), 1.0, 0.01 );
    BOOST_CHECK_CLOSE(C.center()(1), 2.0, 0.01 );
    BOOST_CHECK_CLOSE(C.center()(2), 3.0, 0.01 );
    BOOST_CHECK_CLOSE(C.radius(),    4.0, 0.01 );
  }

}

BOOST_AUTO_TEST_SUITE_END();


#include <boost/test/unit_test.hpp>

#include <random>

#include <memory>

#include "rfr/data_containers/default_data_container.hpp"
#include "rfr/splits/binary_split_one_feature_rss_loss.hpp"
#include "rfr/trees/k_ary_tree.hpp"
#include "rfr/forests/regression_forest.hpp"
#include "rfr/forests/quantile_regression_forest.hpp"
#include "rfr/forests/fanova_forest.hpp"

#include <sstream>
#include <cereal/archives/xml.hpp>
#include <cereal/archives/portable_binary.hpp>
#include <cereal/archives/binary.hpp>
typedef cereal::PortableBinaryInputArchive iarch_type;
typedef cereal::PortableBinaryOutputArchive oarch_type;

typedef double num_t;
typedef double response_t;
typedef unsigned int index_t;
typedef std::default_random_engine rng_t;

typedef rfr::data_containers::default_container<num_t, response_t, index_t> data_container_type;

typedef rfr::splits::binary_split_one_feature_rss_loss<num_t, response_t, index_t, rng_t> split_type;
typedef rfr::nodes::k_ary_node_full<2, split_type, num_t, response_t, index_t, rng_t> node_type;

typedef rfr::nodes::temporary_node<num_t, index_t> tmp_node_type;

typedef rfr::trees::k_ary_random_tree<2, node_type, num_t, response_t, index_t, rng_t> tree_type;

typedef rfr::forests::regression_forest< tree_type, num_t, response_t, index_t, rng_t> forest_type;

typedef rfr::forests::quantile_regression_forest< tree_type, num_t, response_t, index_t, rng_t> qrf_type;

typedef rfr::forests::fANOVA_forest<split_type, num_t, response_t, index_t, rng_t> fANOVAf_type;

data_container_type load_diabetes_data(){
	data_container_type data(10);

    std::string feature_file, response_file;

    feature_file  = std::string(boost::unit_test::framework::master_test_suite().argv[1]) + "diabetes_features.csv";
    response_file = std::string(boost::unit_test::framework::master_test_suite().argv[1]) + "diabetes_responses.csv";

    data.import_csv_files(feature_file, response_file);
    return(data);
}

BOOST_AUTO_TEST_CASE( regression_forest_serialize_test ){

    auto data = load_diabetes_data();

	rfr::trees::tree_options<num_t, response_t, index_t> tree_opts;
	tree_opts.min_samples_to_split = 2;
	tree_opts.min_samples_in_leaf = 1;
	tree_opts.max_features = data.num_features()*3/4;
	tree_opts.max_depth=5;
	tree_opts.max_num_nodes=2048;

	rfr::forests::forest_options<num_t, response_t, index_t> forest_opts(tree_opts);

	forest_opts.num_data_points_per_tree = data.num_data_points()/2;
	forest_opts.num_trees = 16;
	forest_opts.do_bootstrapping = true;
	forest_opts.compute_oob_error= true;

	forest_type the_forest(forest_opts);

	rng_t rng;

	BOOST_REQUIRE(std::isnan(the_forest.out_of_bag_error()));

	the_forest.fit(data, rng);

	BOOST_REQUIRE(!std::isnan(the_forest.out_of_bag_error()));

    auto tmp = the_forest.predict(data.retrieve_data_point(5));

	std::ostringstream oss;

	{
		oarch_type oarchive(oss);
		oarchive(the_forest);
	}

	the_forest.save_to_binary_file("regression_forest_test.bin");

	forest_type the_forest2;

	auto s = the_forest.ascii_string_representation();

	the_forest2.load_from_ascii_string(s);

	forest_type the_forest3;
	the_forest3.load_from_binary_file("regression_forest_test.bin");

	for (auto i=0u; i < data.num_data_points(); ++i){
		auto v1 = the_forest.predict(data.retrieve_data_point(i));
		auto v2 = the_forest2.predict(data.retrieve_data_point(i));
		auto v3 = the_forest3.predict(data.retrieve_data_point(i));

		BOOST_REQUIRE_EQUAL(v1,v2);
		BOOST_REQUIRE_EQUAL(v1,v3);
	}

}

BOOST_AUTO_TEST_CASE( regression_forest_update_downdate_tests ){

	double unique_value = 42.424242;

	auto data = load_diabetes_data();

	rfr::trees::tree_options<num_t, response_t, index_t> tree_opts;
	tree_opts.min_samples_to_split = 2;
	tree_opts.min_samples_in_leaf = 1;
	tree_opts.max_features = 10;

	rfr::forests::forest_options<num_t, response_t, index_t> forest_opts(tree_opts);

	forest_opts.num_data_points_per_tree = data.num_data_points();
	forest_opts.num_trees = 10;
	forest_opts.do_bootstrapping = true;

	forest_type the_forest(forest_opts);

	rng_t rng;

	//fit forest
	the_forest.fit(data, rng);

	// get reference leaf values for one configuration
	std::vector<std::vector< num_t> > before = the_forest.all_leaf_values(data.retrieve_data_point(0));

	// update forest with that configuration and a unique response value
	the_forest.pseudo_update(data.retrieve_data_point(0), unique_value, 1);

	// get new leaf values
	std::vector<std::vector< num_t> > after_update = the_forest.all_leaf_values(data.retrieve_data_point(0));

	// compare them to ensure the data point has been added correctly

	for (auto i=0u; i < before.size(); ++i){
		BOOST_REQUIRE(before[i].size() == after_update[i].size() -1);
		BOOST_CHECK_EQUAL_COLLECTIONS( before[i].begin(), before[i].end(), after_update[i].begin(), --after_update[i].end());
		BOOST_REQUIRE( after_update[i].back() == unique_value);
	}

	// downdate the tree
	the_forest.pseudo_downdate(data.retrieve_data_point(0), unique_value, 1);

	// get new leaf values
	std::vector<std::vector< num_t> > after_downdate = the_forest.all_leaf_values(data.retrieve_data_point(0));

	// compare them to ensure the last data point has been removed
	for (auto i=0u; i < before.size(); ++i)
		BOOST_CHECK_EQUAL_COLLECTIONS( before[i].begin(), before[i].end(), after_downdate[i].begin(), after_downdate[i].end());

	auto m = the_forest.predict(data.retrieve_data_point(0));
}

BOOST_AUTO_TEST_CASE( regression_forest_exceptions_tests ){

    auto data = load_diabetes_data();

	rfr::trees::tree_options<num_t, response_t, index_t> tree_opts;
	tree_opts.min_samples_to_split = 2;
	tree_opts.min_samples_in_leaf = 1;
	tree_opts.max_features = data.num_data_points()*3/4;

	rfr::forests::forest_options<num_t, response_t, index_t> forest_opts(tree_opts);

	forest_opts.num_data_points_per_tree = data.num_data_points();
	forest_opts.num_trees = 8;
	forest_opts.do_bootstrapping = false;
	forest_opts.compute_oob_error= true;

	forest_type the_forest(forest_opts);

	rng_t rng;

	// no trees in the forest
	the_forest.options.num_trees = 0;
	BOOST_REQUIRE_THROW(the_forest.fit(data, rng), std::runtime_error);

	// no datapoints in any trees
	the_forest.options.num_trees = 8;
	the_forest.options.num_data_points_per_tree = 0;
	BOOST_REQUIRE_THROW(the_forest.fit(data, rng), std::runtime_error);

	// not enough datapoints and no bootstrapping
	the_forest.options.num_data_points_per_tree = 32768;
	BOOST_REQUIRE_THROW(the_forest.fit(data, rng), std::runtime_error);

	// a successful training without bootstrapping for covarage
	the_forest.options.num_data_points_per_tree = 128;
	the_forest.fit(data, rng);

	// no features to split
	the_forest.options.num_data_points_per_tree = 128;
	the_forest.options.tree_opts.max_features = 0;
	BOOST_REQUIRE_THROW(the_forest.fit(data, rng), std::runtime_error);
}

BOOST_AUTO_TEST_CASE( quantile_regression_forest_test ){

	auto data = load_diabetes_data();

	rng_t rng;

	rfr::trees::tree_options<num_t, response_t, index_t> tree_opts;
	tree_opts.min_samples_to_split = 2;
	tree_opts.min_samples_in_leaf = 1;
	tree_opts.max_features = 10;

	// don't split anything

	tree_opts.max_num_nodes = 1;
	tree_opts.max_depth = 0;

	rfr::forests::forest_options<num_t, response_t, index_t> forest_opts(tree_opts);

	forest_opts.num_data_points_per_tree = data.num_data_points();
	forest_opts.num_trees = 1;
	forest_opts.do_bootstrapping = false;

	// just to test the default constructor
	qrf_type sudowoodo;
	BOOST_REQUIRE_THROW(sudowoodo.fit(data, rng), std::runtime_error);

	qrf_type the_forest(forest_opts);

	//fit forest
	the_forest.fit(data, rng);

	auto mew = data.retrieve_data_point(0);
	std::vector<num_t> mew2 = {0,0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1};

	auto qv1 = the_forest.predict_quantiles(mew , mew2);

	BOOST_REQUIRE_EQUAL(qv1.size(), mew2.size());

	// check that shuffling doesn't affect the outcome
	std::shuffle(mew2.begin(), mew2.end(), rng);
	auto qv2 = the_forest.predict_quantiles(mew , mew2);
	BOOST_CHECK_EQUAL_COLLECTIONS ( qv1.begin(), qv1.end(), qv2.begin(), qv2.end());

	//
	std::vector<num_t> qv_numpy_ref = {  25.,   60.,   77.,   94.,  115.,  141.,  168.,  197.,  233., 268.,  346.};

	BOOST_CHECK_EQUAL_COLLECTIONS ( qv1.begin(), qv1.end(), qv_numpy_ref.begin(), qv_numpy_ref.end());

	BOOST_REQUIRE_THROW(the_forest.predict_quantiles(mew, {0,-0.5,1}) ,std::runtime_error);
	BOOST_REQUIRE_THROW(the_forest.predict_quantiles(mew, {1.1,0.5}) ,std::runtime_error);
}

/* not interesting right now!
BOOST_AUTO_TEST_CASE( fANOVA_forest_test ){

	auto data = load_diabetes_data();

	rng_t rng;

	rfr::trees::tree_options<num_t, response_t, index_t> tree_opts;
	tree_opts.min_samples_to_split = 2;
	tree_opts.min_samples_in_leaf = 1;
	tree_opts.max_features = 10;

	rfr::forests::forest_options<num_t, response_t, index_t> forest_opts(tree_opts);

	forest_opts.num_data_points_per_tree = data.num_data_points();
	forest_opts.num_trees = 23;
	forest_opts.do_bootstrapping = false;

	fANOVAf_type the_forest(forest_opts);

	//fit forest
	the_forest.fit(data, rng);

	BOOST_REQUIRE_EQUAL(the_forest.num_trees(), 23);
	BOOST_REQUIRE_EQUAL(the_forest.options.num_trees, 23);
	BOOST_REQUIRE_EQUAL(the_forest.options.do_bootstrapping, false);

	the_forest.options.num_trees = 24;
	the_forest.options.do_bootstrapping = true;
	//refit forest
	the_forest.fit(data, rng);
	BOOST_REQUIRE_EQUAL(the_forest.num_trees(), 24);
	BOOST_REQUIRE_EQUAL(the_forest.options.do_bootstrapping, true);

	// 10th and 90th percentile of the response values
	the_forest.set_cutoffs(60,265);

}
*/


// Copyright (c) FRC Team 3512. All Rights Reserved.

#pragma once

#include <vector>

#include <Eigen/Core>
#include <frc/ADIS16470_IMU.h>
#include <frc/AnalogInput.h>
#include <frc/Encoder.h>
#include <frc/controller/ImplicitModelFollower.h>
#include <frc/estimator/AngleStatistics.h>
#include <frc/estimator/KalmanFilterLatencyCompensator.h>
#include <frc/estimator/UnscentedKalmanFilter.h>
#include <frc/filter/LinearFilter.h>
#include <frc/geometry/Pose2d.h>
#include <frc/geometry/Translation2d.h>
#include <frc/motorcontrol/MotorControllerGroup.h>
#include <frc/simulation/ADIS16470_IMUSim.h>
#include <frc/simulation/AnalogInputSim.h>
#include <frc/simulation/DifferentialDrivetrainSim.h>
#include <frc/simulation/EncoderSim.h>
#include <frc/smartdashboard/Field2d.h>
#include <frc/system/plant/LinearSystemId.h>
#include <frc/trajectory/TrajectoryConfig.h>
#include <networktables/NetworkTableEntry.h>
#include <rev/CANSparkMax.h>
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/current.h>
#include <units/length.h>
#include <units/time.h>
#include <units/velocity.h>
#include <wpi/static_circular_buffer.h>

#include "Constants.hpp"
#include "HWConfig.hpp"
#include "NetworkTableUtil.hpp"
#include "controllers/DrivetrainController.hpp"
#include "subsystems/ControlledSubsystemBase.hpp"
#include "subsystems/Vision.hpp"

namespace frc3512 {

/**
 * The drivetrain subsystem.
 *
 * The drivetrain uses an unscented Kalman filter for state estimation.
 */
class Drivetrain : public ControlledSubsystemBase<7, 2, 5> {
public:
    /// The drivetrain length.
    static constexpr units::meter_t kLength = 0.9398_m;

    /**
     * Distance from middle of robot to intake.
     */
    static constexpr units::meter_t kMiddleOfRobotToIntake = 0.656_m;

    /**
     * Producer-consumer queue for global pose measurements from Vision
     * subsystem.
     */
    wpi::static_circular_buffer<Vision::GlobalMeasurement, 8> visionQueue;

    Drivetrain();

    Drivetrain(const Drivetrain&) = delete;
    Drivetrain& operator=(const Drivetrain&) = delete;

    /**
     * Returns the drivetrain's reference pose.
     */
    frc::Pose2d GetReferencePose() const;

    /**
     * Returns the drivetrain's pose estimate.
     */
    frc::Pose2d GetPose() const;

    /**
     * Returns distance from the left ultrasonic sensor.
     */
    units::meter_t GetLeftUltrasonicDistance() const;

    /**
     * Returns distance from the right ultrasonic sensor.
     */
    units::meter_t GetRightUltrasonicDistance() const;

    /**
     * Returns gyro's heading measurement in the global coordinate frame.
     */
    units::radian_t GetAngle() const;

    /**
     * Returns left encoder displacement.
     */
    units::meter_t GetLeftPosition() const;

    /**
     * Returns right encoder displacement.
     */
    units::meter_t GetRightPosition() const;

    /**
     * Returns left encoder velocity.
     */
    units::meters_per_second_t GetLeftVelocity() const;

    /**
     * Returns right encoder velocity.
     */
    units::meters_per_second_t GetRightVelocity() const;

    /**
     * Returns longitudinal acceleration from IMU.
     */
    units::meters_per_second_squared_t GetAccelerationX() const;

    /**
     * Returns lateral acceleration from IMU.
     */
    units::meters_per_second_squared_t GetAccelerationY() const;

    /**
     * Resets all sensors and controller.
     */
    void Reset(const frc::Pose2d& initialPose = frc::Pose2d());

    /**
     * Set global measurements.
     *
     * @param x         X position of the robot in meters.
     * @param y         Y position of the robot in meters.
     * @param timestamp Absolute time the translation data comes from.
     */
    void CorrectWithGlobalOutputs(units::meter_t x, units::meter_t y,
                                  units::second_t timestamp);

    /**
     * Adds a trajectory with the given waypoints.
     *
     * This can be called more than once to create a queue of trajectories.
     * Closed-loop control will be enabled to track the first trajectory.
     *
     * @param start    Starting pose.
     * @param interior Intermediate waypoints excluding heading.
     * @param end      Ending pose.
     */
    void AddTrajectory(const frc::Pose2d& start,
                       const std::vector<frc::Translation2d>& interior,
                       const frc::Pose2d& end);

    /**
     * Adds a trajectory with the given waypoints.
     *
     * This can be called more than once to create a queue of trajectories.
     * Closed-loop control will be enabled to track the first trajectory.
     *
     * @param start    Starting pose.
     * @param interior Intermediate waypoints excluding heading.
     * @param end      Ending pose.
     * @param config   TrajectoryConfig for this trajectory. This can include
     *                 constraints on the trajectory dynamics. If adding custom
     *                 constraints, it is recommended to start with the config
     *                 returned by MakeTrajectoryConfig() so differential drive
     *                 dynamics constraints are included automatically.
     */
    void AddTrajectory(const frc::Pose2d& start,
                       const std::vector<frc::Translation2d>& interior,
                       const frc::Pose2d& end,
                       const frc::TrajectoryConfig& config);

    /**
     * Adds a trajectory with the given waypoints.
     *
     * This can be called more than once to create a queue of trajectories.
     * Closed-loop control will be enabled to track the first trajectory.
     *
     * @param waypoints Waypoints.
     */
    void AddTrajectory(const std::vector<frc::Pose2d>& waypoints);

    /**
     * Adds a trajectory with the given waypoints.
     *
     * This can be called more than once to create a queue of trajectories.
     * Closed-loop control will be enabled to track the first trajectory.
     *
     * @param waypoints Waypoints.
     * @param config    TrajectoryConfig for this trajectory. This can include
     *                  constraints on the trajectory dynamics. If adding custom
     *                  constraints, it is recommended to start with the config
     *                  returned by MakeTrajectoryConfig() so differential drive
     *                  dynamics constraints are included automatically.
     */
    void AddTrajectory(const std::vector<frc::Pose2d>& waypoints,
                       const frc::TrajectoryConfig& config);

    /**
     * Returns a TrajectoryConfig containing a differential drive dynamics
     * constraint with the start and end velocities set to zero.
     */
    static frc::TrajectoryConfig MakeTrajectoryConfig();

    /**
     * Returns a TrajectoryConfig containing a differential drive dynamics
     * constraint and the specified start and end velocities.
     *
     * @param startVelocity The start velocity of the trajectory.
     * @param endVelocity   The end velocity of the trajectory.
     */
    static frc::TrajectoryConfig MakeTrajectoryConfig(
        units::meters_per_second_t startVelocity,
        units::meters_per_second_t endVelocity);

    /**
     * Returns whether the drivetrain controller is at the goal waypoint.
     */
    bool AtGoal() const;

    /**
     * Returns the drivetrain state estimate.
     */
    const Eigen::Vector<double, 7>& GetStates() const;

    /**
     * Returns the drivetrain inputs.
     */
    const Eigen::Vector<double, 2>& GetInputs() const;

    /**
     * Returns current drawn in simulation.
     */
    units::ampere_t GetCurrentDraw() const;

    /**
     * Returns how many times the vision measurement was too far from the
     * drivetrain pose estimate.
     */
    int GetPoseMeasurementFaultCounter();

    /**
     * Returns sim pose.
     */
    frc::Pose2d GetSimPose() const;

    void DisabledInit() override;

    void AutonomousInit() override;

    void TeleopInit() override;

    void TestInit() override;

    void RobotPeriodic() override;

    void TeleopPeriodic() override;

    void TestPeriodic() override;

    void ControllerPeriodic() override;

private:
    static const Eigen::Matrix<double, 2, 2> kGlobalR;

    static const frc::LinearSystem<2, 2, 2> kPlant;

    frc::AnalogInput m_leftUltrasonic{
        HWConfig::Drivetrain::kLeftUltrasonicChannel};
    units::meter_t m_leftUltrasonicDistance;
    frc::AnalogInput m_rightUltrasonic{
        HWConfig::Drivetrain::kRightUltrasonicChannel};
    units::meter_t m_rightUltrasonicDistance;
    frc::LinearFilter<units::meter_t> m_leftDistanceFilter =
        frc::LinearFilter<units::meter_t>::SinglePoleIIR(0.1, 0.02_s);

    frc::LinearFilter<units::meter_t> m_rightDistanceFilter =
        frc::LinearFilter<units::meter_t>::SinglePoleIIR(0.1, 0.02_s);

    rev::CANSparkMax m_leftLeader{HWConfig::Drivetrain::kLeftMotorLeaderID,
                                  rev::CANSparkMax::MotorType::kBrushless};
    rev::CANSparkMax m_leftFollower{HWConfig::Drivetrain::kLeftMotorFollowerID,
                                    rev::CANSparkMax::MotorType::kBrushless};
    frc::MotorControllerGroup m_leftGrbx{m_leftLeader, m_leftFollower};

    rev::CANSparkMax m_rightLeader{HWConfig::Drivetrain::kRightMotorLeaderID,
                                   rev::CANSparkMax::MotorType::kBrushless};
    rev::CANSparkMax m_rightFollower{
        HWConfig::Drivetrain::kRightMotorFollowerID,
        rev::CANSparkMax::MotorType::kBrushless};
    frc::MotorControllerGroup m_rightGrbx{m_rightLeader, m_rightFollower};

    frc::Encoder m_leftEncoder{HWConfig::Drivetrain::kLeftEncoderA,
                               HWConfig::Drivetrain::kLeftEncoderB};

    frc::Encoder m_rightEncoder{HWConfig::Drivetrain::kRightEncoderA,
                                HWConfig::Drivetrain::kRightEncoderB};

    frc::ADIS16470_IMU m_imu;
    units::radian_t m_headingOffset = 0_rad;

    frc::UnscentedKalmanFilter<7, 2, 5> m_observer{
        DrivetrainController::Dynamics,
        DrivetrainController::LocalMeasurementModel,
        {0.002, 0.002, 0.0001, 1.5, 1.5, 0.5, 0.5},
        {0.0001, 0.005, 0.005, 7.0, 7.0},
        frc::AngleMean<7, 7>(2),
        frc::AngleMean<5, 7>(0),
        frc::AngleResidual<7>(2),
        frc::AngleResidual<5>(0),
        frc::AngleAdd<7>(2),
        Constants::kControllerPeriod};
    frc::KalmanFilterLatencyCompensator<7, 2, 5,
                                        frc::UnscentedKalmanFilter<7, 2, 5>>
        m_latencyComp;
    DrivetrainController m_controller;
    Eigen::Vector<double, 2> m_u = Eigen::Vector<double, 2>::Zero();

    nt::NetworkTableEntry m_leftUltrasonicOutputEntry =
        NetworkTableUtil::MakeDoubleEntry(
            "/Diagnostics/Drivetrain/Outputs/Left Ultrasonic Output");

    nt::NetworkTableEntry m_rightUltrasonicOutputEntry =
        NetworkTableUtil::MakeDoubleEntry(
            "/Diagnostics/Drivetrain/Outputs/Right Ultrasonic Output");

    frc::LinearSystem<2, 2, 2> m_imfRef =
        frc::LinearSystemId::IdentifyDrivetrainSystem(
            DrivetrainController::kLinearV,
            DrivetrainController::kLinearA * 5.0,
            DrivetrainController::kAngularV,
            DrivetrainController::kAngularA * 2.0);
    frc::ImplicitModelFollower<2, 2> m_imf{kPlant, m_imfRef, 20_ms};

    int m_poseMeasurementFaultCounter = 0;
    nt::NetworkTableEntry m_poseMeasurementFaultEntry =
        NetworkTableUtil::MakeDoubleEntry("/Diagnostics/Vision/Vision faults");

    // Simulation variables
    frc::sim::DifferentialDrivetrainSim m_drivetrainSim{
        DrivetrainController::GetPlant(), DrivetrainController::kWidth,
        frc::DCMotor::NEO(2), DrivetrainController::kDriveGearRatio,
        DrivetrainController::kWheelRadius};
    frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
    frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
    frc::sim::ADIS16470_IMUSim m_imuSim{m_imu};
    frc::sim::AnalogInputSim m_leftUltrasonicSim{m_leftUltrasonic};
    frc::sim::AnalogInputSim m_rightUltrasonicSim{m_rightUltrasonic};
    frc::Field2d m_field;

    /**
     * Set drivetrain motors to brake mode, which the feedback controllers
     * expect.
     */
    void SetBrakeMode();

    /**
     * Set drivetrain motors to coast mode so the robot is easier to push when
     * it's disabled.
     */
    void SetCoastMode();
};

}  // namespace frc3512


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include "catch.hpp"

#include <iostream>

#include <Eigen/Core>

#include "bi_operators/CollocationIntegrator.hpp"
#include "green/DerivativeTypes.hpp"
#include "cavity/GePolCavity.hpp"
#include "utils/Molecule.hpp"
#include "green/Vacuum.hpp"
#include "green/UniformDielectric.hpp"
#include "solver/CPCMSolver.hpp"
#include "utils/Symmetry.hpp"
#include "TestingMolecules.hpp"

/*! \class CPCMSolver
 *  \test \b NH3GePol tests CPCMSolver using ammonia and a GePol cavity
 */
TEST_CASE("Test solver for the C-PCM with NH3 molecule and a GePol cavity", "[solver][cpcm][cpcm_gepol-NH3]")
{
    Molecule molecule = NH3();

    double area = 0.4;
    double probeRadius = 0.0;
    double minRadius = 100.0;
    GePolCavity cavity = GePolCavity(molecule, area, probeRadius, minRadius);
    cavity.saveCavity("nh3.npz");

    double permittivity = 78.39;
    Vacuum<> gf_i;
    UniformDielectric<> gf_o(permittivity);
    bool symm = true;
    double correction = 0.8;
    CPCMSolver solver(symm, correction);
    solver.buildSystemMatrix(cavity, gf_i, gf_o);

    double Ncharge = 7.0;
    double Hcharge = 1.0;
    int size = cavity.size();
    Eigen::VectorXd fake_mep = computeMEP(molecule, cavity.elements());
    // The total ASC for a conductor is -Q
    // for CPCM it will be -Q*(epsilon-1)/(epsilon + correction)
    Eigen::VectorXd fake_asc = Eigen::VectorXd::Zero(size);
    fake_asc = solver.computeCharge(fake_mep);
    double totalASC = - (Ncharge + 3.0 * Hcharge) * (permittivity - 1) /
                      (permittivity + correction);
    double totalFakeASC = fake_asc.sum();
    CAPTURE(totalASC - totalFakeASC);
    REQUIRE(totalASC == Approx(totalFakeASC).epsilon(1.0e-03));
}


/*
 * Copyright [2015]
 * [Kartik Mohta <kartikmohta@gmail.com>]
 * [Ke Sun <sunke.polyu@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.
 */

#include <ctime>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <tf_conversions/tf_eigen.h>
#include <mocap_vicon/ViconDriver.h>

using namespace std;
using namespace Eigen;
namespace ViconSDK = ViconDataStreamSDK::CPP;

namespace mocap {

bool ViconDriver::init() {

  nh.param("server_address", server_address, string("alkaline2"));
  nh.param("model_list", model_list, vector<string>(0));
  int signed_frame_rate;
  nh.param("frame_rate", signed_frame_rate, 100);
  frame_rate = signed_frame_rate > 0 ? signed_frame_rate : 0;
  nh.param("max_accel", max_accel, 10.0);
  nh.param("publish_tf", publish_tf, false);
  nh.param("fixed_frame_id", fixed_frame_id, string("mocap"));

  frame_interval = 1.0 / static_cast<double>(frame_rate);
  double& dt = frame_interval;
  process_noise.topLeftCorner<6, 6>() =
    0.5*Matrix<double, 6, 6>::Identity()*dt*dt*max_accel;
  process_noise.bottomRightCorner<6, 6>() =
    Matrix<double, 6, 6>::Identity()*dt*max_accel;
  process_noise *= process_noise; // Make it a covariance
  measurement_noise =
    Matrix<double, 6, 6>::Identity()*1e-3;
  measurement_noise *= measurement_noise; // Make it a covariance
  model_set.insert(model_list.begin(), model_list.end());

  timespec ts_sleep;
  ts_sleep.tv_sec = 0;
  ts_sleep.tv_nsec = 100000000;

  // Connect to the server
  ROS_INFO("Connecting to Vicon Datastream server at %s", server_address.c_str());
  bool is_connected = false;
  for (int retry_cnt = 0; retry_cnt < 10; ++retry_cnt) {
    client->Connect(server_address);
    if(client->IsConnected().Connected) {
      is_connected = true;
      break;
    }
    else
      nanosleep(&ts_sleep, NULL);
  }

  // Report if cannot connect
  if (!is_connected) {
    ROS_WARN("Cannot Connect to Vicon server at %s", server_address.c_str());
    return false;
  }

  // Configure the connection
  ROS_INFO("Successfully Connect to Vicon server at %s", server_address.c_str());
  client->SetStreamMode(ViconSDK::StreamMode::ClientPull);
  client->SetAxisMapping(ViconSDK::Direction::Forward,
      ViconSDK::Direction::Left, ViconSDK::Direction::Up);
  client->EnableSegmentData();
  if(!client->IsSegmentDataEnabled().Enabled) {
    ROS_WARN("Segment data cannot be enabled.");
    return false;
  }
  ROS_INFO("Successfully configure Vicon server at %s", server_address.c_str());

  // Need to wait for some time after enabling data else you get junk frames
  //struct timespec ts_sleep;
  ts_sleep.tv_sec = 0;
  ts_sleep.tv_nsec = 100000000;
  nanosleep(&ts_sleep, NULL);

  return true;
}

 bool ViconDriver::run() {
  ViconSDK::Result::Enum result = client->GetFrame().Result;
  if (result != ViconSDK::Result::Success)
    return false;
  handleFrame();
  return true;
}

void ViconDriver::disconnect() {
  ROS_INFO_STREAM("Disconnected with the server at "
      << server_address);
  client->Disconnect();
  return;
}

void ViconDriver::handleFrame() {
  int body_count = client->GetSubjectCount().SubjectCount;
  // Assign each subject with a thread
  vector<boost::thread> subject_threads;
  subject_threads.reserve(body_count);

  for (int i = 0; i< body_count; ++i) {
    string subject_name =
      client->GetSubjectName(i).SubjectName;

    // Process the subject if required
    if (model_set.empty() || model_set.count(subject_name)) {
      // Create a new subject if it does not exist
      if (subjects.find(subject_name) == subjects.end()) {
        subjects[subject_name] = Subject::SubjectPtr(
            new Subject(&nh, subject_name, fixed_frame_id));
        subjects[subject_name]->setParameters(
            process_noise, measurement_noise, frame_rate);
      }
      // Handle the subject in a different thread
      subject_threads.emplace_back(&ViconDriver::handleSubject, this, i);
      //handleSubject(i);
    }
  }

  // Wait for all the threads to stop
  for(auto &thread : subject_threads) {
    thread.join();
  }

  // Send out warnings
  for (auto it = subjects.begin();
      it != subjects.end(); ++it) {
    Subject::Status status = it->second->getStatus();
    if (status == Subject::LOST)
      ROS_WARN_THROTTLE(1, "Lose track of subject %s", (it->first).c_str());
    else if (status == Subject::INITIALIZING)
      ROS_WARN("Initialize subject %s", (it->first).c_str());
  }

  return;
}

void ViconDriver::handleSubject(int sub_idx) {

  boost::unique_lock<boost::shared_mutex> write_lock(mtx);
  // We assume each subject has only one segment
  string subject_name = client->GetSubjectName(sub_idx).SubjectName;
  string segment_name = client->GetSegmentName(subject_name, 0).SegmentName;
  // Get the pose for the subject
  ViconSDK::Output_GetSegmentGlobalTranslation trans =
      client->GetSegmentGlobalTranslation(subject_name, segment_name);
  ViconSDK::Output_GetSegmentGlobalRotationQuaternion quat =
      client->GetSegmentGlobalRotationQuaternion(subject_name, segment_name);
  write_lock.unlock();

  //boost::shared_lock<boost::shared_mutex> read_lock(mtx);
  if(trans.Result != ViconSDK::Result::Success ||
     quat.Result != ViconSDK::Result::Success ||
     trans.Occluded || quat.Occluded) {
    subjects[subject_name]->disable();
    return;
  }

  // Convert the msgs to Eigen type
  Eigen::Quaterniond m_att(quat.Rotation[3],
      quat.Rotation[0], quat.Rotation[1], quat.Rotation[2]);
  Eigen::Vector3d m_pos(trans.Translation[0]/1000,
      trans.Translation[1]/1000, trans.Translation[2]/1000);

  // Re-enable the object if it is lost previously
  if (subjects[subject_name]->getStatus() == Subject::LOST) {
    subjects[subject_name]->enable();
  }

  // Feed the new measurement to the subject
  double time = ros::Time::now().toSec();
  subjects[subject_name]->processNewMeasurement(time, m_att, m_pos);
  //read_lock.unlock();

  // Publish tf if requred
  if (publish_tf &&
      subjects[subject_name]->getStatus() == Subject::TRACKED) {

    Quaterniond att = subjects[subject_name]->getAttitude();
    Vector3d pos = subjects[subject_name]->getPosition();
    tf::Quaternion att_tf;
    tf::Vector3 pos_tf;
    tf::quaternionEigenToTF(att, att_tf);
    tf::vectorEigenToTF(pos, pos_tf);

    tf::StampedTransform stamped_transform =
      tf::StampedTransform(tf::Transform(att_tf, pos_tf),
        ros::Time::now(), fixed_frame_id, subject_name);
    write_lock.lock();
    tf_publisher.sendTransform(stamped_transform);
    write_lock.unlock();
  }

  return;
}

}


// ichnos.cpp : This file contains the 'main' function. Program execution begins and ends there.
//
#define _CRT_SECURE_NO_WARNINGS

//const int dbg_rank = 12;

#include <boost/mpi.hpp>

#include <iostream>
#include <ctime>

#include "ichnos_options.h"
#include "ichnos_structures.h"
#include "velocity_base.h"
#include "ichnos_XYZ_base.h"
#include "trace.h"
#include "iwfmVelocity.h"
#include "npsatVelocity.h"
#include "stochasticVelocity.h"
#include "TransientVelocity.h"

ICHNOS::SingletonGenerator* ICHNOS::SingletonGenerator::_instance = nullptr;

int main(int argc, char* argv[])
{
    boost::mpi::environment env( argc, argv );
    boost::mpi::communicator world;

    //{ // Random number generator
        //ICHNOS::SingletonGenerator* RG = RG->getInstance();
        //int cnt = 0;
        //std::vector<int> prop(10,0);
        //for (int i = 0; i < 100000; ++i) {
            //int r = RG->randomNumber(0, prop.size());
         //   float r = RG->randomNumber();
        //    r = RG->randomNumber(0.0f, 1.0f);
        //    int ri = RG->randomNumber(0, 4);
        //    r = RG->randomNumber(0.f, 4.f);
            //prop[r]++;
        //}
        //for (int i = 0; i < prop.size();++i)
        //    std::cout << i << " : " << prop[i] << std::endl;
        //RG->printSeed();
    //}

    if (world.rank() == 0) {
        std::time_t result = std::time(nullptr);
        std::cout << "Ichnos started at " << std::asctime(std::localtime(&result)) << std::endl;
        std::cout << "Ichnos will run using " << world.size() << " processors" << std::endl;
        //std::cout << "Good luck with that!" << std::endl;
        std::cout << "Reading input data..." << std::endl;
    }

    ICHNOS::options OPT(world);
    if (!OPT.readInput(argc, argv))
        return 0;

    if (OPT.gatherMode) {
        ICHNOS::gather_particles(OPT);
    }
    else {
        ICHNOS::Domain2D domain(OPT.Dopt);
        bool tf;
        switch (OPT.xyztype){
            case ICHNOS::XYZType::CLOUD:
            {
                world.barrier();
                ICHNOS::XYZ_cloud XYZ(world);
                tf = XYZ.readXYZdata(OPT.getVelFname());
                if (!tf){return 0;}

                switch (OPT.velocityFieldType) {
                    case ICHNOS::VelType::TRANS:
                    {
                        world.barrier();
                        TRANS::transVel VF(world);
                        tf = VF.readVelocityField(OPT.getVelFname(), XYZ.getNpnts());
                        if (!tf){return 0;}
                        VF.SetStepOptions(OPT.Popt.StepOpt);

                        ICHNOS::ParticleTrace pt(world, XYZ, VF, domain, OPT.Popt);
                        world.barrier();
                        if (world.rank() == 0)
                            std::cout << "Tracing particles..." << std::endl;
                        pt.Trace();
                        break;
                    }
                    case ICHNOS::VelType::STOCH:
                    {
                        break;
                    }

                }

                break;
            }
            case ICHNOS::XYZType::IWFM:
            {
                ICHNOS::XYZ_IWFM XYZ(world);
                tf = XYZ.readXYZdata(OPT.getVelFname());
                if (!tf){return 0;}

                switch (OPT.velocityFieldType) {
                    case ICHNOS::VelType::TRANS:
                    {
                        TRANS::transVel VF(world);
                        tf = VF.readVelocityField(OPT.getVelFname(), XYZ.getNpnts());
                        if (!tf){return 0;}
                        VF.SetStepOptions(OPT.Popt.StepOpt);

                        ICHNOS::ParticleTrace pt(world, XYZ, VF, domain, OPT.Popt);
                        if (world.rank() == 0)
                            std::cout << "Tracing particles..." << std::endl;
                        pt.Trace();
                        break;
                    }
                    case ICHNOS::VelType::STOCH:
                    {
                        break;
                    }

                break;
            }
        }

//        switch (OPT.velocityFieldType) {
//        case ICHNOS::VelType::STEADY:
//        {
//            NPSAT::npsatVel VF(world, ICHNOS::VelType::STEADY);
//            VF.readVelocityField(OPT.getVelFname());
//            ICHNOS::Domain2D domain(OPT.Dopt);
//            ICHNOS::ParticleTrace pt(world, VF, domain, OPT.Popt);
//            if (world.rank() == 0)
//                std::cout << "Tracing particles..." << std::endl;
//            pt.Trace();
//            break;
//        }
//        case ICHNOS::VelType::TRANS:
//        {
//            TRANS::transVel VF(world, ICHNOS::VelType::TRANS);
//            VF.readVelocityField(OPT.getVelFname());
//            ICHNOS::Domain2D domain(OPT.Dopt);
//            OPT.Popt.bIsTransient = true;
//            ICHNOS::ParticleTrace pt(world, VF, domain, OPT.Popt);
//            if (world.rank() == 0)
//                std::cout << "Tracing particles..." << std::endl;
//            pt.Trace();
//            break;
//        }
//        case ICHNOS::VelType::STOCH:
//        {
//            STOCH::MarkovChainVel VF(world, ICHNOS::VelType::STOCH);
//            ICHNOS::Domain2D domain(OPT.Dopt);
//            VF.readVelocityField(OPT.getVelFname());
//            ICHNOS::ParticleTrace pt(world, VF, domain, OPT.Popt);
//            for (int i = 0; i < OPT.Popt.Nrealizations; ++i) {
//                if (world.rank() == 0)
//                    std::cout << "||======Realization " << i << "======" << std::endl;
//                pt.Trace(i);
//            }
//            break;
//        }
//        default:
//        {
//            std::cout << "Invalid velocity type" << std::endl;
//            break;
//        }
       }
    }

    //std::cout << world.rank() << " has " << VF.getCloudSize() << " points" << std::endl;
    world.barrier();
    if (world.rank() == 0){
        std::time_t result = std::time(nullptr);
        std::cout << "Ichnos Finished at " << std::asctime(std::localtime(&result)) << std::endl;
    }

    return 0;
   }

//100 1 691636.7 4160195 -100.2
//100 1 646753 4263260 - 50.2
//100 1 854295 3966620 -150.2
//100 1 804115 4006950 -200.2 Best so far


#include <armadillo>
//#include <fstream>
//#include <set>
//#include <sstream>
//#include <string>
//#include <algorithm>
//#include <map>
//#include <iterator>
//#include <unordered_set>
//#include <utility>

// #include "utils/hdf5.h"
//#include "utils/string.h"
#include "utils/profiling.h"

extern "C" {
  int mymain( int, arma::uword*, arma::uword*, double* );
    void (*callhaskell)( void );
    void registerfun( void(*)(void) );
}

using namespace util;

extern void (*callhaskell)( void );
void registerfun( void (*f)(void) ) {
    callhaskell=  f;
};

// armadillo: column-major ordering

int mymain( int count, arma::uword* prow, arma::uword* pcol, double* pval ){
    auto timer = Timer{};
    std::vector<uint64_t> vloc(2*count);
    vloc.insert(vloc.begin(),prow,prow+count);
    vloc.insert(vloc.begin()+count,pcol,pcol+count);
    std::vector<double> vval(count);
    vval.assign(pval,pval+count);
    arma::vec value(vval);

    arma::umat locationt ((arma::uword*)vloc.data(), (arma::uword)count, (arma::uword)2);
    arma::umat location = locationt.t();

    arma::sp_mat mat(location, value) ;

    timer.here_then_reset("Move matrix done!\n");

    arma::mat U;
    arma::vec s;
    arma::mat V;

    arma::svds(U, s, V, mat, 100);

    timer.here_then_reset("Completed SVD calculation!\n");

    for( auto d : s ) {
      std::cout << d << "," ;
    }
    std::cout << std::endl;

    std::cout << "Finished!" << std::endl;

    callhaskell();

    std::cout << "After callhaskell()" << std::endl;

    return 0;
}


//
// Created by yche on 12/13/17.
//

#include <iostream>

#include <boost/program_options.hpp>

#include "simrank.h"

int main(int argc, char *argv[]) {
    string data_name = argv[1];
    int a = atoi(argv[2]);
    int b = atoi(argv[3]);
    DirectedG g;
    load_graph("./datasets/edge_list/" + data_name + ".txt", g);

    TruthSim ts(data_name, g, 0.6, 0.00001);
    cout << format("ground truth: %s") % ts.sim(a, b) << endl;
}


#include <I3Test.h>

#include "common.h"
#include "MuonGun/Generator.h"
#include "MuonGun/CORSIKAGenerationProbability.h"
#include "MuonGun/Cylinder.h"

#include <boost/make_shared.hpp>

TEST_GROUP(Generator);

TEST(Addition)
{
	using namespace I3MuonGun;
	using boost::make_shared;

	BundleModel soft = load_model("Hoerandel5_atmod12_SIBYLL");
	BundleModel hard = load_model("GaisserH4a_atmod12_SIBYLL");

	boost::shared_ptr<CORSIKAGenerationProbability> soft_g(
	    new CORSIKAGenerationProbability(make_shared<Cylinder>(1600, 800), soft.flux, soft.radius, soft.energy));
	boost::shared_ptr<CORSIKAGenerationProbability> hard_g(
	    new CORSIKAGenerationProbability(make_shared<Cylinder>(1600, 800), hard.flux, hard.radius, hard.energy));

	ENSURE(soft_g->IsCompatible(soft_g));
	ENSURE_EQUAL((soft_g+soft_g)->GetTotalEvents(), 2.);
	ENSURE((bool)boost::dynamic_pointer_cast<const CORSIKAGenerationProbability>(soft_g+soft_g),
	    "Sum of compatible generators is a generator of the same type");
	ENSURE((bool)boost::dynamic_pointer_cast<const CORSIKAGenerationProbability>(soft_g+soft_g+soft_g),
	    "Sum of compatible generators is a generator of the same type");

	ENSURE(!soft_g->IsCompatible(hard_g));
	ENSURE_EQUAL((soft_g+hard_g)->GetTotalEvents(), 1.);
	ENSURE((bool)boost::dynamic_pointer_cast<const GenerationProbabilityCollection>(soft_g+hard_g),
	    "Sum of incompatible generators is a collection");
}


#ifndef COLOR_HH
#define COLOR_HH

#include <Eigen/Dense>
#include <iostream>

namespace color {

using Color = Eigen::Vector3d;

/**
 * @brief Writes a color into some stream in PPM format
 *
 * @param out Stream to write to
 * @param color Color to write
 */
void write_color(std::ostream& out, Color color);

} // namespace color

#endif // COLOR_HH


//
// Created by AIYOJ on 2019-03-02.
//

#include "calibration/prior_correction.h"
#include <fstream>
#include <boost/lexical_cast.hpp>
#include "log/algocomp_log.h"
#include <iostream>
#include <boost/algorithm/string.hpp>
#include <exception>

namespace algocomp {
    PriorCorrection::PriorCorrection() {
        _logger = spdlog::get(ACLog::_logger_name);
        _logger->info("Initialize Prior Correction.");
    }

    PriorCorrection::~PriorCorrection() {
        _logger->info("Leave Prior Correction.");
    }

    std::vector<float> PriorCorrection::calibrate(const std::vector<float> &scores) {
        std::vector<float> calibrated_scores;
        calibrated_scores.reserve(scores.size());
        std::for_each(scores.begin(), scores.end(), [&calibrated_scores, this](float score) {
            float calibrated_score = score / (score + (1 - score) * (1 / _neg_smp_r));
            calibrated_scores.push_back(calibrated_score);
        });

        return calibrated_scores;
    }

    float PriorCorrection::calibrate(float score) {
        float calibrated_score = score / (score + (1 - score) * (1 / _neg_smp_r));
        return calibrated_score;
    }

    std::unique_ptr<PriorCorrection> PriorCorrection::load(const std::string &path) {
        auto logger = spdlog::get(ACLog::_logger_name);

        try {
            std::ifstream calibration_file;
            calibration_file.open(path);
            if (calibration_file.is_open()) {
                std::unique_ptr<PriorCorrection> output_calibration = std::unique_ptr<PriorCorrection>(
                        new PriorCorrection());
                std::string line;
                int read = 0;
                while (!calibration_file.eof()) {
                    std::getline(calibration_file, line);
                    line = boost::algorithm::trim_copy(line);
                    if (line.length() == 0) {
                        continue;
                    }

                    float neg_smp_r = boost::lexical_cast<float>(line);
                    output_calibration->update(neg_smp_r);

                    read++;
                    if (read == 1) {
                        break;
                    }
                }
                if (calibration_file.is_open()) {
                    calibration_file.close();
                }
                if (output_calibration) {
                    logger->info("Natively loaded prior correction from {}.", path);
                    return output_calibration;
                } else {
                    logger->error("Fail to read model file when natively loading prior correction from {}.", path);
                }

            } else {
                logger->error("Fail to open prior correction file when natively loading prior correction from {}.",
                              path);
            }

        } catch (std::exception &ex) {
            logger->error("Fail to natively load prior correction from {}, detail: {}.", path, ex.what());
        }
        return nullptr;
    }

    void PriorCorrection::update(float neg_smp_r) {
        _neg_smp_r = neg_smp_r;
    }
}


#include <frovedis.hpp>
#include <frovedis/matrix/sliced_vector.hpp>
#include <frovedis/matrix/sliced_matrix.hpp>
#include <frovedis/matrix/blas_wrapper.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    colmajor_matrix_local<float> cm (
             make_rowmajor_matrix_local_load<float>("./sample_6x6"));

    // creating slice matrix from colmajor matrix (error, invalid range)
    bool error_in_slice_matrix_creation = false;
    sliced_colmajor_matrix_local<float> sm;
    try {
      sm = make_sliced_colmajor_matrix_local<float>(cm,1,1,6,6);
    }
    catch (std::exception& e) {
      error_in_slice_matrix_creation = true;
    }

    // creating slice matrix from colmajor matrix (OK)
    sm = make_sliced_colmajor_matrix_local<float>(cm,1,1,4,4);

    // creating slice vectors from slice matrix
    bool error_in_row_vector_creation = false;
    sliced_colmajor_vector_local<float> sv;
    try {
      sv = make_row_vector(sm,4); // error, invalid row-id
    }
    catch (std::exception& e) {
      error_in_row_vector_creation = true;
    }

    // confirming if above expected exceptions occur
    BOOST_CHECK (error_in_slice_matrix_creation);
    BOOST_CHECK (error_in_row_vector_creation);
}