text:
#include "shape_reconstruction/ShapeReconstructionNode.h"

#include <pcl/conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter_indices.h>
#include <sensor_msgs/Image.h>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>

#include <pcl_conversions/pcl_conversions.h>

#include <ros/package.h>

#include <cmath>

#include <ros/console.h>

#include "shape_reconstruction/RangeImagePlanar.hpp"

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/passthrough.h>

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

#include <std_msgs/Bool.h>

using namespace omip;

ShapeReconstructionNode::ShapeReconstructionNode():
    _active(true),
    _frame_ctr(0),
    _max_rb(100),
    _detect_static_environment(false),
    _record_result_bag(false),
    _result_bag_open(false),
    _manually_advance_after_processing(false),
    _sv_estimated(false),
    _refine_shapes_in_each_frame(false),
    _processing_interval(0.0),
    _estimate_sv(false)
{
    this->_rbshapes_combined_publisher = this->_node_handle.advertise<sensor_msgs::PointCloud2>("rbshapes_comb",10);
    this->_rgbd_pc_subscriber.subscribe(this->_node_handle, "/camera/depth_registered/points",1);

    this->_clustered_feats_subscriber.subscribe(this->_node_handle, "/rb_tracker/clustered_tracked_feats", 1);

    this->_node_quit_subscriber = this->_node_handle.subscribe("/omip/shutdown", 1,
                                                               &ShapeReconstructionNode::TrackerQuitCallback, this);

    this->_poses_and_vels_subscriber.subscribe(this->_node_handle, "/rb_tracker/state",1);

    this->_synchronizer_with_clustered_feats =
            new message_filters::Synchronizer<SRSyncPolicyWithClusteredFeatures>(SRSyncPolicyWithClusteredFeatures(10),
                                                                                this->_rgbd_pc_subscriber,
                                                                                this->_poses_and_vels_subscriber,
                                                                                this->_clustered_feats_subscriber);
    this->_synchronizer_with_clustered_feats->registerCallback(boost::bind(&ShapeReconstructionNode::measurementCallbackWithClusteredFeatures, this, _1, _2, _3));

    this->_previous_measurement_time = ros::Time(0.0);

    this->_mesh_generator_srv = this->_node_handle.advertiseService("/shape_reconstruction/mesh_generator_srv", &ShapeReconstructionNode::generateMeshes, this);

    this->_node_handle.getParam("/shape_reconstruction/max_rb", _max_rb);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode","Maximal number of rigid bodies tracked: " << _max_rb);

    this->_node_handle.getParam("/shape_reconstruction/detect_static_environment", this->_detect_static_environment);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode","detect_static_environment = " << this->_detect_static_environment);

    this->_node_handle.getParam("/shape_reconstruction/publish_shapes_in_each_frame", this->_publish_shapes_in_each_frame);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode","publish_shapes_in_each_frame = " << this->_publish_shapes_in_each_frame);

    this->_node_handle.getParam("/shape_reconstruction/refine_shapes_in_each_frame", this->_refine_shapes_in_each_frame);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode","refine_shapes_in_each_frame = " << this->_refine_shapes_in_each_frame);

    this->_node_handle.getParam("/shape_reconstruction/manually_advance_after_processing", this->_manually_advance_after_processing);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode","manually_advance_after_processing = " << this->_manually_advance_after_processing);

    this->_node_handle.getParam("/shape_reconstruction/processing_interval", this->_processing_interval);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode","processing_interval = " << this->_processing_interval);

    this->_node_handle.getParam("/shape_reconstruction/estimate_sv", this->_estimate_sv);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode","_estimate_sv = " << this->_estimate_sv);

    this->_current_pc.reset(new SRPointCloud());
    this->_current_pc_down.reset(new SRPointCloud());
    this->_current_pc_without_nans.reset(new SRPointCloud());

    this->_clustered_feats.reset(new pcl::PointCloud<pcl::PointXYZL>);

    this->_static_environment_ext_d_and_c_pub = this->_node_handle.advertise<sensor_msgs::PointCloud2>("/shape_recons/shape_static_environment", 1);

    this->_shape_models_ext_d_and_c_pub = this->_node_handle.advertise<omip_msgs::ShapeModels>("/shape_recons/state", 1);

    this->_advance_feature_tracker_pub = this->_node_handle.advertise<std_msgs::Bool>("segmentation_info_msg", 1);

    /// For SE shape, defined as PC - RB (it is wrong if there are other RBs)
    this->_se_polygon_data_ptr = vtkSmartPointer<vtkPolyData>::New ();
    this->_se_polygon_writer_ptr = vtkSmartPointer<vtkSTLWriter>::New ();
    this->_se_polygon_writer_ptr->SetFileTypeToBinary();
    this->_se_normal_estimator_ptr.reset(new pcl::NormalEstimationOMP<omip::SRPoint, pcl::Normal>());
    this->_se_normal_estimator_ptr->setKSearch (50);
    this->_se_estimated_normals_pc_ptr.reset(new pcl::PointCloud<pcl::Normal>());
    this->_se_tree_for_normal_estimation_ptr.reset(new pcl::search::KdTree<omip::SRPoint>());
    this->_se_position_color_and_normals_pc_ptr.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
    this->_se_tree_for_triangulation_ptr.reset(new pcl::search::KdTree<pcl::PointXYZRGBNormal>());
    this->_se_greedy_projection_triangulator_ptr.reset(new pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal>());
    // Set typical values for the parameters
    this->_se_greedy_projection_triangulator_ptr->setMu (2.5);
    this->_se_greedy_projection_triangulator_ptr->setMaximumNearestNeighbors (100);
    this->_se_greedy_projection_triangulator_ptr->setMaximumSurfaceAngle(M_PI); // 180 degrees
    this->_se_greedy_projection_triangulator_ptr->setMinimumAngle(M_PI/36); // 5 degrees
    this->_se_greedy_projection_triangulator_ptr->setMaximumAngle(M_PI/2); // 180 degrees
    this->_se_greedy_projection_triangulator_ptr->setNormalConsistency(true);
    this->_se_greedy_projection_triangulator_ptr->setConsistentVertexOrdering(true);
    // Set the maximum distance between connected points (maximum edge length)
    this->_se_greedy_projection_triangulator_ptr->setSearchRadius (0.3);
    this->_se_triangulated_mesh_ptr.reset(new pcl::PolygonMesh());

    this->_node_handle.getParam("/shape_reconstruction/record_result_bag", _record_result_bag);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode","record_result_bag = " << _record_result_bag);

    if (_record_result_bag) {
        if (!this->_node_handle.getParam("/shape_reconstruction/result_bag_path", _result_bag_path)) {
            _result_bag_path = "/tmp";
        }
        ROS_INFO_STREAM_NAMED("ShapeReconstructionNode","storing result bag in = " << _result_bag_path);
    }

    this->_voxel_resolution = 0.008f;
    this->_seed_resolution = 0.2f;
    this->_color_importance = 0.4f;
    this->_spatial_importance = 0.4f;
    this->_normal_importance = 0.8f;

    this->_node_handle.getParam(std::string("/shape_reconstruction/sv_voxel_resolution"), this->_voxel_resolution);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode","sv_voxel_resolution = " << this->_voxel_resolution);

    this->_node_handle.getParam(std::string("/shape_reconstruction/sv_seed_resolution"), this->_seed_resolution);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode","sv_seed_resolution = " << this->_seed_resolution);

    this->_node_handle.getParam(std::string("/shape_reconstruction/sv_color_importance"), this->_color_importance);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode","sv_color_importance = " << this->_color_importance);

    this->_node_handle.getParam(std::string("/shape_reconstruction/sv_spatial_importance"), this->_spatial_importance);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode","sv_spatial_importance = " << this->_spatial_importance);

    this->_node_handle.getParam(std::string("/shape_reconstruction/sv_normal_importance"), this->_normal_importance);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode","sv_normal_importance = " << this->_normal_importance);

    this->_node_handle.getParam(std::string("/shape_reconstruction/sv_visualization"), this->_visualization_sv);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode","sv_visualization = " << this->_visualization_sv);

    this->_node_handle.getParam(std::string("/shape_reconstruction/sv_visualization"), this->_visualization_sv);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode","sv_visualization = " << this->_visualization_sv);

    std::string ci_topic;
    this->getROSParameter<std::string>(std::string("/feature_tracker/camera_info_topic"),ci_topic);
    this->_ci_sub = this->_node_handle.subscribe(ci_topic, 1,
                                                 &ShapeReconstructionNode::CameraInfoCallback, this);
}

ShapeReconstructionNode::~ShapeReconstructionNode()
{
    if(_result_bag_open) {
        this->_result_bag.close();
        this->_video_bag.close();
    }
}

void ShapeReconstructionNode::CameraInfoCallback(const sensor_msgs::CameraInfoConstPtr &ci_msg)
{
    // We have to do this because sometimes the first ci message that is sent is zero (why?)
    if(ci_msg->height != 0)
    {
        this->_ci = sensor_msgs::CameraInfo(*ci_msg);
        this->_ci_sub.shutdown();
        for(std::map<omip::RB_id_t,omip::ShapeReconstructionPtr >::iterator it = this->_rb_shapes.begin(); it!=this->_rb_shapes.end(); it++)
        {
            it->second->setCameraInfo(this->_ci);
        }
    }
}

omip::ShapeReconstructionPtr ShapeReconstructionNode::_createNewShapeReconstruction()
{
    // We create a new ShapeReconstruction object for this RB and we add it to the map
    ROS_WARN_STREAM_NAMED("ShapeReconstructionNode::measurementCallback","Create a new ShapeReconstructor for the newly detected RB.");

    bool to_initial=false;
    this->_node_handle.getParam("/shape_reconstruction/to_initial", to_initial);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","to_initial = " << to_initial);

    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","detect_static_environment = " << this->_detect_static_environment);

    double color_changes_erode_size=false;
    this->_node_handle.getParam("/shape_reconstruction/color_changes_erode_size", color_changes_erode_size);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","color_changes_erode_size = " << color_changes_erode_size);

    bool depth_filling=false;
    this->_node_handle.getParam("/shape_reconstruction/depth_filling", depth_filling);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","depth_filling = " << depth_filling);

    bool remove_inconsistent_points=false;
    this->_node_handle.getParam("/shape_reconstruction/remove_inconsistent_points", remove_inconsistent_points);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","remove_inconsistent_points = " << remove_inconsistent_points);

    bool accumulate_change_candidates=false;
    this->_node_handle.getParam("/shape_reconstruction/accumulate_change_candidates", accumulate_change_candidates);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","accumulate_change_candidates = " << accumulate_change_candidates);

    double min_depth_change;
    this->_node_handle.getParam("/shape_reconstruction/min_depth_change", min_depth_change);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","min_depth_change = " << min_depth_change);

    int min_color_change;
    this->_node_handle.getParam("/shape_reconstruction/min_color_change", min_color_change);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","min_color_change = " << min_color_change);

    double knn_min_radius;
    this->_node_handle.getParam("/shape_reconstruction/knn_min_radius", knn_min_radius);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","knn_min_radius = " << knn_min_radius);

    int min_num_model_pixels_in_sv;
    this->_node_handle.getParam("/shape_reconstruction/sv_min_num_model_pixels", min_num_model_pixels_in_sv);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","sv_min_num_model_pixels = " << min_num_model_pixels_in_sv);

    bool record_videos;
    this->_node_handle.getParam("/shape_reconstruction/record_videos", record_videos);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","record_videos = " << record_videos);

    bool extend_model_to_neighbor_sv;
    this->_node_handle.getParam("/shape_reconstruction/extend_model_to_neighbor_sv", extend_model_to_neighbor_sv);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","extend_model_to_neighbor_sv = " << extend_model_to_neighbor_sv);

    double similarity_in_h;
    this->_node_handle.getParam("/shape_reconstruction/similarity_in_h", similarity_in_h);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","similarity_in_h = " << similarity_in_h);

    double similarity_in_normal;
    this->_node_handle.getParam("/shape_reconstruction/similarity_in_h", similarity_in_normal);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","similarity_in_normal = " << similarity_in_normal);

    bool use_clustered_feats;
    this->_node_handle.getParam("/shape_reconstruction/use_clustered_feats", use_clustered_feats);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","use_clustered_feats = " << use_clustered_feats);

    bool data_from_bag;
    this->_node_handle.getParam("/feature_tracker/data_from_bag", data_from_bag);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","data_from_bag = " << data_from_bag);

    double avg_leaf_size;
    this->_node_handle.getParam("/shape_reconstruction/avg_leaf_size", avg_leaf_size);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","avg_leaf_size = " << avg_leaf_size);

    double ror_radius_search;
    this->_node_handle.getParam("/shape_reconstruction/ror_radius_search", ror_radius_search);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","ror_radius_search = " << ror_radius_search);

    int ror_min_neighbors;
    this->_node_handle.getParam("/shape_reconstruction/ror_min_neighbors", ror_min_neighbors);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","ror_min_neighbors = " << ror_min_neighbors);

    // TODO external parameters for depth_filling and detect_changes_in_color
    omip::ShapeReconstructionPtr shape_ptr = omip::ShapeReconstructionPtr(new omip::ShapeReconstruction());

    shape_ptr->setEstimateSV(_estimate_sv);
    shape_ptr->setSuperVoxelizerPtr(this->_supervoxelizer);
    shape_ptr->setSuperVoxelClustersPtr(this->_supervoxel_clusters);
    shape_ptr->setDetectStaticEnvironment(this->_detect_static_environment);
    shape_ptr->setAccumulateChangeCandidates(accumulate_change_candidates);
    shape_ptr->setMinDepthChange(min_depth_change);
    shape_ptr->setMinColorChange(min_color_change);
    shape_ptr->setRemoveInconsistentPoints(remove_inconsistent_points);
    shape_ptr->setKNNMinRadius(knn_min_radius);
    shape_ptr->setSVMinNumberModelPixels(min_num_model_pixels_in_sv);
    shape_ptr->setExtendToNeighborSV(extend_model_to_neighbor_sv);
    shape_ptr->setSimilarityInH(similarity_in_h);
    shape_ptr->setSimilarityInNormal(similarity_in_normal);
    shape_ptr->setCameraInfo(this->_ci);
    //shape_ptr->setSupportingFeatures(features_of_this_rb);
    shape_ptr->useClusteredFeatures(use_clustered_feats);
    shape_ptr->setDepthFilling(depth_filling);
    shape_ptr->setColorChangesErodeSize(color_changes_erode_size);
    shape_ptr->setToInitial(to_initial);
    shape_ptr->setRecordVideos(record_videos);
    shape_ptr->setLiveStream(!data_from_bag);
    shape_ptr->setApproxVoxelGridLeafSize(avg_leaf_size);
    shape_ptr->setRadiusOutRemovalSearch(ror_radius_search);
    shape_ptr->setRadiusOutRemovalMinNeighbors(ror_min_neighbors);

    return shape_ptr;
}

void ShapeReconstructionNode::measurementCallbackWithClusteredFeatures(const sensor_msgs::PointCloud2ConstPtr &pc_msg,
                                                                       const boost::shared_ptr<omip::rbt_state_t const> &poses_and_vels,
                                                                       const sensor_msgs::PointCloud2ConstPtr &features_pc)
{
    ros::Time ta = ros::Time::now();

    this->_current_pc_down->points.clear();
    this->_current_pc_down->height = IMG_HEIGHT;
    this->_current_pc_down->width = IMG_WIDTH;
    this->_current_pc_down->is_dense = true;

    this->_sv_estimated = false;
    _InitResultRosbag();

    // Extract the time of the upcoming message and estimate the elapsed time since the last message was processed
    _current_measurement_time = pc_msg->header.stamp;
    ros::Duration time_difference = _current_measurement_time - this->_previous_measurement_time;

    if(this->_previous_measurement_time.toSec() != 0.0 )
    {
        ROS_ERROR_STREAM("Time between segmentations: " << time_difference.toSec() << " s");
    }

    // Convert ROS PC message into a pcl point cloud
    pcl::fromROSMsg(*pc_msg, *this->_current_pc);

#ifdef DOWNSAMPLING
    // Downsample the input pcl point cloud
    for(int u=0; u<this->_current_pc->width; u+=2)
    {
        for(int v=0; v<this->_current_pc->height; v+=2)
        {
            this->_current_pc_down->points.push_back(this->_current_pc->at(u, v));
        }
    }
#else
    this->_current_pc_down = this->_current_pc;
#endif

    // Convert clustered features message into a pcl point cloud
    pcl::fromROSMsg(*features_pc, *this->_clustered_feats);

    // Clean the input pcl point cloud of nans
    pcl::removeNaNFromPointCloud<SRPoint>(*this->_current_pc_down,*this->_current_pc_without_nans, this->_not_nan_indices);

    // Iterate over the received rb poses and check if a ShapeReconstructor for each tracked RB already exists
    // If not, create a new ShapeReconstructor and pass the current point cloud as initial point cloud
    // NOTE: This is done no matter how much time passed since the last processed message
    omip::RB_id_t rb_id_temp;
    for(int rb_poses_idx = 1; rb_poses_idx < poses_and_vels->rb_poses_and_vels.size(); rb_poses_idx++)
    {
        rb_id_temp = poses_and_vels->rb_poses_and_vels.at(rb_poses_idx).rb_id;

        if(this->_rb_shapes.find(rb_id_temp) == this->_rb_shapes.end())
        {// If this RB (identified by its id) was not previously tracked, we create a new ShapeReconstruction object for it
            // Unless we reached the max number of rb to reconstruct
            if (_rb_shapes.size() >= _max_rb)
            {
                ROS_WARN_ONCE_NAMED("ShapeReconstructionNode.measurementCallback","Maximum limit of RBs for ShapeReconstructor reached");
                continue;
            }

            omip::ShapeReconstructionPtr shape_ptr = this->_createNewShapeReconstruction();

            shape_ptr->setRBId(rb_id_temp);
            shape_ptr->setInitialFullRGBDPCAndRBT(this->_current_pc_down,
                                                  poses_and_vels->rb_poses_and_vels.at(rb_poses_idx).pose_wc);
            shape_ptr->initialize();

            this->_rb_shapes[rb_id_temp] = shape_ptr;
        }
        else
        {// If this RB (identified by its id) was previously tracked, we already have a ShapeReconstruction object for it
            if(_refine_shapes_in_each_frame)
            {
                this->_rb_shapes[rb_id_temp]->RemoveInconsistentPoints(this->_current_pc_down,
                                                                       poses_and_vels->rb_poses_and_vels.at(rb_poses_idx).pose_wc);
            }

            if(_publish_shapes_in_each_frame)
            {
                this->_rb_shapes[rb_id_temp]->PublishMovedModelAndSegment(_current_measurement_time,
                                                                          poses_and_vels->rb_poses_and_vels.at(rb_poses_idx).pose_wc,
                                                                          this->_video_bag,
                                                                          this->_result_bag_open);
            }
        }

        // Extract all the features that support the rigid body
        pcl::PassThrough<pcl::PointXYZL>::Ptr pt_filter(new pcl::PassThrough<pcl::PointXYZL>());
        pt_filter->setInputCloud(this->_clustered_feats);
        pt_filter->setFilterFieldName("label");
        pcl::PointCloud<pcl::PointXYZL>::Ptr features_of_this_rb(new pcl::PointCloud<pcl::PointXYZL>);

        uint32_t lower_bound = rb_id_temp;
        float lower_bound_float = 0;
        memcpy (&lower_bound_float, &lower_bound, sizeof (float));
        uint32_t upper_bound = rb_id_temp;
        float upper_bound_float = 0;
        memcpy (&upper_bound_float, &upper_bound, sizeof (float));
        pt_filter->setFilterLimits(lower_bound_float, upper_bound_float);
        //std::vector<int> passing_indices;
        pcl::IndicesPtr passing_indices(new std::vector<int>);
        pt_filter->filter(*passing_indices);
        ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.measurementCallbackWithClusteredFeatures",
                              "[RB" << rb_id_temp << "]: " << passing_indices->size() << " of " << this->_clustered_feats->points.size() << " features support this RB.");

        // Extract the features from the clustered features point cloud
        this->_extractor_cf.setInputCloud(this->_clustered_feats);
        this->_extractor_cf.setIndices(passing_indices);
        this->_extractor_cf.filter(*features_of_this_rb);

        // Set the supporting features of this rigid body
        this->_rb_shapes[rb_id_temp]->setSupportingFeatures(features_of_this_rb);
    }

    // Check if the elapsed time since the last processed message is over the processing time interval
    // If not, return and ignore the measurement
    // If yes, process this measurement
    if(time_difference.toSec() < this->_processing_interval && this->_previous_measurement_time.toSec() != 0.0)
    {
        ROS_INFO_STREAM_NAMED("ShapeReconstructionNode::measurementCallback","Not processing this measurement.");
        _AdvanceFeatureTracker();
        return;
    }else{
        ROS_ERROR_STREAM_NAMED("ShapeReconstructionNode::measurementCallback","Processing this measurement.");
    }

    ros::Time te = ros::Time::now();

    // write current point cloud, depth and rgb image
    if (_result_bag_open) {
        this->_result_bag.write(_rgbd_pc_subscriber.getTopic(), _current_measurement_time, *pc_msg);

        // Create a depth map from the current organized depth map
        cv::Mat depth_image_raw(this->_current_pc->height, this->_current_pc->width, CV_32FC1);
        OrganizedPC2DepthMap(this->_current_pc, depth_image_raw);
        //        cv::Mat depth_image(depth_image_raw.rows, depth_image_raw.cols, CV_8UC1);
        //        cv::convertScaleAbs(depth_image_raw, depth_image, 100, 0);
        //        sensor_msgs::ImagePtr depth_msg = cv_bridge::CvImage(std_msgs::Header(), "mono8", depth_image).toImageMsg();
        sensor_msgs::ImagePtr depth_msg;
        DepthImage2CvImage(depth_image_raw, depth_msg);

        this->_result_bag.write("/camera/depth_registered/image_rect", _current_measurement_time, *depth_msg);

        cv::Mat color_image(this->_current_pc->height, this->_current_pc->width, CV_8UC3);
        OrganizedPC2ColorMap(this->_current_pc, color_image);
        sensor_msgs::ImagePtr color_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", color_image).toImageMsg();

        this->_result_bag.write("/camera/rgb/image_rect_color", _current_measurement_time, *color_msg);

    }

    ros::Time te2;
    ros::Time te3;
    // If we process the measurement we pass it to each ShapeReconstructor
    // We create a temp mapping so that any existing RBs get deleted
    std::map<omip::RB_id_t,omip::ShapeReconstructionPtr > rb_shapes_updated;
    for(int rb_poses_idx = 1; rb_poses_idx < poses_and_vels->rb_poses_and_vels.size(); rb_poses_idx++)
    {
        rb_id_temp = poses_and_vels->rb_poses_and_vels.at(rb_poses_idx).rb_id;

        // If this RB (identified by its id) was previously tracked, we already have a ShapeReconstruction object for it
        if(this->_rb_shapes.find(rb_id_temp) != this->_rb_shapes.end())
        {
            rb_shapes_updated[rb_id_temp] = this->_rb_shapes[rb_id_temp];

            if(!this->_sv_estimated && _estimate_sv)
            {
                te2= ros::Time::now();
                this->_EstimateSupervoxels();
                te3= ros::Time::now();
            }

            rb_shapes_updated[rb_id_temp]->setFullRGBDPCandRBT(this->_current_pc_down,
                                                               poses_and_vels->rb_poses_and_vels.at(rb_poses_idx).pose_wc);
            rb_shapes_updated[rb_id_temp]->PublishMovedModelAndSegment(_current_measurement_time,
                                                                       poses_and_vels->rb_poses_and_vels.at(rb_poses_idx).pose_wc,
                                                                       this->_video_bag,
                                                                       this->_result_bag_open);
        }else{
            ROS_WARN_STREAM_NAMED("ShapeReconstructionNode.measurementCallback","There is no ShapeReconstructor associated to this RB id - it was detected before max_rb was reached");
        }
    }
    // Swap the maps (in this way, the entries of the map that were not updated because the RB was lost, get deleted)
    this->_rb_shapes = rb_shapes_updated;

    ros::Time tf = ros::Time::now();

    this->_CollectAndPublishShapeModels();

    // Write the results into bag
    if (_result_bag_open) {
        int rb_poses_idx=1;
        BOOST_FOREACH(shape_reconstructors_map_t::value_type rb_shape_it, this->_rb_shapes)
        {
            rb_shape_it.second->PublishMovedModelAndSegment(_current_measurement_time, poses_and_vels->rb_poses_and_vels.at(rb_poses_idx).pose_wc, _result_bag, _result_bag_open);
            rb_poses_idx++;
        }
    }

    // we have processed the measurement - if the flag
    if (this->_manually_advance_after_processing) {
        std::cout << " Hit enter to advance feature tracker" << std::endl;
        getchar();
    }

    _AdvanceFeatureTracker();

    ros::Time th = ros::Time::now();

//    std::cout << "SV estimation: " << (te3-te2).toSec() << std::endl;
//    std::cout << "Process RGBD frame and publish results (including SV estimation): " << (tf-te).toSec() << std::endl;
//    std::cout << "Total: " << (th-ta).toSec() << std::endl;

    // Update previous time
    this->_previous_measurement_time = _current_measurement_time;
}

void ShapeReconstructionNode::ReadRosBag()
{
}

void ShapeReconstructionNode::_CollectAndPublishShapeModels()
{
    omip_msgs::ShapeModelsPtr shape_models(new omip_msgs::ShapeModels());
    shape_models->header.stamp = this->_current_measurement_time;

    static ros::Publisher* current_pc_repub = NULL;
    if (current_pc_repub == NULL) {
        current_pc_repub = new ros::Publisher;
    }
    *current_pc_repub = this->_node_handle.advertise<sensor_msgs::PointCloud2>("/camera/depth_registered/points", 1);

    // First we publish each shape moved with the latest motion
    BOOST_FOREACH(shape_reconstructors_map_t::value_type rb_shape_it, this->_rb_shapes)
    {
        rb_shape_it.second->getShapeModel(shape_models);

        if (_manually_advance_after_processing) {
            std::cout << "Republishing depth registered points (in order to sync ground truth) [to switch off set manually_advance_after_processing=false" << std::endl;

            SRPointCloud::Ptr current_pc = rb_shape_it.second->getCurrentPointCloud();
            sensor_msgs::PointCloud2 current_pc_msg;
            pcl::toROSMsg(*current_pc, current_pc_msg);
            current_pc_repub->publish(current_pc_msg);
        }
    }

    _shape_models_ext_d_and_c_pub.publish(*shape_models);

    // Then we query all the shapes in the current frame (transformed using the latest motion) and subtract them from the current point cloud to obtaint the static environment
    if(this->_detect_static_environment)
    {

        pcl::PointIndices::Ptr indices_of_moving_bodies_ptr = pcl::PointIndices::Ptr(new pcl::PointIndices());
        ROS_INFO_STREAM_NAMED("ShapeReconstructionNode._PublishShapes",
                              "Subtracting reconstructed shapes (obtained from the extension of both models to supervoxels) from current point cloud to estimate the static environment.");

        this->_set_knn_ptr.reset(new pcl::search::KdTree<omip::SRPoint>());
        this->_set_knn_ptr->setInputCloud(this->_current_pc_without_nans);

        indices_of_moving_bodies_ptr.reset(new pcl::PointIndices());
        BOOST_FOREACH(shape_reconstructors_map_t::value_type rb_shape_it, this->_rb_shapes)
        {
            std::vector<int> empty;
            std::vector<std::vector<int> > indices_of_moving_body;
            std::vector<std::vector<float> > distances_of_moving_body;
            SRPointCloud::Ptr rb_shape_ext_moved_ptr = rb_shape_it.second->getMovedRigidBodyShapeExtDandC();
            this->_set_knn_ptr->radiusSearch(*rb_shape_ext_moved_ptr, empty, 0.02, indices_of_moving_body, distances_of_moving_body, 0);

            for(int idx = 0; idx < indices_of_moving_body.size(); idx++)
            {
                if(indices_of_moving_body.at(idx).size())
                {
                    for(int i = 0; i<indices_of_moving_body.at(idx).size(); i++)
                    {
                        indices_of_moving_bodies_ptr->indices.push_back(indices_of_moving_body.at(idx).at(i));
                    }
                }
            }
        }
        std::set<int> tmp_d_and_c(indices_of_moving_bodies_ptr->indices.begin(), indices_of_moving_bodies_ptr->indices.end());
        indices_of_moving_bodies_ptr->indices.clear();
        std::copy(tmp_d_and_c.begin(), tmp_d_and_c.end(), std::back_inserter(indices_of_moving_bodies_ptr->indices));

        pcl::ExtractIndices<SRPoint> extract_d_and_c;
        this->_static_env_ext_d_and_c_in_current_frame = omip::SRPointCloud::Ptr(new omip::SRPointCloud());
        // Extract the inliers
        extract_d_and_c.setInputCloud (this->_current_pc_without_nans);
        extract_d_and_c.setIndices (indices_of_moving_bodies_ptr);
        extract_d_and_c.setNegative (true);
        extract_d_and_c.filter (*this->_static_env_ext_d_and_c_in_current_frame);

        ROS_INFO_STREAM_NAMED("ShapeReconstructionNode._PublishShapes",
                              "The static environment once subtracted the reconstructed shapes (obtained from the extension of both models to supervoxels) contains "
                              << this->_static_env_ext_d_and_c_in_current_frame->points.size()
                              << " points.");

        sensor_msgs::PointCloud2 static_env_ext_d_and_c_in_current_frame_ros;
        pcl::toROSMsg(*this->_static_env_ext_d_and_c_in_current_frame, static_env_ext_d_and_c_in_current_frame_ros);

        this->_static_environment_ext_d_and_c_pub.publish(static_env_ext_d_and_c_in_current_frame_ros);
    }
}

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
typedef pcl::PointXYZRGBA PointT2;
typedef pcl::PointCloud<PointT2> PointCloudT2;
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointNCloudT;
typedef pcl::PointXYZL PointLT;
typedef pcl::PointCloud<PointLT> PointLCloudT;

void ShapeReconstructionNode::_EstimateSupervoxels()
{
    this->_sv_estimated = true;

    this->_supervoxelizer.reset(new pcl::SupervoxelClustering<SRPoint>(this->_voxel_resolution,this->_seed_resolution, true));
    this->_supervoxelizer->setColorImportance (this->_color_importance);
    this->_supervoxelizer->setSpatialImportance (this->_spatial_importance);
    this->_supervoxelizer->setNormalImportance (this->_normal_importance);

    this->_supervoxelizer->setInputCloud (this->_current_pc_down);

    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode._SegmentPointCloud()", "Estimating supervoxels");
    this->_supervoxelizer->extract (this->_supervoxel_clusters);
    ROS_INFO_STREAM_NAMED("ShapeReconstructionNode._SegmentPointCloud()", "Found " << this->_supervoxel_clusters.size () << " supervoxels.");

    if(this->_visualization_sv)
    {
        boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
        viewer->setBackgroundColor (0, 0, 0);

//        PointCloudT::Ptr voxel_centroid_cloud = this->_supervoxelizer->getVoxelCentroidCloud ();
//        viewer->addPointCloud (voxel_centroid_cloud, "voxel centroids");
//        viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2.0, "voxel centroids");
//        viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.95, "voxel centroids");

        PointCloudT2::Ptr colored_voxel_cloud = this->_supervoxelizer->getColoredVoxelCloud ();
        viewer->addPointCloud (colored_voxel_cloud, "colored voxels");
        viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.8, "colored voxels");

        PointNCloudT::Ptr sv_normal_cloud = this->_supervoxelizer->makeSupervoxelNormalCloud (this->_supervoxel_clusters);
        //We have this disabled so graph is easy to see, uncomment to see supervoxel normals
        viewer->addPointCloudNormals<PointNT> (sv_normal_cloud,1,0.05f, "supervoxel_normals");

        while (!viewer->wasStopped ())
        {
            viewer->spinOnce (100);
        }
    }
}

bool ShapeReconstructionNode::generateMeshes(shape_reconstruction::generate_meshes::Request& request, shape_reconstruction::generate_meshes::Response& response)
{
    // First we generate a triangular mesh for each shape and save them in stl files (done by each ShapeReconstruction object independently)
    BOOST_FOREACH(shape_reconstructors_map_t::value_type rb_shape_it, this->_rb_shapes)
    {
        rb_shape_it.second->generateMesh();
    }

    // Then we generate a mesh of the static environment
    if(this->_detect_static_environment)
    {
        ROS_INFO_STREAM_NAMED("ShapeReconstructionNode.generateMeshes",
                              "Called the generation of 3D triangular mesh from the model of the static environment (based on extension of both models to supervoxels).");
        std::string shape_ext_d_and_c_file_prefix("static_env_ext_d_and_c");
        this->_GenerateMesh(this->_static_env_ext_d_and_c_in_current_frame, shape_ext_d_and_c_file_prefix);
    }
    ROS_INFO_STREAM_NAMED("ShapeReconstruction.generateMesh", "Finished the generation of 3D triangular mesh from the point cloud of the static environment.");

    ROS_INFO_STREAM_NAMED("ShapeReconstruction.generateMesh", "Finished the generation of 3D triangular meshes");

    return true;
}

void ShapeReconstructionNode::_GenerateMesh(const omip::SRPointCloud::Ptr& pc_source, std::string shape_file_prefix)
{
    if(pc_source->points.size())
    {
        // Normal estimation*
        this->_se_tree_for_normal_estimation_ptr->setInputCloud(pc_source);
        this->_se_normal_estimator_ptr->setInputCloud(pc_source);
        this->_se_normal_estimator_ptr->setSearchMethod (this->_se_tree_for_normal_estimation_ptr);
        this->_se_normal_estimator_ptr->compute(*this->_se_estimated_normals_pc_ptr);

        // Concatenate the XYZ and normal fields*
        pcl::concatenateFields (*pc_source, *this->_se_estimated_normals_pc_ptr, *this->_se_position_color_and_normals_pc_ptr);
        this->_se_tree_for_triangulation_ptr->setInputCloud (this->_se_position_color_and_normals_pc_ptr);

        // Get result
        this->_se_greedy_projection_triangulator_ptr->setInputCloud(this->_se_position_color_and_normals_pc_ptr);
        this->_se_greedy_projection_triangulator_ptr->setSearchMethod(this->_se_tree_for_triangulation_ptr);
        this->_se_greedy_projection_triangulator_ptr->reconstruct(*this->_se_triangulated_mesh_ptr);

        // Additional vertex information
        //std::vector<int> parts = gp3.getPartIDs();
        //std::vector<int> states = gp3.getPointStates();

        std::string mesh_path = ros::package::getPath("shape_reconstruction") + std::string("/meshes/");
        std::stringstream se_name_ss;
        se_name_ss << shape_file_prefix << ".stl";

        std::string se_mesh_full_file_name = mesh_path + se_name_ss.str();

        pcl::io::mesh2vtk(*this->_se_triangulated_mesh_ptr, this->_se_polygon_data_ptr);

        this->_se_polygon_writer_ptr->SetInputData (this->_se_polygon_data_ptr);
        this->_se_polygon_writer_ptr->SetFileName (se_mesh_full_file_name.c_str ());
        this->_se_polygon_writer_ptr->Write ();

        ROS_INFO_STREAM_NAMED("ShapeReconstruction.generateMesh", "Resulting triangular mesh written to " << se_mesh_full_file_name);
    }else{
        ROS_WARN_STREAM_NAMED("ShapeReconstruction.generateMesh", "Impossible to generate a triangular mesh for this model, it doesn't contain any points!");
    }
}

void ShapeReconstructionNode::_InitResultRosbag() {
    if (!_record_result_bag)
        return;

    if (_result_bag_open)
        return;

    using namespace boost::filesystem;

    // prepare path
    path result_bag_out(_result_bag_path);
    if (!boost::filesystem::exists(result_bag_out)) {
        if (!boost::filesystem::create_directories(result_bag_out)) {
            ROS_ERROR_NAMED("ShapeReconstruction.InitResultRosbag", "Output directory for result bag does not exist and cannot be created!");
            return;
        }
    }

    // get input rosbag name
    std::string input_bag_name;
    if (!_node_handle.getParam("/feature_tracker/bag_file", input_bag_name)){
        ROS_ERROR_NAMED("ShapeReconstruction.InitResultRosbag", "Cannot infer name of input bag file from feature tracker");
        return;
    }
    path input_bag_path(input_bag_name);

    // construct output rosbag name
    std::stringstream result_bag_name;
    time_t t = time(0);   // get time now
    struct tm * now = localtime( & t );

    result_bag_name << input_bag_path.stem().string() << "_";
    result_bag_name << (now->tm_mon + 1) << "-" << now->tm_mday << "_" << now->tm_hour << "-" << now->tm_min << "-" << now->tm_sec;

    result_bag_out /= path(result_bag_name.str() + ".bag");

    ROS_INFO_STREAM_NAMED("ShapeReconstruction.InitResultRosbag", "Opening result rosbag " << result_bag_out.string());
    this->_result_bag.open(result_bag_out.string(), rosbag::bagmode::Write);

    this->_result_bag_open=true;

    // construct full video rosbag
    // construct output rosbag name
    std::stringstream video_bag_name;

    video_bag_name << input_bag_path.stem().string() << "_";
    video_bag_name << (now->tm_mon + 1) << "-" << now->tm_mday << "_" << now->tm_hour << "-" << now->tm_min << "-" << now->tm_sec;
    video_bag_name << "_VIDEO.bag";

    path video_bag_out(_result_bag_path);
    video_bag_out /= path(video_bag_name.str());

    this->_video_bag.open(video_bag_out.string(), rosbag::bagmode::Write);

    std::string cfg_src_path = ros::package::getPath("shape_reconstruction") + std::string("/cfg/shape_reconstruction_cfg.yaml");
    // copy shape reconstruction cfg
    path cfg_dest_path = _result_bag_path;
    cfg_dest_path /= result_bag_name.str() + ".yaml";

    std::ifstream ifs(cfg_src_path.c_str());
    std::ofstream ofs(cfg_dest_path.string().c_str());
    ofs << ifs.rdbuf();

}

void ShapeReconstructionNode::_AdvanceFeatureTracker() {
//    ros::Duration d(0.09);
//    d.sleep(); // sleep otherwise ROS might lose this message

    ROS_DEBUG_STREAM_NAMED("ShapeReconstruction._AdvanceFeatureTracker", "Advancing Feature Tracker");
    this->_advance_feature_tracker_pub.publish(std_msgs::Bool());
}

void ShapeReconstructionNode::TrackerQuitCallback(const std_msgs::EmptyConstPtr &empty)
{
    ROS_INFO_STREAM_NAMED("ShapeReconstruction.TrackerQuitCallback", "Shape Reconstruction quit stopping!");
    _active = false;
}

// Main program
int main(int argc, char** argv)
{
    ros::init(argc, argv, "ShapeReconstructionNode");
    ShapeReconstructionNode sr_node;

    ros::Rate r(10); // 10 hz
    while (ros::ok() && sr_node.isActive())
    {
        ros::spinOnce();
        r.sleep();
    }

    ROS_ERROR("Shutting down ShapeReconstructionNode" );

    return (0);
}

max_stars_repo_name: tu-rbo/omip

max_issues_repo_name: tu-rbo/omip

max_forks_repo_name: tu-rbo/omip


text:
#ifndef BOOST_STATECHART_STATE_HPP_INCLUDED
#define BOOST_STATECHART_STATE_HPP_INCLUDED
//////////////////////////////////////////////////////////////////////////////
// Copyright 2002-2006 Andreas Huber Doenni
// Distributed under the Boost Software License, Version 1.0. (See accompany-
// ing file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//////////////////////////////////////////////////////////////////////////////

#include <boost/statechart/simple_state.hpp>

#include <boost/mpl/list.hpp>

namespace boost
{
namespace statechart
{

template< class MostDerived,
          class Context,
          class InnerInitial = mpl::list<>,
          history_mode historyMode = has_no_history >
class state : public simple_state<
  MostDerived, Context, InnerInitial, historyMode >
{
  typedef simple_state< MostDerived, Context, InnerInitial, historyMode >
    base_type;

  protected:
    //////////////////////////////////////////////////////////////////////////
    struct my_context
    {
      my_context( typename base_type::context_ptr_type pContext ) :
        pContext_( pContext )
      {
      }

      typename base_type::context_ptr_type pContext_;
    };

    typedef state my_base;

    state( my_context ctx )
    {
      this->set_context( ctx.pContext_ );
    }

    ~state() {}

  public:
    //////////////////////////////////////////////////////////////////////////
    // The following declarations should be private.
    // They are only public because many compilers lack template friends.
    //////////////////////////////////////////////////////////////////////////
    // See base class for documentation
    typedef typename base_type::outermost_context_base_type
      outermost_context_base_type;
    typedef typename base_type::inner_context_ptr_type inner_context_ptr_type;
    typedef typename base_type::context_ptr_type context_ptr_type;
    typedef typename base_type::inner_initial_list inner_initial_list;

    static void initial_deep_construct(
      outermost_context_base_type & outermostContextBase )
    {
      deep_construct( &outermostContextBase, outermostContextBase );
    }

    // See base class for documentation
    static void deep_construct(
      const context_ptr_type & pContext,
      outermost_context_base_type & outermostContextBase )
    {
      const inner_context_ptr_type pInnerContext(
        shallow_construct( pContext, outermostContextBase ) );
      base_type::template deep_construct_inner< inner_initial_list >(
        pInnerContext, outermostContextBase );
    }

    static inner_context_ptr_type shallow_construct(
      const context_ptr_type & pContext,
      outermost_context_base_type & outermostContextBase )
    {
      const inner_context_ptr_type pInnerContext(
        new MostDerived( my_context( pContext ) ) );
      outermostContextBase.add( pInnerContext );
      return pInnerContext;
    }
};

} // namespace statechart
} // namespace boost

#endif

ext: hpp

max_stars_repo_name: randolphwong/mcsema

max_issues_repo_name: randolphwong/mcsema

max_forks_repo_name: randolphwong/mcsema


text:
//******************************************************************
//
// Copyright 2016 Intel Corporation All Rights Reserved.
//
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=

#include <boost/algorithm/string/join.hpp>
#include <boost/tokenizer.hpp>
#include <iostream>
#include <poll.h>
#include <sstream>

#include "cJSON.h"
#include "cacommon.h"
#include "cbor.h"
#include "cborjson.h"
#include "coap/pdu.h"
#include "ocpayload.h"
#include "ocpayloadcbor.h"
#include "ocstack.h"
#include "payload_logging.h"
#if __WITH_DTLS__
#include "pinoxmcommon.h"
#endif

static CborError ConvertJSONToCBOR(CborEncoder *encoder, cJSON *json)
{
    CborEncoder containerEncoder;
    CborError err = CborNoError;

    if (json->string)
    {
        err = cbor_encode_text_stringz(encoder, json->string);
    }

    switch (json->type)
    {
        case cJSON_False:
            err = cbor_encode_boolean(encoder, false);
            break;
        case cJSON_True:
            err = cbor_encode_boolean(encoder, true);
            break;
        case cJSON_NULL:
            err = cbor_encode_null(encoder);
            break;
        case cJSON_Number:
            err = cbor_encode_double(encoder, json->valuedouble);
            break;
        case cJSON_String:
            err = cbor_encode_text_stringz(encoder, json->valuestring);
            break;
        case cJSON_Array:
            err = cbor_encoder_create_array(encoder, &containerEncoder, cJSON_GetArraySize(json));
            for (cJSON *child = json->child; err == CborNoError && child; child = child->next)
            {
                err = ConvertJSONToCBOR(&containerEncoder, child);
            }
            if (err == CborNoError)
            {
                err = cbor_encoder_close_container(encoder, &containerEncoder);
            }
            break;
        case cJSON_Object:
            err = cbor_encoder_create_map(encoder, &containerEncoder, cJSON_GetArraySize(json));
            for (cJSON *child = json->child; err == CborNoError && child; child = child->next)
            {
                err = ConvertJSONToCBOR(&containerEncoder, child);
            }
            if (err == CborNoError)
            {
                err = cbor_encoder_close_container(encoder, &containerEncoder);
            }
            break;
    }
    return err;
}

static size_t ConvertJSONToCBOR(const char *jsonStr, uint8_t *buffer, size_t size)
{
    CborEncoder encoder;
    CborError err = CborNoError;

    cbor_encoder_init(&encoder, buffer, size, 0);
    cJSON *json = cJSON_Parse(jsonStr);
    for (; err == CborNoError && json; json = json->next)
    {
        err = ConvertJSONToCBOR(&encoder, json);
    }
    if (json)
    {
        cJSON_Delete(json);
    }
    if (err == CborNoError)
    {
        return cbor_encoder_get_buffer_size(&encoder, buffer);
    }
    else
    {
        return 0;
    }
}

static void LogResponse(OCMethod method, OCClientResponse *response, OCPayloadFormat format)
{
    if (!response)
    {
        return;
    }

    switch (method)
    {
        case OC_REST_DISCOVER:
        case OC_REST_GET:
        case OC_REST_OBSERVE:
            std::cout << "GET";
            break;
        case OC_REST_POST:
            std::cout << "POST";
            break;
        default:
            std::cout << method;
            break;
    }
    std::cout << " - " << response->result << std::endl;

    std::cout << "Uri-Host: " << response->devAddr.addr << std::endl
              << "Uri-Port: " << response->devAddr.port << std::endl
              << "Uri-Path: " << (response->resourceUri ? response->resourceUri : "") << std::endl;
    if (method & OC_REST_OBSERVE)
    {
        std::cout << "Observe: " << response->sequenceNumber << std::endl;
    }

    uint8_t *buffer = NULL;
    size_t size;
    OCStackResult result = OCConvertPayload(response->payload, format, &buffer, &size);
    if (result != OC_STACK_OK)
    {
        return;
    }
    CborParser parser;
    CborValue it;
    CborError err = cbor_parser_init(buffer, size, CborConvertDefaultFlags, &parser, &it);
    char value[32768];
    FILE *fp = fmemopen(value, 32768, "w");
    while (err == CborNoError)
    {
        err = cbor_value_to_json_advance(fp, &it, CborConvertDefaultFlags);
    }
    fclose(fp);
    cJSON *json = cJSON_Parse(value);
    if (json)
    {
        char *valueStr = cJSON_Print(json);
        std::cout << valueStr << std::endl;
        free(valueStr);
        cJSON_Delete(json);
    }
    else
    {
        std::cout << value << std::endl;
    }
    OICFree(buffer);
}

struct Resource
{
    std::string di;
    OCDevAddr devAddr;
    std::string uri;
};
static std::vector<Resource> g_resources;

static OCStackApplicationResult DiscoverCB(void *context, OCDoHandle,
        OCClientResponse *response)
{
    uint16_t format = (uint16_t)(uintptr_t)context;
    switch (format)
    {
        case COAP_MEDIATYPE_APPLICATION_VND_OCF_CBOR:
            LogResponse(OC_REST_DISCOVER, response, OC_FORMAT_VND_OCF_CBOR);
            break;
        case COAP_MEDIATYPE_APPLICATION_CBOR:
            LogResponse(OC_REST_DISCOVER, response, OC_FORMAT_CBOR);
            break;
    }
    if (response && response->payload && (response->payload->type == PAYLOAD_TYPE_DISCOVERY))
    {
        OCDiscoveryPayload *payload = (OCDiscoveryPayload *) response->payload;
        while (payload)
        {
            OCResourcePayload *resource = (OCResourcePayload *) payload->resources;
            while (resource)
            {
                Resource r;
                r.di = payload->sid;
                if (resource->eps)
                {
                    OCEndpointPayload *ep = resource->eps;
                    if (!strcmp(ep->tps, "coap") || !strcmp(ep->tps, "coaps"))
                    {
                        r.devAddr.adapter = OC_ADAPTER_IP;
                    }
                    else if (!strcmp(ep->tps, "coap+tcp") || !strcmp(ep->tps, "coaps+tcp"))
                    {
                        r.devAddr.adapter = OC_ADAPTER_TCP;
                    }
                    r.devAddr.flags = ep->family;
                    r.devAddr.port = ep->port;
                    strncpy(r.devAddr.addr, ep->addr, MAX_ADDR_STR_SIZE);
                    r.devAddr.ifindex = 0;
                    r.devAddr.routeData[0] = '\0';
                    strncpy(r.devAddr.remoteId, payload->sid, MAX_IDENTITY_SIZE);
                }
                else
                {
                    r.devAddr = response->devAddr;
                    if (resource->secure)
                    {
                        r.devAddr.flags = (OCTransportFlags) (r.devAddr.flags | OC_FLAG_SECURE);
                        if (response->devAddr.adapter == OC_ADAPTER_IP)
                        {
                            r.devAddr.port = resource->port;
                        }
#ifdef TCP_ADAPTER
                        else if (response->devAddr.adapter == OC_ADAPTER_TCP)
                        {
                            r.devAddr.port = resource->tcpPort;
                        }
#endif
                    }
                }
                if (!strncmp(resource->uri, "ocf://", 6))
                {
                    r.uri = strchr(resource->uri + 6, '/');
                }
                else
                {
                    r.uri = resource->uri;
                }
                g_resources.push_back(r);
                resource = resource->next;
            }
            payload = payload->next;
        }
    }
    return OC_STACK_KEEP_TRANSACTION;
}

static OCStackApplicationResult GetCB(void *, OCDoHandle,
        OCClientResponse *response)
{
    LogResponse(OC_REST_GET, response, OC_FORMAT_CBOR);
    return OC_STACK_DELETE_TRANSACTION;
}

static OCStackApplicationResult PostCB(void *, OCDoHandle,
        OCClientResponse *response)
{
    LogResponse(OC_REST_POST, response, OC_FORMAT_CBOR);
    return OC_STACK_DELETE_TRANSACTION;
}

static OCStackApplicationResult ObserveCB(void *, OCDoHandle,
        OCClientResponse *response)
{
    LogResponse(OC_REST_OBSERVE, response, OC_FORMAT_CBOR);
    if (response->payload)
    {
        return OC_STACK_KEEP_TRANSACTION;
    }
    else
    {
        return OC_STACK_DELETE_TRANSACTION;
    }
}

static void usage()
{
    std::cout << "Usage:" << std::endl
              << std::endl
              << "Find smart home bridge devices:" << std::endl
              << "  find rt=oic.d.bridge" << std::endl
              << "Find light devices:" << std::endl
              << "  find rt=oic.d.light" << std::endl
              << "Find all devices:" << std::endl
              << "  find" << std::endl
              << std::endl
              << "List all found devices:" << std::endl
              << "  list" << std::endl
              << std::endl
              << "Get attributes of a resource:" << std::endl
              << "  get INDEX [QUERY PARAM]..." << std::endl
              << std::endl
              << "Post attributes of a resource:" << std::endl
              << "  post INDEX [QUERY PARAM]... [JSON]" << std::endl
              << std::endl
              << "Observe attributes of a resource:" << std::endl
              << "  observe INDEX [QUERY PARAM]" << std::endl;
}

static FILE *PSOpenCB(const char *suffix, const char *mode)
{
    std::string path = std::string("occlient_") + suffix;
    return fopen(path.c_str(), mode);
}

#if __WITH_DTLS__
static void DisplayPinCB(char *pin, size_t pinSize, void *context)
{
    OC_UNUSED(pinSize);
    OC_UNUSED(context);
    std::cout << "DisplayPinCB(pin=" << pin << ",...)" << std::endl;
}

static void ClosePinDisplayCB()
{
    std::cout << "ClosePinDisplayCB()" << std::endl;
}
#endif

int main(int, char **)
{
    std::cout.setf(std::ios::boolalpha);

    OCPersistentStorage ps = { PSOpenCB, fread, fwrite, fclose, unlink };
    if (OCRegisterPersistentStorageHandler(&ps) != OC_STACK_OK)
    {
        std::cerr << "OCRegisterPersistentStorageHandler error" << std::endl;
        return EXIT_FAILURE;
    }
#if __WITH_DTLS__
    if (SetDisplayPinWithContextCB(DisplayPinCB, NULL) != OC_STACK_OK)
    {
        std::cerr << "SetDisplayPinWithContextCB error" << std::endl;
        return EXIT_FAILURE;
    }
    SetClosePinDisplayCB(ClosePinDisplayCB);
    if (SetRandomPinPolicy(OXM_RANDOM_PIN_DEFAULT_SIZE, (OicSecPinType_t) OXM_RANDOM_PIN_DEFAULT_PIN_TYPE) != OC_STACK_OK)
    {
        std::cerr << "SetRandomPinPolicy error" << std::endl;
        return EXIT_FAILURE;
    }
#endif
    if (OCInit1(OC_CLIENT_SERVER, OC_DEFAULT_FLAGS, OC_DEFAULT_FLAGS) != OC_STACK_OK)
    {
        std::cerr << "OCStack init error" << std::endl;
        return EXIT_FAILURE;
    }
    std::cout << "di=" << OCGetServerInstanceIDString() << std::endl;

    if (OCSetPropertyValue(PAYLOAD_TYPE_DEVICE, OC_RSRVD_DEVICE_NAME, "occlient") != OC_STACK_OK)
    {
        std::cerr << "OCSetPropertyValue error" << std::endl;
        return EXIT_FAILURE;
    }
    if (OCSetPropertyValue(PAYLOAD_TYPE_DEVICE, OC_RSRVD_SPEC_VERSION, OC_SPEC_VERSION) != OC_STACK_OK)
    {
        std::cerr << "OCSetPropertyValue error" << std::endl;
        return EXIT_FAILURE;
    }
    if (OCSetPropertyValue(PAYLOAD_TYPE_DEVICE, OC_RSRVD_DATA_MODEL_VERSION, "ocf.res.1.1.0") != OC_STACK_OK)
    {
        std::cerr << "OCSetPropertyValue error" << std::endl;
        return EXIT_FAILURE;
    }
    if (OCSetPropertyValue(PAYLOAD_TYPE_PLATFORM, OC_RSRVD_MFG_NAME, "IoTivity") != OC_STACK_OK)
    {
        std::cerr << "OCSetPropertyValue error" << std::endl;
        return EXIT_FAILURE;
    }

    for (;;)
    {
        struct pollfd pfd = { STDIN_FILENO, POLLIN, 0 };
        int ret = poll(&pfd, 1, 1);
        if (ret != 1)
        {
            if (OCProcess() != OC_STACK_OK)
            {
                std::cerr << "OCStack process error" << std::endl;
                return 0;
            }
            continue;
        }
        std::string line;
        std::getline(std::cin, line);
        boost::char_separator<char> sep(" ");
        boost::tokenizer<boost::char_separator<char>> tokens(line, sep);
        auto token = tokens.begin();
        if (token == tokens.end())
        {
            usage();
            continue;
        }
        std::string cmd = *token++;
        if (cmd == "list")
        {
            for (size_t i = 0; i < g_resources.size(); ++i)
            {
                std::cout << "[" << i << "] "
                          << g_resources[i].di << " "
                          << g_resources[i].uri << std::endl;
            }
        }
        else if (cmd == "find")
        {
            uint16_t optionID = CA_OPTION_ACCEPT;
            uint16_t format = COAP_MEDIATYPE_APPLICATION_VND_OCF_CBOR;
            std::string uri = "/oic/res";
            std::string query;
            while (token != tokens.end())
            {
                std::string str = *token++;
                if (str == "Accept:")
                {
                    str = *token++;
                    if (str == "application/cbor")
                    {
                        format = COAP_MEDIATYPE_APPLICATION_CBOR;
                    }
                }
                else if (str[0] == '/')
                {
                    uri = str;
                }
                else
                {
                    if (!query.empty())
                    {
                        query += ";";
                    }
                    query += str;
                }
            }
            if (!query.empty())
            {
                uri += "?" + query;
            }
            OCCallbackData cbData;
            cbData.cb = DiscoverCB;
            cbData.context = (void *)(uintptr_t)format;
            cbData.cd = NULL;
            OCHeaderOption options[1];
            size_t numOptions = 0;
            OCSetHeaderOption(options, &numOptions, optionID, &format, sizeof(format));
            OCStackResult result = OCDoResource(NULL, OC_REST_DISCOVER, uri.c_str(), NULL, 0,
                    CT_DEFAULT, OC_HIGH_QOS, &cbData, options, numOptions);
            std::cout << "find " << uri << " - " << result << std::endl;
        }
        else if (cmd == "get")
        {
            uint16_t optionID = CA_OPTION_ACCEPT;
            uint16_t format = COAP_MEDIATYPE_APPLICATION_CBOR;
            std::string uri;
            OCDevAddr *devAddr = NULL;
            try
            {
                size_t i = std::stoi(*token);
                uri = g_resources[i].uri;
                devAddr = &g_resources[i].devAddr;
            }
            catch (std::exception ex)
            {
                uri = *token;
            }
            ++token;
            std::string query;
            while (token != tokens.end())
            {
                std::string str = *token++;
                if (str == "Accept:")
                {
                    str = *token++;
                    if (str == "application/vnd.ocf+cbor")
                    {
                        format = COAP_MEDIATYPE_APPLICATION_VND_OCF_CBOR;
                    }
                }
                else
                {
                    if (!query.empty())
                    {
                        query += ";";
                    }
                    query += str;
                }
            }
            if (!query.empty())
            {
                uri += "?" + query;
            }
            OCCallbackData cbData;
            cbData.cb = GetCB;
            cbData.context = NULL;
            cbData.cd = NULL;
            OCHeaderOption options[1];
            size_t numOptions = 0;
            OCSetHeaderOption(options, &numOptions, optionID, &format, sizeof(format));
            OCStackResult result = OCDoResource(NULL, OC_REST_GET, uri.c_str(), devAddr,
                    NULL, CT_DEFAULT, OC_HIGH_QOS, &cbData, options, numOptions);
            std::cout << "get " << uri << " - " << result << std::endl;
        }
        else if (cmd == "put")
        {
            // TODO
        }
        else if (cmd == "post")
        {
            size_t i = std::stoi(*token++);
            std::string query, body;
            while (token != tokens.end())
            {
                std::string str = *token++;
                if (str.find('=') != std::string::npos)
                {
                    if (!query.empty())
                    {
                        query += ";";
                    }
                    query += str;
                }
                else
                {
                    body += str;
                }
            }
            std::string uri = g_resources[i].uri;
            if (!query.empty())
            {
                uri += "?" + query;
            }
            OCRepPayload *payload = NULL;
            size_t size = 32768;
            uint8_t buffer[32768];
            size = ConvertJSONToCBOR(body.c_str(), buffer, size);
            if (size)
            {
                payload = OCRepPayloadCreate();
                OCParsePayload((OCPayload **) &payload, OC_FORMAT_CBOR, PAYLOAD_TYPE_REPRESENTATION,
                        buffer, size);
            }
            OCDevAddr *devAddr = &g_resources[i].devAddr;
            OCCallbackData cbData;
            cbData.cb = PostCB;
            cbData.context = NULL;
            cbData.cd = NULL;
            OCStackResult result = OCDoResource(NULL, OC_REST_POST, uri.c_str(), devAddr,
                    (OCPayload *) payload, CT_DEFAULT, OC_HIGH_QOS, &cbData, NULL, 0);
            std::cout << "post " << g_resources[i].uri << " - " << result << std::endl;
        }
        else if (cmd == "observe")
        {
            uint16_t optionID = CA_OPTION_ACCEPT;
            uint16_t format = COAP_MEDIATYPE_APPLICATION_CBOR;
            size_t i = std::stoi(*token++);
            std::string query;
            while (token != tokens.end())
            {
                std::string str = *token++;
                if (str == "Accept:")
                {
                    str = *token++;
                    if (str == "application/vnd.ocf+cbor")
                    {
                        format = COAP_MEDIATYPE_APPLICATION_VND_OCF_CBOR;
                    }
                }
                else
                {
                    if (!query.empty())
                    {
                        query += ";";
                    }
                    query += str;
                }
            }
            std::string uri = g_resources[i].uri;
            if (!query.empty())
            {
                uri += "?" + query;
            }
            OCDevAddr *devAddr = &g_resources[i].devAddr;
            OCDoHandle handle;
            OCCallbackData cbData;
            cbData.cb = ObserveCB;
            cbData.context = (void *)(uintptr_t)format;
            cbData.cd = NULL;
            OCHeaderOption options[1];
            size_t numOptions = 0;
            OCSetHeaderOption(options, &numOptions, optionID, &format, sizeof(format));
            OCStackResult result = OCDoResource(&handle, OC_REST_OBSERVE, uri.c_str(), devAddr,
                    NULL, CT_DEFAULT, OC_HIGH_QOS, &cbData, options, numOptions);
            std::cout << "observe " << g_resources[i].uri << " (" << handle << ") - " << result
                      << std::endl;
        }
        else if (cmd == "cancel")
        {
            OCDoHandle handle = (OCDoHandle) (intptr_t) std::stoi(*token++, 0, 0);
            OCStackResult result = OCCancel(handle, OC_HIGH_QOS, NULL, 0);
            std::cout << "cancel (" << handle << ") - " << result << std::endl;
        }
        else if (cmd == "delete")
        {
            // TODO
        }
        else if (cmd == "quit")
        {
            break;
        }
    }

    if (OCStop() != OC_STACK_OK)
    {
        std::cerr << "OCStack stop error" << std::endl;
    }

    return EXIT_SUCCESS;
}

ext: cpp

max_stars_repo_name: iotivity/iotivity-alljoyn-bridge

max_issues_repo_name: iotivity/iotivity-alljoyn-bridge

max_forks_repo_name: iotivity/iotivity-alljoyn-bridge


text:
#ifndef SPACEY_NEURON_INPUT_HPP
#define SPACEY_NEURON_INPUT_HPP

#include <boost/functional/hash.hpp>
#include <neat/src/Weight.hpp>

namespace spacey::neat {

class Neuron;
struct NeuronInput {
    Neuron* input;
    Weight weight;
};

inline auto operator==(const NeuronInput first, const NeuronInput second) -> bool {
    return first.input == second.input && first.weight == second.weight;
}

inline auto operator<<(std::ostream& os, const NeuronInput input) -> std::ostream& {
    os << "{ " << input.input << ", " << input.weight << "}" << std::endl;
    return os;
}

}

#endif //SPACEY_NEURON_INPUT_HPP

max_stars_repo_name: saleph/spacey

max_issues_repo_name: saleph/spacey

max_forks_repo_name: saleph/spacey


text:
#ifndef BURST__ALGORITHM__DETAIL__PARALLEL_BY_CHUNKS_HPP
#define BURST__ALGORITHM__DETAIL__PARALLEL_BY_CHUNKS_HPP

#include <burst/type_traits/iterator_difference.hpp>

#include <boost/asio/post.hpp>
#include <boost/asio/thread_pool.hpp>

#include <algorithm>
#include <cstddef>
#include <future>
#include <iterator>
#include <type_traits>
#include <utility>
#include <vector>

namespace burst
{
    namespace detail
    {
        template <typename Executor, typename NullaryFunction>
        auto post (Executor & e, NullaryFunction && f)
        {
            using return_type = std::result_of_t<NullaryFunction()>;
            auto task = std::packaged_task<return_type ()>(std::forward<NullaryFunction>(f));
            auto future = task.get_future();

            boost::asio::post(e, std::move(task));

            return future;
        }

        // TODO: https://github.com/izvolov/burst/issues/150
        template <typename RandomAccessIterator, typename TernaryFunction>
        iterator_difference_t<RandomAccessIterator>
            parallel_by_chunks
            (
                boost::asio::thread_pool & pool,
                iterator_difference_t<RandomAccessIterator> chunk_size,
                RandomAccessIterator first,
                RandomAccessIterator last,
                TernaryFunction process_chunk
            )
        {
            std::vector<std::future<void>> results;

            auto chunk_count = iterator_difference_t<RandomAccessIterator>{0};
            while (first != last)
            {
                using std::distance;
                auto chunk_begin = first;
                auto chunk_end =
                    std::next(chunk_begin, std::min(distance(chunk_begin, last), chunk_size));

                results.push_back(detail::post(pool,
                    [process_chunk, chunk_count, chunk_begin, chunk_end]
                    {
                        process_chunk(chunk_count, chunk_begin, chunk_end);
                    }));

                first = chunk_end;
                ++chunk_count;
            }

            std::for_each(results.begin(), results.end(), [] (auto & r) {r.get();});

            return chunk_count;
        }
    } // namespace detail
} // namespace burst

#endif // BURST__ALGORITHM__DETAIL__PARALLEL_BY_CHUNKS_HPP

max_stars_repo_name: izvolov/thrust

max_issues_repo_name: izvolov/burst

max_forks_repo_name: izvolov/thrust


text:
/*=============================================================================
    Copyright (c) 2001-2011 Joel de Guzman

    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)
==============================================================================*/
#if !defined(FUSION_REVERSE_VIEW_07202005_0836)
#define FUSION_REVERSE_VIEW_07202005_0836

#include <boost/fusion/sequence/intrinsic/begin.hpp>
#include <boost/fusion/sequence/intrinsic/end.hpp>
#include <boost/fusion/sequence/intrinsic/size.hpp>
#include <boost/fusion/support/category_of.hpp>
#include <boost/fusion/support/config.hpp>
#include <boost/fusion/support/detail/access.hpp>
#include <boost/fusion/support/is_view.hpp>
#include <boost/fusion/support/sequence_base.hpp>
#include <boost/fusion/view/reverse_view/detail/at_impl.hpp>
#include <boost/fusion/view/reverse_view/detail/begin_impl.hpp>
#include <boost/fusion/view/reverse_view/detail/end_impl.hpp>
#include <boost/fusion/view/reverse_view/detail/value_at_impl.hpp>
#include <boost/fusion/view/reverse_view/reverse_view_iterator.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/identity.hpp>
#include <boost/mpl/inherit.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_base_of.hpp>

namespace boost {
namespace fusion {
struct reverse_view_tag;
struct fusion_sequence_tag;

template <typename Sequence>
struct reverse_view : sequence_base<reverse_view<Sequence>> {
  typedef reverse_view_tag fusion_tag;
  typedef fusion_sequence_tag tag; // this gets picked up by MPL
  typedef mpl::true_ is_view;

  typedef Sequence seq_type;
  typedef typename traits::category_of<Sequence>::type category;
  typedef typename result_of::begin<Sequence>::type first_type;
  typedef typename result_of::end<Sequence>::type last_type;
  typedef typename result_of::size<Sequence>::type size;

  BOOST_STATIC_ASSERT(
      (is_base_of<bidirectional_traversal_tag,
                  typename traits::category_of<first_type>::type>::value));

  BOOST_CONSTEXPR BOOST_FUSION_GPU_ENABLED reverse_view(Sequence &in_seq)
      : seq(in_seq) {}

  BOOST_CONSTEXPR BOOST_FUSION_GPU_ENABLED first_type first() const {
    return fusion::begin(seq);
  }
  BOOST_CONSTEXPR BOOST_FUSION_GPU_ENABLED last_type last() const {
    return fusion::end(seq);
  }
  typename mpl::if_<traits::is_view<Sequence>, Sequence, Sequence &>::type seq;

private:
  // silence MSVC warning C4512: assignment operator could not be generated
  reverse_view &operator=(reverse_view const &);
};
} // namespace fusion
} // namespace boost

#endif

max_stars_repo_name: henrywarhurst/matrix

max_issues_repo_name: henrywarhurst/matrix

max_forks_repo_name: henrywarhurst/matrix


text:
////////////////////////////////////////////////////////////////////////////////
// This source code is part of RokLegend project.
//         (Ragnarok Online Client)
//     Copyright (c) RokLegend Team, 2013
////////////////////////////////////////////////////////////////////////////////

#include "RoCacheStore.h"
#include <core/RoFileSystem.h>
#include <core/RoStringUtil.h>
#include <boost/filesystem.hpp>

#if roARCHITECTURE_IS_64
#   define roCACHE_ARCH_SUFFIX ".x64"
#else
#   define roCACHE_ARCH_SUFFIX ".x86"
#endif // roARCHITECTURE_IS_64
//------------------------------------------------------------------------------
bool roCacheStoreExists(RoString fileName)
{
    return RoFileSystem::FileExists(roCacheStoreFilePath(fileName).asWStr());
}
//------------------------------------------------------------------------------
RoString roCacheStoreFilePath(RoString fileName)
{
    const RoPath fileNamePath{ fileName };
    auto extension = fileNamePath.extension();
    auto basename = fileNamePath.stem();
    fileName = RoString{ basename.native() + roCACHE_ARCH_SUFFIX + extension.native() };
    return roCacheStoreFileNameForFile(RoStringUtil::Empty, fileName);
}
//------------------------------------------------------------------------------
RoPath roCacheStoreFilePathForFile(const RoString& cacheDir, const RoString& fileName)
{
    const RoPath gameDir = RoFileSystem::GetGameDirectory();
    const RoPath filePath = RoPath(fileName);
    if (cacheDir.empty()) {
        return gameDir / filePath;
    }
    return gameDir / RoPath(cacheDir) / filePath;
}
//------------------------------------------------------------------------------
RoString roCacheStoreFileNameForFile(const RoString& cacheDir, const RoString& fileName)
{
    return RoString(roCacheStoreFilePathForFile(cacheDir, fileName).wstring());
}
//------------------------------------------------------------------------------

max_stars_repo_name: ViteFalcon/RokLegend

max_issues_repo_name: ViteFalcon/RokLegend

max_forks_repo_name: ViteFalcon/RokLegend


text:
#include <utility>
#include <boost/hana/tuple.hpp>
#include <boost/hana/functional/arg.hpp>
#include <boost/hana/equal.hpp>

// for print result
#include <iostream>
#include <boost/hana/for_each.hpp>

namespace hana = boost::hana;

template <auto ... args, auto ... i>
inline constexpr auto make_tuple_helper(std::index_sequence<i...>) noexcept
{
    return hana::make_tuple(hana::arg<i>(args...)...);
}

template <auto itr, auto end, auto ... args, auto ... result_indexes>
inline constexpr auto stalin_impl(std::index_sequence<result_indexes...>) noexcept
{
    if constexpr (sizeof...(args) == 0) return std::index_sequence<>{};

    else if constexpr (itr == end)
        return std::index_sequence<result_indexes...>{};

    else if constexpr (hana::arg<hana::arg<sizeof...(result_indexes)>(result_indexes...)>(args...)
         > hana::arg<itr + 1>(args...))

        return stalin_impl<itr + 1, end, args...>(
             std::index_sequence<result_indexes...>{}
        );

    else return stalin_impl<itr + 1, end, args...>(
        std::index_sequence<result_indexes..., itr + 1>{}
    );
}

template <auto ... args>
inline constexpr auto stalin_sort() noexcept
{
    return make_tuple_helper<args...>(
        stalin_impl<1, sizeof...(args), args...>(std::index_sequence<1>{})
    );
}

auto main() -> int
{
    static_assert(stalin_sort<>() == hana::make_tuple()); // sorted at compile time
    static_assert(stalin_sort<0>() == hana::make_tuple(0));
    static_assert(stalin_sort<2,3>() == hana::make_tuple(2, 3));
    static_assert(stalin_sort<2,1,3>() == hana::make_tuple(2, 3));
    static_assert(stalin_sort<2,1,3,3,2,5,6>() == hana::make_tuple(2,3,3,5,6));

    // print
    constexpr auto result = stalin_sort<3, 1, 4, 1, 5>();
    hana::for_each(result, [](const auto& x){ std::cout << x << " "; }); // 3 4 5
}

max_stars_repo_name: twofist/stalin-sort

max_issues_repo_name: twofist/stalin-sort

max_forks_repo_name: twofist/stalin-sort


text:
//---------------------------------------------------------------------------
/// \file   parser.hpp
/// \brief  Construct a constraint from a string
//
// Copyright 2012-2014, nocte@hippie.nu       Released under the MIT License.
//---------------------------------------------------------------------------

#include <unordered_map>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>

#include "constraint.hpp"
#include "linear_equation.hpp"
#include "linear_inequality.hpp"

namespace rhea
{

typedef boost::spirit::qi::symbols<char, variable> var_map;

template <typename Iterator>
struct constraint_grammar : boost::spirit::qi::grammar<Iterator, constraint()>
{
    typedef boost::spirit::ascii::space_type space_type;

    constraint_grammar(var_map& vars)
        : constraint_grammar::base_type{constr}
    {
        using namespace boost::spirit;
        namespace phx = boost::phoenix;
        using qi::double_;
        using qi::_val;

        constr = lineq[_val = phx::construct<constraint>(_1)]
                 | linineq[_val = phx::construct<constraint>(_1)];

        lineq = (expr >> '='
                 >> expr)[_val = phx::construct<linear_equation>(_1, _2)];

        linineq
            = (expr >> "<=" >> expr)[_val = phx::construct<linear_inequality>(
                                         _1, relation::leq, _2)]
              | (expr >> ">="
                 >> expr)[_val = phx::construct<linear_inequality>(
                              _1, relation::geq, _2)];

        expr = double_[_val = _1]
               //| lexeme[raw[(ascii::alpha >> *(ascii::alnum | '_'))]] [
               // vars.add, _val = vars[_1] ]
               | vars[_val = _1] | (expr >> '+' >> expr)[_val = _1 + _2]
               | (expr >> '-' >> expr)[_val = _1 - _2]
               | (expr >> '*' >> expr)[_val = _1 * _2]
               | (expr >> '/' >> expr)[_val = _1 / _2];

        constr.name("constraint");
        lineq.name("linear equation");
        linineq.name("linear inequality");
        expr.name("expression");

        qi::on_error<qi::fail>(
            constr, std::cout << phx::val("Error! Expecting ")
                              << _4 // what failed?
                              << phx::val(" here: \"")
                              << phx::construct<std::string>(
                                     _3, _2) // iterators to error-pos, end
                              << phx::val("\"") << std::endl);
    }

    boost::spirit::qi::rule<Iterator, constraint()> constr;
    boost::spirit::qi::rule<Iterator, linear_equation()> lineq;
    boost::spirit::qi::rule<Iterator, linear_inequality()> linineq;
    boost::spirit::qi::rule<Iterator, linear_expression()> expr;
};
}

max_stars_repo_name: sablib/ZZLayout

max_issues_repo_name: sablib/ZZLayout

max_forks_repo_name: sablib/ZZLayout


text:
#define BOOST_TEST_MODULE io_json
#include <boost/test/included/unit_test.hpp>

#include <fc/io/json.hpp>
#include <fc/exception/exception.hpp>

using namespace fc;

BOOST_AUTO_TEST_SUITE(json_test_suite)

namespace json_test_util {
   constexpr size_t exception_limit_size = 250;

   const json::yield_function_t yield_deadline_exception_at_start = [](size_t s) {
      FC_CHECK_DEADLINE(fc::time_point::now() - fc::milliseconds(1));
   };

   const json::yield_function_t yield_deadline_exception_in_mid = [](size_t s) {
      if (s >= exception_limit_size) {
         throw fc::timeout_exception(fc::exception_code::timeout_exception_code, "timeout_exception", "execution timed out" );
      }
   };

   const json::yield_function_t yield_length_exception = [](size_t s) {
      FC_ASSERT( s <= exception_limit_size );
   };

   const json::yield_function_t yield_no_limitation = [](size_t s) {
      // no limitation
   };

   const auto time_except_verf_func = [](const fc::timeout_exception& ex) -> bool {
      return (static_cast<int>(ex.code_value) == static_cast<int>(fc::exception_code::timeout_exception_code));
   };

   const auto length_limit_except_verf_func = [](const fc::assert_exception& ex) -> bool {
      return (static_cast<int>(ex.code_value) == static_cast<int>(fc::exception_code::assert_exception_code));
   };

   constexpr size_t repeat_char_num = 512;
   const std::string repeat_chars(repeat_char_num, 'a');
   const string escape_input_str = "\\b\\f\\n\\r\\t-\\-\\\\-\\x0\\x01\\x02\\x03\\x04\\x05\\x06\\x07\\x08\\x09\\x0a\\x0b\\x0c\\x0d\\x0e\\x0f"  \
                                   "\\x10\\x11\\x12\\x13\\x14\\x15\\x16\\x17\\x18\\x19\\x1a\\x1b\\x1c\\x1d\\x1e\\x1f-" + repeat_chars;

}  // namespace json_test_util

BOOST_AUTO_TEST_CASE(to_string_test)
{
   {  // to_string( const variant& v, const fc::time_point& deadline, const uint64_t max_len = max_length_limit, output_formatting format = stringify_large_ints_and_doubles);
      {
         variant v(json_test_util::escape_input_str);
         std::string deadline_exception_at_start_str;
         BOOST_CHECK_EXCEPTION(deadline_exception_at_start_str = json::to_string( v, fc::time_point::min(), json::output_formatting::stringify_large_ints_and_doubles, json::max_length_limit),
                               fc::timeout_exception,
                               json_test_util::time_except_verf_func);
         BOOST_CHECK_EQUAL(deadline_exception_at_start_str.empty(), true);
      }
      {
         constexpr size_t max_len = 10;
         variant v(json_test_util::repeat_chars);
         std::string deadline_exception_at_start_str;
         BOOST_CHECK_EQUAL(deadline_exception_at_start_str.empty(), true);
         BOOST_CHECK_EXCEPTION(deadline_exception_at_start_str = json::to_string( v, fc::time_point::maximum(), json::output_formatting::stringify_large_ints_and_doubles, max_len),
                               fc::assert_exception,
                               json_test_util::length_limit_except_verf_func);
         BOOST_CHECK_EQUAL(deadline_exception_at_start_str.empty(), true);
      }
      {
         variant v(json_test_util::repeat_chars);
         std::string length_exception_in_mid_str;
         BOOST_CHECK_NO_THROW(length_exception_in_mid_str = json::to_string( v, fc::time_point::maximum(), json::output_formatting::stringify_large_ints_and_doubles, json::max_length_limit));
         BOOST_CHECK_EQUAL(length_exception_in_mid_str, "\"" + json_test_util::repeat_chars + "\"");
      }
   }
   {  // to_string( const variant& v, const yield_function_t& yield, output_formatting format = stringify_large_ints_and_doubles);
      const variant v(json_test_util::repeat_chars);
      BOOST_CHECK_LT(json_test_util::exception_limit_size, json_test_util::repeat_chars.size());
      {
         std::string deadline_exception_at_start_str;
         BOOST_CHECK_EXCEPTION(deadline_exception_at_start_str = json::to_string( v, json_test_util::yield_deadline_exception_at_start),
                               fc::timeout_exception,
                               json_test_util::time_except_verf_func);
         BOOST_CHECK_EQUAL(deadline_exception_at_start_str.empty(), true);
      }
      {
         std::string deadline_exception_in_mid_str;
         BOOST_CHECK_EXCEPTION(deadline_exception_in_mid_str = json::to_string( v, json_test_util::yield_deadline_exception_in_mid),
                               fc::timeout_exception,
                               json_test_util::time_except_verf_func);
         BOOST_CHECK_EQUAL(deadline_exception_in_mid_str.empty(), true);
      }
      {
         std::string length_exception_in_mid_str;
         BOOST_CHECK_EXCEPTION(length_exception_in_mid_str = json::to_string( v, json_test_util::yield_length_exception),
                               fc::assert_exception,
                               json_test_util::length_limit_except_verf_func);
         BOOST_CHECK_EQUAL(length_exception_in_mid_str.empty(), true);
      }
      {
         std::string no_exception_str;
         BOOST_CHECK_NO_THROW(no_exception_str = json::to_string( v, json_test_util::yield_no_limitation));
         BOOST_CHECK_EQUAL(no_exception_str, "\"" + json_test_util::repeat_chars + "\"");
      }
   }
   { // to_string template call
      const uint16_t id = 1000;
      const uint64_t len = 3;
      const std::string id_ret_1 = json::to_string( id, fc::time_point::maximum());
      BOOST_CHECK_EQUAL(std::to_string(id), id_ret_1);

      // exceed length
      std::string id_ret_2;
      BOOST_REQUIRE_THROW(id_ret_2 = json::to_string( id, fc::time_point::maximum(), json::output_formatting::stringify_large_ints_and_doubles, len), fc::assert_exception);
      BOOST_CHECK_EQUAL(id_ret_2.empty(), true);

      // time_out
      std::string id_ret_3;
      BOOST_REQUIRE_THROW(id_ret_3 = json::to_string( id, fc::time_point::now() - fc::milliseconds(1) ), fc::timeout_exception);
      BOOST_CHECK_EQUAL(id_ret_3.empty(), true);
   }
}

BOOST_AUTO_TEST_CASE(to_pretty_string_test)
{
   {  // to_pretty_string( const variant& v, const fc::time_point& deadline, const uint64_t max_len = max_length_limit, output_formatting format = stringify_large_ints_and_doubles );
      {
         variant v(json_test_util::escape_input_str);
         std::string deadline_exception_at_start_str;
         BOOST_CHECK_EXCEPTION(deadline_exception_at_start_str = json::to_pretty_string( v, fc::time_point::min(), json::output_formatting::stringify_large_ints_and_doubles, json::max_length_limit),
         fc::timeout_exception,
         json_test_util::time_except_verf_func);
         BOOST_CHECK_EQUAL(deadline_exception_at_start_str.empty(), true);
      }
      {
         constexpr size_t max_len = 10;
         variant v(json_test_util::repeat_chars);
         std::string deadline_exception_at_start_str;
         BOOST_CHECK_EQUAL(deadline_exception_at_start_str.empty(), true);
         BOOST_CHECK_EXCEPTION(deadline_exception_at_start_str = json::to_pretty_string( v, fc::time_point::maximum(), json::output_formatting::stringify_large_ints_and_doubles, max_len),
                               fc::assert_exception,
                               json_test_util::length_limit_except_verf_func);
         BOOST_CHECK_EQUAL(deadline_exception_at_start_str.empty(), true);
      }
      {
         variant v(json_test_util::repeat_chars);
         std::string length_exception_in_mid_str;
         BOOST_CHECK_NO_THROW(length_exception_in_mid_str = json::to_pretty_string( v, fc::time_point::maximum(), json::output_formatting::stringify_large_ints_and_doubles, json::max_length_limit));
         BOOST_CHECK_EQUAL(length_exception_in_mid_str, "\"" + json_test_util::repeat_chars + "\"");
      }
   }
   {  // to_pretty_string( const variant& v, const yield_function_t& yield, output_formatting format = stringify_large_ints_and_doubles );
      const variant v(json_test_util::repeat_chars);
      BOOST_CHECK_LT(json_test_util::exception_limit_size, json_test_util::repeat_chars.size());
      {
         std::string deadline_exception_at_start_str;
         BOOST_CHECK_EXCEPTION(deadline_exception_at_start_str = json::to_pretty_string( v, json_test_util::yield_deadline_exception_at_start),
                               fc::timeout_exception,
                               json_test_util::time_except_verf_func);
         BOOST_CHECK_EQUAL(deadline_exception_at_start_str.empty(), true);
      }
      {
         std::string deadline_exception_in_mid_str;
         BOOST_CHECK_EXCEPTION(deadline_exception_in_mid_str = json::to_pretty_string( v, json_test_util::yield_deadline_exception_in_mid),
                               fc::timeout_exception,
                               json_test_util::time_except_verf_func);
         BOOST_CHECK_EQUAL(deadline_exception_in_mid_str.empty(), true);
      }
      {
         std::string length_exception_in_mid_str;
         BOOST_CHECK_EXCEPTION(length_exception_in_mid_str = json::to_pretty_string( v, json_test_util::yield_length_exception),
                               fc::assert_exception,
                               json_test_util::length_limit_except_verf_func);
         BOOST_CHECK_EQUAL(length_exception_in_mid_str.empty(), true);
      }
      {
         std::string no_exception_str;
         BOOST_CHECK_NO_THROW(no_exception_str = json::to_pretty_string( v, json_test_util::yield_no_limitation));
         BOOST_CHECK_EQUAL(no_exception_str, "\"" + json_test_util::repeat_chars + "\"");
      }
   }
   { // to_pretty_string template call
      const uint16_t id = 1000;
      const uint64_t len = 3;
      const std::string id_ret_1 = json::to_pretty_string( id, fc::time_point::maximum());
      BOOST_CHECK_EQUAL(std::to_string(id), id_ret_1);

      // exceed length
      std::string id_ret_2;
      BOOST_REQUIRE_THROW(id_ret_2 = json::to_pretty_string( id, fc::time_point::maximum(), json::output_formatting::stringify_large_ints_and_doubles, len), fc::assert_exception);
      BOOST_CHECK_EQUAL(id_ret_2.empty(), true);

      // time_out
      std::string id_ret_3;
      BOOST_REQUIRE_THROW(id_ret_3 = json::to_pretty_string( id, fc::time_point::now() - fc::milliseconds(1) ), fc::timeout_exception);
      BOOST_CHECK_EQUAL(id_ret_3.empty(), true);
   }
}

BOOST_AUTO_TEST_CASE(escape_string_test)
{
   std::string escape_out_str;
   escape_out_str = fc::escape_string(json_test_util::escape_input_str, json_test_util::yield_no_limitation);
   BOOST_CHECK_LT(json_test_util::repeat_char_num, json_test_util::escape_input_str.size());
   BOOST_CHECK_LT(json_test_util::escape_input_str.size() - json_test_util::repeat_char_num, json_test_util::exception_limit_size);  // by using size_different to calculate expected string
   {
      // simulate exceed time exception at the beginning of processing
      BOOST_CHECK_EXCEPTION(escape_string(json_test_util::escape_input_str, json_test_util::yield_deadline_exception_at_start),
                            fc::timeout_exception,
                            json_test_util::time_except_verf_func);
   }
   {
      // simulate exceed time exception in the middle of processing
      BOOST_CHECK_EXCEPTION(escape_string(json_test_util::escape_input_str, json_test_util::yield_deadline_exception_in_mid),
                            fc::timeout_exception,
                            json_test_util::time_except_verf_func);
   }
   {
      // length limitation exception in the middle of processing
      BOOST_CHECK_EXCEPTION(escape_string(json_test_util::escape_input_str, json_test_util::yield_length_exception),
                            fc::assert_exception,
                            json_test_util::length_limit_except_verf_func);
   }
}

BOOST_AUTO_TEST_SUITE_END()


text:
#ifndef BOOST_DESCRIBE_ENUM_HPP_INCLUDED
#define BOOST_DESCRIBE_ENUM_HPP_INCLUDED

// Copyright 2020 Peter Dimov
// Distributed under the Boost Software License, Version 1.0.
// https://www.boost.org/LICENSE_1_0.txt

#include <boost/describe/detail/config.hpp>

#if !defined(BOOST_DESCRIBE_CXX14)

#define BOOST_DESCRIBE_ENUM(E, ...)

#else

#include <boost/describe/detail/pp_for_each.hpp>
#include <boost/describe/detail/list.hpp>
#include <type_traits>

namespace boost
{
namespace describe
{
namespace detail
{

template<class D> struct enum_descriptor
{
    // can't use auto here because of the need to supply the definitions below
    static constexpr decltype(D::value()) value = D::value();
    static constexpr decltype(D::name()) name = D::name();
};

// GCC requires these definitions
template<class D> constexpr decltype(D::value()) enum_descriptor<D>::value;
template<class D> constexpr decltype(D::name()) enum_descriptor<D>::name;

template<class... T> auto enum_descriptor_fn_impl( int, T... )
{
    return list<enum_descriptor<T>...>();
}

#define BOOST_DESCRIBE_ENUM_BEGIN(E) \
    inline auto boost_enum_descriptor_fn( E* ) \
    { return boost::describe::detail::enum_descriptor_fn_impl( 0

#define BOOST_DESCRIBE_ENUM_ENTRY(E, e) , []{ struct _boost_desc { \
    static constexpr auto value() noexcept { return E::e; } \
    static constexpr decltype(#e) name() noexcept { return #e; } }; return _boost_desc(); }()

#define BOOST_DESCRIBE_ENUM_END(E) ); }

} // namespace detail

#if defined(_MSC_VER) && !defined(__clang__)

#define BOOST_DESCRIBE_ENUM(E, ...) \
    static_assert(std::is_enum<E>::value, "BOOST_DESCRIBE_ENUM should only be used with enums"); \
    BOOST_DESCRIBE_ENUM_BEGIN(E) \
    BOOST_DESCRIBE_PP_FOR_EACH(BOOST_DESCRIBE_ENUM_ENTRY, E, __VA_ARGS__) \
    BOOST_DESCRIBE_ENUM_END(E)

#else

#define BOOST_DESCRIBE_ENUM(E, ...) \
    static_assert(std::is_enum<E>::value, "BOOST_DESCRIBE_ENUM should only be used with enums"); \
    BOOST_DESCRIBE_ENUM_BEGIN(E) \
    BOOST_DESCRIBE_PP_FOR_EACH(BOOST_DESCRIBE_ENUM_ENTRY, E, ##__VA_ARGS__) \
    BOOST_DESCRIBE_ENUM_END(E)

#endif

} // namespace describe
} // namespace boost

#endif // defined(BOOST_DESCRIBE_CXX14)

#if defined(_MSC_VER) && !defined(__clang__)

#define BOOST_DEFINE_ENUM(E, ...) enum E { __VA_ARGS__ }; BOOST_DESCRIBE_ENUM(E, __VA_ARGS__)
#define BOOST_DEFINE_ENUM_CLASS(E, ...) enum class E { __VA_ARGS__ }; BOOST_DESCRIBE_ENUM(E, __VA_ARGS__)

#define BOOST_DEFINE_FIXED_ENUM(E, Base, ...) enum E: Base { __VA_ARGS__ }; BOOST_DESCRIBE_ENUM(E, __VA_ARGS__)
#define BOOST_DEFINE_FIXED_ENUM_CLASS(E, Base, ...) enum class E: Base { __VA_ARGS__ }; BOOST_DESCRIBE_ENUM(E, __VA_ARGS__)

#else

#define BOOST_DEFINE_ENUM(E, ...) enum E { __VA_ARGS__ }; BOOST_DESCRIBE_ENUM(E, ##__VA_ARGS__)
#define BOOST_DEFINE_ENUM_CLASS(E, ...) enum class E { __VA_ARGS__ }; BOOST_DESCRIBE_ENUM(E, ##__VA_ARGS__)

#define BOOST_DEFINE_FIXED_ENUM(E, Base, ...) enum E: Base { __VA_ARGS__ }; BOOST_DESCRIBE_ENUM(E, ##__VA_ARGS__)
#define BOOST_DEFINE_FIXED_ENUM_CLASS(E, Base, ...) enum class E: Base { __VA_ARGS__ }; BOOST_DESCRIBE_ENUM(E, ##__VA_ARGS__)

#endif

#endif // #ifndef BOOST_DESCRIBE_ENUM_HPP_INCLUDED

max_stars_repo_name: Julien-Blanc-tgcm/describe

max_issues_repo_name: Julien-Blanc-tgcm/describe

max_forks_repo_name: Julien-Blanc-tgcm/describe