#pragma once
#include <Eigen/Dense>
#include <string>
#include <vector>
#include <opencv2/core.hpp>
#include <boost/algorithm/string.hpp>
#include "pointmatcher/PointMatcher.h"

typedef PointMatcher<double> PM;
typedef PM::DataPoints DP;
using namespace PointMatcherSupport;  // NOLINT

/*!
   \brief Retrieves a vector of the (radar) file names in ascending order of time stamp
   \param datadir (absolute) path to the directory that contains (radar) files
   \param radar_files [out] A vector to be filled with a string for each file name
   \param extension Optional argument to specify the desired file extension. Files without this extension are rejected
*/
void get_file_names(std::string datadir, std::vector<std::string> &radar_files, std::string extension = "");

/*!
   \brief Retrieves lidar pointcloud data from an absolute file path.

   The ground truth transform associated with the pointcloud is associated with the timestamp of point [0].

   \param path absolute path to the lidar file
   \param pc 4 x N matrix of points (x, y, z, 1) x N
   \param intensties 1 x N matrix of infrared reflectivities / intensties associated with each point
   \param times vector of timestamps associated with each point. Seconds since the start of the hour in GPS time.

*/
void load_velodyne(std::string path, Eigen::MatrixXd &pc, Eigen::MatrixXd & intensities,
    std::vector<float> &times);

/*!
   \brief Retrieves an image from an absolute file path.
   \param path absolute path to the image file
   \param img The retrieved image will be placed here.
*/
void load_image(std::string path, cv::Mat &img);

/*!
   \brief Retrieve the ground truth pose and velocity information
   \param gtfile absolute path to the ground truth file (lidar_poses.csv)
   \param sensor_file Name of the sensor file for which we want ground truth (<timestamp>.bin)
   \param gt Output vector of ground truth information:
        GPSTime,x,y,z,vel_x,vel_y,vel_z,roll,pitch,heading,ang_vel_z
*/
bool get_groundtruth_data(std::string gtfile, std::string sensor_file, std::vector<double> &gt);

// Same as the function above except for odometry files with the following format:
// TIME1,TIME2,x,y,z,yaw,pitch,roll
// Output vector contents: x,y,z,yaw,pitch,roll
bool get_odom_data(std::string gtfile, std::string file1, std::string file2, std::vector<double> &gt);

Eigen::Matrix3d roll(double r);

Eigen::Matrix3d pitch(double p);

Eigen::Matrix3d yaw(double y);

/*!
   \brief Retrieve the 4x4 homogeneous transformation matrix from the line of groundtruth.
   \param gt Vector of ground truth information: GPSTime,x,y,z,vel_x,vel_y,vel_z,roll,pitch,heading,ang_vel_z
   \return 4x4 homogeneous transform: T_enu_sensor
*/
Eigen::Matrix4d getTransformFromGT(std::vector<double> gt);

/*!
   \brief Save a 4x4 homogeneous transform to a text file.
   \param path absolute path to the output text file.
   \param T input 4x4 homogeneous transform.
*/
void save_transform(std::string path, Eigen::Matrix4d T);

/*!
   \brief Load a 4x4 homogeneous transform from a text file.
   \param path absolute path to the input text file.
   \param T output 4x4 homogeneous transform.
*/
void load_transform(std::string path, Eigen::Matrix4d &T);

/*!
   \brief outputs a booleans depending on whether the frame is within a valid time window.
*/
bool filter_on_time(std::string fname, std::vector<std::vector<int>> valid_times);

void filterFrames(std::vector<std::string> &frames, std::vector<std::vector<int>> valid_times);

/*!
   \brief Extracts the file name, ex: <timestamp> from <timestamp>.png
*/
void get_name_from_file(std::string file, std::string &name);

/*!
   \brief Prints the contents of a vector.
*/
template<typename T>
void print_vec(std::vector<T> v) {
    for (uint i = 0; i < v.size(); ++i) {
        std::cout << v[i] << " ";
    }
    std::cout << std::endl;
}

/*!
   \brief Calculates the BGR pixel value for each point that projects onto the image plane using bilinear interp
   \param pc Input pointcloud in lidar frame
   \param img Input image that corresponds to the lidar frame (closest in time)
   \param P 4x4 homogeneous camera perspective projection matrix [fx 0 cx 0; 0 fy cy 0; 0 0 1 0; 0 0 0 1]
   \param T_cam_lidar 4x4 homogeneous transformation matrix. transforms points from lidar frame to camera frame
   \param point_colors Output matrix (3 x N) of BGR pixel values. Default is zero for points without a color
*/
void get_point_colors(DP pc, cv::Mat img, Eigen::Matrix4d P, Eigen::MatrixXd T_cam_lidar,
    Eigen::MatrixXd &point_colors);

/*!
   \brief Where file names are UNIX EPOCH times, this function finds the closest_frame to the src_file within tgt_files.
*/
bool get_closest_frame(std::string src_file, std::vector<std::string> &tgt_files, std::string &closest_frame);

/*!
   \brief Colorizes a pointcloud by adding blue, green, red descriptors for all points that project onto the image plane
        The closest camera image in time is used for colorization. Default is zero for uncolored points.
   \param cloud Input pointcloud that will be colorized.
   \param T_enu_lidar 4x4 homogeneous transform from lidar frame to ENU frame
   \param P_cam 4x4 homogeneous camera perspective projection matrix [fx 0 cx 0; 0 fy cy 0; 0 0 1 0; 0 0 0 1]
   \param lidar_file Name of the current lidar file
   \param cam_files Vector of camera file names
   \param cam_pose_file path to the ground truth pose file for camera images (../../cam_poses.csv)
   \param root path to the root of the data directory
*/
void colorize_cloud(DP &cloud, Eigen::Matrix4d T_enu_lidar, Eigen::Matrix4d P_cam, std::string lidar_file,
    std::vector<std::string> &cam_files, std::string cam_pose_file, std::string root);

/*!
   \brief Load arguments from the command line and check their validity.
*/
int validateArgs(const int argc, const char *argv[], std::string &root, std::string &config);

void getMapFrames(std::string root, std::vector<std::string> &frame_names);


//  (C) Copyright Gennadiy Rozental 2001-2005.
//  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)

//  See http://www.boost.org/libs/test for the library home page.
//
//  File        : $RCSfile: floating_point_comparison.hpp,v $
//
//  Version     : $Revision: 1.26.2.2 $
//
//  Description : defines algoirthms for comparing 2 floating point values
// ***************************************************************************

#ifndef BOOST_TEST_FLOATING_POINT_COMPARISON_HPP_071894GER
#define BOOST_TEST_FLOATING_POINT_COMPARISON_HPP_071894GER

#include <boost/limits.hpp>  // for std::numeric_limits

#include <boost/test/utils/class_properties.hpp>

#include <boost/test/detail/suppress_warnings.hpp>

//____________________________________________________________________________//

namespace boost {

namespace test_tools {

using unit_test::readonly_property;

// ************************************************************************** //
// **************        floating_point_comparison_type        ************** //
// ************************************************************************** //

enum floating_point_comparison_type {
    FPC_STRONG, // "Very close"   - equation 1' in docs, the default
    FPC_WEAK    // "Close enough" - equation 2' in docs.

};

// ************************************************************************** //
// **************                    details                   ************** //
// ************************************************************************** //

namespace tt_detail {

// FPT is Floating-Point type, float, double, long double or User-Defined.

template<typename FPT>
inline FPT
fpt_abs( FPT arg )
{
    return arg < static_cast<FPT>(0) ? -arg : arg;
}

//____________________________________________________________________________//

// both f1 and f2 are unsigned here
template<typename FPT>
inline FPT
safe_fpt_division( FPT f1, FPT f2 )
{
    // Avoid overflow.
    if( f2 < static_cast<FPT>(1)  && f1 > f2 * (std::numeric_limits<FPT>::max)() )
        return (std::numeric_limits<FPT>::max)();

    // Avoid underflow.
    if( f1 == static_cast<FPT>(0) ||
        f2 > static_cast<FPT>(1) && f1 < f2 * (std::numeric_limits<FPT>::min)() )
        return static_cast<FPT>(0);

    return f1/f2;
}

//____________________________________________________________________________//

} // namespace tt_detail

// ************************************************************************** //
// **************         tolerance presentation types         ************** //
// ************************************************************************** //

template<typename FPT>
struct percent_tolerance_t {
    explicit    percent_tolerance_t( FPT v ) : m_value( v ) {}

    FPT m_value;
};

//____________________________________________________________________________//

template<typename Out,typename FPT>
Out& operator<<( Out& out, percent_tolerance_t<FPT> t )
{
    return out << t.m_value;
}

//____________________________________________________________________________//

template<typename FPT>
inline percent_tolerance_t<FPT>
percent_tolerance( FPT v )
{
    return percent_tolerance_t<FPT>( v );
}

//____________________________________________________________________________//

template<typename FPT>
struct fraction_tolerance_t {
    explicit fraction_tolerance_t( FPT v ) : m_value( v ) {}

    FPT m_value;
};

//____________________________________________________________________________//

template<typename Out,typename FPT>
Out& operator<<( Out& out, fraction_tolerance_t<FPT> t )
{
    return out << t.m_value;
}

//____________________________________________________________________________//

template<typename FPT>
inline fraction_tolerance_t<FPT>
fraction_tolerance( FPT v )
{
    return fraction_tolerance_t<FPT>( v );
}

//____________________________________________________________________________//

// ************************************************************************** //
// **************             close_at_tolerance               ************** //
// ************************************************************************** //

template<typename FPT>
class close_at_tolerance {
public:
    // Public typedefs
    typedef bool result_type;

    // Constructor
    template<typename ToleranceBaseType>
    explicit    close_at_tolerance( percent_tolerance_t<ToleranceBaseType>  tolerance,
                                    floating_point_comparison_type          fpc_type = FPC_STRONG )
    : p_fraction_tolerance( tt_detail::fpt_abs( static_cast<FPT>(0.01)*tolerance.m_value ) )
    , p_strong_or_weak( fpc_type ==  FPC_STRONG )
    {}
    template<typename ToleranceBaseType>
    explicit    close_at_tolerance( fraction_tolerance_t<ToleranceBaseType> tolerance,
                                    floating_point_comparison_type          fpc_type = FPC_STRONG )
    : p_fraction_tolerance( tt_detail::fpt_abs( tolerance.m_value ) )
    , p_strong_or_weak( fpc_type ==  FPC_STRONG )
    {}

    bool        operator()( FPT left, FPT right ) const
    {
        FPT diff = tt_detail::fpt_abs( left - right );
        FPT d1   = tt_detail::safe_fpt_division( diff, tt_detail::fpt_abs( right ) );
        FPT d2   = tt_detail::safe_fpt_division( diff, tt_detail::fpt_abs( left ) );

        return p_strong_or_weak
                   ? (d1 <= p_fraction_tolerance && d2 <= p_fraction_tolerance)
                   : (d1 <= p_fraction_tolerance || d2 <= p_fraction_tolerance);
    }

    // Public properties
    readonly_property<FPT>  p_fraction_tolerance;
    readonly_property<bool> p_strong_or_weak;
};

//____________________________________________________________________________//

// ************************************************************************** //
// **************               check_is_close                 ************** //
// ************************************************************************** //

struct BOOST_TEST_DECL check_is_close_t {
    // Public typedefs
    typedef bool result_type;

    template<typename FPT, typename ToleranceBaseType>
    bool
    operator()( FPT left, FPT right, percent_tolerance_t<ToleranceBaseType> tolerance,
                floating_point_comparison_type fpc_type = FPC_STRONG )
    {
        close_at_tolerance<FPT> pred( tolerance, fpc_type );

        return pred( left, right );
    }
    template<typename FPT, typename ToleranceBaseType>
    bool
    operator()( FPT left, FPT right, fraction_tolerance_t<ToleranceBaseType> tolerance,
                floating_point_comparison_type fpc_type = FPC_STRONG )
    {
        close_at_tolerance<FPT> pred( tolerance, fpc_type );

        return pred( left, right );
    }
};

namespace {
check_is_close_t check_is_close;
}

//____________________________________________________________________________//

// ************************************************************************** //
// **************               check_is_small                 ************** //
// ************************************************************************** //

struct BOOST_TEST_DECL check_is_small_t {
    // Public typedefs
    typedef bool result_type;

    template<typename FPT>
    bool
    operator()( FPT fpv, FPT tolerance )
    {
        return tt_detail::fpt_abs( fpv ) < tt_detail::fpt_abs( tolerance );
    }
};

namespace {
check_is_small_t check_is_small;
}

//____________________________________________________________________________//

} // namespace test_tools

} // namespace boost

//____________________________________________________________________________//

#include <boost/test/detail/enable_warnings.hpp>

// ***************************************************************************
//  Revision History :
//
//  $Log: floating_point_comparison.hpp,v $
//  Revision 1.26.2.2  2006/11/30 14:41:21  jhunold
//  Merge from HEAD: Remove unnecessary export makro.
//
//  Revision 1.26.2.1  2006/05/22 17:39:12  johnmaddock
//  Fix min/max guidelines violation.
//
//  Revision 1.26  2006/03/16 07:31:06  vladimir_prus
//  Fix compile error on MSVC due to max and min being defined as macros.
//
//  Revision 1.25  2006/03/13 18:28:25  rogeeff
//  warnings eliminated
//
//  Revision 1.24  2005/12/14 05:07:28  rogeeff
//  introduced an ability to test on closeness based on either percentage dirven tolerance or fraction driven one
//
//  Revision 1.23  2005/05/29 08:54:57  rogeeff
//  allow bind usage
//
//  Revision 1.22  2005/02/21 10:21:40  rogeeff
//  check_is_small implemented
//  check functions implemented as function objects
//
//  Revision 1.21  2005/02/20 08:27:05  rogeeff
//  This a major update for Boost.Test framework. See release docs for complete list of fixes/updates
//
//  Revision 1.20  2005/02/01 06:40:06  rogeeff
//  copyright update
//  old log entries removed
//  minor stilistic changes
//  depricated tools removed
//
//  Revision 1.19  2005/01/22 19:22:12  rogeeff
//  implementation moved into headers section to eliminate dependency of included/minimal component on src directory
//
// ***************************************************************************

#endif // BOOST_FLOATING_POINT_COMAPARISON_HPP_071894GER


/*
 * GridDistribution.cpp
 *
 *  Created on: 22.02.2018
 *      Author: thies
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/utilities.h>

#include <measurements/GridDistribution.h>
#include <measurements/SensorDistribution.h>

#include <cstdio>
#include <fstream>
#include <iostream>

namespace wavepi {
namespace measurements {
using namespace dealii;

template <>
void GridDistribution<1>::update_grid(const std::vector<double> &times,
                                      const std::vector<std::vector<double>> &points_per_dim) {
  this->points_per_dim = points_per_dim;

  Assert(points_per_dim.size() == 1, ExcInternalError());
  size_t nb_points = points_per_dim[0].size();

  std::vector<Point<1>> points_each_time(nb_points);

  for (size_t ix = 0; ix < points_per_dim[0].size(); ix++)
    points_each_time[ix] = Point<1>(points_per_dim[0][ix]);

  std::vector<std::vector<Point<1>>> points_per_time(times.size());

  for (size_t i = 0; i < times.size(); i++)
    points_per_time[i] = points_each_time;

  update_points(times, points_per_time);

  // update times_per_point and points now (cleared by update_points!)
  points = points_each_time;
  times_per_point.reserve(nb_points);

  for (size_t ix = 0; ix < nb_points; ix++)
    times_per_point.push_back(times);
}

template <>
void GridDistribution<2>::update_grid(const std::vector<double> &times,
                                      const std::vector<std::vector<double>> &points_per_dim) {
  this->points_per_dim = points_per_dim;

  Assert(points_per_dim.size() == 2, ExcInternalError());
  size_t nb_points = points_per_dim[0].size() * points_per_dim[1].size();

  std::vector<Point<2>> points_each_time(nb_points);

  for (size_t ix = 0; ix < points_per_dim[0].size(); ix++)
    for (size_t iy = 0; iy < points_per_dim[1].size(); iy++)
      points_each_time[ix * points_per_dim[1].size() + iy] = Point<2>(points_per_dim[0][ix], points_per_dim[1][iy]);

  std::vector<std::vector<Point<2>>> points_per_time(times.size());

  for (size_t i = 0; i < times.size(); i++)
    points_per_time[i] = points_each_time;

  update_points(times, points_per_time);

  // update times_per_point and points now (cleared by update_points!)
  points = points_each_time;
  times_per_point.reserve(nb_points);

  for (size_t ix = 0; ix < nb_points; ix++)
    times_per_point.push_back(times);
}

template <>
void GridDistribution<3>::update_grid(const std::vector<double> &times,
                                      const std::vector<std::vector<double>> &points_per_dim) {
  this->points_per_dim = points_per_dim;

  Assert(points_per_dim.size() == 3, ExcInternalError());
  size_t nb_points = points_per_dim[0].size() * points_per_dim[1].size() * points_per_dim[2].size();

  std::vector<Point<3>> points_each_time(nb_points);

  for (size_t ix = 0; ix < points_per_dim[0].size(); ix++)
    for (size_t iy = 0; iy < points_per_dim[1].size(); iy++)
      for (size_t iz = 0; iz < points_per_dim[2].size(); iz++)
        points_each_time[ix * points_per_dim[1].size() * points_per_dim[2].size() + iy * points_per_dim[2].size() +
                         iz] = Point<3>(points_per_dim[0][ix], points_per_dim[1][iy], points_per_dim[2][iz]);

  std::vector<std::vector<Point<3>>> points_per_time(times.size());

  for (size_t i = 0; i < times.size(); i++)
    points_per_time[i] = points_each_time;

  update_points(times, points_per_time);

  // update times_per_point and points now (cleared by update_points!)
  points = points_each_time;
  times_per_point.reserve(nb_points);

  for (size_t ix = 0; ix < nb_points; ix++)
    times_per_point.push_back(times);
}

template <int dim>
GridDistribution<dim>::GridDistribution(const std::vector<double> &times,
                                        const std::vector<std::vector<double>> &points_per_dim) {
  update_grid(times, points_per_dim);
}

template <int dim>
void GridDistribution<dim>::declare_parameters(ParameterHandler &prm) {
  prm.enter_subsection("GridDistribution");
  {
    prm.declare_entry("points x", "-1:10:1", Patterns::Anything(),
                      "points for the grid in x-direction. Format: '[lb]:[n_points]:[ub]'. Lower bound and upper bound "
                      "are exclusive.");
    prm.declare_entry("points y", "-1:10:1", Patterns::Anything(),
                      "points for the grid in y-direction. Format: '[lb]:[n_points]:[ub]'. Lower bound and upper bound "
                      "are exclusive.");
    prm.declare_entry("points z", "-1:10:1", Patterns::Anything(),
                      "points for the grid in z-direction. Format: '[lb]:[n_points]:[ub]'. Lower bound and upper bound "
                      "are exclusive.");
    prm.declare_entry("points t", "0:10:6", Patterns::Anything(),
                      "points for the grid in time. Format: '[lower bound]:[n_points]:[ub]'. Upper bound is inclusive, "
                      "lower bound is exclusive iff it equals 0.0.");
  }
  prm.leave_subsection();
}

template <int dim>
void GridDistribution<dim>::get_parameters(ParameterHandler &prm) {
  prm.enter_subsection("GridDistribution");
  {
    AssertThrow(0 <= dim && dim <= 3, ExcInternalError());

    std::vector<std::vector<double>> spatial_points;
    spatial_points.emplace_back(parse_description(prm.get("points x")));
    if (dim > 1) spatial_points.emplace_back(parse_description(prm.get("points y")));
    if (dim > 2) spatial_points.emplace_back(parse_description(prm.get("points z")));

    auto temporal_points = parse_description(prm.get("points t"), true);
    update_grid(temporal_points, spatial_points);
  }
  prm.leave_subsection();
}

template <int dim>
std::vector<double> GridDistribution<dim>::parse_description(const std::string description, bool is_time) {
  double lb, ub;
  size_t nb;

  AssertThrow(std::sscanf(description.c_str(), " %lf : %zu : %lf ", &lb, &nb, &ub) == 3,
              ExcMessage("Could not parse points"));
  AssertThrow((is_time && nb > 1 && lb < ub && lb >= 0.0) || (!is_time && nb >= 1 && lb <= ub),
              ExcMessage("Illegal interval spec: " + description));

  std::vector<double> points(nb);

  if (lb == 0.0 && is_time)  // lb excl, ub incl
    for (size_t i = 0; i < nb; i++)
      points[i] = lb + (i + 1) * (ub - lb) / nb;
  else if (lb > 0.0 && is_time)  // lb incl, ub incl
    for (size_t i = 0; i < nb; i++)
      points[i] = lb + (i + 1) * (ub - lb) / (nb - 1);
  else
    // lb excl, ub excl
    for (size_t i = 0; i < nb; i++)
      points[i] = lb + (i + 1) * (ub - lb) / (nb + 1);

  return points;
}

template <int dim>
size_t GridDistribution<dim>::index_times_per_point(size_t point_index, size_t time_index) {
  return time_index * this->points_per_time[0].size() + point_index;
}

template <int dim>
void GridDistribution<dim>::write_pvd(const std::vector<double> &values, std::string path, std::string filename,
                                      std::string name) {
  size_t extentX           = points_per_dim[0].size() - 1;
  size_t extentY           = dim > 1 ? points_per_dim[1].size() - 1 : 0;
  size_t extentZ           = dim > 2 ? points_per_dim[2].size() - 1 : 0;
  size_t nb_spatial_points = this->points_per_time[0].size();

  std::ofstream fpvd(path + filename + ".pvd", std::ios::out | std::ios::trunc);
  fpvd.precision(8);
  fpvd << std::fixed << "<?xml version=\"1.0\"?>" << std::endl
       << "<VTKFile type=\"Collection\" version=\"0.1\" byte_order=\"LittleEndian\">" << std::endl
       << "<Collection>";

  for (size_t ti = 0; ti < this->times.size(); ti++) {
    fpvd << "<DataSet timestep=\"" << this->times[ti] << "\" group=\"\" part=\"0\" file=\"" << filename << "-"
         << Utilities::to_string(ti, 4) << ".vts\"/>" << std::endl;

    std::ofstream fvts(path + filename + "-" + Utilities::to_string(ti, 4) + ".vts", std::ios::out | std::ios::trunc);

    fvts << std::fixed
         << "<VTKFile type=\"StructuredGrid\" version=\"1.0\" byte_order=\"LittleEndian\" header_type=\"UInt64\">"
         << std::endl
         << "<StructuredGrid WholeExtent=\"0 " << extentX << " 0 " << extentY << " 0 " << extentZ << "\">" << std::endl
         << "<Piece Extent=\"0 " << extentX << " 0 " << extentY << " 0 " << extentZ << "\">" << std::endl
         << "<PointData Scalars=\"" << name << "\">" << std::endl
         << "<DataArray type=\"Float64\" Name=\"" << name << "\" format=\"ascii\">" << std::endl;

    fvts.precision(16);
    fvts << std::scientific;

    for (size_t i = 0; i < nb_spatial_points; i++)
      fvts << values[nb_spatial_points * ti + i] << " ";

    fvts << std::endl
         << "</DataArray>" << std::endl
         << "</PointData>" << std::endl
         << "<CellData></CellData>" << std::endl
         << "<Points>" << std::endl
         << "<DataArray type=\"Float64\" Name=\"Points\" NumberOfComponents=\"3\" format=\"ascii\">" << std::endl;

    fvts.precision(6);
    fvts << std::fixed;

    for (auto p : this->points_per_time[ti])
      fvts << p(0) << " " << (dim > 1 ? p(1) : 0.0) << " " << (dim > 2 ? p(2) : 0.0) << " ";

    fvts << std::endl
         << "</DataArray>" << std::endl
         << "</Points>" << std::endl
         << "</Piece>" << std::endl
         << "</StructuredGrid>" << std::endl
         << "</VTKFile>" << std::endl;
    fvts.close();
  }

  fpvd << "</Collection>" << std::endl << "</VTKFile>" << std::endl;
  fpvd.close();
}

template class GridDistribution<1>;
template class GridDistribution<2>;
template class GridDistribution<3>;

} /* namespace measurements */
} /* namespace wavepi */


#include <mc_rbdyn/RobotLoader.h>
#include <mc_rbdyn/Robots.h>
#include <mc_rbdyn/rpy_utils.h>
#include <boost/test/unit_test.hpp>
#include "utils.h"

mc_rbdyn::Robots & get_robots()
{
  static std::shared_ptr<mc_rbdyn::Robots> robots_ptr = nullptr;
  if(robots_ptr)
  {
    return *robots_ptr;
  }
  configureRobotLoader();
  auto rm = mc_rbdyn::RobotLoader::get_robot_module("JVRC1");
  auto env = mc_rbdyn::RobotLoader::get_robot_module("env", std::string(mc_rtc::MC_ENV_DESCRIPTION_PATH),
                                                     std::string("ground"));
  robots_ptr = mc_rbdyn::loadRobots({rm, env});
  return *robots_ptr;
}

BOOST_AUTO_TEST_CASE(TestRobotPosWVelW)
{
  auto robots = get_robots();

  for(int i = 0; i < 100; ++i)
  {
    Eigen::Vector3d oriRPYRef = Eigen::Vector3d::Random();
    Eigen::Vector3d posRPYRef = Eigen::Vector3d::Random();
    sva::PTransformd refPosW(mc_rbdyn::rpyToMat(oriRPYRef), posRPYRef);
    robots.robot().posW(refPosW);
    BOOST_CHECK(robots.robot().posW().matrix().isApprox(refPosW.matrix()));
  }

  for(int i = 0; i < 100; ++i)
  {
    auto refVal = sva::MotionVecd{Eigen::Vector3d::Random(), Eigen::Vector3d::Random()};
    robots.robot().velW(refVal);
    auto actual = robots.robot().velW();
    BOOST_CHECK_MESSAGE(actual.vector().isApprox(refVal.vector()), "Error in Robot::velW"
                                                                       << "\nExpected:"
                                                                       << "\nangular:" << refVal.angular().transpose()
                                                                       << "\nlinear :" << refVal.linear().transpose()
                                                                       << "\nGot:"
                                                                       << "\nangular:" << actual.angular().transpose()
                                                                       << "\nlinear :" << actual.linear().transpose());
  }
}


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

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

     Represents the negatenz function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct negatenz_ : ext::elementwise_<negatenz_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<negatenz_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_negatenz_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::negatenz_, Site> dispatching_negatenz_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::negatenz_, Site>();
   }
   template<class... Args>
   struct impl_negatenz_;
  }
  /*!
    Returns a0 multiplied by the signnz of a1
    The result is unspecified if a1 is NaN

    @par Semantic:

    @code
    T r = negatenz(a0,a1);
    @endcode

    is similar to:

    @code
    T r = a0*signnz(a1)
    @endcode

    @param a0

    @param a1

    @return a value of same type as the inputs
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::negatenz_, negatenz, 2)
} }

#endif


/*
 * GridTools
 *
 * Copyright (c) 2014-2019, ETH Zurich
 * All rights reserved.
 *
 * Please, refer to the LICENSE file in the root directory.
 * SPDX-License-Identifier: BSD-3-Clause
 */

#pragma once

#include <type_traits>

#include <boost/mpl/arithmetic.hpp>
#include <boost/mpl/comparison.hpp>
#include <boost/mpl/integral_c_tag.hpp>

namespace boost {
    namespace mpl {
        /** \ingroup common
            @{
            \ingroup allmeta
            @{
            \ingroup mplutil
            @{
        */

        /** Tag to make std::integral_constants boost::mpl compatible
         */
        template <class T, T V>
        struct equal_to_tag<std::integral_constant<T, V>> {
            using type = integral_c_tag;
        };

        /** Tag to make std::integral_constants boost::mpl compatible
         */
        template <class T, T V>
        struct not_equal_to_tag<std::integral_constant<T, V>> {
            using type = integral_c_tag;
        };

        /** Tag to make std::integral_constants boost::mpl compatible
         */
        template <class T, T V>
        struct less_tag<std::integral_constant<T, V>> {
            using type = integral_c_tag;
        };

        /** Tag to make std::integral_constants boost::mpl compatible
         */
        template <class T, T V>
        struct less_equal_tag<std::integral_constant<T, V>> {
            using type = integral_c_tag;
        };

        /** Tag to make std::integral_constants boost::mpl compatible
         */
        template <class T, T V>
        struct greater_tag<std::integral_constant<T, V>> {
            using type = integral_c_tag;
        };

        /** Tag to make std::integral_constants boost::mpl compatible
         */
        template <class T, T V>
        struct greater_equal_tag<std::integral_constant<T, V>> {
            using type = integral_c_tag;
        };

        /** Tag to make std::integral_constants boost::mpl compatible
         */
        template <class T, T V>
        struct plus_tag<std::integral_constant<T, V>> {
            using type = integral_c_tag;
        };

        /** Tag to make std::integral_constants boost::mpl compatible
         */
        template <class T, T V>
        struct minus_tag<std::integral_constant<T, V>> {
            using type = integral_c_tag;
        };
        /** @} */
        /** @} */
        /** @} */
    } // namespace mpl
} // namespace boost


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

#include <nt2/swar/functions/cumprod.hpp>
#include <nt2/include/functions/aligned_load.hpp>
#include <nt2/include/functions/aligned_store.hpp>
#include <boost/simd/preprocessor/aligned_type.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <nt2/sdk/complex/meta/as_complex.hpp>
#include <nt2/sdk/complex/meta/as_real.hpp>
#include <nt2/sdk/complex/meta/as_dry.hpp>
#include <nt2/include/functions/imag.hpp>
#include <nt2/include/functions/real.hpp>

#include <algorithm>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( cumprod_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<complex_<arithmetic_<A0> > ,X>))
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<A0>::type stype;
      static const size_t size = boost::simd::meta::cardinal_of<A0>::value;
      BOOST_SIMD_ALIGNED_TYPE(stype) tmp[size];
      boost::simd::aligned_store(a0, &tmp[0], 0);
      for(size_t i=1; i!=size; ++i) tmp[i] *= tmp[i-1];
      return boost::simd::aligned_load<A0>(&tmp[0], 0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( cumprod_, tag::cpu_
                                     , (A0)(X)
                                     , ((simd_<dry_<arithmetic_<A0> >,X>))
                                     )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
      {
        return result_type(cumprod(nt2::real(a0)));
      }
  };
} } }

#endif