/// Personal calibration of kappa angle for use with RGB-D gaze estimation

////////////////////
/// DEPENDENCIES ///
////////////////////

// ROS 2
#include <rclcpp/rclcpp.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_ros/transform_listener.h>

// ROS 2 interfaces
#include <geometry_msgs/msg/point_stamped.hpp>
#include <gaze_msgs/msg/gaze_binocular_stamped.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <std_srvs/srv/set_bool.hpp>

// Eigen
#include <Eigen/Geometry>

// PCL
#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>

// File IO
#include <fstream>

/////////////////
/// CONSTANTS ///
/////////////////

/// The name of this node
const std::string NODE_NAME = "rgbd_gaze_accuracy";
/// Size of the queue size used by the synchronizer in its policy
const uint8_t SYNCHRONIZER_QUEUE_SIZE = 50;

/// Index of the left eye
const uint8_t EYE_LEFT = 0;
/// Index of the right eye
const uint8_t EYE_RIGHT = 1;

const float VISUAL_VISUAL_AXIS_LENGTH = 5.0;
const float VISUAL_VISUAL_AXIS_WIDTH = 0.001;
const float VISUAL_VISUAL_AXIS_COLOR[] = {1.0, 0, 0, 1};

const float VISUAL_REAL_GAZE_WIDTH = 0.0015;
const float VISUAL_REAL_GAZE_COLOR[] = {0, 1.0, 0, 1};

// If false, use radians for angular measurements
const bool DEG = true;

/////////////
/// TYPES ///
/////////////

/// Policy of the synchronizer
typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::msg::PoseStamped,
                                                        gaze_msgs::msg::GazeBinocularStamped,
                                                        geometry_msgs::msg::PointStamped>
    synchronizer_policy;

////////////////////////
/// HELPER FUNCTIONS ///
////////////////////////

namespace Eigen
{
  void fromMsg(const geometry_msgs::msg::Vector3 &input, Eigen::Vector3d &output)
  {
    output.x() = input.x;
    output.y() = input.y;
    output.z() = input.z;
  }
} // namespace Eigen

namespace tf2
{
  Eigen::Vector3d transform(Eigen::Vector3d &input, const Eigen::Isometry3d &transformation)
  {
    return transformation * input;
  }

  Eigen::ParametrizedLine<double, 3> transform(Eigen::ParametrizedLine<double, 3> &parametrized_line, const Eigen::Isometry3d &transformation)
  {
    return Eigen::ParametrizedLine<double, 3>(transformation * parametrized_line.origin(), transformation.rotation() * parametrized_line.direction());
  }
} // namespace tf2

double compute_angle(double x1, double y1, double x2, double y2)
{
  double norm1 = std::sqrt(x1 * x1 + y1 * y1);
  double norm2 = std::sqrt(x2 * x2 + y2 * y2);
  x1 /= norm1;
  y1 /= norm1;
  x2 /= norm2;
  y2 /= norm2;

  return std::atan2(x1 * y2 - y1 * x2, x1 * x2 + y1 * y2);
}

//////////////////
/// NODE CLASS ///
//////////////////

/// Class representation of this node
class RgbdGazeAccuracy : public rclcpp::Node
{
public:
  /// Constructor
  RgbdGazeAccuracy();
  /// Destructor
  ~RgbdGazeAccuracy();

private:
  /// Subscriber to the head pose
  message_filters::Subscriber<geometry_msgs::msg::PoseStamped> sub_head_pose_;
  /// Subscriber to the visual axis
  message_filters::Subscriber<gaze_msgs::msg::GazeBinocularStamped> sub_visual_axes_;
  /// Subscriber to the head pose
  message_filters::Subscriber<geometry_msgs::msg::PointStamped> sub_scene_point_;
  /// Synchronizer of the subscribers
  message_filters::Synchronizer<synchronizer_policy> synchronizer_;

  /// Service for enabling data collection
  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr enable_data_collection_service_;
  /// Flag that determines whether to collect data on the next synchronized callback
  bool enable_data_collection_;
  /// File stream for collecting data
  std::ofstream data_file_;

  /// Publisher of visualisation markers
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_markers_;

  /// Buffer for tf2 transforms
  tf2_ros::Buffer tf2_buffer_;
  /// Listener of tf2 transforms
  tf2_ros::TransformListener tf2_listener_;

  uint64_t epoch_;

  void handle_enable_data_collection_service(
      const std::shared_ptr<rmw_request_id_t> request_header,
      const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
      const std::shared_ptr<std_srvs::srv::SetBool::Response> response);
  /// Callback called each time a message is received on all topics
  void synchronized_callback(
      const geometry_msgs::msg::PoseStamped::SharedPtr msg_head_pose,
      const gaze_msgs::msg::GazeBinocularStamped::SharedPtr msg_visual_axes,
      const geometry_msgs::msg::PointStamped::SharedPtr msg_scene_point);
};

RgbdGazeAccuracy::RgbdGazeAccuracy() : Node(NODE_NAME),
                                       sub_head_pose_(this, "head_pose"),
                                       sub_visual_axes_(this, "visual_axes"),
                                       sub_scene_point_(this, "scene_point"),
                                       synchronizer_(synchronizer_policy(SYNCHRONIZER_QUEUE_SIZE), sub_head_pose_, sub_visual_axes_, sub_scene_point_),
                                       tf2_buffer_(this->get_clock()),
                                       tf2_listener_(tf2_buffer_),
                                       enable_data_collection_(true),
                                       epoch_(0)
{
  // Synchronize the subscriptions under a single callback
  synchronizer_.registerCallback(&RgbdGazeAccuracy::synchronized_callback, this);

  // Parameters of the element
  this->declare_parameter<bool>("publish_markers", true);

  // Register publisher of the visualisation markers
  if (this->get_parameter("publish_markers").get_value<bool>())
  {
    rclcpp::QoS qos_markers = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
    pub_markers_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("visualisation_markers", qos_markers);
  }

  // Register a service that begins data collection
  enable_data_collection_service_ = this->create_service<std_srvs::srv::SetBool>("enable_data_collection",
                                                                                std::bind(&RgbdGazeAccuracy::handle_enable_data_collection_service, this,
                                                                                std::placeholders::_1,
                                                                                std::placeholders::_2,
                                                                                std::placeholders::_3));

  // Open file for writing the data
  data_file_.open("rgbd_gaze_data.csv", std::ios::out | std::ios::app);

  // Pring headers into the file
  data_file_ << "epoch,"
             << "L_visual_axis_x,L_visual_axis_y,L_visual_axis_z,L_real_gaze_x,L_real_gaze_y,L_real_gaze_z,L_alpha,L_beta,L_kappa,"
             << "R_visual_axis_x,R_visual_axis_y,R_visual_axis_z,R_real_gaze_x,R_real_gaze_y,R_real_gaze_z,R_alpha,R_beta,R_kappa,"
             << "head_rotation_roll,head_rotation_pitch,head_rotation_yaw,head_to_camera_distance,head_to_point_distance\n";

  RCLCPP_INFO(this->get_logger(), "Node initialised");
}

RgbdGazeAccuracy::~RgbdGazeAccuracy()
{
  data_file_.flush();
  data_file_.close();
}

void RgbdGazeAccuracy::handle_enable_data_collection_service(
    const std::shared_ptr<rmw_request_id_t> request_header,
    const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
    const std::shared_ptr<std_srvs::srv::SetBool::Response> response)
{
  (void)request_header;
  enable_data_collection_ = request->data;
  response->success = true;
}

void RgbdGazeAccuracy::synchronized_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg_head_pose,
                                             const gaze_msgs::msg::GazeBinocularStamped::SharedPtr msg_visual_axes,
                                             const geometry_msgs::msg::PointStamped::SharedPtr msg_scene_point)
{
  RCLCPP_DEBUG(this->get_logger(), "Received synchronized messages for processing");

  if (!enable_data_collection_)
  {
    return;
  }

  epoch_++;

  // Convert head pose to Eigen
  Eigen::Isometry3d head_pose;
  tf2::fromMsg(msg_head_pose->pose, head_pose);

  // Convert scene point to Eigen
  Eigen::Vector3d scene_point;
  Eigen::fromMsg(msg_scene_point->point, scene_point);

  // Create a parametrized lines from visual axes
  Eigen::ParametrizedLine<double, 3> visual_axes[2];
  for (uint8_t eye = 0; eye < 2; eye++)
  {
    Eigen::Vector3d origin, direction;
    Eigen::fromMsg(msg_visual_axes->gaze[eye].origin, origin);
    Eigen::fromMsg(msg_visual_axes->gaze[eye].direction, direction);
    visual_axes[eye] = Eigen::ParametrizedLine<double, 3>(origin, direction);
  }

  // Convert scene point into coordinate system of the head pose
  if (msg_scene_point->header.frame_id != msg_head_pose->header.frame_id)
  {
    // Attemp to find transformation between the coordinate frames
    geometry_msgs::msg::TransformStamped transform_stamped;
    try
    {
      transform_stamped = tf2_buffer_.lookupTransform(msg_scene_point->header.frame_id, msg_head_pose->header.frame_id, rclcpp::Time(0));
    }
    catch (tf2::TransformException &ex)
    {
      RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
      return;
    }

    // Transform scene point into coordinate system of visual axes
    scene_point = tf2::transform(scene_point, tf2::transformToEigen(transform_stamped.transform));
  }

  // Convert visual axes into coordinate system of the head pose, if needed
  if (msg_visual_axes->header.frame_id != msg_head_pose->header.frame_id)
  {
    // Attemp to find transformation between the coordinate frames
    geometry_msgs::msg::TransformStamped transform_stamped;
    try
    {
      transform_stamped = tf2_buffer_.lookupTransform(msg_visual_axes->header.frame_id, msg_head_pose->header.frame_id, rclcpp::Time(0));
    }
    catch (tf2::TransformException &ex)
    {
      RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
      return;
    }

    // Transform visual axes into coordinate system of visual axes
    for (uint8_t eye = 0; eye < 2; eye++)
    {
      visual_axes[eye] = tf2::transform(visual_axes[eye], tf2::transformToEigen(transform_stamped.transform));
    }
  }

  // Compute kappa
  double alpha[2];
  double beta[2];
  double kappa[2];
  Eigen::Vector3d visual_axes_direction_wrt_head[2];
  Eigen::Vector3d real_gaze_direction_wrt_head[2];
  Eigen::ParametrizedLine<double, 3> real_gaze[2];
  for (uint8_t eye = 0; eye < 2; eye++)
  {
    // Construct the real gaze based on the scene point
    real_gaze[eye] = Eigen::ParametrizedLine<double, 3>(visual_axes[eye].origin(), (scene_point - visual_axes[eye].origin()).normalized());

    // Transform visual axes and real gaze of both eyes to head coordinate frame (kappa is fixed with this frame)
    visual_axes_direction_wrt_head[eye] = head_pose.inverse().rotation() * visual_axes[eye].direction();
    real_gaze_direction_wrt_head[eye] = head_pose.inverse().rotation() * real_gaze[eye].direction();

    // Compute alpha (around x)
    alpha[eye] = compute_angle(visual_axes_direction_wrt_head[eye].y(), visual_axes_direction_wrt_head[eye].z(),
                               real_gaze_direction_wrt_head[eye].y(), real_gaze_direction_wrt_head[eye].z());

    // Compute beta (around y)
    beta[eye] = compute_angle(visual_axes_direction_wrt_head[eye].z(), visual_axes_direction_wrt_head[eye].x(),
                              real_gaze_direction_wrt_head[eye].z(), real_gaze_direction_wrt_head[eye].x());

    kappa[eye] = std::sqrt(alpha[eye] * alpha[eye] + beta[eye] * beta[eye]);
  }

  // Write data in
  {
    auto head_rot_euler = head_pose.rotation().eulerAngles(0, 1, 2);

    if (DEG)
    {
      for (uint8_t eye = 0; eye < 2; eye++)
      {
        alpha[eye] *= 180 / M_PI;
        beta[eye] *= 180 / M_PI;
        kappa[eye] *= 180 / M_PI;
      }
      head_rot_euler *= 180 / M_PI;
    }

    data_file_ << std::to_string(epoch_) << ","
               << std::to_string(visual_axes_direction_wrt_head[EYE_LEFT].x()) << "," << std::to_string(visual_axes_direction_wrt_head[EYE_LEFT].y()) << "," << std::to_string(visual_axes_direction_wrt_head[EYE_LEFT].z()) << "," << std::to_string(real_gaze_direction_wrt_head[EYE_LEFT].x()) << "," << std::to_string(real_gaze_direction_wrt_head[EYE_LEFT].y()) << "," << std::to_string(real_gaze_direction_wrt_head[EYE_LEFT].z()) << "," << std::to_string(alpha[EYE_LEFT]) << "," << std::to_string(beta[EYE_LEFT]) << "," << std::to_string(kappa[EYE_LEFT]) << ","
               << std::to_string(visual_axes_direction_wrt_head[EYE_RIGHT].x()) << "," << std::to_string(visual_axes_direction_wrt_head[EYE_RIGHT].y()) << "," << std::to_string(visual_axes_direction_wrt_head[EYE_RIGHT].z()) << "," << std::to_string(real_gaze_direction_wrt_head[EYE_RIGHT].x()) << "," << std::to_string(real_gaze_direction_wrt_head[EYE_RIGHT].y()) << "," << std::to_string(real_gaze_direction_wrt_head[EYE_RIGHT].z()) << "," << std::to_string(alpha[EYE_RIGHT]) << "," << std::to_string(beta[EYE_RIGHT]) << "," << std::to_string(kappa[EYE_RIGHT]) << ","
               << std::to_string(head_rot_euler.x()) << "," << std::to_string(head_rot_euler.y()) << "," << std::to_string(head_rot_euler.z()) << "," << std::to_string(head_pose.translation().norm()) << "," << std::to_string((head_pose.translation() - scene_point).norm()) << "\n";
  }

  // Publish eyeball as a visualisation marker, if desired
  if (this->get_parameter("publish_markers").get_value<bool>())
  {
    visualization_msgs::msg::MarkerArray markers;
    visualization_msgs::msg::Marker default_marker;
    default_marker.header = msg_head_pose->header;
    default_marker.action = visualization_msgs::msg::Marker::ADD;
    for (uint8_t eye = 0; eye < 2; eye++)
    {
      std::string eye_side;
      if (eye == EYE_LEFT)
      {
        eye_side = "_left";
      }
      else
      {
        eye_side = "_right";
      }
      default_marker.ns = std::string(this->get_namespace()) + "kappa_calibration" + eye_side;

      // Visual axis
      {
        visualization_msgs::msg::Marker visual_axis_marker = default_marker;
        visual_axis_marker.id = 0;
        visual_axis_marker.type = visualization_msgs::msg::Marker::ARROW;
        geometry_msgs::msg::Point start, end;
        start = Eigen::toMsg(visual_axes[eye].origin());
        end = Eigen::toMsg(visual_axes[eye].pointAt(VISUAL_VISUAL_AXIS_LENGTH));
        visual_axis_marker.points.push_back(start);
        visual_axis_marker.points.push_back(end);
        visual_axis_marker.scale.x = VISUAL_VISUAL_AXIS_WIDTH;
        visual_axis_marker.scale.y =
            visual_axis_marker.scale.z = 0;
        visual_axis_marker.color.r = VISUAL_VISUAL_AXIS_COLOR[0];
        visual_axis_marker.color.g = VISUAL_VISUAL_AXIS_COLOR[1];
        visual_axis_marker.color.b = VISUAL_VISUAL_AXIS_COLOR[2];
        visual_axis_marker.color.a = VISUAL_VISUAL_AXIS_COLOR[3];
        markers.markers.push_back(visual_axis_marker);
      }

      // Real gaze
      {
        visualization_msgs::msg::Marker real_gaze_marker = default_marker;
        real_gaze_marker.id = 1;
        real_gaze_marker.type = visualization_msgs::msg::Marker::ARROW;
        geometry_msgs::msg::Point start, end;
        start = Eigen::toMsg(real_gaze[eye].origin());
        end = Eigen::toMsg(scene_point);
        real_gaze_marker.points.push_back(start);
        real_gaze_marker.points.push_back(end);
        real_gaze_marker.scale.x =
            real_gaze_marker.scale.y = VISUAL_REAL_GAZE_WIDTH;
        real_gaze_marker.scale.z = 0.0;
        real_gaze_marker.color.r = VISUAL_REAL_GAZE_COLOR[0];
        real_gaze_marker.color.g = VISUAL_REAL_GAZE_COLOR[1];
        real_gaze_marker.color.b = VISUAL_REAL_GAZE_COLOR[2];
        real_gaze_marker.color.a = VISUAL_REAL_GAZE_COLOR[3];
        markers.markers.push_back(real_gaze_marker);
      }
    }
    pub_markers_->publish(markers);
  }
}

////////////
/// MAIN ///
////////////

/// Main function that initiates an object of `RgbdGazeAccuracy` class as the core of this node.
int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<RgbdGazeAccuracy>());
  rclcpp::shutdown();
  return EXIT_SUCCESS;
}


// SixDegs: based on Kevin bacon game
// functions are search, insert, and remove (type ./sixdegs for details)
// 7.2012

#include <stdlib.h>
#include <stdio.h>
#include <sys/time.h>
#include <time.h>

#include <iostream>
#include <string.h>
#include <fstream>
#include <boost/config.hpp>
#include <boost/tokenizer.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/lexical_cast.hpp>

#include "defines.h"

using namespace std;
using namespace boost;

template <typename DistanceMap>
class bacon_number_recorder : public default_bfs_visitor {
public:
  bacon_number_recorder(DistanceMap dist) : d(dist) { }

  template <typename Edge, typename Graph>
  void tree_edge(Edge e, const Graph& g) const
  {
    typename graph_traits<Graph>::vertex_descriptor
      u = source(e, g), v = target(e, g);
    d[v] = d[u] + 1;
  }
private:
  DistanceMap d;
};

// Convenience function
template <typename DistanceMap>
bacon_number_recorder<DistanceMap>
record_bacon_number(DistanceMap d)
{
  return bacon_number_recorder < DistanceMap > (d);
}

int main(int argc, char **argv)
{
  mcsim_skip_instrs_begin();
  if (argc == 1) {
    printf("\n=========== An Six Degree Usage ============\n");
    printf("Build a scale-free graph, apply to actions to the graph: \n");
    printf("(1) Search for a path of length no more than six between two vertices\n");
    printf("(2) Modify the graph by inserting and removing edges\n");
    printf("./sixdegs --search <num of iterations>\n");
    return 0;
  }

  int i, bench = SEARCH_BENCH, total_iterations = TOTAL_ITERATIONS;
  int base = BASE, diff = DIFF;
  string src_node, other_node;

  for (i = 1; i < argc; i++) {
    if (strncmp(argv[i], "--search", 8) == 0) {
      bench = SEARCH_BENCH;
      total_iterations = atoi(argv[i+1]);
      ++i;
    } else {
      printf("Invalid parameters: '%s'\n", argv[i]);
      return -1;
    }
  }

  ifstream datafile("input.dat");
  if (!datafile) {
    cerr << "No input.dat file" << endl;
    return -1;
  }

  typedef adjacency_list <vecS, vecS, undirectedS, property <vertex_name_t, string>, property <edge_name_t, string> > Graph;
  Graph g;

  typedef property_map < Graph, vertex_name_t >::type actor_name_map_t;
  actor_name_map_t actor_name = get(vertex_name, g);
  typedef property_map < Graph, edge_name_t >::type movie_name_map_t;
  movie_name_map_t connecting_movie = get(edge_name, g);

  typedef graph_traits < Graph >::vertex_descriptor Vertex;
  typedef map < string, Vertex > NameVertexMap;
  NameVertexMap actors;

  // Initialization: create a graph from the input file
  for (string line; getline(datafile, line);) {
    char_delimiters_separator <char>sep(false, "", ";");
    tokenizer <> line_toks(line, sep);
    tokenizer <>::iterator it = line_toks.begin();
    string actors_name = *it++;
    NameVertexMap::iterator pos;
    bool inserted;
    Vertex u, v;
    tie(pos, inserted) = actors.insert(make_pair(actors_name, Vertex()));
    if (inserted) {
      u = add_vertex(g);
      actor_name[u] = actors_name;
      pos->second = u;
    } else
      u = pos->second;

    string movie_name = *it++;

    tie(pos, inserted) = actors.insert(make_pair(*it, Vertex()));
    if (inserted) {
      v = add_vertex(g);
      actor_name[v] = *it;
      pos->second = v;
    } else
      v = pos->second;

    graph_traits < Graph >::edge_descriptor e;
    tie(e, inserted) = add_edge(u, v, g);
    if (inserted)
      connecting_movie[e] = movie_name;

  } // Initialization done

  mcsim_skip_instrs_end();

  // Search for a path of length no more than six between two vertices
  if (bench == SEARCH_BENCH) {
    cout << "num_edges before search: " << num_edges(g) << endl;
    string movie_name;
    Vertex u, v;
    bool inserted;
    for (i=0; i != total_iterations; ++i) {

      // insert

      //mcsim_skip_instrs_begin();
      src_node = lexical_cast<string>(i+base);
      other_node = lexical_cast<string>(i+base + diff-1);

      movie_name = src_node + "_with_" + other_node;
      u = actors[src_node];
      v = actors[other_node];
      graph_traits < Graph >::edge_descriptor e;
      //mcsim_skip_instrs_end();

      mcsim_tx_begin();
      tie(e, inserted) = add_edge(u, v, g);

      if (inserted) {
	connecting_movie[e] = movie_name;
	cout << src_node << " and " << other_node
	     << " now connected." << endl;
      }
      mcsim_tx_end();

      // remove
      //mcsim_skip_instrs_begin();
      src_node = lexical_cast<string>(i+base);
      other_node = lexical_cast<string>(i+ diff + base/2);

      u = actors[src_node];
      v = actors[other_node];
      //mcsim_skip_instrs_end();

      mcsim_tx_begin();
      remove_edge(u, v, g);
      mcsim_tx_end();

      cout << src_node << " and " << other_node
	   << " now disconnected." << endl;
    }
    cout << "num_edges after searches: " << num_edges(g) << endl;
  }

  return 0;
}


/*
 [auto_generated]
 boost/numeric/odeint/integrate/detail/integrate_times.hpp

 [begin_description]
 Default integrate times implementation.
 [end_description]

 Copyright 2011-2012 Mario Mulansky
 Copyright 2012 Karsten Ahnert
 Copyright 2012 Christoph Koke

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_TIMES_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_TIMES_HPP_INCLUDED

#include <stdexcept>

#include <boost/config.hpp>
#include <boost/throw_exception.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/stepper/controlled_step_result.hpp>
#include <boost/numeric/odeint/util/detail/less_with_sign.hpp>

namespace boost {
namespace numeric {
namespace odeint {
namespace detail {

/*
 * integrate_times for simple stepper
 */
template< class Stepper , class System , class State , class TimeIterator , class Time , class Observer >
size_t integrate_times(
        Stepper stepper , System system , State &start_state ,
        TimeIterator start_time , TimeIterator end_time , Time dt ,
        Observer observer , stepper_tag
)
{
    typename odeint::unwrap_reference< Observer >::type &obs = observer;
    typename odeint::unwrap_reference< Stepper >::type &st = stepper;
    typedef typename unit_value_type<Time>::type time_type;

    size_t steps = 0;
    Time current_dt = dt;
    while( true )
    {
        Time current_time = *start_time++;
        obs( start_state , current_time );
        if( start_time == end_time )
            break;
        while( less_with_sign( current_time , static_cast<time_type>(*start_time) , current_dt ) )
        {
            current_dt = min_abs( dt , *start_time - current_time );
            st.do_step( system , start_state , current_time , current_dt );
            current_time += current_dt;
            steps++;
        }
    }
    return steps;
}

/*
 * integrate_times for controlled stepper
 */
template< class Stepper , class System , class State , class TimeIterator , class Time , class Observer >
size_t integrate_times(
        Stepper stepper , System system , State &start_state ,
        TimeIterator start_time , TimeIterator end_time , Time dt ,
        Observer observer , controlled_stepper_tag
)
{
    typename odeint::unwrap_reference< Observer >::type &obs = observer;
    typename odeint::unwrap_reference< Stepper >::type &st = stepper;
    typedef typename unit_value_type<Time>::type time_type;

    const size_t max_attempts = 1000;
    const char *error_string = "Integrate adaptive : Maximal number of iterations reached. A step size could not be found.";
    size_t steps = 0;
    while( true )
    {
        size_t fail_steps = 0;
        Time current_time = *start_time++;
        obs( start_state , current_time );
        if( start_time == end_time )
            break;
        while( less_with_sign( current_time , static_cast<time_type>(*start_time) , dt ) )
        {
            // adjust stepsize to end up exactly at the observation point
            Time current_dt = min_abs( dt , *start_time - current_time );
            if( st.try_step( system , start_state , current_time , current_dt ) == success )
            {
                ++steps;
                // continue with the original step size if dt was reduced due to observation
                dt = max_abs( dt , current_dt );
            }
            else
            {
                ++fail_steps;
                dt = current_dt;
            }
            if( fail_steps == max_attempts ) BOOST_THROW_EXCEPTION( std::overflow_error( error_string ));
        }
    }
    return steps;
}

/*
 * integrate_times for dense output stepper
 */
template< class Stepper , class System , class State , class TimeIterator , class Time , class Observer >
size_t integrate_times(
        Stepper stepper , System system , State &start_state ,
        TimeIterator start_time , TimeIterator end_time , Time dt ,
        Observer observer , dense_output_stepper_tag
)
{
    typename odeint::unwrap_reference< Observer >::type &obs = observer;
    typename odeint::unwrap_reference< Stepper >::type &st = stepper;
    typedef typename unit_value_type<Time>::type time_type;

    if( start_time == end_time )
        return 0;

    TimeIterator last_time_iterator = end_time;
    --last_time_iterator;
    Time last_time_point = static_cast<time_type>(*last_time_iterator);

    st.initialize( start_state , *start_time , dt );
    obs( start_state , *start_time++ );

    size_t count = 0;
    while( start_time != end_time )
    {
        while( ( start_time != end_time ) && less_eq_with_sign( static_cast<time_type>(*start_time) , st.current_time() , st.current_time_step() ) )
        {
            st.calc_state( *start_time , start_state );
            obs( start_state , *start_time );
            start_time++;
        }

        // we have not reached the end, do another real step
        if( less_eq_with_sign( st.current_time() + st.current_time_step() ,
                               last_time_point ,
                               st.current_time_step() ) )
        {
            st.do_step( system );
            ++count;
        }
        else if( start_time != end_time )
        { // do the last step ending exactly on the end point
            st.initialize( st.current_state() , st.current_time() , last_time_point - st.current_time() );
            st.do_step( system );
            ++count;
        }
    }
    return count;
}

} // namespace detail
} // namespace odeint
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_ADAPTIVE_HPP_INCLUDED


#pragma once
#include <trajopt_utils/macros.h>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <Eigen/Dense>
#include <functional>
#include <memory>
TRAJOPT_IGNORE_WARNINGS_POP

/*
 * Numerical derivatives
 */

namespace sco
{
class ScalarOfVector;
class VectorOfVector;
class MatrixOfVector;
typedef std::shared_ptr<ScalarOfVector> ScalarOfVectorPtr;
typedef std::shared_ptr<VectorOfVector> VectorOfVectorPtr;
typedef std::shared_ptr<MatrixOfVector> MatrixOfVectorPtr;

class ScalarOfVector
{
public:
  virtual double operator()(const Eigen::VectorXd& x) const = 0;
  double call(const Eigen::VectorXd& x) const
  {
    return operator()(x);
  }
  virtual ~ScalarOfVector()
  {
  }
  typedef std::function<double(Eigen::VectorXd)> func;
  static ScalarOfVectorPtr construct(const func&);
  //  typedef VectorXd (*c_func)(const VectorXd&);
  //  static ScalarOfVectorPtr construct(const c_func&);
};
class VectorOfVector
{
public:
  virtual Eigen::VectorXd operator()(const Eigen::VectorXd& x) const = 0;
  Eigen::VectorXd call(const Eigen::VectorXd& x) const
  {
    return operator()(x);
  }
  virtual ~VectorOfVector()
  {
  }
  typedef std::function<Eigen::VectorXd(Eigen::VectorXd)> func;
  static VectorOfVectorPtr construct(const func&);
  //  typedef VectorXd (*c_func)(const VectorXd&);
  //  static VectorOfVectorPtr construct(const c_func&);
};
class MatrixOfVector
{
public:
  virtual Eigen::MatrixXd operator()(const Eigen::VectorXd& x) const = 0;
  Eigen::MatrixXd call(const Eigen::VectorXd& x) const
  {
    return operator()(x);
  }
  virtual ~MatrixOfVector()
  {
  }
  typedef std::function<Eigen::MatrixXd(Eigen::VectorXd)> func;
  static MatrixOfVectorPtr construct(const func&);
  //  typedef VectorMatrixXd (*c_func)(const VectorXd&);
  //  static MatrixOfVectorPtr construct(const c_func&);
};

Eigen::VectorXd calcForwardNumGrad(const ScalarOfVector& f, const Eigen::VectorXd& x, double epsilon);
Eigen::MatrixXd calcForwardNumJac(const VectorOfVector& f, const Eigen::VectorXd& x, double epsilon);
void calcGradAndDiagHess(const ScalarOfVector& f, const Eigen::VectorXd& x, double epsilon, double& y,
                         Eigen::VectorXd& grad, Eigen::VectorXd& hess);
void calcGradHess(ScalarOfVectorPtr f, const Eigen::VectorXd& x, double epsilon, double& y, Eigen::VectorXd& grad,
                  Eigen::MatrixXd& hess);
VectorOfVectorPtr forwardNumGrad(ScalarOfVectorPtr f, double epsilon);
MatrixOfVectorPtr forwardNumJac(VectorOfVectorPtr f, double epsilon);
}


/**
 * Copyright 2004-present Facebook. All Rights Reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include "source/conversion/PointCloudUtil.h"

#include <regex>

#include <boost/algorithm/string/predicate.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/format.hpp>

#include "source/util/ThreadPool.h"

using Image = cv::Mat_<cv::Vec3b>;
using DisparityImage = cv::Mat_<float>;
using CoordinateImage = cv::Mat_<cv::Point3f>;

namespace fb360_dep {
namespace point_cloud_util {

std::vector<PointCloudProjection> generateProjectedImages(
    const PointCloud& pointCloud,
    const Camera::Rig& rig) {
  std::vector<PointCloudProjection> projections;
  for (const auto& camera : rig) {
    PointCloudProjection projection;
    projection.image =
        cv::Mat(camera.resolution[1], camera.resolution[0], CV_8UC3, cv::Scalar(0, 0, 0));
    projection.disparityImage =
        cv::Mat(camera.resolution[1], camera.resolution[0], CV_32F, cv::Scalar(0));
    projection.coordinateImage =
        cv::Mat(camera.resolution[1], camera.resolution[0], CV_32FC3, cv::Scalar(0, 0, 0));
    projections.emplace_back(projection);
  }

  for (const auto& point : pointCloud) {
    for (int i = 0; i < int(rig.size()); ++i) {
      if (rig[i].sees(point.coords)) {
        const Camera::Vector2& projectedCoors = rig[i].pixel(point.coords);
        PointCloudProjection& projection = projections[i];
        const float depth = (point.coords - rig[i].position).norm();
        const float disparity = 1.0f / depth;
        if (projection.disparityImage(projectedCoors.y(), projectedCoors.x()) < disparity) {
          projection.disparityImage(projectedCoors.y(), projectedCoors.x()) = disparity;
          projection.image(projectedCoors.y(), projectedCoors.x()) = point.bgrColor;
          projection.coordinateImage(projectedCoors.y(), projectedCoors.x()).x = point.coords.x();
          projection.coordinateImage(projectedCoors.y(), projectedCoors.x()).y = point.coords.y();
          projection.coordinateImage(projectedCoors.y(), projectedCoors.x()).z = point.coords.z();
        }
      }
    }
  }
  return projections;
}

void goToLine(std::ifstream& file, const int lineNumber) {
  file.seekg(0, std::ios::beg);
  for (int i = 1; i < lineNumber; ++i) {
    file.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
  }
}

// PCL header entries must be specified in the following order
// Comment
// VERSION
// FIELDS
// SIZE
// TYPE
// COUNT
// WIDTH
// HEIGHT
// VIEWPOINT
// POINTS
// DATA
void verifyPCLHeader(std::ifstream& file) {
  std::string line;

  // Make sure FIELDS starts with x y z
  const int lineFields = 3;
  goToLine(file, lineFields);
  std::getline(file, line);
  CHECK(boost::starts_with(line, "FIELDS x y z")) << "PCL header: FIELDS must start with x y z";

  // Make DATA is ascii
  const int lineData = 11;
  goToLine(file, lineData);
  std::getline(file, line);
  CHECK_EQ(line, "DATA ascii") << "PCL header: DATA must be ascii";
}

int extractPCLPointCount(std::ifstream& file) {
  goToLine(file, 0);

  std::string line;
  const int linePoints = 10;
  goToLine(file, linePoints);
  std::getline(file, line);
  CHECK(boost::starts_with(line, "POINTS"))
      << boost::format("PCL header: expected point count in line %1%, got %2%") % linePoints % line;

  std::regex rgx("POINTS (\\w+)");
  std::smatch match;
  const std::string lineConst = line;
  CHECK(std::regex_search(lineConst.begin(), lineConst.end(), match, rgx))
      << "Could not parse point count from line " << line;
  const std::string pointCountStr = match[1];
  try {
    return boost::lexical_cast<int>(pointCountStr);
  } catch (boost::bad_lexical_cast&) {
    CHECK(false) << boost::format("PCL header: invalid point count %1%") % pointCountStr;
  }
}

int extractASCIIPointCount(std::ifstream& file) {
  goToLine(file, 0);

  std::string line;
  file >> line;
  try {
    return boost::lexical_cast<int>(line);
  } catch (boost::bad_lexical_cast&) {
    CHECK(false) << "First line should contain point count: " << line;
  }
}

int getPointCount(const std::string& pointCloudFile) {
  // Check if first line is point counter
  std::ifstream srcFile(pointCloudFile);
  CHECK(srcFile.good()) << "File does not exist: " << pointCloudFile;

  const std::string pcExt = filesystem::path(pointCloudFile).extension().string();
  if (pcExt == ".pcd") {
    verifyPCLHeader(srcFile);
    return extractPCLPointCount(srcFile);
  } else {
    return extractASCIIPointCount(srcFile);
  }
}

PointCloud
extractPoints(const std::string& pointCloudFile, const int pointCount, const int maxThreads) {
  LOG(INFO) << boost::format("Extracting %1% points from %2%...") % pointCount % pointCloudFile;

  ThreadPool threadPool(maxThreads);
  const int threads = threadPool.getMaxThreads();

  // Evenly distribute lines across threads
  const int pointsPerThread = float(pointCount) / threads;
  const int remain = pointCount % threads;

  const std::string pcExt = filesystem::path(pointCloudFile).extension().string();
  const int headerNumLines = pcExt == ".pcd" ? 11 : 1;

  PointCloud points(pointCount);
  for (int i = 0; i < threads; ++i) {
    threadPool.spawn([&, i] {
      const int begin = headerNumLines + 1 + // skip header
          (i < remain ? i * (pointsPerThread + 1) : pointCount - (threads - i) * pointsPerThread);
      const int end = begin + pointsPerThread + (i < remain);

      // Lines may not have the same length in characters, we have to loop our way to the start
      // line
      std::ifstream file(pointCloudFile);
      goToLine(file, begin);

      for (int j = begin; j < end; ++j) {
        // Read XYZ coordinates and discard the rest of the line
        BGRPoint& point = points[j];
        int ignored, r, g, b;
        file >> point.coords.x() >> point.coords.y() >> point.coords.z() >> ignored >> r >> g >> b;
        point.bgrColor[0] = b;
        point.bgrColor[1] = g;
        point.bgrColor[2] = r;
        file.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
      }
    });
  }
  threadPool.join();

  LOG(INFO) << boost::format("Extracted %1% points.") % points.size();
  if (pointCount > 0) {
    CHECK_EQ(pointCount, points.size()) << boost::format(
        "Point count in header (%1%) does not match number of extracted points (%2%)")
        % pointCount
        % points.size();
  }

  return points;
}

PointCloud extractPoints(const std::string& pointCloudFile, const int maxThreads) {
  const int pointCount = getPointCount(pointCloudFile);
  return extractPoints(pointCloudFile, pointCount, maxThreads);
}

} // namespace point_cloud_util
} // namespace fb360_dep


//  Unit test for a subrange type template for C++
//
//  Copyright © 2005, 2010, 2013  Russel Winder
//
//  Distributed under the Boost Software License, Version 1.0.  See
//  http://www.boost.org/LICENSE_1_0.txt.

/**
 *  @file
 *
 *  A test program (using Boost.Test) for the subrange type.
 *
 *
This is in serious need of extending with far more test cases.

 *
 *
Copyright © 2005, 2010, 2013  Russel Winder.

 *
 *  @author Russel Winder
 *  @version 1.3
 *  @date 2013-04-12T11:16+01:00
 */

#define BOOST_TEST_MODULE SubrangeTest_Boost
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>

#include "subrange.hpp"

enum Day { monday, tuesday, wednesday, thursday, friday, saturday, sunday };

typedef subrange::subrange<subrange::ordinal_range<Day, monday, friday>> Weekday;

BOOST_AUTO_TEST_CASE(createDefaultTestEnum) {
  Weekday a;
  BOOST_REQUIRE_EQUAL(a, monday);
}

BOOST_AUTO_TEST_CASE(assignTestEnum) {
  Weekday a {monday};
  Weekday b {tuesday};
  a = b;
  BOOST_REQUIRE(a == b);
}

BOOST_AUTO_TEST_CASE(equalsTestEnum) {
  Weekday a {monday};
  Weekday b {monday};
  BOOST_REQUIRE(a == b);
}

BOOST_AUTO_TEST_CASE(notEqualsTestEnum) {
  Weekday a {monday};
  Weekday b {tuesday};
  BOOST_REQUIRE(a != b);
}

BOOST_AUTO_TEST_CASE(lessThanTestEnum) {
  Weekday a {monday};
  Weekday b {tuesday};
  BOOST_REQUIRE(a = b));
}

BOOST_AUTO_TEST_CASE(lessThanEqualToTestEnum) {
  Weekday a {monday};
  Weekday b {monday};
  BOOST_REQUIRE(a <= b);
  BOOST_REQUIRE(!(a > b));
}

BOOST_AUTO_TEST_CASE(greaterThanTestEnum) {
  Weekday a {monday};
  Weekday b {tuesday};
  BOOST_REQUIRE(b > a);
  BOOST_REQUIRE(!(b <= a));
}

BOOST_AUTO_TEST_CASE(greaterThanEqualToTestEnum) {
  Weekday a {monday};
  Weekday b {monday};
  BOOST_REQUIRE(a >= b);
  BOOST_REQUIRE(!(a < b));
}

typedef subrange::subrange<subrange::ordinal_range<short, 1, 99>> SubIntException;

BOOST_AUTO_TEST_CASE(createDefaultTestIntException) {
  SubIntException a;
  BOOST_REQUIRE_EQUAL(a, 1);
}

BOOST_AUTO_TEST_CASE(preincrementTestIntException) {
  SubIntException a {9};
  ++a;
  BOOST_REQUIRE_EQUAL(a, 10);
}

BOOST_AUTO_TEST_CASE(postincrementTestIntException) {
  SubIntException a {9};
  a++;
  BOOST_REQUIRE_EQUAL(a, 10);
}

BOOST_AUTO_TEST_CASE(predecrementTestIntException) {
  SubIntException a {9};
  --a;
  BOOST_REQUIRE_EQUAL(a, 8);
}

BOOST_AUTO_TEST_CASE(postdecrementTestIntException) {
  SubIntException a {9};
  a--;
  BOOST_REQUIRE_EQUAL(a, 8);
}

BOOST_AUTO_TEST_CASE(assignLiteralTestIntException) {
  SubIntException a {9};
  a = 20;
  BOOST_REQUIRE_EQUAL(a, 20);
  BOOST_REQUIRE_THROW(a = 200, subrange::range_error);
  BOOST_REQUIRE_THROW(a = -5, subrange::range_error);
}

BOOST_AUTO_TEST_CASE(assignTestIntException) {
  SubIntException a {9};
  SubIntException b {20};
  a = b;
  BOOST_REQUIRE_EQUAL(a, b);
  BOOST_REQUIRE_EQUAL(a, 20);
}

BOOST_AUTO_TEST_CASE(addAssignTestIntException) {
  SubIntException a {9};
  SubIntException b {20};
  a += b;
  BOOST_REQUIRE_EQUAL(a, 29);
  a += 10;
  BOOST_REQUIRE_EQUAL(a, 39);
  a += -10;
  BOOST_REQUIRE_EQUAL(a, 29);
  BOOST_REQUIRE_THROW(a += 200, subrange::range_error);
}

BOOST_AUTO_TEST_CASE(subtractAssignTestIntException) {
  SubIntException a {20};
  SubIntException b {9};
  a -= b;
  BOOST_REQUIRE_EQUAL(a, 11);
  a -= 5;
  BOOST_REQUIRE_EQUAL(a, 6);
  a -= -5;
  BOOST_REQUIRE_EQUAL(a, 11);
  BOOST_REQUIRE_THROW (b -= a, subrange::range_error);
}

BOOST_AUTO_TEST_CASE(multiplyAssignTestIntException) {
  SubIntException a {20};
  SubIntException b {3};
  a *= b;
  BOOST_REQUIRE_EQUAL(a, 60);
  BOOST_REQUIRE_THROW (a *= b, subrange::range_error);
}

BOOST_AUTO_TEST_CASE(divideAssignTestIntException) {
  SubIntException a {20};
  SubIntException b {3};
  a /= b;
  BOOST_REQUIRE_EQUAL(a, 6);
}

BOOST_AUTO_TEST_CASE(remainderAssignTestIntException) {
  SubIntException a {20};
  SubIntException b {3};
  a %= b;
  BOOST_REQUIRE_EQUAL(a, 2);
}

BOOST_AUTO_TEST_CASE(xorAssignTestIntException) {
  SubIntException a {0x12};
  SubIntException b {0x03};
  a ^= b;
  BOOST_REQUIRE_EQUAL(a, 0x11);
}

BOOST_AUTO_TEST_CASE(andAssignTestIntException) {
  SubIntException a {0x12};
  SubIntException b {0x03};
  a &= b;
  BOOST_REQUIRE_EQUAL(a, 0x02);
}

BOOST_AUTO_TEST_CASE(orAssignTestIntException) {
  SubIntException a {0x12};
  SubIntException b {0x03};
  a |= b;
  BOOST_REQUIRE_EQUAL(a, 0x13);
}

BOOST_AUTO_TEST_CASE(shiftLeftAssignTestIntException) {
  SubIntException a {0x12};
  a <<= 1;
  BOOST_REQUIRE_EQUAL(a, 0x24);
}

BOOST_AUTO_TEST_CASE(shiftRightAssignTestIntException) {
  SubIntException a {0x12};
  a >>= 1;
  BOOST_REQUIRE_EQUAL(a, 0x09);
}

BOOST_AUTO_TEST_CASE(addTestIntException) {
  SubIntException a {9};
  SubIntException b {20};
  a = a + b;
  BOOST_REQUIRE_EQUAL(a, 29);
  a = a + 10;
  BOOST_REQUIRE_EQUAL(a, 39);
  a = a + -10;
  BOOST_REQUIRE_EQUAL(a, 29);
  a = 10 + a;
  BOOST_REQUIRE_EQUAL(a, 39);
  a = -10 + a;
  BOOST_REQUIRE_EQUAL(a, 29);
}

BOOST_AUTO_TEST_CASE(subtractTestIntException) {
  SubIntException a {20};
  SubIntException b {9};
  a = a - b;
  BOOST_REQUIRE_EQUAL(a, 11);
}

BOOST_AUTO_TEST_CASE(multiplyTestIntException) {
  SubIntException a {20};
  SubIntException b {3};
  a = a * b;
  BOOST_REQUIRE_EQUAL(a, 60);
}

BOOST_AUTO_TEST_CASE(divideTestIntException) {
  SubIntException a {20};
  SubIntException b {3};
  a = a / b;
  BOOST_REQUIRE_EQUAL(a, 6);
}

BOOST_AUTO_TEST_CASE(remainderTestIntException) {
  SubIntException a {20};
  SubIntException b {3};
  a = a % b;
  BOOST_REQUIRE_EQUAL(a, 2);
}

BOOST_AUTO_TEST_CASE(xorTestIntException) {
  SubIntException a {0x12};
  SubIntException b {0x03};
  a = a ^ b;
  BOOST_REQUIRE_EQUAL(a, 0x11);
}

BOOST_AUTO_TEST_CASE(andTestIntException) {
  SubIntException a {0x12};
  SubIntException b {0x03};
  a = a & b;
  BOOST_REQUIRE_EQUAL(a, 0x02);
}

BOOST_AUTO_TEST_CASE(orTestIntException) {
  SubIntException a {0x12};
  SubIntException b {0x03};
  a = a | b;
  BOOST_REQUIRE_EQUAL(a, 0x13);
}

BOOST_AUTO_TEST_CASE(shiftLeftTestIntException) {
  SubIntException a {0x12};
  a = a << 1;
  BOOST_REQUIRE_EQUAL(a, 0x24);
  BOOST_REQUIRE_THROW(a = a << 3, subrange::range_error);
}

BOOST_AUTO_TEST_CASE(shiftRightTestIntException) {
  SubIntException a {0x12};
  a = a >> 1;
  BOOST_REQUIRE_EQUAL(a, 0x09);
  BOOST_REQUIRE_THROW(a = a >> 5, subrange::range_error);
}

BOOST_AUTO_TEST_CASE(equalsTestIntException) {
  SubIntException a {9};
  SubIntException b {9};
  BOOST_REQUIRE_EQUAL(a, b);
}

BOOST_AUTO_TEST_CASE(notEqualsTestIntException) {
  SubIntException a {9};
  SubIntException b {8};
  BOOST_REQUIRE(a != b);
}

BOOST_AUTO_TEST_CASE(lessThanTestIntException) {
  SubIntException a {8};
  SubIntException b {9};
  BOOST_REQUIRE(a = b));
}

BOOST_AUTO_TEST_CASE(lessThanEqualToTestIntException) {
  SubIntException a {8};
  SubIntException b {8};
  BOOST_REQUIRE(a <= b);
  BOOST_REQUIRE(!(a > b));
}

BOOST_AUTO_TEST_CASE(greaterThanTestIntException) {
  SubIntException a {8};
  SubIntException b {9};
  BOOST_REQUIRE(b > a);
  BOOST_REQUIRE(!(b <= a));
}

BOOST_AUTO_TEST_CASE(greaterThanEqualToTestIntException) {
  SubIntException a {8};
  SubIntException b {8};
  BOOST_REQUIRE(a >= b);
  BOOST_REQUIRE(!(a < b));
}

typedef subrange::subrange<subrange::ordinal_range<short, 1, 99>, subrange::modulo_arithmetic> SubIntModulo;

BOOST_AUTO_TEST_CASE(createDefaultTestIntModulo) {
  SubIntModulo a;
  BOOST_REQUIRE_EQUAL(a, 1);
}

BOOST_AUTO_TEST_CASE(preincrementTestIntModulo) {
  SubIntModulo a {9};
  ++a;
  BOOST_REQUIRE_EQUAL(a, 10);
}

BOOST_AUTO_TEST_CASE(postincrementTestIntModulo) {
  SubIntModulo a {9};
  a++;
  BOOST_REQUIRE_EQUAL(a, 10);
}

BOOST_AUTO_TEST_CASE(predecrementTestIntModulo) {
  SubIntModulo a {9};
  --a;
  BOOST_REQUIRE_EQUAL(a, 8);
}

BOOST_AUTO_TEST_CASE(postdecrementTestIntModulo) {
  SubIntModulo a {9};
  a--;
  BOOST_REQUIRE_EQUAL(a, 8);
}

BOOST_AUTO_TEST_CASE(assignLiteralTestIntModulo) {
  SubIntModulo a {9};
  a = 20;
  BOOST_REQUIRE_EQUAL(a, 20);
  a = 200;
  BOOST_REQUIRE_EQUAL(a, 4);
  a = -5;
  BOOST_REQUIRE_EQUAL(a, 93);
}

BOOST_AUTO_TEST_CASE(assignTestIntModulo) {
  SubIntModulo a {9};
  SubIntModulo b {20};
  a = b;
  BOOST_REQUIRE_EQUAL(a, b);
  BOOST_REQUIRE_EQUAL(a, 20);
}

BOOST_AUTO_TEST_CASE(addAssignTestIntModulo) {
  SubIntModulo a {9};
  SubIntModulo b {20};
  a += b;
  BOOST_REQUIRE_EQUAL(a, 29);
  a += 10;
  BOOST_REQUIRE_EQUAL(a, 39);
  a += -10;
  BOOST_REQUIRE_EQUAL(a, 29);
  a += 200;
  BOOST_REQUIRE_EQUAL(a, 33);
}

BOOST_AUTO_TEST_CASE(subtractAssignTestIntModulo) {
  SubIntModulo a {20};
  SubIntModulo b {9};
  a -= b;
  BOOST_REQUIRE_EQUAL(a, 11);
  a -= 5;
  BOOST_REQUIRE_EQUAL(a, 6);
  a -= -5;
  BOOST_REQUIRE_EQUAL(a, 11);
  b -= a;
  BOOST_REQUIRE(b == 96);
}

BOOST_AUTO_TEST_CASE(multiplyAssignTestIntModulo) {
  SubIntModulo a {20};
  SubIntModulo b {3};
  a *= b;
  BOOST_REQUIRE_EQUAL(a, 60);
  a *= b;
  BOOST_REQUIRE_EQUAL(a, 82);
}

BOOST_AUTO_TEST_CASE(divideAssignTestIntModulo) {
  SubIntModulo a {20};
  SubIntModulo b {3};
  a /= b;
  BOOST_REQUIRE_EQUAL(a, 6);
}

BOOST_AUTO_TEST_CASE(remainderAssignTestIntModulo) {
  SubIntModulo a {20};
  SubIntModulo b {3};
  a %= b;
  BOOST_REQUIRE_EQUAL(a, 2);
}

BOOST_AUTO_TEST_CASE(xorAssignTestIntModulo) {
  SubIntModulo a {0x12};
  SubIntModulo b {0x03};
  a ^= b;
  BOOST_REQUIRE_EQUAL(a, 0x11);
}

BOOST_AUTO_TEST_CASE(andAssignTestIntModulo) {
  SubIntModulo a {0x12};
  SubIntModulo b {0x03};
  a &= b;
  BOOST_REQUIRE_EQUAL(a, 0x02);
}

BOOST_AUTO_TEST_CASE(orAssignTestIntModulo) {
  SubIntModulo a {0x12};
  SubIntModulo b {0x03};
  a |= b;
  BOOST_REQUIRE_EQUAL(a, 0x13);
}

BOOST_AUTO_TEST_CASE(shiftLeftAssignTestIntModulo) {
  SubIntModulo a {0x12};
  a <<= 1;
  BOOST_REQUIRE_EQUAL(a, 0x24);
}

BOOST_AUTO_TEST_CASE(shiftRightAssignTestIntModulo) {
  SubIntModulo a {0x12};
  a >>= 1;
  BOOST_REQUIRE_EQUAL(a, 0x09);
}

BOOST_AUTO_TEST_CASE(addTestIntModulo) {
  SubIntModulo a {9};
  SubIntModulo b {20};
  a = a + b;
  BOOST_REQUIRE_EQUAL(a, 29);
  a = a + 10;
  BOOST_REQUIRE_EQUAL(a, 39);
  a = a + -10;
  BOOST_REQUIRE_EQUAL(a, 29);
  a = 10 + a;
  BOOST_REQUIRE_EQUAL(a, 39);
  a = -10 + a;
  BOOST_REQUIRE_EQUAL(a, 29);
}

BOOST_AUTO_TEST_CASE(subtractTestIntModulo) {
  SubIntModulo a {20};
  SubIntModulo b {9};
  a = a - b;
  BOOST_REQUIRE_EQUAL(a, 11);
}

BOOST_AUTO_TEST_CASE(multiplyTestIntModulo) {
  SubIntModulo a {20};
  SubIntModulo b {3};
  a = a * b;
  BOOST_REQUIRE_EQUAL(a, 60);
}

BOOST_AUTO_TEST_CASE(divideTestIntModulo) {
  SubIntModulo a {20};
  SubIntModulo b {3};
  a = a / b;
  BOOST_REQUIRE_EQUAL(a, 6);
}

BOOST_AUTO_TEST_CASE(remainderTestIntModulo) {
  SubIntModulo a {20};
  SubIntModulo b {3};
  a = a % b;
  BOOST_REQUIRE_EQUAL(a, 2);
}

BOOST_AUTO_TEST_CASE(xorTestIntModulo) {
  SubIntModulo a {0x12};
  SubIntModulo b {0x03};
  a = a ^ b;
  BOOST_REQUIRE_EQUAL(a, 0x11);
}

BOOST_AUTO_TEST_CASE(andTestIntModulo) {
  SubIntModulo a {0x12};
  SubIntModulo b {0x03};
  a = a & b;
  BOOST_REQUIRE_EQUAL(a, 0x02);
}

BOOST_AUTO_TEST_CASE(orTestIntModulo) {
  SubIntModulo a {0x12};
  SubIntModulo b {0x03};
  a = a | b;
  BOOST_REQUIRE_EQUAL(a, 0x13);
}

BOOST_AUTO_TEST_CASE(shiftLeftTestIntModulo) {
  SubIntModulo a {0x12};
  a = a << 1;
  BOOST_REQUIRE_EQUAL(a, 0x24);
  a = a << 3;
  BOOST_REQUIRE_EQUAL(a, 92);
}

BOOST_AUTO_TEST_CASE(shiftRightTestIntModulo) {
  SubIntModulo a {0x12};
  a = a >> 1;
  BOOST_REQUIRE_EQUAL(a, 0x09);
  a = a >> 5;
  BOOST_REQUIRE_EQUAL(a, 98);
}

BOOST_AUTO_TEST_CASE(equalsTestIntModulo) {
  SubIntModulo a {9};
  SubIntModulo b {9};
  BOOST_REQUIRE_EQUAL(a, b);
}

BOOST_AUTO_TEST_CASE(notEqualsTestIntModulo) {
  SubIntModulo a {9};
  SubIntModulo b {8};
  BOOST_REQUIRE(a != b);
}

BOOST_AUTO_TEST_CASE(lessThanTestIntModulo) {
  SubIntModulo a {8};
  SubIntModulo b {9};
  BOOST_REQUIRE(a = b));
}

BOOST_AUTO_TEST_CASE(lessThanEqualToTestIntModulo) {
  SubIntModulo a {8};
  SubIntModulo b {8};
  BOOST_REQUIRE(a <= b);
  BOOST_REQUIRE(!(a > b));
}

BOOST_AUTO_TEST_CASE(greaterThanTestIntModulo) {
  SubIntModulo a {8};
  SubIntModulo b {9};
  BOOST_REQUIRE(b > a);
  BOOST_REQUIRE(!(b <= a));
}

BOOST_AUTO_TEST_CASE(greaterThanEqualToTestIntModulo) {
  SubIntModulo a {8};
  SubIntModulo b {8};
  BOOST_REQUIRE(a >= b);
  BOOST_REQUIRE(!(a < b));
}

typedef subrange::subrange<subrange::ordinal_range<short, 1, 99>, subrange::saturated_arithmetic> SubIntSaturated;

BOOST_AUTO_TEST_CASE(createDefaultTestIntSaturated) {
  SubIntSaturated a;
  BOOST_REQUIRE_EQUAL(a, 1);
}

BOOST_AUTO_TEST_CASE(preincrementTestIntSaturated) {
  SubIntSaturated a {9};
  ++a;
  BOOST_REQUIRE_EQUAL(a, 10);
}

BOOST_AUTO_TEST_CASE(postincrementTestIntSaturated) {
  SubIntSaturated a {9};
  a++;
  BOOST_REQUIRE_EQUAL(a, 10);
}

BOOST_AUTO_TEST_CASE(predecrementTestIntSaturated) {
  SubIntSaturated a {9};
  --a;
  BOOST_REQUIRE_EQUAL(a, 8);
}

BOOST_AUTO_TEST_CASE(postdecrementTestIntSaturated) {
  SubIntSaturated a {9};
  a--;
  BOOST_REQUIRE_EQUAL(a, 8);
}

BOOST_AUTO_TEST_CASE(assignLiteralTestIntSaturated) {
  SubIntSaturated a {9};
  a = 20;
  BOOST_REQUIRE_EQUAL(a, 20);
  a = 200;
  BOOST_REQUIRE_EQUAL(a, 99);
  a = -5;
  BOOST_REQUIRE_EQUAL(a, 1);
}

BOOST_AUTO_TEST_CASE(assignTestIntSaturated) {
  SubIntSaturated a {9};
  SubIntSaturated b {20};
  a = b;
  BOOST_REQUIRE_EQUAL(a, b);
  BOOST_REQUIRE_EQUAL(a, 20);
}

BOOST_AUTO_TEST_CASE(addAssignTestIntSaturated) {
  SubIntSaturated a {9};
  SubIntSaturated b {20};
  a += b;
  BOOST_REQUIRE_EQUAL(a, 29);
  a += 10;
  BOOST_REQUIRE_EQUAL(a, 39);
  a += -10;
  BOOST_REQUIRE_EQUAL(a, 29);
  a += 200;
  BOOST_REQUIRE_EQUAL(a, 99);
}

BOOST_AUTO_TEST_CASE(subtractAssignTestIntSaturated) {
  SubIntSaturated a {20};
  SubIntSaturated b {9};
  a -= b;
  BOOST_REQUIRE_EQUAL(a, 11);
  a -= 5;
  BOOST_REQUIRE_EQUAL(a, 6);
  a -= -5;
  BOOST_REQUIRE_EQUAL(a, 11);
  b -= a;
  BOOST_REQUIRE(b == 1);
}

BOOST_AUTO_TEST_CASE(multiplyAssignTestIntSaturated) {
  SubIntSaturated a {20};
  SubIntSaturated b {3};
  a *= b;
  BOOST_REQUIRE_EQUAL(a, 60);
  a *= b;
  BOOST_REQUIRE_EQUAL(a, 99);
}

BOOST_AUTO_TEST_CASE(divideAssignTestIntSaturated) {
  SubIntSaturated a {20};
  SubIntSaturated b {3};
  a /= b;
  BOOST_REQUIRE_EQUAL(a, 6);
}

BOOST_AUTO_TEST_CASE(remainderAssignTestIntSaturated) {
  SubIntSaturated a {20};
  SubIntSaturated b {3};
  a %= b;
  BOOST_REQUIRE_EQUAL(a, 2);
}

BOOST_AUTO_TEST_CASE(xorAssignTestIntSaturated) {
  SubIntSaturated a {0x12};
  SubIntSaturated b {0x03};
  a ^= b;
  BOOST_REQUIRE_EQUAL(a, 0x11);
}

BOOST_AUTO_TEST_CASE(andAssignTestIntSaturated) {
  SubIntSaturated a {0x12};
  SubIntSaturated b {0x03};
  a &= b;
  BOOST_REQUIRE_EQUAL(a, 0x02);
}

BOOST_AUTO_TEST_CASE(orAssignTestIntSaturated) {
  SubIntSaturated a {0x12};
  SubIntSaturated b {0x03};
  a |= b;
  BOOST_REQUIRE_EQUAL(a, 0x13);
}

BOOST_AUTO_TEST_CASE(shiftLeftAssignTestIntSaturated) {
  SubIntSaturated a {0x12};
  a <<= 1;
  BOOST_REQUIRE_EQUAL(a, 0x24);
}

BOOST_AUTO_TEST_CASE(shiftRightAssignTestIntSaturated) {
  SubIntSaturated a {0x12};
  a >>= 1;
  BOOST_REQUIRE_EQUAL(a, 0x09);
}

BOOST_AUTO_TEST_CASE(addTestIntSaturated) {
  SubIntSaturated a {9};
  SubIntSaturated b {20};
  a = a + b;
  BOOST_REQUIRE_EQUAL(a, 29);
  a = a + 10;
  BOOST_REQUIRE_EQUAL(a, 39);
  a = a + -10;
  BOOST_REQUIRE_EQUAL(a, 29);
  a = 10 + a;
  BOOST_REQUIRE_EQUAL(a, 39);
  a = -10 + a;
  BOOST_REQUIRE_EQUAL(a, 29);
}

BOOST_AUTO_TEST_CASE(subtractTestIntSaturated) {
  SubIntSaturated a {20};
  SubIntSaturated b {9};
  a = a - b;
  BOOST_REQUIRE_EQUAL(a, 11);
}

BOOST_AUTO_TEST_CASE(multiplyTestIntSaturated) {
  SubIntSaturated a {20};
  SubIntSaturated b {3};
  a = a * b;
  BOOST_REQUIRE_EQUAL(a, 60);
}

BOOST_AUTO_TEST_CASE(divideTestIntSaturated) {
  SubIntSaturated a {20};
  SubIntSaturated b {3};
  a = a / b;
  BOOST_REQUIRE_EQUAL(a, 6);
}

BOOST_AUTO_TEST_CASE(remainderTestIntSaturated) {
  SubIntSaturated a {20};
  SubIntSaturated b {3};
  a = a % b;
  BOOST_REQUIRE_EQUAL(a, 2);
}

BOOST_AUTO_TEST_CASE(xorTestIntSaturated) {
  SubIntSaturated a {0x12};
  SubIntSaturated b {0x03};
  a = a ^ b;
  BOOST_REQUIRE_EQUAL(a, 0x11);
}

BOOST_AUTO_TEST_CASE(andTestIntSaturated) {
  SubIntSaturated a {0x12};
  SubIntSaturated b {0x03};
  a = a & b;
  BOOST_REQUIRE_EQUAL(a, 0x02);
}

BOOST_AUTO_TEST_CASE(orTestIntSaturated) {
  SubIntSaturated a {0x12};
  SubIntSaturated b {0x03};
  a = a | b;
  BOOST_REQUIRE_EQUAL(a, 0x13);
}

BOOST_AUTO_TEST_CASE(shiftLeftTestIntSaturated) {
  SubIntSaturated a {0x12};
  a = a << 1;
  BOOST_REQUIRE_EQUAL(a, 0x24);
  a = a << 3;
  BOOST_REQUIRE_EQUAL(a, 99);
}

BOOST_AUTO_TEST_CASE(shiftRightTestIntSaturated) {
  SubIntSaturated a {0x12};
  a = a >> 1;
  BOOST_REQUIRE_EQUAL(a, 0x09);
  a = a >> 5;
  BOOST_REQUIRE_EQUAL(a, 1);
}

BOOST_AUTO_TEST_CASE(equalsTestIntSaturated) {
  SubIntSaturated a {9};
  SubIntSaturated b {9};
  BOOST_REQUIRE_EQUAL(a, b);
}

BOOST_AUTO_TEST_CASE(notEqualsTestIntSaturated) {
  SubIntSaturated a {9};
  SubIntSaturated b {8};
  BOOST_REQUIRE(a != b);
}

BOOST_AUTO_TEST_CASE(lessThanTestIntSaturated) {
  SubIntSaturated a {8};
  SubIntSaturated b {9};
  BOOST_REQUIRE(a = b));
}

BOOST_AUTO_TEST_CASE(lessThanEqualToTestIntSaturated) {
  SubIntSaturated a {8};
  SubIntSaturated b {8};
  BOOST_REQUIRE(a <= b);
  BOOST_REQUIRE(!(a > b));
}

BOOST_AUTO_TEST_CASE(greaterThanTestIntSaturated) {
  SubIntSaturated a {8};
  SubIntSaturated b {9};
  BOOST_REQUIRE(b > a);
  BOOST_REQUIRE(!(b <= a));
}

BOOST_AUTO_TEST_CASE(greaterThanEqualToTestIntSaturated) {
  SubIntSaturated a {8};
  SubIntSaturated b {8};
  BOOST_REQUIRE(a >= b);
  BOOST_REQUIRE(!(a < b));
}

typedef subrange::ordinal_range<short, 1, 99> RangeTraits;
typedef subrange::subrange<RangeTraits, subrange::NaN_arithmetic> SubIntNaN;

inline bool isNaN(const SubIntNaN & s) { return subrange::NaN_arithmetic<RangeTraits>::isNaN(s); }

BOOST_AUTO_TEST_CASE(createDefaultTestIntNaN) {
  SubIntNaN a;
  BOOST_REQUIRE_EQUAL(a, 1);
}

BOOST_AUTO_TEST_CASE(preincrementTestIntNaN) {
  SubIntNaN a {9};
  ++a;
  BOOST_REQUIRE_EQUAL(a, 10);
}

BOOST_AUTO_TEST_CASE(postincrementTestIntNaN) {
  SubIntNaN a {9};
  a++;
  BOOST_REQUIRE_EQUAL(a, 10);
}

BOOST_AUTO_TEST_CASE(predecrementTestIntNaN) {
  SubIntNaN a {9};
  --a;
  BOOST_REQUIRE_EQUAL(a, 8);
}

BOOST_AUTO_TEST_CASE(postdecrementTestIntNaN) {
  SubIntNaN a {9};
  a--;
  BOOST_REQUIRE_EQUAL(a, 8);
}

BOOST_AUTO_TEST_CASE(assignLiteralTestIntNaN) {
  SubIntNaN a {9};
  a = 20;
  BOOST_REQUIRE_EQUAL(a, 20);
  a = 200;
  BOOST_REQUIRE(isNaN(a));
  a = -5;
  BOOST_REQUIRE(isNaN(a));
}

BOOST_AUTO_TEST_CASE(assignTestIntNaN) {
  SubIntNaN a {9};
  SubIntNaN b {20};
  a = b;
  BOOST_REQUIRE_EQUAL(a, b);
  BOOST_REQUIRE_EQUAL(a, 20);
}

BOOST_AUTO_TEST_CASE(addAssignTestIntNaN) {
  SubIntNaN a {9};
  SubIntNaN b {20};
  a += b;
  BOOST_REQUIRE_EQUAL(a, 29);
  a += 10;
  BOOST_REQUIRE_EQUAL(a, 39);
  a += -10;
  BOOST_REQUIRE_EQUAL(a, 29);
  a += 200;
  BOOST_REQUIRE(isNaN(a));
}

BOOST_AUTO_TEST_CASE(subtractAssignTestIntNaN) {
  SubIntNaN a {20};
  SubIntNaN b {9};
  a -= b;
  BOOST_REQUIRE_EQUAL(a, 11);
  a -= 5;
  BOOST_REQUIRE_EQUAL(a, 6);
  a -= -5;
  BOOST_REQUIRE_EQUAL(a, 11);
  b -= a;
  BOOST_REQUIRE(isNaN(b));
}

BOOST_AUTO_TEST_CASE(multiplyAssignTestIntNaN) {
  SubIntNaN a {20};
  SubIntNaN b {3};
  a *= b;
  BOOST_REQUIRE_EQUAL(a, 60);
  a *= b;
  BOOST_REQUIRE(isNaN(a));
}

BOOST_AUTO_TEST_CASE(divideAssignTestIntNaN) {
  SubIntNaN a {20};
  SubIntNaN b {3};
  a /= b;
  BOOST_REQUIRE_EQUAL(a, 6);
}

BOOST_AUTO_TEST_CASE(remainderAssignTestIntNaN) {
  SubIntNaN a {20};
  SubIntNaN b {3};
  a %= b;
  BOOST_REQUIRE_EQUAL(a, 2);
}

BOOST_AUTO_TEST_CASE(xorAssignTestIntNaN) {
  SubIntNaN a {0x12};
  SubIntNaN b {0x03};
  a ^= b;
  BOOST_REQUIRE_EQUAL(a, 0x11);
}

BOOST_AUTO_TEST_CASE(andAssignTestIntNaN) {
  SubIntNaN a {0x12};
  SubIntNaN b {0x03};
  a &= b;
  BOOST_REQUIRE_EQUAL(a, 0x02);
}

BOOST_AUTO_TEST_CASE(orAssignTestIntNaN) {
  SubIntNaN a {0x12};
  SubIntNaN b {0x03};
  a |= b;
  BOOST_REQUIRE_EQUAL(a, 0x13);
}

BOOST_AUTO_TEST_CASE(shiftLeftAssignTestIntNaN) {
  SubIntNaN a {0x12};
  a <<= 1;
  BOOST_REQUIRE_EQUAL(a, 0x24);
}

BOOST_AUTO_TEST_CASE(shiftRightAssignTestIntNaN) {
  SubIntNaN a {0x12};
  a >>= 1;
  BOOST_REQUIRE_EQUAL(a, 0x09);
}

BOOST_AUTO_TEST_CASE(addTestIntNaN) {
  SubIntNaN a {9};
  SubIntNaN b {20};
  a = a + b;
  BOOST_REQUIRE_EQUAL(a, 29);
  a = a + 10;
  BOOST_REQUIRE_EQUAL(a, 39);
  a = a + -10;
  BOOST_REQUIRE_EQUAL(a, 29);
  a = 10 + a;
  BOOST_REQUIRE_EQUAL(a, 39);
  a = -10 + a;
  BOOST_REQUIRE_EQUAL(a, 29);
}

BOOST_AUTO_TEST_CASE(subtractTestIntNaN) {
  SubIntNaN a {20};
  SubIntNaN b {9};
  a = a - b;
  BOOST_REQUIRE_EQUAL(a, 11);
}

BOOST_AUTO_TEST_CASE(multiplyTestIntNaN) {
  SubIntNaN a {20};
  SubIntNaN b {3};
  a = a * b;
  BOOST_REQUIRE_EQUAL(a, 60);
}

BOOST_AUTO_TEST_CASE(divideTestIntNaN) {
  SubIntNaN a {20};
  SubIntNaN b {3};
  a = a / b;
  BOOST_REQUIRE_EQUAL(a, 6);
}

BOOST_AUTO_TEST_CASE(remainderTestIntNaN) {
  SubIntNaN a {20};
  SubIntNaN b {3};
  a = a % b;
  BOOST_REQUIRE_EQUAL(a, 2);
}

BOOST_AUTO_TEST_CASE(xorTestIntNaN) {
  SubIntNaN a {0x12};
  SubIntNaN b {0x03};
  a = a ^ b;
  BOOST_REQUIRE_EQUAL(a, 0x11);
}

BOOST_AUTO_TEST_CASE(andTestIntNaN) {
  SubIntNaN a {0x12};
  SubIntNaN b {0x03};
  a = a & b;
  BOOST_REQUIRE_EQUAL(a, 0x02);
}

BOOST_AUTO_TEST_CASE(orTestIntNaN) {
  SubIntNaN a {0x12};
  SubIntNaN b {0x03};
  a = a | b;
  BOOST_REQUIRE_EQUAL(a, 0x13);
}

BOOST_AUTO_TEST_CASE(shiftLeftTestIntNaN) {
  SubIntNaN a {0x12};
  a = a << 1;
  BOOST_REQUIRE_EQUAL(a, 0x24);
  a = a << 3;
  BOOST_REQUIRE(isNaN(a));
}

BOOST_AUTO_TEST_CASE(shiftRightTestIntNaN) {
  SubIntNaN a {0x12};
  a = a >> 1;
  BOOST_REQUIRE_EQUAL(a, 0x09);
  a = a >> 5;
  BOOST_REQUIRE(isNaN(a));
}

BOOST_AUTO_TEST_CASE(equalsTestIntNaN) {
  SubIntNaN a {9};
  SubIntNaN b {9};
  BOOST_REQUIRE_EQUAL(a, b);
}

BOOST_AUTO_TEST_CASE(notEqualsTestIntNaN) {
  SubIntNaN a {9};
  SubIntNaN b {8};
  BOOST_REQUIRE(a != b);
}

BOOST_AUTO_TEST_CASE(lessThanTestIntNaN) {
  SubIntNaN a {8};
  SubIntNaN b {9};
  BOOST_REQUIRE(a = b));
}

BOOST_AUTO_TEST_CASE(lessThanEqualToTestIntNaN) {
  SubIntNaN a {8};
  SubIntNaN b {8};
  BOOST_REQUIRE(a <= b);
  BOOST_REQUIRE(!(a > b));
}

BOOST_AUTO_TEST_CASE(greaterThanTestIntNaN) {
  SubIntNaN a {8};
  SubIntNaN b {9};
  BOOST_REQUIRE(b > a);
  BOOST_REQUIRE(!(b <= a));
}

BOOST_AUTO_TEST_CASE(greaterThanEqualToTestIntNaN) {
  SubIntNaN a {8};
  SubIntNaN b {8};
  BOOST_REQUIRE(a >= b);
  BOOST_REQUIRE(!(a < b));
}

//  Can't use any templates for defining floating point range traits as C++ forbids template parameters
//  that are not of ordinal type so we haver to do that manually.

class float_range {
 public :
  typedef double value_type;
  typedef subrange::range_error exception_type;
  static double min() { return 1.0; }
  static double max() { return 99.0; }
};

typedef subrange::subrange<float_range> SubFloat;

BOOST_AUTO_TEST_CASE(createDefaultTestFloat) {
  SubFloat a;
  BOOST_REQUIRE_EQUAL(a, 1.0);
}

BOOST_AUTO_TEST_CASE(preincrementTestFloat) {
  SubFloat a {9.0};
  ++a;
  BOOST_REQUIRE_EQUAL(a, 10.0);
}

BOOST_AUTO_TEST_CASE(postincrementTestFloat) {
  SubFloat a {9.0};
  a++;
  BOOST_REQUIRE_EQUAL(a, 10.0);
}

BOOST_AUTO_TEST_CASE(predecrementTestFloat) {
  SubFloat a {9.0};
  --a;
  BOOST_REQUIRE_EQUAL(a, 8.0);
}

BOOST_AUTO_TEST_CASE(postdecrementTestFloat) {
  SubFloat a {9.0};
  a--;
  BOOST_REQUIRE_EQUAL(a, 8.0);
}

BOOST_AUTO_TEST_CASE(assignLiteralTestFloat) {
  SubFloat a {9.0};
  a = 20.0;
  BOOST_REQUIRE_EQUAL(a, 20.0);
  BOOST_REQUIRE_THROW(a = 200.0, subrange::range_error);
  BOOST_REQUIRE_THROW(a = -5.0, subrange::range_error);
}

BOOST_AUTO_TEST_CASE(assignTestFloat) {
  SubFloat a {9.0};
  SubFloat b {20.0};
  a = b;
  BOOST_REQUIRE_EQUAL(a, b);
  BOOST_REQUIRE_EQUAL(a, 20.0);
}

BOOST_AUTO_TEST_CASE(addAssignTestFloat) {
  SubFloat a {9.0};
  SubFloat b {20.0};
  a += b;
  BOOST_REQUIRE_EQUAL(a, 29.0);
  a += 10;
  BOOST_REQUIRE_EQUAL(a, 39.0);
  a += -10;
  BOOST_REQUIRE_EQUAL(a, 29.0);
  BOOST_REQUIRE_THROW(a += 200.0, subrange::range_error);
}

BOOST_AUTO_TEST_CASE(subtractAssignTestFloat) {
  SubFloat a {20.0};
  SubFloat b {9.0};
  a -= b;
  BOOST_REQUIRE_EQUAL(a, 11.0);
  a -= 5.0;
  BOOST_REQUIRE_EQUAL(a, 6.0);
  a -= -5.0;
  BOOST_REQUIRE_EQUAL(a, 11.0);
  BOOST_REQUIRE_THROW(b -= a, subrange::range_error);
}

BOOST_AUTO_TEST_CASE(multiplyAssignTestFloat) {
  SubFloat a {20.0};
  SubFloat b {3.0};
  a *= b;
  BOOST_REQUIRE_EQUAL(a, 60.0);
  BOOST_REQUIRE_THROW(a *= b, subrange::range_error);
}

BOOST_AUTO_TEST_CASE(divideAssignTestFloat) {
  SubFloat a {20.0};
  SubFloat b {4.0};
  a /= b;
  BOOST_REQUIRE_EQUAL(a, 5.0);
}

BOOST_AUTO_TEST_CASE(addTestFloat) {
  SubFloat a {9.0};
  SubFloat b {20.0};
  a = a + b;
  BOOST_REQUIRE_EQUAL(a, 29.0);
  a = a + 10.0;
  BOOST_REQUIRE_EQUAL(a, 39.0);
  a = a + -10.0;
  BOOST_REQUIRE_EQUAL(a, 29.0);
  a = 10.0 + a;
  BOOST_REQUIRE_EQUAL(a, 39.0);
  a = -10.0 + a;
  BOOST_REQUIRE_EQUAL(a, 29.0);
}

BOOST_AUTO_TEST_CASE(subtractTestFloat) {
  SubFloat a {20.0};
  SubFloat b {9.0};
  a = a - b;
  BOOST_REQUIRE_EQUAL(a, 11.0);
}

BOOST_AUTO_TEST_CASE(multiplyTestFloat) {
  SubFloat a {20.0};
  SubFloat b {3.0};
  a = a * b;
  BOOST_REQUIRE_EQUAL(a, 60.0);
}

BOOST_AUTO_TEST_CASE(divideTestFloat) {
  SubFloat a {20.0};
  SubFloat b {4.0};
  a = a / b;
  BOOST_REQUIRE_EQUAL(a, 5.0);
}

BOOST_AUTO_TEST_CASE(equalsTestFloat) {
  SubFloat a {9.0};
  SubFloat b {9.0};
  BOOST_REQUIRE_EQUAL(a, b);
}

BOOST_AUTO_TEST_CASE(notEqualsTestFloat) {
  SubFloat a {9.0};
  SubFloat b {8.0};
  BOOST_REQUIRE(a != b);
}

BOOST_AUTO_TEST_CASE(lessThanTestFloat) {
  SubFloat a {8.0};
  SubFloat b {9.0};
  BOOST_REQUIRE(a = b));
}

BOOST_AUTO_TEST_CASE(lessThanEqualToTestFloat) {
  SubFloat a {8.0};
  SubFloat b {8.0};
  BOOST_REQUIRE(a <= b);
  BOOST_REQUIRE(!(a > b));
}

BOOST_AUTO_TEST_CASE(greaterThanTestFloat) {
  SubFloat a {8.0};
  SubFloat b {9.0};
  BOOST_REQUIRE(b > a);
  BOOST_REQUIRE(!(b <= a));
}

BOOST_AUTO_TEST_CASE(greaterThanEqualToTestFloat) {
  SubFloat a {8.0};
  SubFloat b {8.0};
  BOOST_REQUIRE(a >= b);
  BOOST_REQUIRE(!(a < b));
}


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

#include <boost/hana/maybe.hpp>

#include <boost/hana/bool.hpp>
#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/minimal/comparable.hpp>

#include <tuple>
using namespace boost::hana;

BOOST_HANA_CONSTEXPR_LAMBDA auto f = [](auto x) {
    return std::make_tuple(x);
};

struct invalid { };

template <int i>
constexpr auto x = detail::minimal::comparable<>(i);

template <bool b>
BOOST_HANA_CONSTEXPR_LAMBDA auto pred = [](auto) {
    return bool_<b>; // we need a compile-time logical
};

int main() {
    BOOST_HANA_CONSTEXPR_ASSERT(only_when(pred<true>, f, x<0>) == just(f(x<0>)));
    BOOST_HANA_CONSTANT_ASSERT(only_when(pred<false>, f, x<0>) == nothing);
    BOOST_HANA_CONSTANT_ASSERT(only_when(pred<false>, invalid{}, x<0>) == nothing);
}


//==================================================================================================
/*!

  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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/all.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

STF_CASE_TPL ( "all", STF_ALL_TYPES)
{
  namespace bs = boost::simd;
  using bs::all;

  STF_EQUAL( all(T(1)) , true  );
  STF_EQUAL( all(T(0)), false );
}


/**
    anest.cc
    Purpose: Perform the Displacement Method of Analysis
    for structural analysis.

    @author Adolfo Correa
    @version 0.1 02/03/20
*/

#include<iostream>
#include<vector>
#include <armadillo>
#include <fstream>
#include <string>

using namespace std;
using namespace arma;

void preparq(int, int, int, int, int);
// void nos(int nno, int nnv, vector<double> x);

void nos(int nno, int nnv, vector<double> x, \
vector<double> y, vector<vector<double>> fa, \
vector<vector<int>> d, vector<vector<int>> v, \
vector<int> nv, int gln);

void testing_armadillo(void);

int main(int argc, char* argv[])
{
	int nno, nel, ngl, nnv, gln, nelt, nelp, \
	nll, ntc = nno = nel = ngl = nnv = gln = nelt \
	= nelp = nll = 0;

	vector<int> nv, te, nc;

	/// Vector 2D
	vector<vector<int>> no, g, v, d, il;
	vector<double> x, y, e, a, iz, ra, fp, f, u, \
	qx, qy;
	/// Vector 2D
	vector<vector<double>> fa, sg, esf;

/// Read input files
	preparq(nno, nel, ntc, ngl, gln);

	nno = nel = 4; ntc = 3; gln = 3; ngl = gln * nno;

// Dimensionar vetores e matrizes
// matrix.resize(M, vector<int>(N, default_value));

/**
* \brief Initialize the variables
*/
	d.resize(nno, vector<int>(gln, 0));
	v.resize(nno, vector<int>(gln, 0));
	no.resize(2, vector<int>(nel, 0));
	g.resize(nel, vector<int>(2*gln, 0));
	il.resize(nel, vector<int>(2*gln, 0));

	fa.resize(nno, vector<double>(gln, 0));
	esf.resize(6, vector<double>(nel, 0));
	x.resize(nno, 0);
	y.resize(nno, 0);
	nv.resize(nno, 0);
	te.resize(nel, 0);
	qx.resize(nel, 0);
	qy.resize(nel, 0);
	e.resize(ntc, 0);
	a.resize(ntc, 0);
	iz.resize(ntc, 0);
	fp.resize(nel, 0);
	nc.resize(nel, 0);

//	nos(nno, nnv, x);
	nos(nno, nnv, x, y, fa, d, v, nv, gln);

	/// Testing Armadillo library
	testing_armadillo();

	return 0;
}

void preparq(int nno, int nel, int ntc, int ngl, int gln)
{
	cout<<"Reading input files"<<endl;
	ifstream file;
	ofstream auxFile;
	string line, filename = "exemplo";
	file.open(filename);
	auxFile.open("anest.dat");

	// Read lines of input file
	for(int i = 0; !file.eof(); i++)
	{
		//file.get(line);
		getline(file, line);
		cout<<line<<endl;
		if(line.substr(0,1) != ";") auxFile<<line<<endl;
	}

	file.close();
	auxFile.close();

}

/**
 * Read the nodes coordinates of the structure from
 * the input file.
 *
 * @param[in] nno number of nodes
 * @param[in] gln degree of freedom per node
 * @param[out] x x-coordinates of the nodes
 * @param[out] y y-coordinates of the nodes
 */
void nos(int nno, int nnv, vector<double> x, \
vector<double> y, vector<vector<double>> fa, \
vector<vector<int>> d, vector<vector<int>> v, \
vector<int> nv, int gln)
{
	cout<<"Reading nos"<<endl;
}

/**
 * Create two matrices with random values
 * and multiply them
 *
 * @param void.
 * @return none, only print on the screen.
 */
void testing_armadillo(void)
{
	cout<<endl<<"Testing armadillo::mat"<<endl<<endl;

	mat A(4, 5, fill::randu); ///< matrix A filled with random unsigned integers
	mat B(4, 5, fill::randn); ///< matrix B filled with randoms integers

	cout<<"A = "<<A<<endl;
	cout<<"B = "<<B<<endl;

	/**
	  \f[
  	    C = A \cross B^T
	  \f]
	*/
	cout<<"A*B.t() = "<<A*B.t()<<endl; ///< Plot the product of the two matrices

}


#ifndef LIS2MDL_HEADER
#define LIS2MDL_HEADER
#include "I2CDevice.hpp"
#include "stdint.h"
#include <iostream>
#include <Eigen/Dense>
#include "measurement.hpp"

#define LIS2MDL_ADDR           0X1E
#define LIS2MDL_WHO_AM_I_VALUE 0X40
#define LIS2MDL_SCALE_FACTOR   1.5
#define LIS2MDL_TEMP_SCALE     8

enum class Lis2RegisterMap : uint16_t
{
    WHO_AM_I   = 0x4F,
    CFG_REG_A  = 0x60,
    OUTX_L_REG = 0x68,
    OUTX_H_REG = 0x69,
    OUTY_L_REG = 0x6A,
    OUTY_H_REG = 0x6B,
    OUTZ_L_REG = 0x6C,
    OUTZ_H_REG = 0x6D,
};

class LIS2MDL : public exploringBB::I2CDevice
{
    public:
    LIS2MDL(uint16_t i2cBus);
    ~LIS2MDL(){};

    uint8_t getWhoAmI();
    bool checkWhoAmI() { return getWhoAmI() == LIS2MDL_WHO_AM_I_VALUE; }

    /// \brief Returns true when the sensor is configured and connection has been verified
    bool isConfigured() { return configured_; }

    /// \brief Gets a measurement from the magnetometer
    void getMeasurement(Measurement& meas);

    void calibrateSensor();

    void printCalibrationMatrices();

    private:
    void setConfigRegA();

    bool configured_;

    Eigen::MatrixXf a_matrix_;
    Eigen::MatrixXf b_matrix_;
};
#endif


/*=============================================================================
    Copyright (c) 2010 Christopher Schmidt

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
==============================================================================*/

#include <boost/config.hpp>
#include <boost/detail/lightweight_test.hpp>
#include <boost/fusion/sequence.hpp>
#include <boost/fusion/iterator.hpp>
#include <boost/fusion/algorithm/transformation/reverse.hpp>
#include <boost/fusion/algorithm/iteration/fold.hpp>
#include <boost/fusion/algorithm/iteration/reverse_fold.hpp>
#include <boost/fusion/algorithm/iteration/iter_fold.hpp>
#include <boost/fusion/algorithm/iteration/reverse_iter_fold.hpp>
#include <boost/fusion/container/vector/convert.hpp>
#include <boost/fusion/container/vector/vector.hpp>
#include <boost/fusion/adapted/mpl.hpp>
#include <boost/fusion/support/pair.hpp>
#include <boost/fusion/mpl.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/back_inserter.hpp>
#include <boost/mpl/always.hpp>
#include <boost/mpl/copy.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <iostream>

namespace mpl=boost::mpl;
namespace fusion=boost::fusion;

#ifdef BOOST_FUSION_TEST_REVERSE_FOLD
#   ifdef BOOST_FUSION_TEST_ITER_FOLD
#       define BOOST_FUSION_TEST_FOLD_NAME reverse_iter_fold
#   else
#       define BOOST_FUSION_TEST_FOLD_NAME reverse_fold
#   endif
#else
#   ifdef BOOST_FUSION_TEST_ITER_FOLD
#       define BOOST_FUSION_TEST_FOLD_NAME iter_fold
#   else
#       define BOOST_FUSION_TEST_FOLD_NAME fold
#   endif
#endif

struct sum
{
    template<typename Sig>
    struct result;

    template<typename Self, typename State, typename T>
    struct result<Self(State,T)>
      : boost::fusion::result_of::make_pair<
            mpl::int_<
                boost::remove_reference<
                    State
                >::type::first_type::value+1
            >
          , int
        >
    {
        BOOST_MPL_ASSERT((typename boost::is_reference<State>::type));
        BOOST_MPL_ASSERT((typename boost::is_reference<T>::type));
    };

#ifdef BOOST_FUSION_TEST_ITER_FOLD
    template<typename State, typename It>
    typename result<sum const&(State const&,It const&)>::type
    operator()(State const& state, It const& it)const
    {
        static const int n=State::first_type::value;
        return fusion::make_pair<mpl::int_<n+1> >(
            state.second+fusion::deref(it)*n);
    }
#else
    template<typename State>
    typename result<sum const&(State const&, int const&)>::type
    operator()(State const& state, int const& e)const
    {
        static const int n=State::first_type::value;
        return fusion::make_pair<mpl::int_<n+1> >(state.second+e*n);
    }
#endif
};

struct meta_sum
{
    template<typename Sig>
    struct result;

    template<typename Self, typename State, typename T>
    struct result<Self(State,T)>
    {
        BOOST_MPL_ASSERT((typename boost::is_reference<State>::type));
        BOOST_MPL_ASSERT((typename boost::is_reference<T>::type));

        typedef typename boost::remove_reference<State>::type state;
        static const int n=mpl::front<state>::type::value;

#ifdef BOOST_FUSION_TEST_ITER_FOLD
        typedef typename
            boost::fusion::result_of::value_of<
                typename boost::remove_reference<T>::type
            >::type
        t;
#else
        typedef typename boost::remove_reference<T>::type t;
#endif

        typedef
            mpl::vector<
                mpl::int_<n+1>
              , mpl::int_<
                    mpl::back<state>::type::value+t::value*n
                >
            >
        type;
    };

    template<typename State, typename T>
    typename result<meta_sum const&(State const&,T const&)>::type
    operator()(State const&, T const&)const;
};

struct fold_test_n
{
    template<typename I>
    void
    operator()(I)const
    {
        static const int n=I::value;
        typedef mpl::range_c<int, 0, n> range;

        static const int squares_sum=n*(n+1)*(2*n+1)/6;

        {
            mpl::range_c<int, 1, n+1> init_range;
            typename boost::fusion::result_of::as_vector<
                typename mpl::transform<
                    range
                  , mpl::always<int>
                  , mpl::back_inserter<mpl::vector<> >
                >::type
            >::type vec(
#ifdef BOOST_FUSION_TEST_REVERSE_FOLD
                fusion::reverse(init_range)
#else
                init_range
#endif
            );

            int result=BOOST_FUSION_TEST_FOLD_NAME(
                vec,
                fusion::make_pair<mpl::int_<1> >(0),
                sum()).second;
            std::cout << n << ": " << result << std::endl;
            BOOST_TEST(result==squares_sum);
        }

        {
            typedef typename
#ifdef BOOST_FUSION_TEST_REVERSE_FOLD
                boost::fusion::result_of::as_vector<
                    typename mpl::copy<
                        mpl::range_c<int, 1, n+1>
                      , mpl::front_inserter<fusion::vector<> >
                    >::type
                >::type
#else
                boost::fusion::result_of::as_vector<mpl::range_c<int, 1, n+1> >::type
#endif
            vec;

            typedef
                boost::is_same<
                    typename boost::fusion::result_of::BOOST_FUSION_TEST_FOLD_NAME<
                        vec
                      , mpl::vector<mpl::int_<1>, mpl::int_<0> >
                      , meta_sum
                    >::type
                  , typename mpl::if_c<
                        !n
                      , mpl::vector<mpl::int_<1>, mpl::int_<0> >&
                      , mpl::vector<mpl::int_<n+1>, mpl::int_<squares_sum> >
                    >::type
                >
            result_test;

            BOOST_MPL_ASSERT((result_test));
        }
    }
};

int
main()
{
    mpl::for_each<mpl::range_c<int, 0, 10> >(fold_test_n());

    return boost::report_errors();
}

#undef BOOST_FUSION_TEST_FOLD_NAME


/*
 * Copyright (C) Alex Nekipelov (alex@nekipelov.net)
 * License: BSD
 */

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>
#include <boost/foreach.hpp>

#include "value.h"

using namespace cpptl;

BOOST_AUTO_TEST_SUITE(value)

BOOST_AUTO_TEST_CASE(value_nums)
{
    Value v1( static_cast<int64_t>(5) );
    Value v2( static_cast<uint64_t>(5) );
    Value v3( static_cast<double>(5.0) );
    Value v4( static_cast<int>(5) );
    Value v5( static_cast<unsigned int>(5) );

    BOOST_VERIFY(v1.toInt() == 5);
    BOOST_VERIFY(v2.toInt() == 5);
    BOOST_VERIFY(v3.toInt() == 5);
    BOOST_VERIFY(v4.toInt() == 5);
    BOOST_VERIFY(v5.toInt() == 5);

    BOOST_VERIFY(v1.toUInt() == 5);
    BOOST_VERIFY(v2.toUInt() == 5);
    BOOST_VERIFY(v3.toUInt() == 5);
    BOOST_VERIFY(v4.toUInt() == 5);
    BOOST_VERIFY(v5.toUInt() == 5);

    BOOST_VERIFY(v1.toInt64() == 5);
    BOOST_VERIFY(v2.toInt64() == 5);
    BOOST_VERIFY(v3.toInt64() == 5);
    BOOST_VERIFY(v4.toInt64() == 5);
    BOOST_VERIFY(v5.toInt64() == 5);

    BOOST_VERIFY(v1.toUInt64() == 5);
    BOOST_VERIFY(v2.toUInt64() == 5);
    BOOST_VERIFY(v3.toUInt64() == 5);
    BOOST_VERIFY(v4.toUInt64() == 5);
    BOOST_VERIFY(v5.toUInt64() == 5);

    BOOST_VERIFY(v1.toDouble() == 5.0);
    BOOST_VERIFY(v2.toDouble() == 5.0);
    BOOST_VERIFY(v3.toDouble() == 5.0);
    BOOST_VERIFY(v4.toDouble() == 5.0);
    BOOST_VERIFY(v5.toDouble() == 5.0);

    BOOST_VERIFY( (v1 + v2).toInt() == 10 );
    BOOST_VERIFY( (v2 + v3).toInt() == 10 );
    BOOST_VERIFY( (v3 + v4).toInt() == 10 );
    BOOST_VERIFY( (v1 + v3).toInt() == 10 );
    BOOST_VERIFY( (v1 + v4).toInt() == 10 );
    BOOST_VERIFY( (v2 + v4).toInt() == 10 );
    BOOST_VERIFY( (v5 + v4).toInt() == 10 );
    BOOST_VERIFY( (v1 + v2 + v3 + v4 + v5).toInt() == 25 );

    BOOST_VERIFY( (v1 - v2).toInt() == 0 );
    BOOST_VERIFY( (v2 - v3).toInt() == 0 );
    BOOST_VERIFY( (v3 - v4).toInt() == 0 );
    BOOST_VERIFY( (v1 - v3).toInt() == 0 );
    BOOST_VERIFY( (v1 - v4).toInt() == 0 );
    BOOST_VERIFY( (v2 - v4).toInt() == 0 );
    BOOST_VERIFY( (v2 - v5).toInt() == 0 );
    BOOST_VERIFY( (v1 + v2 - v3 - v4 - v5).toInt() == -5 );

    BOOST_VERIFY( (v1 / v2).toInt() == 1 );
    BOOST_VERIFY( (v2 / v3).toInt() == 1 );
    BOOST_VERIFY( (v3 / v4).toInt() == 1 );
    BOOST_VERIFY( (v1 / v3).toInt() == 1 );
    BOOST_VERIFY( (v1 / v4).toInt() == 1 );
    BOOST_VERIFY( (v2 / v4).toInt() == 1 );
    BOOST_VERIFY( (v2 / v5).toInt() == 1 );
    BOOST_VERIFY( (Value(25) / v4).toInt() == 5 );
    BOOST_VERIFY( (v1 / v2 / v3 / v4 / v5).toInt() == 0 );

    BOOST_VERIFY( (v1 * v2).toInt() == 25 );
    BOOST_VERIFY( (v2 * v3).toInt() == 25 );
    BOOST_VERIFY( (v3 * v4).toInt() == 25 );
    BOOST_VERIFY( (v1 * v3).toInt() == 25 );
    BOOST_VERIFY( (v1 * v4).toInt() == 25 );
    BOOST_VERIFY( (v2 * v4).toInt() == 25 );
    BOOST_VERIFY( (v1 * v5).toInt() == 25 );
    BOOST_VERIFY( (v2 * v5).toInt() == 25 );
    BOOST_VERIFY( (v1 * v2 * v3 * v4 * v5).toInt() == 5*5*5*5*5 );

    BOOST_VERIFY( (v1 + v2) == (v3 + v4) );
    BOOST_VERIFY( v1 == v5 );
    BOOST_VERIFY( v2 == v4 );

    BOOST_VERIFY( (v1 + 1) != v1 );
    BOOST_VERIFY( (v2 + 1) != v2 );
    BOOST_VERIFY( (v3 + 1) != v3 );
    BOOST_VERIFY( (v4 + 1) != v4 );
    BOOST_VERIFY( (v5 + 1) != v4 );
    BOOST_VERIFY( (v1 + v2) != v3 );

    BOOST_VERIFY( v1 > 1 && !(v1 > 10) );
    BOOST_VERIFY( v2 > 1 && !(v2 > 10) );
    BOOST_VERIFY( v3 > 1 && !(v3 > 10) );
    BOOST_VERIFY( v4 > 1 && !(v4 > 10) );

    BOOST_VERIFY( v1 < 10 && !(v1 < 1) );
    BOOST_VERIFY( v2 < 10 && !(v2 < 1) );
    BOOST_VERIFY( v3 < 10 && !(v3 < 1) );
    BOOST_VERIFY( v4 < 10 && !(v4 < 1) );

    BOOST_VERIFY( v1 >= 1 && v1 >= 5 && !(v1 > 50) );
    BOOST_VERIFY( v2 >= 1 && v2 >= 5 && !(v2 > 50) );
    BOOST_VERIFY( v3 >= 1 && v3 >= 5 && !(v3 > 50) );
    BOOST_VERIFY( v4 >= 1 && v4 >= 5 && !(v4 > 50) );

    BOOST_VERIFY( v1 <= 50 && v1 <= 5 && !(v1 <= 1) );
    BOOST_VERIFY( v2 <= 50 && v2 <= 5 && !(v2 <= 1) );
    BOOST_VERIFY( v3 <= 50 && v3 <= 5 && !(v3 <= 1) );
    BOOST_VERIFY( v4 <= 50 && v4 <= 5 && !(v4 <= 1) );
}

BOOST_AUTO_TEST_CASE(value_strings)
{
    Value v1("test");
    Value v2(" ");
    Value v3("string");

    BOOST_VERIFY(v1 == "test");
    BOOST_VERIFY(v2 == " ");
    BOOST_VERIFY(v3 == "string");

    BOOST_VERIFY(v1.toString() == "test");
    BOOST_VERIFY(v2.toString() == " ");
    BOOST_VERIFY(v3.toString() == "string");

    BOOST_VERIFY(v1 + v2 + v3 == "test string");

    Value s1 = v1;
    Value s2 = v2;
    Value s3 = v3;

    BOOST_VERIFY(v1 == s1);
    BOOST_VERIFY(v2 == s2);
    BOOST_VERIFY(v3 == s3);

    BOOST_VERIFY(v1 != s2);
    BOOST_VERIFY(v2 != s3);
    BOOST_VERIFY(v3 != s1);

    v1 = "10";
    v2 = "10.5";
    v3 = "true";
    Value v4 = "false";

    BOOST_VERIFY(v1.toInt() == 10);
    BOOST_VERIFY(v2.toDouble() == 10.5);
    BOOST_VERIFY(v3.toBool() == true);
    BOOST_VERIFY(v4.toBool() == false);
}

BOOST_AUTO_TEST_CASE(value_bool)
{
    Value v1(true);
    Value v2(false);

    BOOST_VERIFY(v1 == true);
    BOOST_VERIFY(v2 == false);

    BOOST_VERIFY(v1.toString() == "true");
    BOOST_VERIFY(v2.toString() == "false");
    BOOST_VERIFY(v1.toInt() == 1);

}

BOOST_AUTO_TEST_CASE(value_fromValue)
{
    Value v1 = Value::fromValue(true);
    Value v2 = Value::fromValue(false);
    Value v3 = Value::fromValue(std::string("string"));
    Value v4 = Value::fromValue(10);
    Value v5 = Value::fromValue(-10);
    Value v6 = Value::fromValue(10.10);
    Value v7 = Value::fromValue(-10.10);

    BOOST_VERIFY(v1.type() == Value::Bool);
    BOOST_VERIFY(v2.type() == Value::Bool);
    BOOST_VERIFY(v3.type() == Value::String);
    BOOST_VERIFY(v4.type() == Value::Int);
    BOOST_VERIFY(v5.type() == Value::Int);
    BOOST_VERIFY(v6.type() == Value::Double);
    BOOST_VERIFY(v7.type() == Value::Double);

    BOOST_VERIFY(v1 == true);
    BOOST_VERIFY(v2 == false);
    BOOST_VERIFY(v3 == "string");
    BOOST_VERIFY(v4 == 10);
    BOOST_VERIFY(v5 == -10);
    BOOST_VERIFY(v6 == 10.10);
    BOOST_VERIFY(v7 == -10.10);
}

BOOST_AUTO_TEST_CASE(value_null)
{
    Value nil;

    BOOST_VERIFY(nil.isNull());
    BOOST_VERIFY(nil.isEmpty());
    BOOST_VERIFY(nil.isArray() == false);
    BOOST_VERIFY(nil.isObject() == false);
    BOOST_VERIFY(nil.size() == 0);
    BOOST_VERIFY(nil.toString() == "");
    BOOST_VERIFY(nil.toInt() == 0);
    BOOST_VERIFY(nil.type() == Value::Null);
}

BOOST_AUTO_TEST_CASE(value_object)
{
    Value obj(Value::Object);

    BOOST_VERIFY(obj.isNull() == false);
    BOOST_VERIFY(obj.isEmpty());
    BOOST_VERIFY(obj.isObject());
    BOOST_VERIFY(obj.isArray() == false);
    BOOST_VERIFY(obj.size() == 0);
    BOOST_VERIFY(obj.type() == Value::Object);

    obj["property1"] = 10;
    obj["property2"] = "string";
    obj["property3"] = true;

    BOOST_VERIFY(obj.size() == 3);
    BOOST_VERIFY(obj["property1"] == 10);
    BOOST_VERIFY(obj["property2"] == "string");
    BOOST_VERIFY(obj["property3"] == true);

    BOOST_VERIFY(obj.hasMember("property1") == true);
    BOOST_VERIFY(obj.hasMember("property2") == true);
    BOOST_VERIFY(obj.hasMember("property3") == true);
    BOOST_VERIFY(obj.hasMember("invalid_property") == false);
}

BOOST_AUTO_TEST_CASE(value_array)
{
    Value obj(Value::Array);

    BOOST_VERIFY(obj.isNull() == false);
    BOOST_VERIFY(obj.isEmpty());
    BOOST_VERIFY(obj.isArray());
    BOOST_VERIFY(obj.isObject() == false);
    BOOST_VERIFY(obj.size() == 0);
    BOOST_VERIFY(obj.type() == Value::Array);

    obj.append(10);
    obj[1] = std::string("string");
    obj[2] = true;

    BOOST_VERIFY(obj.size() == 3);
    BOOST_VERIFY(obj[0] == 10);
    BOOST_VERIFY(obj[1] == "string");
    BOOST_VERIFY(obj[2] == true);
    BOOST_VERIFY(obj.at(0) == 10);
    BOOST_VERIFY(obj.at(1) == "string");
    BOOST_VERIFY(obj.at(2) == true);
}

BOOST_AUTO_TEST_CASE(value_user_type)
{
    Value obj;

    {
        std::list<Value> list;

        list.push_back(10);
        list.push_back("test");
        list.push_back(false);

        obj = Value::fromValue(list);
    }

    BOOST_VERIFY(obj.isNull() == false);
    BOOST_VERIFY(obj.isEmpty() == false);
    BOOST_VERIFY(obj.type() == Value::UserType);

    {
        std::list<Value> list = obj.toValue< std::list<Value> >();
        std::list<Value>::iterator it = list.begin();

        BOOST_VERIFY(list.size() == 3);

        if( list.size() == 3 )
        {
            Value v1 = *it++;
            Value v2 = *it++;
            Value v3 = *it++;

            BOOST_VERIFY(v1 == 10);
            BOOST_VERIFY(v2 == "test");
            BOOST_VERIFY(v3 == false);
        }
    }
}

BOOST_AUTO_TEST_CASE(value_ctor)
{
    Value obj1(Value::Bool);
    Value obj2(Value::Int);
    Value obj3(Value::Integer);
    Value obj4(Value::Double);
    Value obj5(Value::String);
    Value obj6(Value::Array);
    Value obj7(Value::Object);
    Value obj8(Value::UserType);

    BOOST_VERIFY(obj1.type() == Value::Bool);
    BOOST_VERIFY(obj2.type() == Value::Int);
    BOOST_VERIFY(obj3.type() == Value::Integer);
    BOOST_VERIFY(obj4.type() == Value::Double);
    BOOST_VERIFY(obj5.type() == Value::String);
    BOOST_VERIFY(obj6.type() == Value::Array);
    BOOST_VERIFY(obj7.type() == Value::Object);
    BOOST_VERIFY(obj8.type() == Value::UserType);
}

BOOST_AUTO_TEST_SUITE_END()