text:
#include <waypoint_control/waypoint_controller.h>
#include <boost/algorithm/clamp.hpp>
#include <math.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/transform_datatypes.h>
#include <tf2_ros/transform_listener.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Transform.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Transform.h>

#include <boost/math/constants/constants.hpp>

using namespace waypoint_control;

using boost::algorithm::clamp;
using boost::math::double_constants::pi;
using std::signbit;
using std::abs;
using utilities::simpleLowPassFilter;

WaypointController::WaypointController(Config config, iVescAccess *front_left, iVescAccess *front_right,
    iVescAccess *back_right, iVescAccess *back_left) :
    fl(front_left), fr(front_right), br(back_right), bl(back_left), config(config),
    state(ControlState::manual), waypoint_state(WaypointState::ready), last_left(0.0), last_right(0.0),
    battery_voltage(0.0)
{
  dt = 1.0/config.rate();
  ROS_INFO("[WaypointController::WaypointController]: Waiting for VESCs to come online");
  while (ros::ok())
  {
    if (fl->isAlive() && fr->isAlive() && br->isAlive() && bl->isAlive())
    {
      break;
    }
    ROS_WARN("[WaypointController::WaypointController]: VESCs not online");
    ros::Duration(1.0).sleep();
  }
  ROS_INFO("[WaypointController::WaypointController]: VESCs online");
}

void WaypointController::setControlState(ControlState state)
{
  switch (state)
  {
    case ControlState::ready:
    case ControlState::manual:
    {
      waypoint_state = WaypointState::ready;
      waypoints.clear();
      if (this->state != state)
      {
        stop();
        this->state = state;
        ROS_INFO("[WaypointController::setControlState]: %s to %s",
                 to_string(this->state).c_str(), to_string(state).c_str());
      }
      else
      {
        ROS_WARN("[WaypointController::setControlState]: %s to %s",
                 to_string(this->state).c_str(), to_string(state).c_str());
      }

      break;
    }
    case ControlState::cancel:
    {
      stop();
      this->state = ControlState::ready;
      ROS_INFO("[WaypointController::setControlState]: %s to %s",
               to_string(this->state).c_str(), to_string(state).c_str());
      break;
    }
    case ControlState::error:
    case ControlState::new_goal:
    case ControlState::in_progress:
    {
      stop();
      this->state = ControlState::error;
      ROS_WARN("[WaypointController::setControlState]: Invalid transition %s to %s",
               to_string(this->state).c_str(), to_string(state).c_str());
      break;
    }
  }
}

void WaypointController::setControlState(ControlState state, const Waypoints &waypoints)
{
  stop();
  if (state == ControlState::new_goal)
  {
    if (!this->waypoints.empty())
    {
      ROS_WARN("[WaypointController::setControlState]: Flushing current waypoints");
      this->waypoints.clear();
    }
    ROS_INFO("[WaypointController::setControlState]: %s to %s, %lu waypoints",
             to_string(this->state).c_str(),
             to_string(ControlState::in_progress).c_str(),
             waypoints.size());
    this->waypoints = waypoints;
    this->state = ControlState::in_progress;
    this->waypoint_state = WaypointState::ready;
  }
  else
  {
    this->state = ControlState::error;
    ROS_WARN("[WaypointController::setControlState]: Invalid transition %s to %s",
             to_string(this->state).c_str(),
             to_string(state).c_str());
  }
}

void WaypointController::update(const Pose2D &pose, bool manual_safety, bool autonomy_safety, double left, double right)
{
  debug_info.waypoint = Waypoint();
  switch (state)
  {
    case ControlState::in_progress:
    {
      if (autonomy_safety)
      {
        if (waypoints.empty())
        {
          ROS_INFO("[WaypointController::update]: Processed all waypoints");
          state = ControlState::finished;
        }
        else
        {
          debug_info.waypoint = waypoints.back();
          updateControls(pose);
        }
      }
      else
      {
        stop();
      }
      break;
    }
    case ControlState::manual:
    {
      if (manual_safety)
      {
        setPoint(left, right);
      }
      else
      {
        stop();
      }
      break;
    }
    case ControlState::ready:
    case ControlState::finished:
    case ControlState::cancel:
    case ControlState::new_goal:
    case ControlState::error:
    {
      stop();
      break;
    }
  }
  this->last_pose = pose;

  // Update debug info
  debug_info.command_state = to_string(state);
  debug_info.waypoint_state = to_string(waypoint_state);
  debug_info.feedback.dx = feedback.x();
  debug_info.feedback.dy = feedback.y();
  debug_info.feedback.dr = feedback.r();
  debug_info.feedback.tr = feedback.theta();
  debug_info.feedback.td = feedback.theta()/pi*180.0;
  debug_info.battery_voltage = battery_voltage;
  debug_info.pose = pose;
}

void WaypointController::updateControls(const geometry_msgs::Pose2D& pose)
{
  if (waypoints.empty())
  {
    ROS_WARN("[WaypointController::updateControls::empty]: No waypoints");
    return;
  }

  Waypoint waypoint = waypoints.back();
  last_feedback = feedback;
  feedback = Feedback(pose, waypoint);

  switch (waypoint_state)
  {
    case WaypointState::error:
    {
      ROS_ERROR("[WaypointController::updateControls::error]: Error state");
      break;
    }
    case WaypointState::ready:
    {
      ROS_INFO("[WaypointController::updateControls::ready]: [P%i]: %s to %s",
               waypoint.header.seq,
               to_string(waypoint_state).c_str(),
               to_string(WaypointState::driving).c_str());
      waypoint_state = WaypointState::initial_angle_correction;
      break;
    }
    case WaypointState::initial_angle_correction:
    {
      //ROS_INFO("[WaypointController::updateControls::initial_angle_correction]: angle = %f",
      //         feedback.theta()/pi*180.0);
      if (waypoint.drive_profile == 3)
      {
        setPoint(0.0, 0.0, waypoint.reverse);
        waypoint_state = WaypointState::driving;
      }
      if (signbit(feedback.theta()) != signbit(last_feedback.theta()))
      {
        ROS_INFO("[WaypointController::updateControls::initial_angle_correction]: %s to %s, angle = %f",
                 to_string(waypoint_state).c_str(),
                 to_string(WaypointState::driving).c_str(),
                 feedback.theta()/pi*180.0);
        setPoint(0.0, 0.0, waypoint.reverse);
        waypoint_state = WaypointState::driving;
      }
      else
      {
        int i = waypoint.drive_profile;
        double duty = clamp(config.driveProfile(i).inPlaceK() * abs(feedback.theta()),
            config.driveProfile(i).minInPlaceDuty(), config.driveProfile(i).maxInPlaceDuty());
        //ROS_INFO("Duty %f", duty);
        if (feedback.theta() > 0.0)
        {
          setPoint(-duty, duty, waypoint.reverse);
        }
        else
        {
          setPoint(duty, -duty, waypoint.reverse);
        }
      }
      break;
    }
    case WaypointState::driving:
    {
      if (signbit(feedback.x()) != signbit(last_feedback.x()))
      {
        ROS_INFO("[WaypointController::updateControls::driving]: %s to %s, dx = %3f, dy = %3f",
                 to_string(waypoint_state).c_str(), to_string(WaypointState::driving).c_str(),
                 feedback.x(), feedback.y());
        setPoint(0.0, 0.0, waypoint.reverse);
        waypoint_state = WaypointState::final_angle_correction;
      }
      else
      {
        int i = waypoint.drive_profile;
        double kx  = config.driveProfile(i).drivingKx();
        double ky  = config.driveProfile(i).drivingKy();
        double min = config.driveProfile(i).minDrivingDuty();
        double max = config.driveProfile(i).maxDrivingDuty();
        double p = clamp(kx * abs(feedback.x()), 0.0, 1.0);
        double high = clamp(p*max, min, max);
        double low = clamp(p*max*(1 - ky * abs(feedback.y())), min, max);
        if (feedback.y() > 0.0)
        {
          setPoint(low, high, waypoint.reverse);
        }
        else
        {
          setPoint(high, low, waypoint.reverse);
        }
      }
      break;
    }
    case WaypointState::angle_correction:
    {
      ROS_WARN("[WaypointController::updateControls::angle_correction]: Not implemented yet");
      break;
    }
    case WaypointState::final_angle_correction:
    {
      //ROS_WARN("[WaypointController::updateControls::final_angle_correction]: Not implemented yet");
      waypoint_state = WaypointState::finished;
      break;
    }
    case WaypointState::finished:
    {
      ROS_INFO("[WaypointController::updateControls::finish]: Finished waypoint %i (%f, %f)",
          waypoint.header.seq, waypoint.pose.x, waypoint.pose.y);
      waypoints.pop_back();
      waypoint_state = WaypointState::ready;
      break;
    }
  }
}

void WaypointController::updateBatteryVoltage()
{
  double sample = (fl->getVin() + fr->getVin() + br->getVin() + bl->getVin()) / 4.0;
  simpleLowPassFilter<double>(battery_voltage, sample, config.batteryFilterK());
  if (battery_voltage < config.minVoltage())
  {
    ROS_WARN("[WaypointController::setPoint]: Vin = %f < %f", battery_voltage, config.minVoltage());
    battery_voltage = config.minVoltage();
  }
}

void WaypointController::setPoint(double left, double right, bool reverse)
{
  // Clamp duty
  double tmp_left, tmp_right;
  tmp_left  = clamp((reverse ? -right : left),  -config.maxDuty(), config.maxDuty());
  tmp_right = clamp((reverse ?  -left : right), -config.maxDuty(), config.maxDuty());
  left  = clampAcceleration(tmp_left,  last_left, config.maxAcceleration(), dt);
  right = clampAcceleration(tmp_right, last_right, config.maxAcceleration(), dt);
  last_left = left;
  last_right = right;

  // Compensate for change in battery voltage
  updateBatteryVoltage();
  if (config.voltageCompensation())
  {
      left = clamp(left * config.fullVoltage() / battery_voltage,
          -1.0*config.maxCompensatedDuty(), config.maxCompensatedDuty());
      right = clamp(right * config.fullVoltage() / battery_voltage,
          -1.0*config.maxCompensatedDuty(), config.maxCompensatedDuty());
  }

  // Set duty
  fl->setDuty((float)left);
  bl->setDuty((float)left);
  fr->setDuty((float)right);
  br->setDuty((float)right);

  debug_info.command.left = left;
  debug_info.command.right = right;
}

void WaypointController::stop()
{
  setPoint(0.0, 0.0);
}

ControlState WaypointController::getControlState() const
{
  return state;
}

size_t WaypointController::remainingWaypoints()
{
  return waypoints.size();
}

Debug WaypointController::getDebugInfo() const
{
  return debug_info;
}

double waypoint_control::clampAcceleration(double value, double last_value, double limit, double dt)
{
  double acceleration = (value - last_value) / dt;
  if (abs(acceleration) > limit)
  {
    return (acceleration >= 0.0 ? 1.0 : -1.0) * limit * dt + last_value;
  }
  return value;
}


text:
#include <chainbase/undo_index.hpp>

#include <boost/multi_index/member.hpp>
#include <boost/multi_index/ordered_index.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/monomorphic.hpp>
#include <boost/test/data/test_case.hpp>

namespace {
int exception_counter = 0;
int throw_at = -1;
struct test_exception_base {};
template<typename E>
struct test_exception : E, test_exception_base {
  template<typename... A>
  test_exception(A&&... a) : E{static_cast<E&&>(a)...} {}
};
template<typename E, typename... A>
void throw_point(A&&... a) {
   if(throw_at != -1 && exception_counter++ >= throw_at) {
     throw test_exception<E>{static_cast<A&&>(a)...};
   }
}
template<typename F>
void test_exceptions(F&& f) {
   for(throw_at = 0; ; ++throw_at) {
      exception_counter = 0;
      try {
         f();
         break;
      } catch(test_exception_base&) {}
   }
   throw_at = -1;
   exception_counter = 0;
}

struct throwing_copy {
   throwing_copy() { throw_point<std::bad_alloc>(); }
   throwing_copy(const throwing_copy&) { throw_point<std::bad_alloc>(); }
   throwing_copy(throwing_copy&&) noexcept = default;
   throwing_copy& operator=(const throwing_copy&) { throw_point<std::bad_alloc>(); return *this; }
   throwing_copy& operator=(throwing_copy&&) noexcept = default;
};

template<typename T>
struct test_allocator : std::allocator<T> {
   test_allocator() = default;
   template<typename U>
   test_allocator(const test_allocator<U>&) {}
   template<typename U>
   struct rebind { using other = test_allocator<U>; };
   T* allocate(std::size_t count) {
      throw_point<std::bad_alloc>();
      return std::allocator<T>::allocate(count);
   }
};

template<typename F>
struct scope_fail {
   scope_fail(F&& f) : _f{static_cast<F&&>(f)}, _exception_count{std::uncaught_exceptions()} {}
   ~scope_fail() {
      if(_exception_count != std::uncaught_exceptions()) _f();
   }
   F _f;
   int _exception_count;
};

struct basic_element_t {
   template<typename C, typename A>
   basic_element_t(C&& c, const std::allocator<A>&) { c(*this); }
   uint64_t id;
   throwing_copy dummy;
};

// TODO: Replace with boost::multi_index::key once we bump our minimum Boost version to at least 1.69
template<typename T>
struct key_impl;
template<typename C, typename T>
struct key_impl<T C::*> { template<auto F> using fn = boost::multi_index::member<C, T, F>; };

template<auto Fn>
using key = typename key_impl<decltype(Fn)>::template fn<Fn>;

}

BOOST_AUTO_TEST_SUITE(undo_index_tests)

#define EXCEPTION_TEST_CASE(name)                               \
   void name##impl();                                         \
   BOOST_AUTO_TEST_CASE(name) { test_exceptions(&name##impl); } \
   void name##impl ()

EXCEPTION_TEST_CASE(test_simple) {
   chainbase::undo_index<basic_element_t, test_allocator<basic_element_t>, boost::multi_index::ordered_unique<key<&basic_element_t::id>>> i0;
   i0.emplace([](basic_element_t& elem) {});
   const basic_element_t* element = i0.find(0);
   BOOST_TEST((element != nullptr && element->id == 0));
   const basic_element_t* e1 = i0.find(1);
   BOOST_TEST(e1 == nullptr);
   i0.emplace([](basic_element_t& elem) {});
   const basic_element_t* e2 = i0.find(1);
   BOOST_TEST((e2 != nullptr && e2->id == 1));

   i0.modify(*element, [](basic_element_t& elem) {});
   i0.remove(*element);
   element = i0.find(0);
   BOOST_TEST(element == nullptr);
}

struct test_element_t {
   template<typename C, typename A>
   test_element_t(C&& c, const std::allocator<A>&) { c(*this); }
   uint64_t id;
   int secondary;
   throwing_copy dummy;
};

// If an exception is thrown while an undo session is active, undo will restore the state.
template<typename C>
auto capture_state(const C& index) {
   std::vector<std::pair<test_element_t, const test_element_t*>> vec;
   for(const auto& elem : index) {
     vec.emplace_back(elem, &elem);
   }
   return scope_fail{[vec = std::move(vec), &index]{
      BOOST_TEST(index.size() == vec.size());
      for(const auto& [elem, ptr] : vec) {
         auto * actual0 = index.find(elem.id);
         BOOST_TEST(actual0 == ptr); // reference stability is guaranteed
         if (actual0 != nullptr) {
            BOOST_TEST(actual0->id == elem.id);
            BOOST_TEST(actual0->secondary == elem.secondary);
         }
         auto actual1iter = index.template get<1>().find(elem.secondary);
         BOOST_TEST((actual1iter != index.template get<1>().end() && &*actual1iter == actual0));
      }
   }};
}

EXCEPTION_TEST_CASE(test_insert_undo) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary> > > i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session = i0.start_undo_session(true);
   i0.emplace([](test_element_t& elem) { elem.secondary = 12; });
   BOOST_TEST(i0.find(1)->secondary == 12);
   }
   BOOST_TEST(i0.find(0)->secondary == 42);
   BOOST_TEST(i0.find(1) == nullptr);
}

EXCEPTION_TEST_CASE(test_insert_squash) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary> > > i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session0 = i0.start_undo_session(true);
   auto session1 = i0.start_undo_session(true);
   i0.emplace([](test_element_t& elem) { elem.secondary = 12; });
   BOOST_TEST(i0.find(1)->secondary == 12);
   session1.squash();
   BOOST_TEST(i0.find(1)->secondary == 12);
   }
   BOOST_TEST(i0.find(0)->secondary == 42);
   BOOST_TEST(i0.find(1) == nullptr);
}

EXCEPTION_TEST_CASE(test_insert_push) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary> > > i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session = i0.start_undo_session(true);
   i0.emplace([](test_element_t& elem) { elem.secondary = 12; });
   BOOST_TEST(i0.find(1)->secondary == 12);
   session.push();
   i0.commit(i0.revision());
   }
   BOOST_TEST(!i0.has_undo_session());
   BOOST_TEST(i0.find(0)->secondary == 42);
   BOOST_TEST(i0.find(1)->secondary == 12);
}

EXCEPTION_TEST_CASE(test_modify_undo) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session = i0.start_undo_session(true);
   i0.modify(*i0.find(0), [](test_element_t& elem) { elem.secondary = 18; });
   BOOST_TEST(i0.find(0)->secondary == 18);
   }
   BOOST_TEST(i0.find(0)->secondary == 42);
}

EXCEPTION_TEST_CASE(test_modify_squash) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session0 = i0.start_undo_session(true);
   auto session1 = i0.start_undo_session(true);
   i0.modify(*i0.find(0), [](test_element_t& elem) { elem.secondary = 18; });
   BOOST_TEST(i0.find(0)->secondary == 18);
   session1.squash();
   BOOST_TEST(i0.find(0)->secondary == 18);
   }
   BOOST_TEST(i0.find(0)->secondary == 42);
}

EXCEPTION_TEST_CASE(test_modify_push) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session = i0.start_undo_session(true);
   i0.modify(*i0.find(0), [](test_element_t& elem) { elem.secondary = 18; });
   BOOST_TEST(i0.find(0)->secondary == 18);
   session.push();
   i0.commit(i0.revision());
   }
   BOOST_TEST(!i0.has_undo_session());
   BOOST_TEST(i0.find(0)->secondary == 18);
}

EXCEPTION_TEST_CASE(test_remove_undo) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session = i0.start_undo_session(true);
   i0.remove(*i0.find(0));
   BOOST_TEST(i0.find(0) == nullptr);
   }
   BOOST_TEST(i0.find(0)->secondary == 42);
}

EXCEPTION_TEST_CASE(test_remove_squash) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session0 = i0.start_undo_session(true);
   auto session1 = i0.start_undo_session(true);
   i0.remove(*i0.find(0));
   BOOST_TEST(i0.find(0) == nullptr);
   session1.squash();
   BOOST_TEST(i0.find(0) == nullptr);
   }
   BOOST_TEST(i0.find(0)->secondary == 42);
}

EXCEPTION_TEST_CASE(test_remove_push) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session = i0.start_undo_session(true);
   i0.remove(*i0.find(0));
   BOOST_TEST(i0.find(0) == nullptr);
   session.push();
   i0.commit(i0.revision());
   }
   BOOST_TEST(!i0.has_undo_session());
   BOOST_TEST(i0.find(0) == nullptr);
}

EXCEPTION_TEST_CASE(test_insert_modify) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   i0.emplace([](test_element_t& elem) { elem.secondary = 12; });
   BOOST_TEST(i0.find(1)->secondary == 12);
   i0.modify(*i0.find(1), [](test_element_t& elem) { elem.secondary = 24; });
   BOOST_TEST(i0.find(1)->secondary == 24);
}

EXCEPTION_TEST_CASE(test_insert_modify_undo) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session = i0.start_undo_session(true);
   i0.emplace([](test_element_t& elem) { elem.secondary = 12; });
   BOOST_TEST(i0.find(1)->secondary == 12);
   i0.modify(*i0.find(1), [](test_element_t& elem) { elem.secondary = 24; });
   BOOST_TEST(i0.find(1)->secondary == 24);
   }
   BOOST_TEST(i0.find(0)->secondary == 42);
   BOOST_TEST(i0.find(1) == nullptr);
}

EXCEPTION_TEST_CASE(test_insert_modify_squash) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session1 = i0.start_undo_session(true);
   i0.emplace([](test_element_t& elem) { elem.secondary = 12; });
   BOOST_TEST(i0.find(1)->secondary == 12);
   auto session2 = i0.start_undo_session(true);
   i0.modify(*i0.find(1), [](test_element_t& elem) { elem.secondary = 24; });
   BOOST_TEST(i0.find(1)->secondary == 24);
   session2.squash();
   }
   BOOST_TEST(i0.find(0)->secondary == 42);
   BOOST_TEST(i0.find(1) == nullptr);
}

EXCEPTION_TEST_CASE(test_insert_remove_undo) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session = i0.start_undo_session(true);
   i0.emplace([](test_element_t& elem) { elem.secondary = 12; });
   BOOST_TEST(i0.find(1)->secondary == 12);
   i0.remove(*i0.find(1));
   BOOST_TEST(i0.find(1) == nullptr);
   }
   BOOST_TEST(i0.find(0)->secondary == 42);
   BOOST_TEST(i0.find(1) == nullptr);
}

EXCEPTION_TEST_CASE(test_insert_remove_squash) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session1 = i0.start_undo_session(true);
   i0.emplace([](test_element_t& elem) { elem.secondary = 12; });
   BOOST_TEST(i0.find(1)->secondary == 12);
   auto session2 = i0.start_undo_session(true);
   i0.remove(*i0.find(1));
   BOOST_TEST(i0.find(1) == nullptr);
   session2.squash();
   }
   BOOST_TEST(i0.find(0)->secondary == 42);
   BOOST_TEST(i0.find(1) == nullptr);
}

EXCEPTION_TEST_CASE(test_modify_modify_undo) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session = i0.start_undo_session(true);
   i0.modify(*i0.find(0), [](test_element_t& elem) { elem.secondary = 18; });
   BOOST_TEST(i0.find(0)->secondary == 18);
   i0.modify(*i0.find(0), [](test_element_t& elem) { elem.secondary = 24; });
   BOOST_TEST(i0.find(0)->secondary == 24);
   }
   BOOST_TEST(i0.find(0)->secondary == 42);
}

EXCEPTION_TEST_CASE(test_modify_modify_squash) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session1 = i0.start_undo_session(true);
   i0.modify(*i0.find(0), [](test_element_t& elem) { elem.secondary = 18; });
   BOOST_TEST(i0.find(0)->secondary == 18);
   auto session2 = i0.start_undo_session(true);
   i0.modify(*i0.find(0), [](test_element_t& elem) { elem.secondary = 24; });
   BOOST_TEST(i0.find(0)->secondary == 24);
   session2.squash();
   }
   BOOST_TEST(i0.find(0)->secondary == 42);
}

EXCEPTION_TEST_CASE(test_modify_remove_undo) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session = i0.start_undo_session(true);
   i0.modify(*i0.find(0), [](test_element_t& elem) { elem.secondary = 18; });
   BOOST_TEST(i0.find(0)->secondary == 18);
   i0.remove(*i0.find(0));
   BOOST_TEST(i0.find(0) == nullptr);
   }
   BOOST_TEST(i0.find(0)->secondary == 42);
}

EXCEPTION_TEST_CASE(test_modify_remove_squash) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   auto undo_checker = capture_state(i0);
   auto session1 = i0.start_undo_session(true);
   i0.modify(*i0.find(0), [](test_element_t& elem) { elem.secondary = 18; });
   BOOST_TEST(i0.find(0)->secondary == 18);
   auto session2 = i0.start_undo_session(true);
   i0.remove(*i0.find(0));
   BOOST_TEST(i0.find(0) == nullptr);
   session2.squash();
   }
   BOOST_TEST(i0.find(0)->secondary == 42);
}

EXCEPTION_TEST_CASE(test_squash_one) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   {
   i0.modify(*i0.find(0), [](test_element_t& elem) { elem.secondary = 18; });
   BOOST_TEST(i0.find(0)->secondary == 18);
   auto session2 = i0.start_undo_session(true);
   i0.remove(*i0.find(0));
   BOOST_TEST(i0.find(0) == nullptr);
   session2.squash();
   }
}

EXCEPTION_TEST_CASE(test_insert_non_unique) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.find(0)->secondary == 42);
   BOOST_CHECK_THROW(i0.emplace([](test_element_t& elem) { elem.secondary = 42; }),  std::exception);
   BOOST_TEST(i0.find(0)->secondary == 42);
}

struct conflict_element_t {
   template<typename C, typename A>
   conflict_element_t(C&& c, const test_allocator<A>&) { c(*this); }
   uint64_t id;
   int x0;
   int x1;
   int x2;
   throwing_copy dummy;
};

EXCEPTION_TEST_CASE(test_modify_conflict) {
   chainbase::undo_index<conflict_element_t, test_allocator<conflict_element_t>,
                         boost::multi_index::ordered_unique<key<&conflict_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&conflict_element_t::x0>>,
                         boost::multi_index::ordered_unique<key<&conflict_element_t::x1>>,
                         boost::multi_index::ordered_unique<key<&conflict_element_t::x2>>> i0;
   // insert 3 elements
   i0.emplace([](conflict_element_t& elem) { elem.x0 = 0; elem.x1 = 10; elem.x2 = 10; });
   i0.emplace([](conflict_element_t& elem) { elem.x0 = 11; elem.x1 = 1; elem.x2 = 11; });
   i0.emplace([](conflict_element_t& elem) { elem.x0 = 12; elem.x1 = 12; elem.x2 = 2; });
   {
   auto session = i0.start_undo_session(true);
   // set them to a different value
   i0.modify(*i0.find(0), [](conflict_element_t& elem) { elem.x0 = 10; elem.x1 = 10; elem.x2 = 10; });
   i0.modify(*i0.find(1), [](conflict_element_t& elem) { elem.x0 = 11; elem.x1 = 11; elem.x2 = 11; });
   i0.modify(*i0.find(2), [](conflict_element_t& elem) { elem.x0 = 12; elem.x1 = 12; elem.x2 = 12; });
   // create a circular conflict with the original values
   i0.modify(*i0.find(0), [](conflict_element_t& elem) { elem.x0 = 10; elem.x1 = 1; elem.x2 = 10; });
   i0.modify(*i0.find(1), [](conflict_element_t& elem) { elem.x0 = 11; elem.x1 = 11; elem.x2 = 2; });
   i0.modify(*i0.find(2), [](conflict_element_t& elem) { elem.x0 = 0; elem.x1 = 12; elem.x2 = 12; });
   }
   BOOST_TEST(i0.find(0)->x0 == 0);
   BOOST_TEST(i0.find(1)->x1 == 1);
   BOOST_TEST(i0.find(2)->x2 == 2);
   // Check lookup in the other indices
   BOOST_TEST(i0.get<1>().find(0)->x0 == 0);
   BOOST_TEST(i0.get<1>().find(11)->x0 == 11);
   BOOST_TEST(i0.get<1>().find(12)->x0 == 12);
   BOOST_TEST(i0.get<2>().find(10)->x1 == 10);
   BOOST_TEST(i0.get<2>().find(1)->x1 == 1);
   BOOST_TEST(i0.get<2>().find(12)->x1 == 12);
   BOOST_TEST(i0.get<3>().find(10)->x2 == 10);
   BOOST_TEST(i0.get<3>().find(11)->x2 == 11);
   BOOST_TEST(i0.get<3>().find(2)->x2 == 2);
}

BOOST_DATA_TEST_CASE(test_insert_fail, boost::unit_test::data::make({true, false}), use_undo) {
   chainbase::undo_index<conflict_element_t, test_allocator<conflict_element_t>,
                         boost::multi_index::ordered_unique<key<&conflict_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&conflict_element_t::x0>>,
                         boost::multi_index::ordered_unique<key<&conflict_element_t::x1>>,
                         boost::multi_index::ordered_unique<key<&conflict_element_t::x2>>> i0;
   // insert 3 elements
   i0.emplace([](conflict_element_t& elem) { elem.x0 = 10; elem.x1 = 10; elem.x2 = 10; });
   i0.emplace([](conflict_element_t& elem) { elem.x0 = 11; elem.x1 = 11; elem.x2 = 11; });
   i0.emplace([](conflict_element_t& elem) { elem.x0 = 12; elem.x1 = 12; elem.x2 = 12; });
   {
   auto session = i0.start_undo_session(true);
   // Insert a value with a duplicate
   BOOST_CHECK_THROW(i0.emplace([](conflict_element_t& elem) { elem.x0 = 81; elem.x1 = 11; elem.x2 = 91; }), std::logic_error);
   }
   BOOST_TEST(i0.find(0)->x0 == 10);
   BOOST_TEST(i0.find(1)->x1 == 11);
   BOOST_TEST(i0.find(2)->x2 == 12);
   // Check lookup in the other indices
   BOOST_TEST(i0.get<1>().find(10)->x0 == 10);
   BOOST_TEST(i0.get<1>().find(11)->x0 == 11);
   BOOST_TEST(i0.get<1>().find(12)->x0 == 12);
   BOOST_TEST(i0.get<2>().find(10)->x1 == 10);
   BOOST_TEST(i0.get<2>().find(11)->x1 == 11);
   BOOST_TEST(i0.get<2>().find(12)->x1 == 12);
   BOOST_TEST(i0.get<3>().find(10)->x2 == 10);
   BOOST_TEST(i0.get<3>().find(11)->x2 == 11);
   BOOST_TEST(i0.get<3>().find(12)->x2 == 12);
}

EXCEPTION_TEST_CASE(test_modify_fail) {
   chainbase::undo_index<conflict_element_t, test_allocator<conflict_element_t>,
                         boost::multi_index::ordered_unique<key<&conflict_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&conflict_element_t::x0>>,
                         boost::multi_index::ordered_unique<key<&conflict_element_t::x1>>,
                         boost::multi_index::ordered_unique<key<&conflict_element_t::x2>>> i0;
   // insert 3 elements
   i0.emplace([](conflict_element_t& elem) { elem.x0 = 10; elem.x1 = 10; elem.x2 = 10; });
   i0.emplace([](conflict_element_t& elem) { elem.x0 = 11; elem.x1 = 11; elem.x2 = 11; });
   i0.emplace([](conflict_element_t& elem) { elem.x0 = 12; elem.x1 = 12; elem.x2 = 12; });
   {
   auto session = i0.start_undo_session(true);
   // Insert a value with a duplicate
   i0.emplace([](conflict_element_t& elem) { elem.x0 = 71; elem.x1 = 81; elem.x2 = 91; });
   BOOST_CHECK_THROW(i0.modify(i0.get(3), [](conflict_element_t& elem) { elem.x0 = 71; elem.x1 = 10; elem.x2 = 91; }), std::logic_error);
   }
   BOOST_TEST(i0.get<0>().size() == 3);
   BOOST_TEST(i0.get<1>().size() == 3);
   BOOST_TEST(i0.get<2>().size() == 3);
   BOOST_TEST(i0.get<3>().size() == 3);
   BOOST_TEST(i0.find(0)->x0 == 10);
   BOOST_TEST(i0.find(1)->x1 == 11);
   BOOST_TEST(i0.find(2)->x2 == 12);
   // Check lookup in the other indices
   BOOST_TEST(i0.get<1>().find(10)->x0 == 10);
   BOOST_TEST(i0.get<1>().find(11)->x0 == 11);
   BOOST_TEST(i0.get<1>().find(12)->x0 == 12);
   BOOST_TEST(i0.get<2>().find(10)->x1 == 10);
   BOOST_TEST(i0.get<2>().find(11)->x1 == 11);
   BOOST_TEST(i0.get<2>().find(12)->x1 == 12);
   BOOST_TEST(i0.get<3>().find(10)->x2 == 10);
   BOOST_TEST(i0.get<3>().find(11)->x2 == 11);
   BOOST_TEST(i0.get<3>().find(12)->x2 == 12);
}

struct by_secondary {};

BOOST_AUTO_TEST_CASE(test_project) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<boost::multi_index::tag<by_secondary>, key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 42; });
   BOOST_TEST(i0.project<by_secondary>(i0.begin()) == i0.get<by_secondary>().begin());
   BOOST_TEST(i0.project<by_secondary>(i0.end()) == i0.get<by_secondary>().end());
   BOOST_TEST(i0.project<1>(i0.begin()) == i0.get<by_secondary>().begin());
   BOOST_TEST(i0.project<1>(i0.end()) == i0.get<by_secondary>().end());
}

EXCEPTION_TEST_CASE(test_remove_tracking_session) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 20; });
   auto session = i0.start_undo_session(true);
   auto tracker = i0.track_removed();
   i0.emplace([](test_element_t& elem) { elem.secondary = 21; });
   const test_element_t& elem0 = *i0.find(0);
   const test_element_t& elem1 = *i0.find(1);
   BOOST_CHECK(!tracker.is_removed(elem0));
   BOOST_CHECK(!tracker.is_removed(elem1));
   tracker.remove(elem0);
   tracker.remove(elem1);
   BOOST_CHECK(tracker.is_removed(elem0));
   BOOST_CHECK(tracker.is_removed(elem1));
}

EXCEPTION_TEST_CASE(test_remove_tracking_no_session) {
   chainbase::undo_index<test_element_t, test_allocator<test_element_t>,
                         boost::multi_index::ordered_unique<key<&test_element_t::id>>,
                         boost::multi_index::ordered_unique<key<&test_element_t::secondary>>> i0;
   i0.emplace([](test_element_t& elem) { elem.secondary = 20; });
   auto tracker = i0.track_removed();
   i0.emplace([](test_element_t& elem) { elem.secondary = 21; });
   const test_element_t& elem0 = *i0.find(0);
   const test_element_t& elem1 = *i0.find(1);
   BOOST_CHECK(!tracker.is_removed(elem0));
   BOOST_CHECK(!tracker.is_removed(elem1));
   tracker.remove(elem0);
   tracker.remove(elem1);
   BOOST_CHECK(tracker.is_removed(elem0));
   BOOST_CHECK(tracker.is_removed(elem1));
}

BOOST_AUTO_TEST_SUITE_END()

ext: cpp


text:
#include <iostream>
#include <sstream>
#include <string>

//~ #include <boost/filesystem.hpp>

#include <boost/algorithm/string.hpp>

// #include "rose.h"
#include "sageInterface.h"
#include "sageGeneric.h"

#include "dotify.h"

namespace sb = SageBuilder;
namespace si = SageInterface;

namespace RoseUtils
{
  SgName nameOf(const SgVarRefExp& var_ref);
}

namespace dotify
{
  /// converts object of type E to T via string conversion
  template <class T, class E>
  static inline
  T conv(const E& el)
  {
    std::stringstream s;
    T                 res;

    s << el;
    s >> res;
    return res;
  }

  void edge(std::ostream& os, const void* src, const void* tgt, std::string lbl, std::string attr)
  {
    os << "  p" << size_t(src) << " -> p" << size_t(tgt)
       << "[ taillabel = \"" << lbl << "\" " << attr << "];" << std::endl;
  }

  void node(std::ostream& os, const void* n, std::string lbl, std::string attr)
  {
    os << "  p" << size_t(n) << "[ label = \"" << lbl << "\" " << attr << "];" << std::endl;
  }

  std::string short_name(SgNode& n)
  {
    std::string s = typeid(n).name();
    size_t      pre = s.find_first_of("Sg");

    ROSE_ASSERT(pre != std::string::npos);
    return s.substr(pre+2);
  }

  std::string short_name(SgNode* n)
  {
    if (n == NULL)
      return "<null>";

    return short_name(*n);
  }

  template <class T>
  static
  bool freshlyDiscovered(std::set<const T*>& aset, const T* elem)
  {
    if (aset.find(elem) != aset.end())
      return false;

    aset.insert(elem);
    return true;
  }

  template <class T>
  static inline
  bool freshlyDiscovered(std::set<const T*>& aset, const T& elem)
  {
    return freshlyDiscovered(aset, &elem);
  }

  static inline
  std::string nameIfNeeded(std::string s)
  {
    if (s.empty()) return "<noname>";

    return s;
  }

  struct DotPrinter
  {
    DotPrinter(std::ostream& s, attr_set options)
    : os(&s), opts(options), has_project(false), known_types(), known_init_names(), known_symbols()
    {}

    bool with_opt(int o) const { return (opts & o) == o; }

    void recurse(SgNode& n)
    {
      *this = sg::traverseChildren(*this, n);
    }

    void descend(SgNode* n)
    {
      //~ *this = sg::forAllNodes(*this, n, preorder);
      *this = sg::dispatch(*this, n);

      // recurse(*n);
    }

    void write_type(SgExpression& n, attr_set opt = ::dotify::types)
    {
      if (!with_opt(opt))
        return;

      prn_node_type(n.get_type());
      edge(*os, &n, n.get_type(), "", type_color);
    }

    void prn_node_type(SgType* n)
    {
      if (!freshlyDiscovered(known_types, n))
        return;

      *this = sg::dispatch(*this, n);
    }

    void prn_decllink(SgNode* src, SgInitializedName* decl, std::string edgelbl, std::string edgeattr)
    {
      edge(*os, src, decl, edgelbl, edgeattr);
    }

    void prn_decllink(SgNode* src, SgVariableSymbol* sy, std::string edgeattr)
    {
      std::string edgelbl = shortcut_lbl;

      if (with_opt(::dotify::symbols))
      {
        // include extra nodes and edges

        if (freshlyDiscovered(known_symbols, static_cast<SgSymbol*>(sy)))
          node(*os, sy, short_name(sy), decl_color);

        edge(*os, src, sy, "", edgeattr);

        edgelbl = "";
        src     = sy;
      }

      prn_decllink(src, sy ? sy->get_declaration() : 0, edgelbl, edgeattr);
    }

    void normal_node(SgLocatedNode& n, std::string lbl, std::string nodeattr)
    {
      //~ if (n.isCompilerGenerated()) return;

      node(*os, &n, lbl, nodeattr);
      edge(*os, n.get_parent(), &n, "", "");
    }

    void normal_node(SgLocatedNode& n, std::string nodeattr)
    {
      normal_node(n, short_name(n), nodeattr);
    }

    void expr_node(SgExpression& n, std::string id, attr_set opt = ::dotify::types)
    {
      node(*os, &n, id, expr_color);
      edge(*os, n.get_parent(), &n, "", "");
      write_type(n, opt);
      recurse(n);
    }

    void expr_node(SgExpression& n, attr_set opt = ::dotify::types)
    {
      expr_node(n, short_name(n), opt);
    }

    template <class SgValNode>
    void value_expression(SgValNode& n)
    {
      std::string val = conv<std::string>(n.get_value());
      //~ std::string val = n.get_valueString();

      node(*os, &n, short_name(n) + ": " + val, expr_color);
      edge(*os, n.get_parent(), &n, "", "");

      write_type(n, ::dotify::moretypes);
    }

    void handle(SgNode& n)
    {
      node(*os, &n, short_name(n), udef_color);
    }

    void handle(SgSourceFile& n)
    {
      node(*os, &n, short_name(n), udef_color);

      if (has_project)
        edge(*os, sg::ancestor<SgProject>(&n), &n, shortcut_lbl, "");

      recurse(n);
    }

    void handle(SgProject& n)
    {
      has_project = true;

      node(*os, &n, short_name(n), udef_color);

      for (int sz = n.numberOfFiles(); sz > 0; --sz)
      {
        descend(&n.get_file(sz-1));
      }
    }

    void handle(SgLocatedNode& n)
    {
      normal_node(n, short_name(n), udef_color);
      recurse(n);
    }

    void handle(SgType& n)
    {
      node(*os, &n, short_name(n), type_color);
    }

    void handle(SgArrayType& n)
    {
      std::string arrname = short_name(n);

      arrname = arrname + "[" + conv<std::string>(n.get_number_of_elements()) + "]"
                        + " @" + conv<std::string>(n.get_rank())
                        ;

      node(*os, &n, arrname, type_color);

      prn_node_type(n.get_base_type());
      descend(n.get_index());
      descend(n.get_dim_info());

      edge(*os, &n, n.get_base_type(), "",  type_color);
      //~ edge(*os, &n, n.get_index(),     "index", type_color);
      //~ edge(*os, &n, n.get_dim_info(),  "dim",   type_color);
    }

    void handle(SgInitializedName& n)
    {
      if (!freshlyDiscovered(known_init_names, n))
        return;

      known_init_names.insert(&n);
      normal_node(n, nameIfNeeded(n.get_name()), decl_color);

      if (!with_opt(::dotify::types))
        return;

      prn_node_type(n.get_type());
      edge(*os, &n, n.get_type(), "", type_color);
      recurse(n);
    }

    void handle(SgStatement& n)
    {
      normal_node(n, stmt_color);
      recurse(n);
    }

    void handle(SgRenamePair& n)
    {
      normal_node(n, udef_color);
      recurse(n);
    }

    void handle(SgDeclarationStatement& n)
    {
      normal_node(n, decl_color);
      recurse(n);
    }

    void handle(SgFunctionDeclaration& n)
    {
      if (  !with_opt(attr_set::builtins)
         && boost::starts_with(std::string(n.get_name()), "__")
         )
        return;

      normal_node(n, decl_color);
      recurse(n);
    }

    void handle(SgFunctionParameterList& n)
    {
      SgInitializedNamePtrList& args = n.get_args();

      // normal_node(n, short_name(n) + ": " + conv<std::string>(args.size()), udef_color);
      normal_node(n, short_name(n), udef_color);

      for (size_t i = 0; i < args.size(); ++i)
      {
        SgInitializedName& child = sg::deref(args.at(i));

        handle(child);

        // fortran special ..
        if (child.get_parent() != &n)
          edge(*os, &n, &child, unusual_lbl, udef_color);
      }
    }

    void handle(SgIntVal& n)         { value_expression(n); }
    void handle(SgFloatVal& n)       { value_expression(n); }
    void handle(SgDoubleVal& n)      { value_expression(n); }
    void handle(SgBoolValExp& n)     { value_expression(n); }
    // void handle(SgComplexVal& n) { value_expression(n); }

    void handle(SgExpression& n)     { expr_node(n); }
    void handle(SgExprListExp& n)    { expr_node(n, ::dotify::moretypes); }
    void handle(SgNullExpression& n) { expr_node(n, ::dotify::moretypes); }

    void handle(SgVarRefExp& n)
    {
      std::string ident("VarRefExp: ");

      ident += RoseUtils::nameOf(n);

      expr_node(n, ident);

      if (!with_opt(::dotify::decllinks))
        return;

      prn_decllink(&n, n.get_symbol(), "style = dotted");
    }

    std::ostream*                      os;
    attr_set                           opts;
    bool                               has_project;
    std::set<const SgType*>            known_types;
    std::set<const SgInitializedName*> known_init_names;
    std::set<const SgSymbol*>          known_symbols;

    static constexpr const char* decl_color = "color=gold";
    static constexpr const char* stmt_color = "color=blue";
    static constexpr const char* type_color = "color=red";
    static constexpr const char* expr_color = "color=indigo";
    static constexpr const char* udef_color = "color=green";

    static constexpr const char* shortcut_lbl = "**";
    static constexpr const char* unusual_lbl  = "??";
  };

  void dot_header(std::ostream& os, std::string s = "dotify")
  {
    os << "digraph " << s << " {" << std::endl;
  }

  void dot_footer(std::ostream& os)
  {
    os << "}" << std::endl;
  }

  void dot_noheader(std::ostream& os, SgNode& n, attr_set options)
  {
    // print type nodes
    //~ if ((attributes & attr::types) == attr::types)
    //~ {
      //~ dot_types(os, prog);

      //~ dot_type_relations(os, prog);
    //~ }

    sg::dispatch(DotPrinter(os, options), &n);
  }

  /// generates a dot file ( https://www.graphviz.org/doc/info/lang.html )
  ///   for the AST rooted in p. The attribute set determine if types and
  ///   decl-backlinks are included in the graph.
  void print_dot(std::ostream& os, SgNode& n, attr_set options)
  {
    dot_header(os);
    dot_noheader(os, n, options);
    dot_footer(os);
  }

  void save_dot(std::string s, SgNode& n, attr_set options)
  {
    std::fstream out(s, std::fstream::out);

    print_dot(out, n, options);
  }
}

ext: cc

max_stars_repo_name: ouankou/rose

max_issues_repo_name: ouankou/rose

max_forks_repo_name: ouankou/rose


text:
#include "solver.h"

#include <boost/program_options.hpp>

#include <chrono>
#include <fstream>
#include <iostream>
#include <mutex>
#include <random>
#include <thread>

std::mutex global_mutex;
std::vector<Field> all_fields;
std::vector<int32_t> all_scores;
std::atomic_size_t games_count = 0;
size_t scores_sum = 0;
size_t total_games;
size_t ready_games = 0;
std::vector<Figure> figures;
std::chrono::high_resolution_clock::time_point start_time;

int32_t play_game(const Field& start_field, std::shared_ptr<basic_solver> solver, std::vector<Field>& fields, std::vector<int32_t>& scores, std::default_random_engine& rng)
{
    std::uniform_int_distribution<size_t> distr(0, figures.size() - 1);
    auto field = start_field;
    fields.clear();
    scores.clear();
    int32_t total_score = 0;
    while (true)
    {
        const TriFigures figs = {
            std::cref(figures[distr(rng)]),
            std::cref(figures[distr(rng)]),
            std::cref(figures[distr(rng)])
        };
        if (!std::any_of(figs.begin(), figs.end(), [&field](const Figure& fig) { return field.has_placements(fig); }))
            continue;
        fields.push_back(field);
        scores.push_back(total_score);
//        std::cout << fields.size() << " ";
        size_t score;
        auto alternatives = field.get_all_next(figs, score);
        total_score += (int32_t)score;
        if (alternatives.empty())
            break;
        Field::random_shrink(alternatives, 10000, rng);
        auto best = solver->choose_best(alternatives);
        field = alternatives[best];
    }
//    std::cout << std::endl;
    for (auto& score : scores)
        score = total_score - score;
    return total_score;
}

void worker(std::shared_ptr<basic_solver> solver, size_t jnum)
{
    std::vector<Field> fields;
    std::vector<int32_t> scores;
    std::random_device rd;
    std::default_random_engine rng(rd());
    while (++games_count <= total_games)
    {
        size_t total_score = play_game(Field(), solver, fields, scores, rng);
        std::lock_guard lock(global_mutex);
        scores_sum += total_score;
        all_fields.insert(all_fields.end(), fields.begin(), fields.end());
        all_scores.insert(all_scores.end(), scores.begin(), scores.end());
        auto t = std::chrono::high_resolution_clock::now();
        auto dt = std::chrono::duration_cast<std::chrono::milliseconds>(t - start_time).count();
        ++ready_games;
        std::cout << "[" << jnum+1 << "] " << ready_games << ": Score=" << total_score << ". T=" << dt << std::endl;
    }
}

int main(int argc, char* argv[])
{
    namespace po = boost::program_options;
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("figures,f", po::value<std::string>()->default_value("figures.txt"), "path to figures list")
        ("model,m", po::value<std::string>()->default_value("model.torch"), "path to pytorch module")
        ("games,g", po::value<size_t>()->default_value(1000), "number of games to play")
        ("out,o", po::value<std::string>()->default_value("games.data"), "output file")
        ("jobs,j", po::value<size_t>()->default_value(1), "number of working jobs")
        ;
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout << desc << std::endl;
        return 1;
    }

    {
        const auto figures_file = vm["figures"].as<std::string>();
        std::ifstream ifs(figures_file);
        if (!ifs)
        {
            std::cerr << "Cannot read figures file: " << figures_file << std::endl;
            return 2;
        }
        figures = read_figures(ifs);
    }
    const auto model_file = vm["model"].as<std::string>();
    auto factory = create_solver_factory(model_file);
    const auto jobs = vm["jobs"].as<size_t>();
    total_games = vm["games"].as<size_t>();
    std::vector<std::jthread> workers;
    workers.reserve(jobs);
    start_time = std::chrono::high_resolution_clock::now();
    for (size_t i = 0; i < jobs; ++i)
    {
        auto solver = factory->get_cuda_count() > 0
            ? factory->get_cuda_solver(i % factory->get_cuda_count())
            : factory->get_cpu_solver();
        workers.emplace_back(worker, solver, i);
    }

    for (auto& w : workers)
        w.join();

    std::cout << "Average score: " << scores_sum / total_games << std::endl;
    auto t = std::chrono::high_resolution_clock::now();
    auto dt = std::chrono::duration_cast<std::chrono::milliseconds>(t - start_time).count();
    std::cout << "Average time: " << dt / total_games << " ms" << std::endl;

    const auto output_file = vm["out"].as<std::string>();
    FILE* f = fopen(output_file.c_str(), "wb");
    if (!f)
    {
        std::cerr << "Cannot write output file: " << output_file << std::endl;
        return 3;
    }

    std::vector<int32_t> fields_buffer(all_fields.size() * 3);
    Field::copy_to(all_fields, fields_buffer.data());
    fwrite(fields_buffer.data(), sizeof(fields_buffer[0]), fields_buffer.size(), f);
    fwrite(all_scores.data(), sizeof(all_scores[0]), all_scores.size(), f);
    fclose(f);

    return 0;
}

max_stars_repo_name: evsergey/woodoku-nn

max_issues_repo_name: evsergey/woodoku-nn

max_forks_repo_name: evsergey/woodoku-nn


text: #include <boost/hana/fwd/basic_tuple.hpp>

ext: hpp

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file math.hpp
 * @author Vaclav Blazek <vaclav.blazek@citationtech.net>
 *
 * 3D mesh operations
 */

#include <unordered_set>
#include <unordered_map>

#include "meshop.hpp"
#include "parse-obj.hpp"
#include "parse-ply.hpp"
#include "triclip.hpp"

#include "utility/expect.hpp"
#include "utility/small_list.hpp"

#include <set>
#include <boost/algorithm/string.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/lexical_cast.hpp>

namespace geometry {

namespace ublas = boost::numeric::ublas;
namespace ba = boost::algorithm;

Obj asObj(const Mesh &mesh)
{
    Obj obj;
    for ( math::Point3 vertex : mesh.vertices ) {

        geometry::Obj::Vector3d overtex;
        overtex.x = vertex[0]; overtex.y = vertex[1]; overtex.z = vertex[2];

        obj.addVertex( overtex );

        //LOG( info1 ) << "[" << overtex.x << ", " << overtex.y << ", "
        //    << overtex.z << "]";
    }

    for ( math::Point2 texture : mesh.tCoords ) {

        geometry::Obj::Vector3d otexture;
        otexture.x = texture[0]; otexture.y = texture[1]; otexture.z = 0.0;

        obj.addTexture( otexture );
    }

    for ( geometry::Face face : mesh.faces ) {

        geometry::Obj::Facet facet;

        facet.v[0] = int(face.a);
        facet.v[1] = int(face.b);
        facet.v[2] = int(face.c);

        facet.t[0] = int(face.ta);
        facet.t[1] = int(face.tb);
        facet.t[2] = int(face.tc);

        obj.addFacet( facet );
    }

    return obj;
}

Mesh::pointer asMesh(const Obj &obj){
    auto newMesh(std::make_shared<geometry::Mesh>());
    for( const auto&v : obj.vertices ){
        newMesh->vertices.push_back(v);
    }

    for( const auto&t : obj.texcoords ){
        newMesh->tCoords.emplace_back( t(0), t(1) );
    }

    for( const auto&f : obj.facets ){
        newMesh->addFace(f.v[0], f.v[1], f.v[2], f.t[0], f.t[1], f.t[2]);
    }

    return newMesh;
}

std::string ObjMaterial::name(std::size_t index) const
{
    if (index < names.size()) { return names[index]; }
    return boost::lexical_cast<std::string>(index);
}

namespace {

void addMtl(std::ostream &out, const ObjMaterial &mtl, unsigned int imageId)
{
    if (mtl.libs.empty()) { return; }
    out << "usemtl " << mtl.name(imageId) << '\n';
}

} // namespace

void saveAsObj(const Mesh &mesh, std::ostream &out
               , const ObjMaterial &mtl
               , const boost::filesystem::path &filepath
               , const ObjStreamSetup &streamSetup)
{
    // keep current numeric precision for future use
    const auto oldPrecision(out.precision());

    for (const auto &lib : mtl.libs) {
        out << "mtllib " << lib << '\n';
    }

    if (!streamSetup.vertex(out)) {
        out.setf(std::ios::scientific, std::ios::floatfield);
    }

    for (const auto &vertex : mesh.vertices) {
        out << "v " << vertex(0) << ' ' << vertex(1) << ' '  << vertex(2)
            << '\n';
    }

    if (!streamSetup.tx(out)) {
        out.setf(std::ios::scientific, std::ios::floatfield);
        // reset precision to recorded one
        out.precision(oldPrecision);
    }

    for (const auto &tCoord : mesh.tCoords) {
        out << "vt " << tCoord(0) << ' ' << tCoord(1) << '\n';
    }

    const bool textured(!mesh.tCoords.empty());

    unsigned int currentImageId(static_cast<unsigned int>(-1));

    for (const auto &face : mesh.faces) {
        if (face.degenerate()) {
            continue;
        }

        if (textured && (face.imageId != currentImageId)) {
            addMtl(out, mtl, face.imageId);
            currentImageId = face.imageId;
        }

        if (textured) {
            out << "f " << face.a + 1 << '/' << face.ta + 1 << "/ "
                << face.b + 1 << '/' << face.tb + 1 << "/ "
                << face.c + 1 << '/' << face.tc + 1 << "/\n";
        } else {
            out << "f " << face.a + 1 << ' '
                << face.b + 1 << ' '
                << face.c + 1 << "\n";
        }
    }

    if (!out) {
        LOGTHROW(err3, std::runtime_error)
            << "Unable to save mesh to <" << filepath << ">.";
    }
}

void saveAsObj(const Mesh &mesh, std::ostream &out
               , const ObjMaterial &mtl
               , const boost::filesystem::path &filepath
               , bool setFormat)
{
    struct DontSetFormat : ObjStreamSetup {
        virtual bool vertex(std::ostream&) const { return true; }
        virtual bool tx(std::ostream&) const { return true; }
    };

    const ObjStreamSetup &streamSetup(setFormat
                                      ? ObjStreamSetup()
                                      : DontSetFormat());

    saveAsObj(mesh, out, mtl, filepath, streamSetup);
}

void saveAsObj(const Mesh &mesh, const boost::filesystem::path &filepath
               , const ObjMaterial &mtl, const ObjStreamSetup &streamSetup)
{
    LOG(info2) << "Saving mesh to file <" << filepath << ">.";

    std::ofstream f;
    f.exceptions(std::ios::badbit | std::ios::failbit);
    try {
        f.open(filepath.string(), std::ios_base::out | std::ios_base::trunc);
    } catch (const std::exception&) {
        LOGTHROW(err3, std::runtime_error)
            << "Unable to save mesh to <" << filepath << ">.";
    }

    saveAsObj(mesh, f, mtl, filepath, streamSetup);
}

void saveAsPly( const Mesh &mesh, const boost::filesystem::path &filepath){
    LOG(info2) << "Saving mesh to file <" << filepath << ">.";

    std::ofstream out;
    out.exceptions(std::ios::badbit | std::ios::failbit);
    out.open(filepath.string(), std::ios_base::out | std::ios_base::trunc);
    out.setf(std::ios::scientific, std::ios::floatfield);

    unsigned validFaces(0);
    for (const auto &face : mesh.faces) {
        if (!face.degenerate() && mesh.good(face))
            validFaces++;
    }

    out << "ply\n"
        << "format ascii 1.0\n"
        << "comment generated by window-mesh\n"
        << "element vertex " << mesh.vertices.size() << '\n'
        << "property float x\n"
        << "property float y\n"
        << "property float z\n"
        << "element face " << validFaces << '\n'
        << "property list uchar int vertex_indices\n"
        << "end_header\n";

    for (const auto &vertex : mesh.vertices)
    {
        out << vertex(0) << ' ' << vertex(1) << ' '  << vertex(2) << '\n';
    }

    for (const auto &face : mesh.faces)
    {
        if (face.degenerate()) {
            continue;
        }
        if (!mesh.good(face)) {
            LOG(warn2) << "Invalid vertex index in face.";
            continue;
        }
        out << "3 " << face.a << ' ' << face.b << ' ' << face.c << '\n';
    }

    out.close();

    if (!out) {
        LOGTHROW(err3, std::runtime_error)
            << "Unable to save mesh to <" << filepath << ">.";
    }
}

Mesh loadPly(const boost::filesystem::path& filepath)
{
    class SimplePlyParser : public PlyParserBase
    {
    public:
        Mesh mesh;

        void loadHeader(const std::vector<PlyElement>& elements,
                        const std::vector<std::string>& /* comments */) override
        {
            for (const auto& el : elements)
            {
                if (el.name == "vertex")
                {
                    mesh.vertices.reserve(el.count);

                    // check vertex properties
                    if (el.props.size() != 3)
                    {
                        LOGTHROW(err4, std::runtime_error)
                            << "No additional vertex properties supported in "
                               "simple parser";
                    }
                    utility::expect(el.props[0].name == "x");
                    utility::expect(el.props[1].name == "y");
                    utility::expect(el.props[2].name == "z");
                }
                else if (el.name == "face")
                {
                    mesh.faces.reserve(el.count);

                    // check face properties
                    if (el.props.size() != 1)
                    {
                        LOGTHROW(err4, std::runtime_error)
                            << "No additional face properties supported in "
                               "simple parser";
                    }
                    if (!ba::starts_with(el.props[0].type, "list"))
                    {
                        LOGTHROW(err4, std::runtime_error)
                            << "Expected face property type to be a list, but "
                               "got: "
                            << el.props[0].type;
                    }
                    // does not check the name which should generally be
                    // "vertex_index" but might vary
                }
                else
                {
                    LOGTHROW(err4, std::runtime_error)
                        << "Unexpected element in header - not supported by "
                           "simple parser: "
                        << el.name;
                }
            }
        }

        void addVertex(std::istream& f) override
        {
            double x, y, z;
            f >> x >> y >> z;
            mesh.vertices.emplace_back(x, y, z);
        }

        void addFace(std::istream& f) override
        {
            int n, a, b, c;
            f >> n >> a >> b >> c;
            if (n != 3)
            {
                LOGTHROW(err4, std::runtime_error)
                    << "Simple parser only supports loading triangular meshes, "
                       "but got face with " << n << " vertices.";
            }
            mesh.faces.emplace_back(a, b, c);
        }
    } simpleParser;

    parsePly(simpleParser, filepath);

    LOG(info1) << "Loaded mesh with " << simpleParser.mesh.vertices.size()
               << " vertices and " << simpleParser.mesh.faces.size()
               << " faces";

    return simpleParser.mesh;
}

const unsigned int imageIdLimit(1<<16);

void loadObj( ObjParserBase &parser
            , const boost::filesystem::path &filename)
{
    std::ifstream file;
    file.exceptions(std::ios::badbit | std::ios::failbit);
    file.open(filename.string());

    if (!parser.parse(file)) {
        LOGTHROW(err2, std::runtime_error)
            << "LoadObj failed to parse file " << filename << ".";
    }
}

Mesh loadObj(const boost::filesystem::path &filename, ObjMaterial *mtl)
{
    struct Obj2MeshParser : public ObjParserBase {
        Obj2MeshParser() : imageId(), namedCount() {}

        void addVertex( const Vector3d &v ) {
            mesh.vertices.push_back(v);
        }

        void addTexture( const Vector3d &t ) {
            mesh.tCoords.emplace_back(t.x, t.y);
        }

        void addFacet( const Facet &f ) {
            mesh.addFace( f.v[0], f.v[1], f.v[2]
                        , f.t[0], f.t[1], f.t[2] );
            mesh.faces.back().imageId = imageId;
        }

        void addNormal( const Vector3d& ) { }

        void materialLibrary(const std::string &lib) { seenLibs.insert(lib); }
        void useMaterial(const std::string &name) {
            auto fmtlMap(mtlMap.find(name));
            if (fmtlMap == mtlMap.end()) {
                unsigned int id(0);
                try {
                    // is id a (non negative) number?
                    auto number(boost::lexical_cast<unsigned int>(name));
                    id = number;
                } catch (const boost::bad_lexical_cast&) {
                    // not a number
                    id = imageIdLimit + namedCount++;
                }

                fmtlMap = mtlMap.insert(MtlMap::value_type(name, id)).first;
            }

            imageId = fmtlMap->second;
        }

        Mesh mesh;
        unsigned int imageId;
        std::set<std::string> seenLibs;
        typedef std::map<std::string, int> MtlMap;
        MtlMap mtlMap;
        int namedCount;
    } parser_;

    loadObj(parser_, filename);

    std::vector<std::string> materials;

    // regenerate IDs if there was any non-numeric material
    if (parser_.namedCount) {
        typedef std::map<unsigned int, unsigned int> Mapping;
        Mapping mapping;
        for (const auto &item : parser_.mtlMap) {
            mapping[item.second] = materials.size();
            materials.push_back(item.first);
        }

        for (auto &face : parser_.mesh.faces) {
            face.imageId = mapping[face.imageId];
        }
    }

    if (mtl) {
        mtl->libs.assign(parser_.seenLibs.begin(), parser_.seenLibs.end());
        mtl->names = std::move(materials);
    }

    return parser_.mesh;
}

namespace {

void clipImpl(const Mesh &omesh, Mesh &mesh
              , const std::vector<ClipPlane> &planes)
{
    ClipTriangle::list clipped;
    for (const auto &face : omesh.faces) {
        clipped.emplace_back(
              omesh.vertices[face.a]
            , omesh.vertices[face.b]
            , omesh.vertices[face.c]);
    }

    std::vector<double> tinfos;
    for (const auto &plane : planes) {
        clipped = clipTriangles(clipped, plane, tinfos);
    }

    typedef math::Points3::size_type Index;
    std::map<math::Point3, Index> pMap;
    Index next = 0;

    for (const auto &triangle : clipped)
    {
        Index indices[3];
        for (int i = 0; i < 3; i++)
        {
            auto pair = pMap.insert(std::make_pair(triangle.pos[i], next));
            if (pair.second) {
                next++;
            }
            indices[i] = pair.first->second;

            if (indices[i] >= mesh.vertices.size()) {
                mesh.vertices.push_back(triangle.pos[i]);
            }
        }
        // do not add degenerated faces
        if ((indices[0] != indices[1]) &&
            (indices[1] != indices[2]) &&
            (indices[0] != indices[2]))
        {
            mesh.addFace(indices[0], indices[1], indices[2]);
        }
    }
}

std::vector<ClipPlane> planes(const math::Extents2 &extents)
{
    std::vector<ClipPlane> planes;
    planes.emplace_back(+1.,  0., 0., extents.ll[0]);
    planes.emplace_back(-1.,  0., 0., -extents.ur[0]);
    planes.emplace_back(0.,  +1., 0., extents.ll[1]);
    planes.emplace_back(0.,  -1., 0., -extents.ur[1]);
    return planes;
}

std::vector<ClipPlane> planes(const math::Extents3 &extents)
{
    std::vector<ClipPlane> planes;
    planes.emplace_back(+1.,  0., 0., extents.ll[0]);
    planes.emplace_back(-1.,  0., 0., -extents.ur[0]);
    planes.emplace_back(0.,  +1., 0., extents.ll[1]);
    planes.emplace_back(0.,  -1., 0., -extents.ur[1]);
    planes.emplace_back(0.,  0., +1., extents.ll[2]);
    planes.emplace_back(0.,  0., -1., -extents.ur[2]);
    return planes;
}

} // namespace

Mesh clip(const Mesh &omesh, const math::Extents2 &extents)
{
    Mesh out;
    clipImpl(omesh, out, planes(extents));
    return out;
}

Mesh clip(const Mesh &omesh, const math::Extents3 &extents)
{
    Mesh out;
    clipImpl(omesh, out, planes(extents));
    return out;
}

Mesh::pointer removeNonManifoldEdges(Mesh omesh)
{
    auto ofaces = omesh.faces;
    auto pmesh(std::make_shared<geometry::Mesh>(std::move(omesh)));
    auto& mesh(*pmesh);
    mesh.faces.clear();
    mesh.faces.shrink_to_fit();

    typedef Face::index_type index_type;

    struct EdgeKey
    {
        index_type v1, v2; // vertex indices

        EdgeKey(index_type v1, index_type v2)
        {
            this->v1 = std::min(v1, v2);
            this->v2 = std::max(v1, v2);
        }

        bool operator< (const EdgeKey& other) const
        {
            return (v1 == other.v1) ? (v2 < other.v2) : (v1 < other.v1);
        }
    };

    struct Edge {
        utility::small_list<index_type, 2> facesIndices;
    };

    //count faces for each edge
    std::map<EdgeKey,Edge> edgeMap;
    for(index_type fi=0; fi<ofaces.size(); fi++){
        const auto & face(ofaces[fi]);
        EdgeKey edgeKeys[3] = { EdgeKey(face.a,face.b)
                           , EdgeKey(face.b,face.c)
                           , EdgeKey(face.c,face.a) };
        for(const auto & key : edgeKeys){
            auto it = edgeMap.find(key);
            if(it==edgeMap.end()){
                //if edge is not present insert it with current face
                Edge edge;
                edge.facesIndices.insert(fi);
                edgeMap.insert(std::make_pair(key,edge));
            }
            else{
                //if edge is present add current face to it
                it->second.facesIndices.insert(fi);
            }
        }
    }

    //collect faces incident with non-manifold edge
    std::set<index_type> facesToOmit;
    for(auto it = edgeMap.begin(); it!=edgeMap.end(); it++){
        if(it->second.facesIndices.size()>2){
            it->second.facesIndices.for_each([&](index_type fi) {
                facesToOmit.insert(fi);
            });
        }
    }

    for(index_type fi=0; fi<ofaces.size(); fi++){
        const auto & face(ofaces[fi]);
        if(facesToOmit.find(fi)==facesToOmit.end()){
            mesh.addFace( face.a, face.b, face.c
                        , face.ta, face.tb, face.tc );
        }
    }

    return pmesh;
}

Mesh::pointer removeIsolatedVertices( const Mesh& imesh ){
    auto pmesh(std::make_shared<geometry::Mesh>());
    auto & mesh(*pmesh);

    std::map<math::Points3::size_type, math::Points3::size_type> vertexMap;
    std::map<math::Points2::size_type, math::Points2::size_type> tCoordsMap;

    for( const auto& face: imesh.faces ){
        math::Points3::size_type vindices[3] { face.a , face.b, face.c };
        math::Points2::size_type tindices[3] { face.ta , face.tb, face.tc };

        for(unsigned int i=0; i<3; ++i){
            auto vit = vertexMap.find(vindices[i]);
            auto tit = tCoordsMap.find(tindices[i]);

            if(vit == vertexMap.end()){
                mesh.vertices.push_back(imesh.vertices[vindices[i]]);
                vit = vertexMap.insert(std::make_pair(vindices[i],mesh.vertices.size()-1)).first;
            }
            if(imesh.tCoords.size() > 0 && tit == tCoordsMap.end()){
                mesh.tCoords.push_back(imesh.vertices[tindices[i]]);
                tit = tCoordsMap.insert(std::make_pair(tindices[i],mesh.tCoords.size()-1)).first;
            }

            vindices[i] = vit->second;
            tindices[i] = tit->second;
        }

        if(imesh.tCoords.size() > 0){
            mesh.addFace( vindices[0], vindices[1], vindices[2]
                        , tindices[0], tindices[1], tindices[2] );
        }
        else{
            mesh.addFace( vindices[0], vindices[1], vindices[2] );
        }
    }

    return pmesh;
}

Mesh::pointer refine( const Mesh & omesh, unsigned int maxFacesCount)
{
    auto pmesh(std::make_shared<geometry::Mesh>(omesh));
    auto & mesh(*pmesh);

    struct EdgeKey
    {
        std::size_t v1, v2; // vertex indices

        EdgeKey(std::size_t v1, std::size_t v2)
        {
            this->v1 = std::min(v1, v2);
            this->v2 = std::max(v1, v2);
        }

        bool operator< (const EdgeKey& other) const
        {
            return (v1 == other.v1) ? (v2 < other.v2) : (v1 < other.v1);
        }
    };

    struct Edge {
        typedef enum {
            AB,
            BC,
            CA
        } EdgeType;

        std::size_t v1, v2;
        int f1, f2;
        EdgeType et1, et2;

        float length;

        Edge(std::size_t pv1, std::size_t pv2, float plength)
            :v1(std::min(pv1,pv2)),v2(std::max(pv1,pv2)), length(plength){
            f1 = -1;
            f2 = -1;
        }

        void addFace(std::size_t pv1, std::size_t pv2, int fid, EdgeType type)
        {
            if(pv1<pv2){
                f1=fid;
                et1 = type;
            }
            else{
                f2=fid;
                et2 = type;
            }
        }

        bool operator< (const Edge& other) const
        {
            return length < other.length;
        }
    };

    struct EdgeMap {
        std::map<EdgeKey, std::shared_ptr<Edge>> map;
        std::vector<std::shared_ptr<Edge>> heap;

        bool compareEdgePtr(const std::shared_ptr<Edge> &a, const std::shared_ptr<Edge> &b){
            return a->length<b->length;
        }

        void addFaceEdge(std::size_t pv1, std::size_t pv2, int fid
                         , Edge::EdgeType type, float length)
        {
            EdgeKey key(pv1, pv2);
            auto it(map.find(key));
            if(it!=map.end()){
                it->second->addFace(pv1, pv2, fid, type );
            }
            else{
                heap.push_back(std::make_shared<Edge>(pv1, pv2, length));
                heap.back()->addFace(pv1, pv2, fid, type );
                map.insert(std::make_pair(key,heap.back()));
                std::push_heap(heap.begin(),heap.end(), [this]( const std::shared_ptr<Edge> &a
                                                              , const std::shared_ptr<Edge> &b){
                    return this->compareEdgePtr(a,b);
                });
            }
        }

        Edge pop_top_edge(){
            auto edge = *heap[0];
            std::pop_heap(heap.begin(), heap.end(), [this]( const std::shared_ptr<Edge> &a
                                                          , const std::shared_ptr<Edge> &b){
                return this->compareEdgePtr(a,b);
            });
            heap.pop_back();
            map.erase(EdgeKey(edge.v1, edge.v2));
            return edge;
        }

        Edge top_edge(){
            return *heap[0];
        }

        void addFaceEdges(const Mesh & mesh, int fid){
            const auto& f = mesh.faces[fid];
            auto e1Length =  float(ublas::norm_2(mesh.vertices[f.a]-mesh.vertices[f.b]));
            addFaceEdge(f.a, f.b, fid, Edge::EdgeType::AB, e1Length);

            auto e2Length =  float(ublas::norm_2(mesh.vertices[f.b]-mesh.vertices[f.c]));
            addFaceEdge(f.b, f.c, fid, Edge::EdgeType::BC, e2Length);

            auto e3Length =  float(ublas::norm_2(mesh.vertices[f.c]-mesh.vertices[f.a]));
            addFaceEdge(f.c, f.a, fid, Edge::EdgeType::CA, e3Length);
        }

        std::size_t size(){
            return heap.size();
        }

    };

    EdgeMap edgeMap;

    auto splitEdge = [&mesh,&edgeMap]( int fid, Edge::EdgeType type
                       , std::size_t vid) mutable -> void{
        auto & face = mesh.faces[fid];
        switch(type){
            case Edge::EdgeType::AB:
                {
                    if(mesh.tCoords.size()>0){
                        math::Point2 tcMiddle = ( mesh.tCoords[face.ta]
                                                + mesh.tCoords[face.tb]) * 0.5;
                        mesh.tCoords.push_back(tcMiddle);
                    }
                    mesh.addFace( mesh.faces[fid].b, mesh.faces[fid].c
                                , vid
                                , mesh.faces[fid].tb, mesh.faces[fid].tc
                                , mesh.tCoords.size()-1);

                    mesh.faces[fid].b = vid;
                    mesh.faces[fid].tb = mesh.tCoords.size()-1;

                    edgeMap.addFaceEdges(mesh, fid);
                    edgeMap.addFaceEdges(mesh, int(mesh.faces.size()-1));
                }
                break;
            case Edge::EdgeType::BC:
                {
                    if(mesh.tCoords.size()>0){
                        math::Point2 tcMiddle = (mesh.tCoords[face.tb]
                                                + mesh.tCoords[face.tc]) * 0.5;
                        mesh.tCoords.push_back(tcMiddle);
                    }
                    mesh.addFace( mesh.faces[fid].c,mesh.faces[fid].a, vid
                                , mesh.faces[fid].tc,mesh.faces[fid].ta
                                , mesh.tCoords.size()-1);

                    mesh.faces[fid].c = vid;
                    mesh.faces[fid].tc = mesh.tCoords.size()-1;

                    edgeMap.addFaceEdges(mesh, fid);
                    edgeMap.addFaceEdges(mesh, int(mesh.faces.size()-1));
                }
                break;
            case Edge::EdgeType::CA:
                {
                    if(mesh.tCoords.size()>0){
                        math::Point2 tcMiddle = (mesh.tCoords[face.tc]
                                                + mesh.tCoords[face.ta]) * 0.5;
                        mesh.tCoords.push_back(tcMiddle);
                    }

                    mesh.addFace( mesh.faces[fid].a,mesh.faces[fid].b, vid
                                , mesh.faces[fid].ta,mesh.faces[fid].tb
                                , mesh.tCoords.size()-1);

                    mesh.faces[fid].a = vid;
                    mesh.faces[fid].ta = mesh.tCoords.size()-1;

                    edgeMap.addFaceEdges(mesh, fid);
                    edgeMap.addFaceEdges(mesh, int(mesh.faces.size()-1));
                }
                break;
        }
    };

    for (std::size_t i=0; i<mesh.faces.size(); ++i ) {
        //add all 3 edges
        edgeMap.addFaceEdges(mesh, int(i));
    }

    //sort edges by length
    while( mesh.faces.size() < maxFacesCount && edgeMap.size()>0 ){
        //split edge
        auto edge = edgeMap.pop_top_edge();

        //find middle
        math::Point3 middle = (mesh.vertices[edge.v1]
                            + mesh.vertices[edge.v2]) * 0.5;
        mesh.vertices.push_back(middle);

        //split first face
        if(edge.f1>=0){
            splitEdge(edge.f1, edge.et1, mesh.vertices.size()-1);
        }

        //split second face
        if(edge.f2>=0){
            splitEdge(edge.f2, edge.et2, mesh.vertices.size()-1);
        }
    }

    return pmesh;
}

MeshInfo measurePly(std::istream &is, const boost::filesystem::path &path)
{
    // read header
    std::string line;
    int nvert = -1, ntris = -1;
    do {
        if (getline(is, line).eof()) break;
        std::sscanf(line.c_str(), "element vertex %d", &nvert);
        std::sscanf(line.c_str(), "element face %d", &ntris);
    } while (line != "end_header");

    if (nvert < 0 || ntris < 0) {
        LOGTHROW(err2, std::runtime_error)
            << "Unknown PLY format in file " << path << ".";
    }

    return MeshInfo(nvert, ntris);
}

void loadPly(ObjParserBase &parser, std::istream &is
             , const boost::filesystem::path &path)
{
    const auto mi(measurePly(is, path));

    // load points
    ObjParserBase::Vector3d v;
    for (std::size_t i = 0; i < mi.vertexCount; i++) {
        is >> v.x >> v.y >> v.z;
        parser.addVertex(v);
    }

    // load triangles
    ObjParserBase::Facet f;
    int n;
    for (std::size_t i = 0; i < mi.faceCount; i++) {
        is >> n >> f.v[0] >> f.v[1] >> f.v[2];
        utility::expect(n == 3
                        , "Only triangles are supported in PLY files (&s)."
                        , path);
        parser.addFacet(f);
    }
}

MeshInfo measurePly(const boost::filesystem::path &path)
{
    std::ifstream f(path.string());
    if (!f.good()) {
        LOGTHROW(err2, std::runtime_error)
            << "Can't open PLY file " << path << ".";
    }

    f.exceptions(std::ios::badbit | std::ios::failbit);
    const auto mi(measurePly(f, path));
    f.close();

    return mi;
}

bool objHasVertexOrFace(const boost::filesystem::path &path)
{
    bool hasVorF(false);

    std::ifstream f(path.string());
    if (!f.good()) {
        LOGTHROW(err2, std::runtime_error)
            << "Can't open OBJ file " << path << ".";
    }

    f.exceptions(std::ios::badbit);

    std::string line;
    do {
        getline(f, line);
        if (f.eof()) break;
        if (f.fail()) {
            LOGTHROW(err2, std::runtime_error)
                << "Failbit when reading <" << path << ">";
        }

        if (line.empty()) continue;
        if (line[0] == 'v' || line[0] == 'f') {
            hasVorF = true;
            break;
        }
    } while (true);
    f.close();

    return hasVorF;
}

void loadPly(ObjParserBase &parser, const boost::filesystem::path &path)
{
    std::ifstream f(path.string());
    if (!f.good()) {
        LOGTHROW(err2, std::runtime_error)
            << "Can't open PLY file " << path << ".";
    }

    f.exceptions(std::ios::badbit | std::ios::failbit);

    loadPly(parser, f, path);

    f.close();
}

void append(Mesh &mesh, const Mesh &added)
{
    // remember vertex indexing shift and append vertices
    const auto vShift(mesh.vertices.size());
    mesh.vertices.insert(mesh.vertices.end(), added.vertices.begin()
                         , added.vertices.end());

    // remember texturing coord indexing shift and append texturing coordinates
    const auto tcShift(mesh.tCoords.size());
    mesh.tCoords.insert(mesh.tCoords.end(), added.tCoords.begin()
                        , added.tCoords.end());

    // append faces with shifts applied
    for (const auto &f : added.faces) {
        mesh.addFace(f.a + vShift, f.b + vShift, f.c + vShift
                     , f.ta + tcShift, f.tb + tcShift, f.tc + tcShift
                     , f.imageId);
    }
}

Mesh::list splitById(const Mesh &mesh)
{
    typedef unsigned int ImageId;

    struct MeshBuilder {
        typedef std::unordered_map<ImageId, MeshBuilder> map;

        typedef Face::index_type Index;

        typedef std::unordered_map<Index, Index> VMap;

        VMap vertices;
        Mesh mesh;

        Index vertex(const Mesh &m, Index v) {
            auto fvertices(vertices.find(v));
            if (fvertices == vertices.end()) {
                auto nv(mesh.vertices.size());
                mesh.vertices.push_back(m.vertices[v]);
                fvertices = vertices.insert(VMap::value_type(v, nv)).first;
            }
            return fvertices->second;
        }

        void add(const Mesh &m, const Face &face) {
            mesh.faces.emplace_back(vertex(m, face.a)
                                    , vertex(m, face.b)
                                    , vertex(m, face.c)
                                    , 0, 0, 0, face.imageId);
        }
    };

    MeshBuilder::map builders;
    std::unordered_set<ImageId> unique;
    for (const auto &face : mesh.faces) {
        auto fbuilders(builders.find(face.imageId));
        if (fbuilders == builders.end()) {
            fbuilders
                = builders.insert(MeshBuilder::map::value_type
                                  (face.imageId, MeshBuilder())).first;
        }
        fbuilders->second.add(mesh, face);
    }

    Mesh::list out;
    out.reserve(builders.size());
    for (auto &builder : builders) {
        out.push_back(std::move(builder.second.mesh));
    }
    return out;
}

} // namespace geometry

max_stars_repo_name: Melown/libgeometry

max_issues_repo_name: Melown/libgeometry

max_forks_repo_name: Melown/libgeometry


text:
#pragma once

#include <string>

#include <Eigen/Core>
#include <opencv2/core/core.hpp>

#include <tf/transform_broadcaster.h>

#include "rcvio/tracker.hpp"
#include "rcvio/updater.hpp"
#include "rcvio/input_buffer.hpp"
#include "rcvio/pre_integrator.hpp"

namespace rcvio
{
    class System
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        System(const std::string &settings_file);

        inline void pushImuData(ImuData *data) { input_buffer_->pushImuData(data); }
        inline void pushImageData(ImageData *data) { input_buffer_->pushImageData(data); }

        void initialize(const Eigen::Vector3d &w, const Eigen::Vector3d &a,
                        const int imu_data, const bool enable_alignment);

        void monoVIO();

    private:
        double imu_rate_;
        double cam_time_offset_;

        int sliding_window_size_;
        int min_clone_states_;

        bool enable_alignment_;
        bool record_outputs_;

        bool is_moving_;
        bool is_ready_;

        double threshold_angle_;
        double threshold_displacement_;

        double gravity_;

        double sigma_gyro_noise_;
        double sigma_gyro_bias_;
        double sigma_accel_noise_;
        double sigma_accel_bias_;

        Eigen::VectorXd xkk;
        Eigen::MatrixXd Pkk;

        Tracker::Ptr tracker_;
        Updater::Ptr updater_;
        InputBuffer::Ptr input_buffer_;
        PreIntegrator::Ptr pre_integrator_;

        ros::NodeHandle system_node_;
        ros::Publisher path_publish_;
        ros::Publisher odometry_publish_;
        tf::TransformBroadcaster tf_publish_;
    };
}

max_stars_repo_name: sufalroy/RC-VIO

max_issues_repo_name: sufalroy/RC-VIO

max_forks_repo_name: sufalroy/RC-VIO


text:
//Copyright (c) 2013 Singapore-MIT Alliance for Research and Technology
//Licensed under the terms of the MIT License, as described in the file:
//   license.txt   (http://opensource.org/licenses/MIT)

#include "ConnectionServer.hpp"

#include <boost/shared_ptr.hpp>
#include <boost/bind.hpp>

#include "entities/commsim/connection/CloudHandler.hpp"
#include "entities/commsim/connection/ConnectionHandler.hpp"
#include "entities/commsim/connection/WhoAreYouProtocol.hpp"
#include "logging/Log.hpp"
#include "entities/commsim/broker/Broker.hpp"

using namespace sim_mob;

sim_mob::ConnectionServer::ConnectionServer(sim_mob::BrokerBase& broker, unsigned short port) :
    broker(broker),
    acceptor(io_service,boost::asio::ip::tcp::endpoint(boost::asio::ip::tcp::v4(), port))
{
}

sim_mob::ConnectionServer::~ConnectionServer()
{
    acceptor.cancel();
    io_service.stop();
    threads.join_all();
}

void sim_mob::ConnectionServer::start(unsigned int numThreads)
{
    //Pend the first client accept.
    creatSocketAndAccept();

    //Create several threads for the io_service to make use of.
    for(unsigned int i=0; i<numThreads; i++) {
      threads.create_thread(boost::bind(&boost::asio::io_service::run, &io_service));
    }

    //NOTE: Always print this, even if output is disabled.
    std::cout << "Accepting clients on " <<numThreads <<" threads.\n";
}

boost::shared_ptr<CloudHandler> sim_mob::ConnectionServer::connectToCloud(const std::string& host, int port)
{
    //Make and track a new session pointer.
    boost::shared_ptr<CloudHandler> conn(new CloudHandler(io_service, broker, host, port));
    {
    boost::lock_guard<boost::mutex> lock(knownCloudConnectionsMUTEX);
    knownCloudConnections.push_back(conn);
    }

    boost::asio::async_connect(conn->socket, conn->getResolvedIterator(), boost::bind(&ConnectionServer::handle_cloud_connect, this, conn, boost::asio::placeholders::error));
    return conn;
}

void sim_mob::ConnectionServer::creatSocketAndAccept()
{
    //Make and track a new session pointer.
    boost::shared_ptr<ConnectionHandler> conn(new ConnectionHandler(io_service, broker));
    {
    boost::lock_guard<boost::mutex> lock(knownConnectionsMUTEX);
    knownConnections.push_back(conn);
    }

    //Accept the next connection.
    acceptor.async_accept(conn->socket,
        boost::bind(&ConnectionServer::handle_accept, this, conn, boost::asio::placeholders::error)
    );
}

void sim_mob::ConnectionServer::handle_accept(boost::shared_ptr<ConnectionHandler> conn, const boost::system::error_code& e)
{
    if (e) {
        std::cout<< "Failed to accept connection: " <<e.message() << std::endl;  //NOTE: Always print this, even if output is disabled.
        return;
    }

    //NOTE: Always print this, even if output is disabled.
    std::cout<< "Accepted a connection.\n";

    //Turn off Nagle's algorithm; it's slow on small packets.
    conn->socket.set_option(boost::asio::ip::tcp::no_delay(true));

    //Start listening for the first client message.
    conn->readHeader();

    //Inform the Broker that a new connection is available.
    broker.onNewConnection(conn);

    //Continue; accept the next connection.
    creatSocketAndAccept();
}

void sim_mob::ConnectionServer::handle_cloud_connect(boost::shared_ptr<CloudHandler> conn, const boost::system::error_code& e)
{
    if (e) {
        std::cout<< "Failed to connect to the cloud: " <<e.message() << std::endl;  //NOTE: Always print this, even if output is disabled.
        return;
    }

    //Turn off Nagle's algorithm; it's slow on small packets.
    conn->socket.set_option(boost::asio::ip::tcp::no_delay(true));

    //Inform the Broker that a new connection is available.
    broker.onNewCloudConnection(conn);

}


text:
#include "queue.hpp"
//#include <boost/date_time/microsec_time_clock.hpp>

namespace sailbot {

bool Queue::testing_ = false;

Queue::Queue(const char *name, bool writer, size_t queue_len, size_t msg_size)
    : writer_(writer) {
  strncpy(name_, name, sizeof(name_));

  if (!testing_) {
    namespace bst = boost::interprocess;

    semaphore_ = new bst::named_semaphore(bst::open_or_create, name_, 0/*Initial Value*/);
    if (writer) {
      queue_ =
          new bst::message_queue(bst::open_or_create, name_, queue_len, msg_size);
      semaphore_->post();
    } else {
      // Wait for a writer to start up before doing anything.
      while (!util::IsShutdown()) {
        boost::posix_time::ptime time(
            boost::date_time::microsec_clock<
                boost::posix_time::ptime>::universal_time() +
            boost::posix_time::millisec(1000));
        if (semaphore_->timed_wait(time)) {
          semaphore_->post();
          queue_ = new bst::message_queue(bst::open_only, name_);
          break;
        }
      }
    }
  } else {
    // Testing
    test_impl_.reset(new testing::TestQueue(name_, msg_size));
  }
}

Queue::~Queue() {
  if (!testing_) {
    // We need to delete the queue_ before calling bst::message_queue::remove().
    delete queue_;

    if (writer_) {
      semaphore_->wait();

      // Check if the semaphore has reached zero and, if so, delete it.
      if (!semaphore_->try_wait()) {
        delete semaphore_;
        remove(name_);
      }
      else {
        // If the try_wait succeeded, then re-increment the semaphore_.
        semaphore_->post();
      }
    }
  }
}

void Queue::send(const void *msg, size_t size) {
  if (!writer_) {
    LOG(FATAL) << "You said you wouldn't be writing on this queue.";
  }
  if (!testing_) {
    queue_->send(msg, size);
  } else {
    test_impl_->send(msg, size);
  }
}

bool Queue::receive(void *msg, size_t size, size_t &rcvd) {
  if (!testing_) {
    return queue_->timed_receive(
        msg, size, rcvd, boost::date_time::microsec_clock<
                             boost::posix_time::ptime>::universal_time() +
                             boost::posix_time::milliseconds(1000));
  } else {
    return test_impl_->receive(msg, size, rcvd);
  }
}

void Queue::remove(const char *name) {
  namespace bst = boost::interprocess;
  bst::named_semaphore::remove(name);
  bst::message_queue::remove(name);
}

} // namespace sailbot

max_stars_repo_name: wpisailbot/boat

max_issues_repo_name: wpisailbot/boat

max_forks_repo_name: wpisailbot/boat


text:
//  (C) Copyright Edward Diener 2015
//  Use, modification and distribution are subject to the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt).

#if !defined(BOOST_VMD_SEQ_SIZE_HPP)
#define BOOST_VMD_SEQ_SIZE_HPP

#include <boost/vmd/detail/setup.hpp>

#if BOOST_PP_VARIADICS

#include <boost/preprocessor/control/iif.hpp>
#include <boost/preprocessor/seq/size.hpp>
#include <boost/vmd/identity.hpp>
#include <boost/vmd/is_empty.hpp>

/*

  The succeeding comments in this file are in doxygen format.

*/

/** \file
*/

/** \def BOOST_VMD_SEQ_SIZE(seq)

    \brief  expands to the size of the seq passed to it.

    seq = seq whose size is to be extracted.

    If the seq is an empty seq its size is 0.
    Otherwise the result is the number of elements in the seq.
*/

#define BOOST_VMD_SEQ_SIZE(seq) \
    BOOST_VMD_IDENTITY_RESULT \
        ( \
        BOOST_PP_IIF \
            ( \
            BOOST_VMD_IS_EMPTY(seq), \
            BOOST_VMD_IDENTITY(0), \
            BOOST_PP_SEQ_SIZE \
            ) \
        (seq) \
        ) \
/**/

#endif /* BOOST_PP_VARIADICS */
#endif /* BOOST_VMD_SEQ_SIZE_HPP */


text:
//---------------------------------------------------------------------------//
// Copyright (c) 2014 Roshan <thisisroshansmail@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

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

#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/copy_n.hpp>
#include <boost/compute/algorithm/find_end.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/types/fundamental.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace bc = boost::compute;

BOOST_AUTO_TEST_CASE(find_end_int)
{
    int data[] = {1, 4, 2, 6, 3, 2, 6, 3, 4, 6};
    bc::vector<bc::int_> vectort(data, data + 10, queue);

    int datap[] = {2, 6};
    bc::vector<bc::int_> vectorp(datap, datap + 2, queue);

    bc::vector<bc::int_>::iterator iter =
        bc::find_end(vectort.begin(), vectort.end(),
                    vectorp.begin(), vectorp.end(), queue);

    BOOST_VERIFY(iter == vectort.begin() + 5);

    vectorp[1] = 9;

    iter =
        bc::find_end(vectort.begin(), vectort.end(),
                    vectorp.begin(), vectorp.end(), queue);

    BOOST_VERIFY(iter == vectort.begin() + 10);
}

BOOST_AUTO_TEST_CASE(find_end_string)
{
    char text[] = "sdabababacabskjabacab";
    bc::vector<bc::char_> vectort(text, text + 21, queue);

    char pattern[] = "aba";
    bc::vector<bc::char_> vectorp(pattern, pattern + 3, queue);

    bc::vector<bc::char_>::iterator iter =
        bc::find_end(vectort.begin(), vectort.end(),
                    vectorp.begin(), vectorp.end(), queue);

    BOOST_VERIFY(iter == vectort.begin() + 15);
}

BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: mikestaub/arangodb