<commit_before>/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the Universite de Sherbrooke nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include <tf/transform_listener.h>

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <stereo_msgs/DisparityImage.h>

#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>

#include <image_geometry/pinhole_camera_model.h>

#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/subscriber.h>

#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>

#include <rtabmap_ros/MsgConversion.h>

#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d_mapping.h"
#include "rtabmap/core/util3d_transforms.h"

namespace rtabmap_ros
{

class ObstaclesDetection : public nodelet::Nodelet
{
public:
	ObstaclesDetection() :
		frameId_("base_link"),
		normalEstimationRadius_(0.05),
		groundNormalAngle_(M_PI_4),
		minClusterSize_(20),
		maxFloorHeight_(-1),
		maxObstaclesHeight_(1.5),
		waitForTransform_(false),
		simpleSegmentation_(false)
	{}

	virtual ~ObstaclesDetection()
	{}

private:
	virtual void onInit()
	{
		ros::NodeHandle & nh = getNodeHandle();
		ros::NodeHandle & pnh = getPrivateNodeHandle();

		int queueSize = 10;
		pnh.param("queue_size", queueSize, queueSize);
		pnh.param("frame_id", frameId_, frameId_);
		pnh.param("normal_estimation_radius", normalEstimationRadius_, normalEstimationRadius_);
		pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_);
		pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
		pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
		pnh.param("max_floor_height", maxFloorHeight_, maxFloorHeight_);
		pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
		pnh.param("simple_segmentation", simpleSegmentation_, simpleSegmentation_);

		cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this);

		groundPub_ = nh.advertise<sensor_msgs::PointCloud2>("ground", 1);
		obstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles", 1);

		this->_lastFrameTime = ros::Time::now();
	}

	void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
	{
		if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0)
		{
			// no one wants the results
			return;
		}

		ROS_ERROR("1111111111111111111111");

		rtabmap::Transform localTransform;
		try
		{
			if(waitForTransform_)
			{
				if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
				{
					ROS_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
					return;
				}
			}

			ROS_ERROR("2222222222222222222222222222222");

			tf::StampedTransform tmp;
			tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
			localTransform = rtabmap_ros::transformFromTF(tmp);
		}
		catch(tf::TransformException & ex)
		{
			ROS_ERROR("%s",ex.what());
			return;
		}

		pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::fromROSMsg(*cloudMsg, *originalCloud);
		if(originalCloud->size() == 0)
		{
			ROS_ERROR("Recieved empty point cloud!");
			return;
		}
		originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);

		ROS_ERROR("3333333333333333333333333");

		/////////////////////////////////////////////////////////////////////////////

		pcl::PointCloud<pcl::PointXYZ>::Ptr hypotheticalGroundCloud(new pcl::PointCloud<pcl::PointXYZ>);
		hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_);

		pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
		obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_);

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

		pcl::IndicesPtr ground, obstacles;
		pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);

		if (!simpleSegmentation_){
			ROS_ERROR("44444444444444444444444444444");

			rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(hypotheticalGroundCloud,
					ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);

			if(ground.get() && ground->size())
			{
				pcl::copyPointCloud(*hypotheticalGroundCloud, *ground, *groundCloud);
			}
			if(obstacles.get() && obstacles->size())
			{
				pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesNearFloorCloud(new pcl::PointCloud<pcl::PointXYZ>);
				pcl::copyPointCloud(*hypotheticalGroundCloud, *obstacles, *obstaclesNearFloorCloud);
				*obstaclesCloud += *obstaclesNearFloorCloud;
			}
		}
		else{
			ROS_ERROR("555555555555555555555555");
			groundCloud = hypotheticalGroundCloud;
		}

		if(groundPub_.getNumSubscribers())
		{
			sensor_msgs::PointCloud2 rosCloud;
			pcl::toROSMsg(*groundCloud, rosCloud);
			rosCloud.header.stamp = cloudMsg->header.stamp;
			rosCloud.header.frame_id = frameId_;

			//publish the message
			groundPub_.publish(rosCloud);
		}

		if(obstaclesPub_.getNumSubscribers())
		{
			sensor_msgs::PointCloud2 rosCloud;
			pcl::toROSMsg(*obstaclesCloud, rosCloud);
			rosCloud.header.stamp = cloudMsg->header.stamp;
			rosCloud.header.frame_id = frameId_;

			//publish the message
			obstaclesPub_.publish(rosCloud);
		}

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

		ros::Duration process_duration = curtime - lasttime;
		ros::Duration between_frames = curtime - this->_lastFrameTime;
		this->_lastFrameTime = curtime;
		std::stringstream buffer;
		buffer << "cloud=" << originalCloud->size() << " ground=" << hypotheticalGroundCloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size();
		buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz";
		//ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str());

	}

private:
	std::string frameId_;
	double normalEstimationRadius_;
	double groundNormalAngle_;
	int minClusterSize_;
	double maxObstaclesHeight_;
	double maxFloorHeight_;
	bool waitForTransform_;
	bool simpleSegmentation_;

	tf::TransformListener tfListener_;

	ros::Publisher groundPub_;
	ros::Publisher obstaclesPub_;

	ros::Subscriber cloudSub_;
	ros::Time _lastFrameTime;
};

PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetection, nodelet::Nodelet);
}

<commit_msg>Comment for debugging<commit_after>/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the Universite de Sherbrooke nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include <tf/transform_listener.h>

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <stereo_msgs/DisparityImage.h>

#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>

#include <image_geometry/pinhole_camera_model.h>

#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/subscriber.h>

#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>

#include <rtabmap_ros/MsgConversion.h>

#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d_mapping.h"
#include "rtabmap/core/util3d_transforms.h"

namespace rtabmap_ros
{

class ObstaclesDetection : public nodelet::Nodelet
{
public:
	ObstaclesDetection() :
		frameId_("base_link"),
		normalEstimationRadius_(0.05),
		groundNormalAngle_(M_PI_4),
		minClusterSize_(20),
		maxFloorHeight_(-1),
		maxObstaclesHeight_(1.5),
		waitForTransform_(false),
		simpleSegmentation_(false)
	{}

	virtual ~ObstaclesDetection()
	{}

private:
	virtual void onInit()
	{
		ros::NodeHandle & nh = getNodeHandle();
		ros::NodeHandle & pnh = getPrivateNodeHandle();

		int queueSize = 10;
		pnh.param("queue_size", queueSize, queueSize);
		pnh.param("frame_id", frameId_, frameId_);
		pnh.param("normal_estimation_radius", normalEstimationRadius_, normalEstimationRadius_);
		pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_);
		pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
		pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
		pnh.param("max_floor_height", maxFloorHeight_, maxFloorHeight_);
		pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
		pnh.param("simple_segmentation", simpleSegmentation_, simpleSegmentation_);

		cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this);

		groundPub_ = nh.advertise<sensor_msgs::PointCloud2>("ground", 1);
		obstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles", 1);

		this->_lastFrameTime = ros::Time::now();
	}

	void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
	{
		if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0)
		{
			// no one wants the results
			return;
		}

		rtabmap::Transform localTransform;
		try
		{
			if(waitForTransform_)
			{
				if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
				{
					ROS_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
					return;
				}
			}

			ROS_ERROR("2222222222222222222222222222222");

			tf::StampedTransform tmp;
			tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
			localTransform = rtabmap_ros::transformFromTF(tmp);
		}
		catch(tf::TransformException & ex)
		{
			ROS_ERROR("%s",ex.what());
			return;
		}

		pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::fromROSMsg(*cloudMsg, *originalCloud);
		if(originalCloud->size() == 0)
		{
			ROS_ERROR("Recieved empty point cloud!");
			return;
		}
		originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);

		ROS_ERROR("3333333333333333333333333");

		/////////////////////////////////////////////////////////////////////////////

		pcl::PointCloud<pcl::PointXYZ>::Ptr hypotheticalGroundCloud(new pcl::PointCloud<pcl::PointXYZ>);
		hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_);

		ROS_ERROR("AAAa3333333333333333333333333");

		pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
		obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_);

		ROS_ERROR("BBBb3333333333333333333333333");

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

		pcl::IndicesPtr ground, obstacles;
		pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);

		if (!simpleSegmentation_){
			ROS_ERROR("44444444444444444444444444444");

			rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(hypotheticalGroundCloud,
					ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);

			if(ground.get() && ground->size())
			{
				pcl::copyPointCloud(*hypotheticalGroundCloud, *ground, *groundCloud);
			}
			if(obstacles.get() && obstacles->size())
			{
				pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesNearFloorCloud(new pcl::PointCloud<pcl::PointXYZ>);
				pcl::copyPointCloud(*hypotheticalGroundCloud, *obstacles, *obstaclesNearFloorCloud);
				*obstaclesCloud += *obstaclesNearFloorCloud;
			}
		}
		else{
			ROS_ERROR("555555555555555555555555");
			groundCloud = hypotheticalGroundCloud;
		}

		if(groundPub_.getNumSubscribers())
		{
			sensor_msgs::PointCloud2 rosCloud;
			pcl::toROSMsg(*groundCloud, rosCloud);
			rosCloud.header.stamp = cloudMsg->header.stamp;
			rosCloud.header.frame_id = frameId_;

			//publish the message
			groundPub_.publish(rosCloud);
		}

		if(obstaclesPub_.getNumSubscribers())
		{
			sensor_msgs::PointCloud2 rosCloud;
			pcl::toROSMsg(*obstaclesCloud, rosCloud);
			rosCloud.header.stamp = cloudMsg->header.stamp;
			rosCloud.header.frame_id = frameId_;

			//publish the message
			obstaclesPub_.publish(rosCloud);
		}

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

		ros::Duration process_duration = curtime - lasttime;
		ros::Duration between_frames = curtime - this->_lastFrameTime;
		this->_lastFrameTime = curtime;
		std::stringstream buffer;
		buffer << "cloud=" << originalCloud->size() << " ground=" << hypotheticalGroundCloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size();
		buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz";
		//ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str());

	}

private:
	std::string frameId_;
	double normalEstimationRadius_;
	double groundNormalAngle_;
	int minClusterSize_;
	double maxObstaclesHeight_;
	double maxFloorHeight_;
	bool waitForTransform_;
	bool simpleSegmentation_;

	tf::TransformListener tfListener_;

	ros::Publisher groundPub_;
	ros::Publisher obstaclesPub_;

	ros::Subscriber cloudSub_;
	ros::Time _lastFrameTime;
};

PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetection, nodelet::Nodelet);
}

<|endoftext|>


<commit_before>#ifndef <<<headerguard>>>
#define <<<headerguard>>>

#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <stddef.h>
#include <sstream>
#include <iostream>

#include "ComponentEssentials.h"
#include "ComponentUtilities.h"

#include "FMI/fmi_import_context.h"
#include "FMI1/fmi1_import.h"
#include "FMI2/fmi2_import.h"
#include "JM/jm_portability.h"

void localLogger(jm_callbacks *c, jm_string module, jm_log_level_enu_t log_level, jm_string message) { }

namespace hopsan {

//!
//! @brief
//! @ingroup HydraulicComponents
//!
class <<<className>>> : public ComponentQ
{

private:
    //Node data pointers
    <<<localvars>>>

                  fmi2_callback_functions_t callBackFunctions;
    jm_callbacks callbacks;
    fmi_import_context_t* context;
    fmi_version_enu_t version;
    jm_status_enu_t status;
    fmi2_status_t fmistatus;
    int k;

    double tlast;

    fmi2_real_t hcur;
    size_t n_states;
    size_t n_event_indicators;
    fmi2_real_t* states;
    fmi2_real_t* states_der;
    fmi2_real_t* event_indicators;
    fmi2_real_t* event_indicators_prev;
    fmi2_boolean_t callEventUpdate;
    fmi2_boolean_t terminateSimulation;
    fmi2_boolean_t toleranceControlled;
    fmi2_real_t relativeTolerance;
    fmi2_event_info_t eventInfo;

    fmi2_import_t* fmu;

public:
    static Component *Creator()
    {
        return new <<<className>>>();
    }

    void configure()
    {
        //Add constants
        <<<addconstants>>>

        //Add input variables
        <<<addinputs>>>

        //Add output variables
        <<<addoutputs>>>

        //Add ports
        <<<addports>>>
    }

    void initialize()
    {
        <<<setnodedatapointers>>>

        addInfoMessage("Initializing FMU 2.0 import");

        fmi2_boolean_t terminateSimulation = fmi2_false;
        fmi2_boolean_t toleranceControlled = fmi2_true;
        fmi2_real_t relativeTolerance = 0.001;

        const char* FMUPath = "<<<fmupath>>>";
        const char* tmpPath = "<<<temppath>>>";

        callbacks.malloc = malloc;
        callbacks.calloc = calloc;
        callbacks.realloc = realloc;
        callbacks.free = free;
        callbacks.logger = localLogger;
        callbacks.log_level = jm_log_level_debug;
        callbacks.context = 0;

        context = fmi_import_allocate_context(&callbacks);

        version = fmi_import_get_fmi_version(context, FMUPath, tmpPath);

        if(version != fmi_version_2_0_enu)
        {
            addErrorMessage("The code only supports version 2.0\n");
            stopSimulation();
            return;
        }

        fmu = fmi2_import_parse_xml(context, tmpPath, 0);

        if(!fmu)
        {
            addErrorMessage("Error parsing XML, exiting\n");
            stopSimulation();
            return;
        }

        if(fmi2_import_get_fmu_kind(fmu) != fmi2_fmu_kind_me)
        {
            addErrorMessage("Only ME 2.0 is supported by this code\n");
            stopSimulation();
             return;
        }

        callBackFunctions.logger = fmi2_log_forwarding;
        callBackFunctions.allocateMemory = calloc;
        callBackFunctions.freeMemory = free;
        callBackFunctions.componentEnvironment = fmu;

        status = fmi2_import_create_dllfmu(fmu, fmi2_fmu_kind_me, &callBackFunctions);
        if (status == jm_status_error)
        {
            std::stringstream ss;
            ss << "Could not create the DLL loading mechanism(C-API) (error: " << fmi2_import_get_last_error(fmu) << ").";
            addErrorMessage(ss.str().c_str());
            stopSimulation();
            return;
        }

        n_states = fmi2_import_get_number_of_continuous_states(fmu);
        n_event_indicators = fmi2_import_get_number_of_event_indicators(fmu);

        states = new fmi2_real_t[n_states];
        states_der = new fmi2_real_t[n_states];
        event_indicators = new fmi2_real_t[n_event_indicators];
        event_indicators_prev = new fmi2_real_t[n_event_indicators];

        //Instantiate FMU
        fmi2_string_t instanceName = "Test CS model instance";
        fmi2_string_t fmuLocation = "";
        fmi2_boolean_t visible = fmi2_false;
        jm_status_enu_t jmstatus = fmi2_import_instantiate(fmu, instanceName, fmi2_cosimulation, fmuLocation, visible);
        if (jmstatus == jm_status_error)
        {
            addErrorMessage("fmi2_import_instantiate() failed!");
            stopSimulation();
            return;
        }

        //Setup experiment
        fmi2_real_t relativeTol = 1e-4;
        fmistatus = fmi2_import_setup_experiment(fmu, fmi2_true, relativeTol, mTime, fmi2_false, 10);
        if(fmistatus != fmi2_status_ok)
        {
            addErrorMessage("fmi2_import_setup_experiment() failed!");
            stopSimulation();
            return;
        }

        //Enter initialization mode
        fmistatus = fmi2_import_enter_initialization_mode(fmu);
        if(fmistatus != fmi2_status_ok)
        {
            addErrorMessage("fmi2_import_enter_initialization_mode() failed!");
            stopSimulation();
            return;
        }

        //Set parameters
        double value;
        fmi2_value_reference_t vr;
>>>setpars>>>        vr = <<<vr>>>;
        value = <<<var>>>;
        fmistatus = fmi2_import_set_real(fmu, &vr, 1, &value);
        <<<setpars<<<

        //Exit initialization mode
        fmistatus = fmi2_import_exit_initialization_mode(fmu);
        if(fmistatus != fmi2_status_ok)
        {
            addErrorMessage("fmi2_import_exit_initialization_mode() failed!");
            stopSimulation();
            return;
        }

        callEventUpdate = fmi2_false;

        eventInfo.newDiscreteStatesNeeded           = fmi2_false;
        eventInfo.terminateSimulation               = fmi2_false;
        eventInfo.nominalsOfContinuousStatesChanged = fmi2_false;
        eventInfo.valuesOfContinuousStatesChanged   = fmi2_true;
        eventInfo.nextEventTimeDefined              = fmi2_false;
        eventInfo.nextEventTime                     = -0.0;

        /* fmiExitInitializationMode leaves FMU in event mode */
        do_event_iteration(fmu, &eventInfo);
        fmi2_import_enter_continuous_time_mode(fmu);

        fmistatus = fmi2_import_get_continuous_states(fmu, states, n_states);
        fmistatus = fmi2_import_get_event_indicators(fmu, event_indicators, n_event_indicators);
    }

    void simulateOneTimestep()
    {
        //Read inputs
        fmi2_value_reference_t vr;
        double value;
>>>readvars>>>        vr = <<<vr>>>;
        value = (*<<<var>>>);
        fmistatus = fmi2_import_set_real(fmu, &vr, 1, &value);
        <<<readvars<<<

        double tStop = mTime+mTimestep;

        while(mTime < tStop)
        {
            size_t k;
            fmi2_real_t tlast;
            int zero_crossing_event = 0;

            fmistatus = fmi2_import_set_time(fmu, mTime);

            { /* Swap event_indicators and event_indicators_prev so that we can get new indicators */
                fmi2_real_t *temp = event_indicators;
                event_indicators = event_indicators_prev;
                event_indicators_prev = temp;
            }
            fmistatus = fmi2_import_get_event_indicators(fmu, event_indicators, n_event_indicators);

              /* Check if an event indicator has triggered */
            for (k = 0; k < n_event_indicators; k++)
            {
                if ((event_indicators[k] > 0) != (event_indicators_prev[k] > 0))
                {
                    zero_crossing_event = 1;
                    break;
                }
            }

            /* Handle any events */
            if (callEventUpdate || zero_crossing_event ||
                    (eventInfo.nextEventTimeDefined && mTime == eventInfo.nextEventTime))
            {
                fmistatus = fmi2_import_enter_event_mode(fmu);
                do_event_iteration(fmu, &eventInfo);
                fmistatus = fmi2_import_enter_continuous_time_mode(fmu);

                fmistatus = fmi2_import_get_continuous_states(fmu, states, n_states);
                fmistatus = fmi2_import_get_event_indicators(fmu, event_indicators, n_event_indicators);
            }

            /* Calculate next time step */
            tlast = mTime;
            mTime = tStop;
            if (eventInfo.nextEventTimeDefined && (mTime >= eventInfo.nextEventTime))
            {
                mTime = eventInfo.nextEventTime;
            }
            hcur = mTime - tlast;

            /* Integrate a step */
            fmistatus = fmi2_import_get_derivatives(fmu, states_der, n_states);
            for (k = 0; k < n_states; k++)
            {
                states[k] = states[k] + hcur*states_der[k];
            }

            /* Set states */
            fmistatus = fmi2_import_set_continuous_states(fmu, states, n_states);

            /* Step is complete */
            fmistatus = fmi2_import_completed_integrator_step(fmu, fmi2_true, &callEventUpdate,
                                                              &terminateSimulation);
        }

        double rValue;
>>>writevars>>>        vr = <<<vr>>>;
        fmistatus = fmi2_import_get_real(fmu, &vr, 1, &rValue);
        (*<<<var>>>) = rValue;
        <<<writevars<<<
    }

    void finalize()
    {
        fmistatus = fmi2_import_terminate(fmu);

        fmi2_import_free_instance(fmu);

        fmi2_import_destroy_dllfmu(fmu);

        fmi2_import_free(fmu);
        fmi_import_free_context(context);

        addInfoMessage("Everything seems to be OK since you got this far=)!");
    }

    void do_event_iteration(fmi2_import_t *fmu, fmi2_event_info_t *eventInfo)
    {
        eventInfo->newDiscreteStatesNeeded = fmi2_true;
        eventInfo->terminateSimulation     = fmi2_false;
        while (eventInfo->newDiscreteStatesNeeded && !eventInfo->terminateSimulation)
        {
            fmi2_import_new_discrete_states(fmu, eventInfo);
        }
    }
};
}

#endif
<commit_msg>Fixed issue with timestep size for integration in FMI for model exchange. Related to #1366.<commit_after>#ifndef <<<headerguard>>>
#define <<<headerguard>>>

#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <stddef.h>
#include <sstream>
#include <iostream>

#include "ComponentEssentials.h"
#include "ComponentUtilities.h"

#include "FMI/fmi_import_context.h"
#include "FMI1/fmi1_import.h"
#include "FMI2/fmi2_import.h"
#include "JM/jm_portability.h"

void localLogger(jm_callbacks *c, jm_string module, jm_log_level_enu_t log_level, jm_string message) { }

namespace hopsan {

//!
//! @brief
//! @ingroup HydraulicComponents
//!
class <<<className>>> : public ComponentQ
{

private:
    //Node data pointers
    <<<localvars>>>

                  fmi2_callback_functions_t callBackFunctions;
    jm_callbacks callbacks;
    fmi_import_context_t* context;
    fmi_version_enu_t version;
    jm_status_enu_t status;
    fmi2_status_t fmistatus;
    int k;

    double tlast;

    fmi2_real_t hcur;
    size_t n_states;
    size_t n_event_indicators;
    fmi2_real_t* states;
    fmi2_real_t* states_der;
    fmi2_real_t* event_indicators;
    fmi2_real_t* event_indicators_prev;
    fmi2_boolean_t callEventUpdate;
    fmi2_boolean_t terminateSimulation;
    fmi2_boolean_t toleranceControlled;
    fmi2_real_t relativeTolerance;
    fmi2_event_info_t eventInfo;

    fmi2_import_t* fmu;

public:
    static Component *Creator()
    {
        return new <<<className>>>();
    }

    void configure()
    {
        //Add constants
        <<<addconstants>>>

        //Add input variables
        <<<addinputs>>>

        //Add output variables
        <<<addoutputs>>>

        //Add ports
        <<<addports>>>
    }

    void initialize()
    {
        <<<setnodedatapointers>>>

        addInfoMessage("Initializing FMU 2.0 import");

        fmi2_boolean_t terminateSimulation = fmi2_false;
        fmi2_boolean_t toleranceControlled = fmi2_true;
        fmi2_real_t relativeTolerance = 0.001;

        const char* FMUPath = "<<<fmupath>>>";
        const char* tmpPath = "<<<temppath>>>";

        callbacks.malloc = malloc;
        callbacks.calloc = calloc;
        callbacks.realloc = realloc;
        callbacks.free = free;
        callbacks.logger = localLogger;
        callbacks.log_level = jm_log_level_debug;
        callbacks.context = 0;

        context = fmi_import_allocate_context(&callbacks);

        version = fmi_import_get_fmi_version(context, FMUPath, tmpPath);

        if(version != fmi_version_2_0_enu)
        {
            addErrorMessage("The code only supports version 2.0\n");
            stopSimulation();
            return;
        }

        fmu = fmi2_import_parse_xml(context, tmpPath, 0);

        if(!fmu)
        {
            addErrorMessage("Error parsing XML, exiting\n");
            stopSimulation();
            return;
        }

        if(fmi2_import_get_fmu_kind(fmu) != fmi2_fmu_kind_me)
        {
            addErrorMessage("Only ME 2.0 is supported by this code\n");
            stopSimulation();
             return;
        }

        callBackFunctions.logger = fmi2_log_forwarding;
        callBackFunctions.allocateMemory = calloc;
        callBackFunctions.freeMemory = free;
        callBackFunctions.componentEnvironment = fmu;

        status = fmi2_import_create_dllfmu(fmu, fmi2_fmu_kind_me, &callBackFunctions);
        if (status == jm_status_error)
        {
            std::stringstream ss;
            ss << "Could not create the DLL loading mechanism(C-API) (error: " << fmi2_import_get_last_error(fmu) << ").";
            addErrorMessage(ss.str().c_str());
            stopSimulation();
            return;
        }

        n_states = fmi2_import_get_number_of_continuous_states(fmu);
        n_event_indicators = fmi2_import_get_number_of_event_indicators(fmu);

        states = new fmi2_real_t[n_states];
        states_der = new fmi2_real_t[n_states];
        event_indicators = new fmi2_real_t[n_event_indicators];
        event_indicators_prev = new fmi2_real_t[n_event_indicators];

        //Instantiate FMU
        fmi2_string_t instanceName = "Test CS model instance";
        fmi2_string_t fmuLocation = "";
        fmi2_boolean_t visible = fmi2_false;
        jm_status_enu_t jmstatus = fmi2_import_instantiate(fmu, instanceName, fmi2_cosimulation, fmuLocation, visible);
        if (jmstatus == jm_status_error)
        {
            addErrorMessage("fmi2_import_instantiate() failed!");
            stopSimulation();
            return;
        }

        //Setup experiment
        fmi2_real_t relativeTol = 1e-4;
        fmistatus = fmi2_import_setup_experiment(fmu, fmi2_true, relativeTol, mTime, fmi2_false, 10);
        if(fmistatus != fmi2_status_ok)
        {
            addErrorMessage("fmi2_import_setup_experiment() failed!");
            stopSimulation();
            return;
        }

        //Enter initialization mode
        fmistatus = fmi2_import_enter_initialization_mode(fmu);
        if(fmistatus != fmi2_status_ok)
        {
            addErrorMessage("fmi2_import_enter_initialization_mode() failed!");
            stopSimulation();
            return;
        }

        //Set parameters
        double value;
        fmi2_value_reference_t vr;
>>>setpars>>>        vr = <<<vr>>>;
        value = <<<var>>>;
        fmistatus = fmi2_import_set_real(fmu, &vr, 1, &value);
        <<<setpars<<<

        //Exit initialization mode
        fmistatus = fmi2_import_exit_initialization_mode(fmu);
        if(fmistatus != fmi2_status_ok)
        {
            addErrorMessage("fmi2_import_exit_initialization_mode() failed!");
            stopSimulation();
            return;
        }

        callEventUpdate = fmi2_false;

        eventInfo.newDiscreteStatesNeeded           = fmi2_false;
        eventInfo.terminateSimulation               = fmi2_false;
        eventInfo.nominalsOfContinuousStatesChanged = fmi2_false;
        eventInfo.valuesOfContinuousStatesChanged   = fmi2_true;
        eventInfo.nextEventTimeDefined              = fmi2_false;
        eventInfo.nextEventTime                     = -0.0;

        /* fmiExitInitializationMode leaves FMU in event mode */
        do_event_iteration(fmu, &eventInfo);
        fmi2_import_enter_continuous_time_mode(fmu);

        fmistatus = fmi2_import_get_continuous_states(fmu, states, n_states);
        fmistatus = fmi2_import_get_event_indicators(fmu, event_indicators, n_event_indicators);
    }

    void simulateOneTimestep()
    {
        //Read inputs
        fmi2_value_reference_t vr;
        double value;
>>>readvars>>>        vr = <<<vr>>>;
        value = (*<<<var>>>);
        fmistatus = fmi2_import_set_real(fmu, &vr, 1, &value);
        <<<readvars<<<

        double time = mTime-mTimestep;
        double tStop = mTime;

        while(time < tStop)
        {
            size_t k;
            fmi2_real_t tlast;
            int zero_crossing_event = 0;

            fmistatus = fmi2_import_set_time(fmu, time);

            { /* Swap event_indicators and event_indicators_prev so that we can get new indicators */
                fmi2_real_t *temp = event_indicators;
                event_indicators = event_indicators_prev;
                event_indicators_prev = temp;
            }
            fmistatus = fmi2_import_get_event_indicators(fmu, event_indicators, n_event_indicators);

              /* Check if an event indicator has triggered */
            for (k = 0; k < n_event_indicators; k++)
            {
                if ((event_indicators[k] > 0) != (event_indicators_prev[k] > 0))
                {
                    zero_crossing_event = 1;
                    break;
                }
            }

            /* Handle any events */
            if (callEventUpdate || zero_crossing_event ||
                    (eventInfo.nextEventTimeDefined && time == eventInfo.nextEventTime))
            {
                fmistatus = fmi2_import_enter_event_mode(fmu);
                do_event_iteration(fmu, &eventInfo);
                fmistatus = fmi2_import_enter_continuous_time_mode(fmu);

                fmistatus = fmi2_import_get_continuous_states(fmu, states, n_states);
                fmistatus = fmi2_import_get_event_indicators(fmu, event_indicators, n_event_indicators);
            }

            /* Calculate next time step */
            tlast = time;
            time = tStop;
            if (eventInfo.nextEventTimeDefined && (time >= eventInfo.nextEventTime))
            {
                time = eventInfo.nextEventTime;
            }
            hcur = time - tlast;

            /* Integrate a step */
            fmistatus = fmi2_import_get_derivatives(fmu, states_der, n_states);
            for (k = 0; k < n_states; k++)
            {
                states[k] = states[k] + hcur*states_der[k];
            }

            /* Set states */
            fmistatus = fmi2_import_set_continuous_states(fmu, states, n_states);

            /* Step is complete */
            fmistatus = fmi2_import_completed_integrator_step(fmu, fmi2_true, &callEventUpdate,
                                                              &terminateSimulation);
        }

        double rValue;
>>>writevars>>>        vr = <<<vr>>>;
        fmistatus = fmi2_import_get_real(fmu, &vr, 1, &rValue);
        (*<<<var>>>) = rValue;
        <<<writevars<<<
    }

    void finalize()
    {
        fmistatus = fmi2_import_terminate(fmu);

        fmi2_import_free_instance(fmu);

        fmi2_import_destroy_dllfmu(fmu);

        fmi2_import_free(fmu);
        fmi_import_free_context(context);

        addInfoMessage("Everything seems to be OK since you got this far=)!");
    }

    void do_event_iteration(fmi2_import_t *fmu, fmi2_event_info_t *eventInfo)
    {
        eventInfo->newDiscreteStatesNeeded = fmi2_true;
        eventInfo->terminateSimulation     = fmi2_false;
        while (eventInfo->newDiscreteStatesNeeded && !eventInfo->terminateSimulation)
        {
            fmi2_import_new_discrete_states(fmu, eventInfo);
        }
    }
};
}

#endif
<|endoftext|>


<commit_before>/***********************************************************************************************************************
 **
 ** Copyright (c) 2015 ETH Zurich
 ** All rights reserved.
 **
 ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 ** following conditions are met:
 **
 **    * Redistributions of source code must retain the above copyright notice, this list of conditions and the
 **      following disclaimer.
 **    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 **      following disclaimer in the documentation and/or other materials provided with the distribution.
 **    * Neither the name of the ETH Zurich nor the names of its contributors may be used to endorse or promote products
 **      derived from this software without specific prior written permission.
 **
 **
 ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 ** INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 ** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 ** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 ** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 ** WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 ** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 **
 **********************************************************************************************************************/

#include "CAddNodeToViewByName.h"

#include "VisualizationBase/src/items/ViewItem.h"
#include "ModelBase/src/model/TreeManager.h"

namespace Interaction {

CAddNodeToViewByName::CAddNodeToViewByName() : Command{"add"}{}

bool CAddNodeToViewByName::canInterpret(Visualization::Item*, Visualization::Item*,
		const QStringList& commandTokens, const std::unique_ptr<Visualization::Cursor>&)
{
	return commandTokens.size() >= 1 && commandTokens.first() == name();
}

CommandResult* CAddNodeToViewByName::execute(Visualization::Item*, Visualization::Item* target,
		const QStringList& commandTokens, const std::unique_ptr<Visualization::Cursor>&)
{
	if (commandTokens.size() < 2)
		return new CommandResult(new CommandError("Please specify a node to add"));

	auto tokens = commandTokens.mid(1);
	tokens.removeAll(".");
	if (auto node = findNode(tokens,
			Model::AllTreeManagers::instance().loadedManagers().first()->root()))
	{
		target->scene()->currentViewItem()->insertNode(node);
		return new CommandResult();
	}
	else
		return new CommandResult(new CommandError("Could not find node with name " + commandTokens[1]));
}

QList<CommandSuggestion*> CAddNodeToViewByName::suggest(Visualization::Item*, Visualization::Item*,
		const QString& textSoFar, const std::unique_ptr<Visualization::Cursor>&)
{
	QList<CommandSuggestion*> suggestions;

	if (textSoFar.trimmed().startsWith("add ", Qt::CaseInsensitive))
	{
		auto nodeName = textSoFar.trimmed().mid(4);
		auto names = findNames(nodeName.split("."),
						Model::AllTreeManagers::instance().loadedManagers().first()->root());
		//Shorter names usually have less parts to the fully qualified name -> suggest them first
		std::sort(names.begin(), names.end(), [](QString first, QString second)
											{ return first.length() < second.length(); });
		//Limit the number of suggestions
		names = names.mid(0, 10);
		for (auto name : names)
			suggestions.append(new CommandSuggestion("add " + name, "Add node " + name + " to the view"));
	}
	else if (QString("add ").startsWith(textSoFar.trimmed(), Qt::CaseInsensitive) )
			suggestions.append(new CommandSuggestion("add ", "Add nodes to the current view"));

	return suggestions;
}

QStringList CAddNodeToViewByName::findNames(QStringList nameParts, Model::Node* root)
{
	QStringList result;

	//If it doesn't define a symbol, just pass it on
	if (!root->definesSymbol())
		for (auto child : root->children())
			result.append(findNames(nameParts, child));

	//If it defines a symbol, and the name matches, take the part of name it matches and search for the rest
	else if (nameParts.size() > 0 && root->symbolName().startsWith(nameParts[0])
			 && isSuggestable(root->symbolType()))
	{
		for (auto child : root->children())
			for (auto name : findNames(nameParts.mid(1), child))
				result.append(root->symbolName() + "." + name);
		if (nameParts.size() == 1)
			result.append(root->symbolName());
	}
	//If we don't have any name left, accept anything which defines a suggestable symbol
	else if (nameParts.size() == 0 && isSuggestable(root->symbolType()))
	{
		for (auto child : root->children())
			for (auto name : findNames(nameParts, child))
				result.append(root->symbolName() + "." + name);
		result.append(root->symbolName());
	}
	return result;
}

Model::Node* CAddNodeToViewByName::findNode(QStringList fullyQualifiedName, Model::Node* root)
{
	QList<Model::Node*> toSearch{root};
	int currentPart = 0;
	while (!toSearch.isEmpty() && currentPart < fullyQualifiedName.size())
	{
		auto current = toSearch.takeLast();
		if (current->definesSymbol() && current->symbolName() == fullyQualifiedName[currentPart]
				&& currentPart == fullyQualifiedName.size() - 1)
			return current;

		//If it doesn't define a symbol, check in the children
		//Else only continue the path if the name fits
		if (!current->definesSymbol())
			toSearch.append(current->children());
		else if (current->symbolName() == fullyQualifiedName[currentPart])
		{
			toSearch.clear();
			toSearch.append(current->children());
			currentPart++;
		}
	}
	return nullptr;
}

bool CAddNodeToViewByName::isSuggestable(Model::Node::SymbolTypes symbolType)
{
	return symbolType == Model::Node::METHOD || symbolType == Model::Node::CONTAINER;
}

}
<commit_msg>Can add nodes of different projects.<commit_after>/***********************************************************************************************************************
 **
 ** Copyright (c) 2015 ETH Zurich
 ** All rights reserved.
 **
 ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 ** following conditions are met:
 **
 **    * Redistributions of source code must retain the above copyright notice, this list of conditions and the
 **      following disclaimer.
 **    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 **      following disclaimer in the documentation and/or other materials provided with the distribution.
 **    * Neither the name of the ETH Zurich nor the names of its contributors may be used to endorse or promote products
 **      derived from this software without specific prior written permission.
 **
 **
 ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 ** INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 ** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 ** SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 ** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 ** WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 ** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 **
 **********************************************************************************************************************/

#include "CAddNodeToViewByName.h"

#include "VisualizationBase/src/items/ViewItem.h"
#include "ModelBase/src/model/TreeManager.h"

namespace Interaction {

CAddNodeToViewByName::CAddNodeToViewByName() : Command{"add"}{}

bool CAddNodeToViewByName::canInterpret(Visualization::Item*, Visualization::Item*,
		const QStringList& commandTokens, const std::unique_ptr<Visualization::Cursor>&)
{
	return commandTokens.size() >= 1 && commandTokens.first() == name();
}

CommandResult* CAddNodeToViewByName::execute(Visualization::Item*, Visualization::Item* target,
		const QStringList& commandTokens, const std::unique_ptr<Visualization::Cursor>&)
{
	if (commandTokens.size() < 2)
		return new CommandResult(new CommandError("Please specify a node to add"));

	auto tokens = commandTokens.mid(1);
	tokens.removeAll(".");
	for (auto manager : Model::AllTreeManagers::instance().loadedManagers())
		if (auto node = findNode(tokens, manager->root()))
		{
			target->scene()->currentViewItem()->insertNode(node);
			return new CommandResult();
		}
	return new CommandResult(new CommandError("Could not find node with name " + commandTokens[1]));
}

QList<CommandSuggestion*> CAddNodeToViewByName::suggest(Visualization::Item*, Visualization::Item*,
		const QString& textSoFar, const std::unique_ptr<Visualization::Cursor>&)
{
	QList<CommandSuggestion*> suggestions;

	if (textSoFar.trimmed().startsWith("add ", Qt::CaseInsensitive))
	{
		auto nodeName = textSoFar.trimmed().mid(4);
		QStringList names;
		for (auto manager : Model::AllTreeManagers::instance().loadedManagers())
			names.append(findNames(nodeName.split("."), manager->root()));

		//Shorter names usually have less parts to the fully qualified name -> suggest them first
		std::sort(names.begin(), names.end(), [](QString first, QString second)
											{ return first.length() < second.length(); });
		//Limit the number of suggestions
		names = names.mid(0, 10);
		for (auto name : names)
			suggestions.append(new CommandSuggestion("add " + name, "Add node " + name + " to the view"));
	}
	else if (QString("add ").startsWith(textSoFar.trimmed(), Qt::CaseInsensitive) )
			suggestions.append(new CommandSuggestion("add ", "Add nodes to the current view"));

	return suggestions;
}

QStringList CAddNodeToViewByName::findNames(QStringList nameParts, Model::Node* root)
{
	QStringList result;

	//If it doesn't define a symbol, just pass it on
	if (!root->definesSymbol())
		for (auto child : root->children())
			result.append(findNames(nameParts, child));

	//If it defines a symbol, and the name matches, take the part of name it matches and search for the rest
	else if (nameParts.size() > 0 && root->symbolName().startsWith(nameParts[0])
			 && isSuggestable(root->symbolType()))
	{
		for (auto child : root->children())
			for (auto name : findNames(nameParts.mid(1), child))
				result.append(root->symbolName() + "." + name);
		if (nameParts.size() == 1)
			result.append(root->symbolName());
	}
	//If we don't have any name left, accept anything which defines a suggestable symbol
	else if (nameParts.size() == 0 && isSuggestable(root->symbolType()))
	{
		for (auto child : root->children())
			for (auto name : findNames(nameParts, child))
				result.append(root->symbolName() + "." + name);
		result.append(root->symbolName());
	}
	return result;
}

Model::Node* CAddNodeToViewByName::findNode(QStringList fullyQualifiedName, Model::Node* root)
{
	QList<Model::Node*> toSearch{root};
	int currentPart = 0;
	while (!toSearch.isEmpty() && currentPart < fullyQualifiedName.size())
	{
		auto current = toSearch.takeLast();
		if (current->definesSymbol() && current->symbolName() == fullyQualifiedName[currentPart]
				&& currentPart == fullyQualifiedName.size() - 1)
			return current;

		//If it doesn't define a symbol, check in the children
		//Else only continue the path if the name fits
		if (!current->definesSymbol())
			toSearch.append(current->children());
		else if (current->symbolName() == fullyQualifiedName[currentPart])
		{
			toSearch.clear();
			toSearch.append(current->children());
			currentPart++;
		}
	}
	return nullptr;
}

bool CAddNodeToViewByName::isSuggestable(Model::Node::SymbolTypes symbolType)
{
	return symbolType == Model::Node::METHOD || symbolType == Model::Node::CONTAINER;
}

}
<|endoftext|>


<commit_before>#include "bulkDataCallback.h"

using namespace ACSBulkDataError;
using namespace ACSBulkDataStatus;

BulkDataCallback::BulkDataCallback()
{
    ACS_TRACE("BulkDataCallback::BulkDataCallback");

    state_m = CB_UNS;
    substate_m = CB_SUB_UNS;

    dim_m = 0;
    count_m = 0;

    //loop_m = 5;
    loop_m = 1000;
    //waitPeriod_m.set(0L, 400000L);
    waitPeriod_m.set(0L, 10L); // set to 0.01 ms to improve Receiver performance
                               // (similar to Distributor callback)

    frameCount_m = 0;

    bufParam_p = 0;

    timeout_m = false;
    working_m = false;
    error_m = false;

    errComp_p = 0;

    flowTimeout_m = 0;

    isFepAlive_m = true;
}

BulkDataCallback::~BulkDataCallback()
{
    ACS_TRACE("BulkDataCallback::~BulkDataCallback");

    if (error_m == true)
	{
	if (errComp_p != 0)
	    {
	    delete errComp_p;
	    errComp_p = 0;
	    }
	}
    error_m = false;

    if(bufParam_p)
	{
	bufParam_p->release();
	bufParam_p = 0;
	}
}

int BulkDataCallback::handle_start(void)
{

    // cout << "BulkDataCallback::handle_start - state_m: " << state_m << endl;
    //if(timeout_m == true)
    //ACS_SHORT_LOG((LM_INFO,"BulkDataCallback::handle_start - timeout_m == true !!!"));

    timeout_m = false;

    if (error_m == true)
	{
	if (errComp_p != 0)
	    {
	    delete errComp_p;
	    errComp_p = 0;
	    }
	}
    // error is cleared
    error_m = false;

    // parameter buffer is cleared (just in case)
    if(bufParam_p)
	{
	bufParam_p->release();
	bufParam_p = 0;
	}

    state_m = CB_UNS;
    substate_m = CB_SUB_UNS;

    frameCount_m = 1; // we need to wait for 1 following frame before doing anything else

    if (flowTimeout_m != 0)
	startTime_m = ACE_OS::gettimeofday();

    return 0;
}

int BulkDataCallback::handle_stop (void)
{
    //ACS_TRACE("BulkDataCallback::handle_stop");

    // cout << "CCCCCCCCCCCCCCCCC enter stop state " << state_m << " " << substate_m << endl;

    try
	{
	int locLoop;
	int res;

	locLoop = loop_m;
	while ( (frameCount_m != 0) && locLoop > 0)
	    {
	    ACE_OS::sleep(waitPeriod_m);
	    locLoop--;
	    } // we didn't receive the first frame yet

	if(state_m == CB_UNS)
	    {
	    res = cbStop();
	    return 0;
	    }

	if((state_m == CB_SEND_PARAM) || (state_m == CB_SEND_DATA))
	    {

	    if  (substate_m == CB_SUB_INIT)
		{
		substate_m = CB_SUB_UNS;
		return 0;
		}

	    if (error_m == true)
		{
		timeout_m = true;
		throw CORBA::BAD_OPERATION();
		}

	    locLoop = loop_m;
	    while((count_m != dim_m) && locLoop > 0)
		{
		ACE_OS::sleep(waitPeriod_m);
		locLoop--;
		checkFlowTimeout();

		// re-check error_m in case that is not set the first time
		if (error_m == true)
		    {
		    timeout_m = true;
		    throw CORBA::BAD_OPERATION();
		    }
		}

	    if ( locLoop == 0 )
		{
		ACS_SHORT_LOG((LM_ERROR,"BulkDataCallback::handle_stop timeout ( (%ld sec + %ld usec) * %ld ) expired for flow %s - not all data received", waitPeriod_m.sec(), waitPeriod_m.usec(), loop_m, flowname_m.c_str()));

		timeout_m = true;
		//cleaning the recv buffer
		///cleanRecvBuffer();
		//cout << "BulkDataCallback::handle_stop - handle removed: " << getHandle() << endl;
		TAO_AV_CORE::instance()->reactor()->remove_handler(getHandle(),ACE_Event_Handler::READ_MASK);
		ACS_SHORT_LOG((LM_DEBUG,"BulkDataCallback::handle_stop - receiver handler removed from the ACE reactor"));
		//ACE_OS::sleep(1);  // seems necessary to give time to remove
		                   // the handler from the reactor
		throw CORBA::TIMEOUT();
		}

	    if(state_m == CB_SEND_PARAM)
		{
		//res ignored by reactor
		if (dim_m != 0)
		    res = cbStart(bufParam_p);

		if(bufParam_p)
		    {
		    bufParam_p->release();
		    bufParam_p = 0;
		    }
		checkFlowTimeout();
		}

	    state_m = CB_UNS;
	    substate_m = CB_SUB_UNS;

	    return 0;
	    }

	}
    catch(ACSErr::ACSbaseExImpl &ex)
	{
	AVCallbackErrorExImpl err = AVCallbackErrorExImpl(ex,__FILE__,__LINE__,"BulkDataCallback::handle_stop");
	err.log();
	// add to the completion
	errComp_p = new AVCbErrorCompletion(err, __FILE__, __LINE__, "BulkDataCallback::handle_stop");
	error_m = true;

	if(bufParam_p)
	    {
	    bufParam_p->release();
	    bufParam_p = 0;
	    }

	state_m = CB_UNS;
	substate_m = CB_SUB_UNS;

	throw CORBA::BAD_OPERATION();
	//throw CORBA::TIMEOUT();
	}
    catch(CORBA::Exception &ex)
	{
	AVCallbackErrorExImpl err = AVCallbackErrorExImpl(__FILE__,__LINE__,"BulkDataCallback::handle_stop");
	err.addData("CORBA::Exception", ex._info());
	err.log();

	if (errComp_p == 0)
	    {
	    errComp_p = new AVCbErrorCompletion(err, __FILE__, __LINE__, "BulkDataCallback::handle_stop");
	    error_m = true;
	    }

	if(bufParam_p)
	    {
	    bufParam_p->release();
	    bufParam_p = 0;
	    }

	state_m = CB_UNS;
	substate_m = CB_SUB_UNS;

	throw;
	//throw CORBA::TIMEOUT();
	}
    catch(...)
	{
	AVCallbackErrorExImpl err = AVCallbackErrorExImpl(__FILE__,__LINE__,"BulkDataCallback::handle_stop");
	err.addData("UNKNOWN Exception", "Unknown ex in BulkDataCallback::handle_stop");
	err.log();
//	error_m = true;

	if(bufParam_p)
	    {
	    bufParam_p->release();
	    bufParam_p = 0;
	    }

	state_m = CB_UNS;
	substate_m = CB_SUB_UNS;

	throw;
	//throw CORBA::TIMEOUT();
	}

    state_m = CB_UNS;
    substate_m = CB_SUB_UNS;

    return 0;
}

int BulkDataCallback::handle_destroy (void)
{
    //ACS_TRACE("BulkDataCallback::handle_destroy");

    //cout << "BulkDataCallback::handle_destroy" << endl;

    isFepAlive_m = false;

    return 0;
}

int BulkDataCallback::receive_frame (ACE_Message_Block *frame, TAO_AV_frame_info *frame_info, const ACE_Addr &)
{
    //cout << "BulkDataCallback::receive_frame - state_m: " << state_m << endl;
    working_m = true;

    if(error_m == true)
	{
	///cleanRecvBuffer();
	working_m = false;
	return 0;
	}

    //ACS_SHORT_LOG((LM_INFO,"BulkDataCallback::receive_frame for flow %s", flowname_m.c_str()));
    // cout << "BulkDataCallback::receive_frame - state_m: " << state_m << endl;

    try
	{
	CORBA::ULong val1, val2;
	int res;

	if(state_m == CB_UNS)
	    {
	    char tmp[255];
	    ACE_OS::strcpy(tmp, frame->rd_ptr());

	    std::string strDataInfo(tmp);

	    std::istringstream iss(strDataInfo);

	    iss >> val1 >> val2;

	    // cout << "CCCCCCCCCCCCCCC " << val1 << " " << val2 << endl;

	    if(val1 == 1)
		{
		state_m = CB_SEND_PARAM;
		substate_m = CB_SUB_INIT;

		if(val2 != 0)
		    bufParam_p = new ACE_Message_Block(val2);
		else
		    bufParam_p = 0;
		}
	    else if(val1 == 2)
		{
		state_m = CB_SEND_DATA;
		substate_m = CB_SUB_INIT;
		}
	    else
		{
		state_m = CB_UNS;
		substate_m = CB_SUB_UNS;
		}

	    dim_m = val2;
	    count_m = 0;

	    frameCount_m = 0;
	    working_m = false;

	    return 0;
	    }

	if(state_m == CB_SEND_PARAM)
	    {

	    if ( dim_m == 0 )
		{
		res = cbStart();

		checkFlowTimeout();
		working_m = false;
		return 0;
		}

	    bufParam_p->copy(frame->rd_ptr(),frame->length());
	    count_m += frame->length();

	    working_m = false;
	    return 0;
	    }

	if (state_m == CB_SEND_DATA)
	    {
	    res = cbReceive(frame);
	    count_m += frame->length();

	    checkFlowTimeout();
	    working_m = false;
	    return 0;
	    }

	}
    catch(ACSErr::ACSbaseExImpl &ex)
	{
	AVCallbackErrorExImpl err = AVCallbackErrorExImpl(ex,__FILE__,__LINE__,"BulkDataCallback::receive_frame");
	err.log();
	// add to the completion
	errComp_p = new AVCbErrorCompletion(err, __FILE__, __LINE__, "BulkDataCallback::receive_frame");
	error_m = true;
	}
    catch(CORBA::Exception &ex)
	{
	AVCallbackErrorExImpl err = AVCallbackErrorExImpl(__FILE__,__LINE__,"BulkDataCallback::receive_frame");
	err.addData("CORBA::Exception", ex._info());
	err.log();
	error_m = true;
	}
    catch(std::exception &stdex)
        {
    	AVCallbackErrorExImpl err = AVCallbackErrorExImpl(__FILE__,__LINE__,"BulkDataCallback::receive_frame");
    	err.addData("std::exception", stdex.what());
    	err.log();
    	error_m = true;
        }
    catch(...)
	{
	AVCallbackErrorExImpl err = AVCallbackErrorExImpl(__FILE__,__LINE__,"BulkDataCallback::receive_frame");
	err.addData("UNKNOWN Exception", "Unknown ex in BulkDataCallback::receive_frame");
	err.log();
	error_m = true;
	}

    working_m = false;
    return 0;
}

void BulkDataCallback::setFlowname (const char * flowname_p)
{
    ACS_TRACE("BulkDataCallback::setFlowname");
    //ACS_SHORT_LOG((LM_INFO,"RRRRRRRRRRRRRRRRR BulkDataCallback::flowname for flow %s", flow_name));
    flowname_m = flowname_p;

    std::string flwName(flowname_p);
    std::string flwNumber(flwName, 4, 1);

    flowNumber_m = atol(flwNumber.c_str());
}

void BulkDataCallback::setReceiverName(ACE_CString recvName)
{
    recvName_m = recvName;
}

void BulkDataCallback::setSleepTime(ACE_Time_Value locWaitPeriod)
{
    ACS_TRACE("BulkDataCallback::setSleepTime");

    waitPeriod_m = locWaitPeriod;
}

void BulkDataCallback::setSafeTimeout(CORBA::ULong locLoop)
{
    ACS_TRACE("BulkDataCallback::setSafeTimeout");

    loop_m = locLoop;
}

CORBA::Boolean BulkDataCallback::isTimeout()
{
    ACS_TRACE("BulkDataCallback::isTimeout");

    return timeout_m;
}

CORBA::Boolean BulkDataCallback::isWorking()
{
    ACS_TRACE("BulkDataCallback::isWorking");

    return working_m;
}

CORBA::Boolean BulkDataCallback::isError()
{
//    ACS_TRACE("BulkDataCallback::isError");

    return error_m;
}

CompletionImpl *BulkDataCallback::getErrorCompletion()
{
    ACS_TRACE("BulkDataCallback::getErrorCompletion");

    // error is cleared; completion cannot be retrieved two times
    error_m = false;

    return errComp_p;
}

void BulkDataCallback::cleanRecvBuffer()
{
    ACE_Svc_Handler<ACE_SOCK_STREAM,ACE_NULL_SYNCH> *svch = dynamic_cast<ACE_Svc_Handler<ACE_SOCK_STREAM,ACE_NULL_SYNCH> *>(handler_);

    char buf[BUFSIZ];
    int bufSize = sizeof(buf);
    int nn = 1;
    while(nn > 0)
	{
	nn = svch->peer().recv(buf,bufSize);
	}

//    svch->peer().close();
}

void BulkDataCallback::setFlowTimeout(CORBA::ULong timeout)
{
    flowTimeout_m = timeout;
}

void BulkDataCallback::checkFlowTimeout()
{
    if(flowTimeout_m == 0)
	return;

    ACE_Time_Value elapsedTime = ACE_OS::gettimeofday() - startTime_m;
    double dtime = (elapsedTime.sec() * 1000.) + ( elapsedTime.usec() / 1000. );
    if(dtime > flowTimeout_m)
	{
	///cleanRecvBuffer();
	timeout_m = true;
	AVCbFlowTimeoutExImpl err = AVCbFlowTimeoutExImpl(__FILE__,__LINE__,"BulkDataCallback::checkFlowTimeout");
	throw err;
	}
}

void BulkDataCallback::closePeer()
{
    ACE_Svc_Handler<ACE_SOCK_STREAM,ACE_NULL_SYNCH> *svch =
	dynamic_cast<ACE_Svc_Handler<ACE_SOCK_STREAM,ACE_NULL_SYNCH> *>(handler_);

    if (svch != 0)
	{
	svch->peer().close();
	}
}

ACE_HANDLE BulkDataCallback::getHandle()
{
    ACS_TRACE("BulkDataCallback::getHandle");

    ACE_Event_Handler *event_handler = handler_->event_handler();

    ACE_HANDLE handle = event_handler->get_handle();

    return handle;
}
<commit_msg>added flow name to commented logs in case we need them at some point<commit_after>#include "bulkDataCallback.h"

using namespace ACSBulkDataError;
using namespace ACSBulkDataStatus;

BulkDataCallback::BulkDataCallback()
{
    ACS_TRACE("BulkDataCallback::BulkDataCallback");

    state_m = CB_UNS;
    substate_m = CB_SUB_UNS;

    dim_m = 0;
    count_m = 0;

    //loop_m = 5;
    loop_m = 1000;
    //waitPeriod_m.set(0L, 400000L);
    waitPeriod_m.set(0L, 10L); // set to 0.01 ms to improve Receiver performance
                               // (similar to Distributor callback)

    frameCount_m = 0;

    bufParam_p = 0;

    timeout_m = false;
    working_m = false;
    error_m = false;

    errComp_p = 0;

    flowTimeout_m = 0;

    isFepAlive_m = true;
}

BulkDataCallback::~BulkDataCallback()
{
    ACS_TRACE("BulkDataCallback::~BulkDataCallback");

    if (error_m == true)
	{
	if (errComp_p != 0)
	    {
	    delete errComp_p;
	    errComp_p = 0;
	    }
	}
    error_m = false;

    if(bufParam_p)
	{
	bufParam_p->release();
	bufParam_p = 0;
	}
}

int BulkDataCallback::handle_start(void)
{

    //cout << "BulkDataCallback::handle_start " << flowname_m << "- state_m: " << state_m << endl;
    //if(timeout_m == true)
    //ACS_SHORT_LOG((LM_INFO,"BulkDataCallback::handle_start - timeout_m == true !!!"));

    timeout_m = false;

    if (error_m == true)
	{
	if (errComp_p != 0)
	    {
	    delete errComp_p;
	    errComp_p = 0;
	    }
	}
    // error is cleared
    error_m = false;

    // parameter buffer is cleared (just in case)
    if(bufParam_p)
	{
	bufParam_p->release();
	bufParam_p = 0;
	}

    state_m = CB_UNS;
    substate_m = CB_SUB_UNS;

    frameCount_m = 1; // we need to wait for 1 following frame before doing anything else

    if (flowTimeout_m != 0)
	startTime_m = ACE_OS::gettimeofday();

    return 0;
}

int BulkDataCallback::handle_stop (void)
{
    //ACS_TRACE("BulkDataCallback::handle_stop");
    // cout << "CCCCCCCCCCCCCCCCC " << flowname_m << "enter stop state " << state_m << " " << substate_m << endl;

    try
	{
	int locLoop;
	int res;

	locLoop = loop_m;
	while ( (frameCount_m != 0) && locLoop > 0)
	    {
	    ACE_OS::sleep(waitPeriod_m);
	    locLoop--;
	    } // we didn't receive the first frame yet

	if(state_m == CB_UNS)
	    {
	    res = cbStop();
	    return 0;
	    }

	if((state_m == CB_SEND_PARAM) || (state_m == CB_SEND_DATA))
	    {

	    if  (substate_m == CB_SUB_INIT)
		{
		substate_m = CB_SUB_UNS;
		return 0;
		}

	    if (error_m == true)
		{
		timeout_m = true;
		throw CORBA::BAD_OPERATION();
		}

	    locLoop = loop_m;
	    while((count_m != dim_m) && locLoop > 0)
		{
		ACE_OS::sleep(waitPeriod_m);
		locLoop--;
		checkFlowTimeout();

		// re-check error_m in case that is not set the first time
		if (error_m == true)
		    {
		    timeout_m = true;
		    throw CORBA::BAD_OPERATION();
		    }
		}

	    if ( locLoop == 0 )
		{
		ACS_SHORT_LOG((LM_ERROR,"BulkDataCallback::handle_stop timeout ( (%ld sec + %ld usec) * %ld ) expired for flow %s - not all data received", waitPeriod_m.sec(), waitPeriod_m.usec(), loop_m, flowname_m.c_str()));

		timeout_m = true;
		//cleaning the recv buffer
		///cleanRecvBuffer();
		//cout << "BulkDataCallback::handle_stop - handle removed: " << getHandle() << endl;
		TAO_AV_CORE::instance()->reactor()->remove_handler(getHandle(),ACE_Event_Handler::READ_MASK);
		ACS_SHORT_LOG((LM_DEBUG,"BulkDataCallback::handle_stop - receiver handler removed from the ACE reactor"));
		//ACE_OS::sleep(1);  // seems necessary to give time to remove
		                   // the handler from the reactor
		throw CORBA::TIMEOUT();
		}

	    if(state_m == CB_SEND_PARAM)
		{
		//res ignored by reactor
		if (dim_m != 0)
		    res = cbStart(bufParam_p);

		if(bufParam_p)
		    {
		    bufParam_p->release();
		    bufParam_p = 0;
		    }
		checkFlowTimeout();
		}

	    state_m = CB_UNS;
	    substate_m = CB_SUB_UNS;

	    return 0;
	    }

	}
    catch(ACSErr::ACSbaseExImpl &ex)
	{
	AVCallbackErrorExImpl err = AVCallbackErrorExImpl(ex,__FILE__,__LINE__,"BulkDataCallback::handle_stop");
	err.log();
	// add to the completion
	errComp_p = new AVCbErrorCompletion(err, __FILE__, __LINE__, "BulkDataCallback::handle_stop");
	error_m = true;

	if(bufParam_p)
	    {
	    bufParam_p->release();
	    bufParam_p = 0;
	    }

	state_m = CB_UNS;
	substate_m = CB_SUB_UNS;

	throw CORBA::BAD_OPERATION();
	//throw CORBA::TIMEOUT();
	}
    catch(CORBA::Exception &ex)
	{
	AVCallbackErrorExImpl err = AVCallbackErrorExImpl(__FILE__,__LINE__,"BulkDataCallback::handle_stop");
	err.addData("CORBA::Exception", ex._info());
	err.log();

	if (errComp_p == 0)
	    {
	    errComp_p = new AVCbErrorCompletion(err, __FILE__, __LINE__, "BulkDataCallback::handle_stop");
	    error_m = true;
	    }

	if(bufParam_p)
	    {
	    bufParam_p->release();
	    bufParam_p = 0;
	    }

	state_m = CB_UNS;
	substate_m = CB_SUB_UNS;

	throw;
	//throw CORBA::TIMEOUT();
	}
    catch(...)
	{
	AVCallbackErrorExImpl err = AVCallbackErrorExImpl(__FILE__,__LINE__,"BulkDataCallback::handle_stop");
	err.addData("UNKNOWN Exception", "Unknown ex in BulkDataCallback::handle_stop");
	err.log();
//	error_m = true;

	if(bufParam_p)
	    {
	    bufParam_p->release();
	    bufParam_p = 0;
	    }

	state_m = CB_UNS;
	substate_m = CB_SUB_UNS;

	throw;
	//throw CORBA::TIMEOUT();
	}

    state_m = CB_UNS;
    substate_m = CB_SUB_UNS;

    return 0;
}

int BulkDataCallback::handle_destroy (void)
{
    //ACS_TRACE("BulkDataCallback::handle_destroy");

    //cout << "BulkDataCallback::handle_destroy" << endl;

    isFepAlive_m = false;

    return 0;
}

int BulkDataCallback::receive_frame (ACE_Message_Block *frame, TAO_AV_frame_info *frame_info, const ACE_Addr &)
{
    //cout << "BulkDataCallback::receive_frame - state_m: " << state_m << endl;
    working_m = true;

    if(error_m == true)
	{
	///cleanRecvBuffer();
	working_m = false;
	return 0;
	}

    //ACS_SHORT_LOG((LM_INFO,"BulkDataCallback::receive_frame for flow %s", flowname_m.c_str()));
    // cout << "BulkDataCallback::receive_frame - state_m: " << state_m << endl;

    try
	{
	CORBA::ULong val1, val2;
	int res;

	if(state_m == CB_UNS)
	    {
	    char tmp[255];
	    ACE_OS::strcpy(tmp, frame->rd_ptr());

	    std::string strDataInfo(tmp);

	    std::istringstream iss(strDataInfo);

	    iss >> val1 >> val2;

	    // cout << "CCCCCCCCCCCCCCC " << val1 << " " << val2 << endl;

	    if(val1 == 1)
		{
		state_m = CB_SEND_PARAM;
		substate_m = CB_SUB_INIT;

		if(val2 != 0)
		    bufParam_p = new ACE_Message_Block(val2);
		else
		    bufParam_p = 0;
		}
	    else if(val1 == 2)
		{
		state_m = CB_SEND_DATA;
		substate_m = CB_SUB_INIT;
		}
	    else
		{
		state_m = CB_UNS;
		substate_m = CB_SUB_UNS;
		}

	    dim_m = val2;
	    count_m = 0;

	    frameCount_m = 0;
	    working_m = false;

	    return 0;
	    }

	if(state_m == CB_SEND_PARAM)
	    {

	    if ( dim_m == 0 )
		{
		res = cbStart();

		checkFlowTimeout();
		working_m = false;
		return 0;
		}

	    bufParam_p->copy(frame->rd_ptr(),frame->length());
	    count_m += frame->length();

	    working_m = false;
	    return 0;
	    }

	if (state_m == CB_SEND_DATA)
	    {
	    res = cbReceive(frame);
	    count_m += frame->length();

	    checkFlowTimeout();
	    working_m = false;
	    return 0;
	    }

	}
    catch(ACSErr::ACSbaseExImpl &ex)
	{
	AVCallbackErrorExImpl err = AVCallbackErrorExImpl(ex,__FILE__,__LINE__,"BulkDataCallback::receive_frame");
	err.log();
	// add to the completion
	errComp_p = new AVCbErrorCompletion(err, __FILE__, __LINE__, "BulkDataCallback::receive_frame");
	error_m = true;
	}
    catch(CORBA::Exception &ex)
	{
	AVCallbackErrorExImpl err = AVCallbackErrorExImpl(__FILE__,__LINE__,"BulkDataCallback::receive_frame");
	err.addData("CORBA::Exception", ex._info());
	err.log();
	error_m = true;
	}
    catch(std::exception &stdex)
        {
    	AVCallbackErrorExImpl err = AVCallbackErrorExImpl(__FILE__,__LINE__,"BulkDataCallback::receive_frame");
    	err.addData("std::exception", stdex.what());
    	err.log();
    	error_m = true;
        }
    catch(...)
	{
	AVCallbackErrorExImpl err = AVCallbackErrorExImpl(__FILE__,__LINE__,"BulkDataCallback::receive_frame");
	err.addData("UNKNOWN Exception", "Unknown ex in BulkDataCallback::receive_frame");
	err.log();
	error_m = true;
	}

    working_m = false;
    return 0;
}

void BulkDataCallback::setFlowname (const char * flowname_p)
{
    ACS_TRACE("BulkDataCallback::setFlowname");
    //ACS_SHORT_LOG((LM_INFO,"RRRRRRRRRRRRRRRRR BulkDataCallback::flowname for flow %s", flow_name));
    flowname_m = flowname_p;

    std::string flwName(flowname_p);
    std::string flwNumber(flwName, 4, 1);

    flowNumber_m = atol(flwNumber.c_str());
}

void BulkDataCallback::setReceiverName(ACE_CString recvName)
{
    recvName_m = recvName;
}

void BulkDataCallback::setSleepTime(ACE_Time_Value locWaitPeriod)
{
    ACS_TRACE("BulkDataCallback::setSleepTime");

    waitPeriod_m = locWaitPeriod;
}

void BulkDataCallback::setSafeTimeout(CORBA::ULong locLoop)
{
    ACS_TRACE("BulkDataCallback::setSafeTimeout");

    loop_m = locLoop;
}

CORBA::Boolean BulkDataCallback::isTimeout()
{
    ACS_TRACE("BulkDataCallback::isTimeout");

    return timeout_m;
}

CORBA::Boolean BulkDataCallback::isWorking()
{
    ACS_TRACE("BulkDataCallback::isWorking");

    return working_m;
}

CORBA::Boolean BulkDataCallback::isError()
{
//    ACS_TRACE("BulkDataCallback::isError");

    return error_m;
}

CompletionImpl *BulkDataCallback::getErrorCompletion()
{
    ACS_TRACE("BulkDataCallback::getErrorCompletion");

    // error is cleared; completion cannot be retrieved two times
    error_m = false;

    return errComp_p;
}

void BulkDataCallback::cleanRecvBuffer()
{
    ACE_Svc_Handler<ACE_SOCK_STREAM,ACE_NULL_SYNCH> *svch = dynamic_cast<ACE_Svc_Handler<ACE_SOCK_STREAM,ACE_NULL_SYNCH> *>(handler_);

    char buf[BUFSIZ];
    int bufSize = sizeof(buf);
    int nn = 1;
    while(nn > 0)
	{
	nn = svch->peer().recv(buf,bufSize);
	}

//    svch->peer().close();
}

void BulkDataCallback::setFlowTimeout(CORBA::ULong timeout)
{
    flowTimeout_m = timeout;
}

void BulkDataCallback::checkFlowTimeout()
{
    if(flowTimeout_m == 0)
	return;

    ACE_Time_Value elapsedTime = ACE_OS::gettimeofday() - startTime_m;
    double dtime = (elapsedTime.sec() * 1000.) + ( elapsedTime.usec() / 1000. );
    if(dtime > flowTimeout_m)
	{
	///cleanRecvBuffer();
	timeout_m = true;
	AVCbFlowTimeoutExImpl err = AVCbFlowTimeoutExImpl(__FILE__,__LINE__,"BulkDataCallback::checkFlowTimeout");
	throw err;
	}
}

void BulkDataCallback::closePeer()
{
    ACE_Svc_Handler<ACE_SOCK_STREAM,ACE_NULL_SYNCH> *svch =
	dynamic_cast<ACE_Svc_Handler<ACE_SOCK_STREAM,ACE_NULL_SYNCH> *>(handler_);

    if (svch != 0)
	{
	svch->peer().close();
	}
}

ACE_HANDLE BulkDataCallback::getHandle()
{
    ACS_TRACE("BulkDataCallback::getHandle");

    ACE_Event_Handler *event_handler = handler_->event_handler();

    ACE_HANDLE handle = event_handler->get_handle();

    return handle;
}
<|endoftext|>


<commit_before>#include "Halide.h"

#include <utility>
#include <vector>

using namespace Halide;

const Expr
    _REAL = 0
  , _IMAG = 1
  ;

const int _CMPLX_SIZE = 2;

struct Complex {
  Expr real, imag;
  Complex(Expr r, Expr i) : real(r), imag(i) {}
  Complex operator+(const Complex &other) const {
    return{ real + other.real, imag + other.imag };
  }
  Complex operator*(const Complex &other) const {
    return{
        real * other.real - imag * other.imag
      , real * other.imag + imag * other.real
      };
  }
  Expr unpack(Expr c) { return select(c == _REAL, real, imag); }
};

/* Not need it ATM
inline Func flushAndContinue(const char * new_name, Func & f) {
  Func ret(new_name);
  f.compute_root();
  ret(f.args()) = f(f.args());
  return ret;
}
*/

#define P(a) a(#a)
#define IP(a,b,c) a(b,c,#a)

// We generate the kernel for GPU which works on the subset of baselines
//   having the same convolution size as we did in MS3.
// But we also simplify the algorithm a bit using
//  GCF-per-baseline (with the size equal to convolution size)
//  instead of GCF-per-point approach.
//  We've done this for all halide_scatter_1bl_1gcf... family of gridders.
int main(/* int argc, char **argv */) {
  int
      over = 8
    , grid_size = 2048
    , max_gcf_size = 128
    ;

  Param<double>
      P(scale)
    , P(wstep)
    ;
  Param<int>
      P(gcf_layer_size)
    , P(num_of_baselines)
    , P(ts_ch)
    ;
  ImageParam IP(uvwf, type_of<double>(), 3);
  ImageParam IP(vis, type_of<double>(), 3);
  ImageParam IP(gcf, type_of<double>(), 2);
  ImageParam IP(vismap, type_of<int>(), 1);

  Func
      P(uvs)
    , P(uv)
    , P(overc)
    , P(gcfoff)
    , P(bloff)
    ;

  Var P(t)
    , P(uvdim)
    , P(bl)
    ;
  const Expr
      _U = 0
    , _V = 1
    ;

  uvs(uvdim, t, bl) = uvwf(uvdim, t, bl) * scale;
  overc(uvdim, t, bl) = clamp(cast<int>(round(over * (uvs(uvdim, t, bl) - floor(uvs(uvdim, t, bl))))), 0, 7);
  uv(uvdim, t, bl) = cast<int>(round(uvs(uvdim, t, bl)) + grid_size / 2 - gcf_layer_size / 2);
  gcfoff(t, bl) = (overc(_U, t, bl) * over + overc(_V, t, bl)) * gcf_layer_size * gcf_layer_size;
  bloff(bl) = ts_ch * vismap(bl);

  RDom red(
      0, _CMPLX_SIZE
    , 0, gcf_layer_size
    , 0, ts_ch
    , 0, gcf_layer_size
    , 0, num_of_baselines
    );
  RVar P(rcmplx), P(rgcfx), P(rvis), P(rgcfy), P(rbl);
  rcmplx = red.x;
  rgcfx  = red.y;
  rvis   = red.z;
  rgcfy  = red.w;
  rbl    = red[4];

  Expr gcf_pix_off = gcfoff(rvis, rbl) + rgcfx + rgcfy * gcf_layer_size;
  Complex gcfC(
      gcf(_REAL, gcf_pix_off)
    , gcf(_IMAG, gcf_pix_off)
    );

  Complex visC(
      vis(_REAL, rvis, rbl)
    , vis(_IMAG, rvis, rbl)
    );

  Func P(uvg);
  Var P(cmplx)
    , P(x)
    , P(y)
    ;
  uvg(cmplx, x, y) = undef<double>();

  Expr
      clampedU = clamp(uv(_U, rvis, rbl), 0, grid_size - 1 - max_gcf_size)
    , clampedV = clamp(uv(_V, rvis, rbl), 0, grid_size - 1 - max_gcf_size)
    , U = clampedU + rgcfx
    , V = clampedV + rgcfy
    ;

  uvg(
      rcmplx
    , U
    , V
    ) += (visC * gcfC).unpack(rcmplx)
    ;

  uvg.bound(cmplx, 0, _CMPLX_SIZE);
  uvg.bound(x, 0, grid_size);
  uvg.bound(y, 0, grid_size);

  std::vector<Halide::Argument> compile_args = {
      scale
    , wstep
    , num_of_baselines
    , ts_ch
    , vismap
    , uvwf
    , vis
    , gcf_layer_size
    , gcf
  };
  uvg.compile_to_lowered_stmt("uvg11.html", compile_args, HTML);
  uvg.compile_to_file(
      "uvg11_full"
    , compile_args
    , get_target_from_environment()
        .with_feature(Target::CUDA)
        .with_feature(Target::CUDACapability35)
    );
}
<commit_msg>Forgot to turn baseline mapping on.<commit_after>#include "Halide.h"

#include <utility>
#include <vector>

using namespace Halide;

const Expr
    _REAL = 0
  , _IMAG = 1
  ;

const int _CMPLX_SIZE = 2;

struct Complex {
  Expr real, imag;
  Complex(Expr r, Expr i) : real(r), imag(i) {}
  Complex operator+(const Complex &other) const {
    return{ real + other.real, imag + other.imag };
  }
  Complex operator*(const Complex &other) const {
    return{
        real * other.real - imag * other.imag
      , real * other.imag + imag * other.real
      };
  }
  Expr unpack(Expr c) { return select(c == _REAL, real, imag); }
};

/* Not need it ATM
inline Func flushAndContinue(const char * new_name, Func & f) {
  Func ret(new_name);
  f.compute_root();
  ret(f.args()) = f(f.args());
  return ret;
}
*/

#define P(a) a(#a)
#define IP(a,b,c) a(b,c,#a)

// We generate the kernel for GPU which works on the subset of baselines
//   having the same convolution size as we did in MS3.
// But we also simplify the algorithm a bit using
//  GCF-per-baseline (with the size equal to convolution size)
//  instead of GCF-per-point approach.
//  We've done this for all halide_scatter_1bl_1gcf... family of gridders.
int main(/* int argc, char **argv */) {
  int
      over = 8
    , grid_size = 2048
    , max_gcf_size = 128
    ;

  Param<double>
      P(scale)
    , P(wstep)
    ;
  Param<int>
      P(gcf_layer_size)
    , P(num_of_baselines)
    , P(ts_ch)
    ;
  ImageParam IP(uvwf, type_of<double>(), 3);
  ImageParam IP(vis, type_of<double>(), 3);
  ImageParam IP(gcf, type_of<double>(), 2);
  ImageParam IP(vismap, type_of<int>(), 1);

  Func
      P(uvs)
    , P(uv)
    , P(overc)
    , P(gcfoff)
    , P(bloff)
    ;

  Var P(t)
    , P(uvdim)
    , P(bl)
    ;
  const Expr
      _U = 0
    , _V = 1
    ;

  uvs(uvdim, t, bl) = uvwf(uvdim, t, bl) * scale;
  overc(uvdim, t, bl) = clamp(cast<int>(round(over * (uvs(uvdim, t, bl) - floor(uvs(uvdim, t, bl))))), 0, 7);
  uv(uvdim, t, bl) = cast<int>(round(uvs(uvdim, t, bl)) + grid_size / 2 - gcf_layer_size / 2);
  gcfoff(t, bl) = (overc(_U, t, bl) * over + overc(_V, t, bl)) * gcf_layer_size * gcf_layer_size;
  bloff(bl) = clamp(ts_ch * vismap(bl), 0, ts_ch * num_of_baselines);

  RDom red(
      0, _CMPLX_SIZE
    , 0, gcf_layer_size
    , 0, ts_ch
    , 0, gcf_layer_size
    , 0, num_of_baselines
    );
  RVar P(rcmplx), P(rgcfx), P(rvis), P(rgcfy), P(rbl0);
  rcmplx = red.x;
  rgcfx  = red.y;
  rvis   = red.z;
  rgcfy  = red.w;
  rbl0   = red[4];
  Expr rbl = bloff(rbl0);

  Expr gcf_pix_off = gcfoff(rvis, rbl) + rgcfx + rgcfy * gcf_layer_size;
  Complex gcfC(
      gcf(_REAL, gcf_pix_off)
    , gcf(_IMAG, gcf_pix_off)
    );

  Complex visC(
      vis(_REAL, rvis, rbl)
    , vis(_IMAG, rvis, rbl)
    );

  Func P(uvg);
  Var P(cmplx)
    , P(x)
    , P(y)
    ;
  uvg(cmplx, x, y) = undef<double>();

  Expr
      clampedU = clamp(uv(_U, rvis, rbl), 0, grid_size - 1 - max_gcf_size)
    , clampedV = clamp(uv(_V, rvis, rbl), 0, grid_size - 1 - max_gcf_size)
    , U = clampedU + rgcfx
    , V = clampedV + rgcfy
    ;

  uvg(
      rcmplx
    , U
    , V
    ) += (visC * gcfC).unpack(rcmplx)
    ;

  uvg.bound(cmplx, 0, _CMPLX_SIZE);
  uvg.bound(x, 0, grid_size);
  uvg.bound(y, 0, grid_size);

  std::vector<Halide::Argument> compile_args = {
      scale
    , wstep
    , num_of_baselines
    , ts_ch
    , vismap
    , uvwf
    , vis
    , gcf_layer_size
    , gcf
  };
  uvg.compile_to_lowered_stmt("uvg11.html", compile_args, HTML);
  uvg.compile_to_file(
      "uvg11_full"
    , compile_args
    , get_target_from_environment()
        .with_feature(Target::CUDA)
        .with_feature(Target::CUDACapability35)
    );
}
<|endoftext|>


<commit_before>//
// Copyright 2012 Francisco Jerez
//
// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
// THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
// WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF
// OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//

#include "api/util.hpp"
#include "core/memory.hpp"
#include "core/format.hpp"

using namespace clover;

PUBLIC cl_mem
clCreateBuffer(cl_context ctx, cl_mem_flags flags, size_t size,
               void *host_ptr, cl_int *errcode_ret) try {
   if (!ctx)
      throw error(CL_INVALID_CONTEXT);

   if (bool(host_ptr) != bool(flags & (CL_MEM_USE_HOST_PTR |
                                       CL_MEM_COPY_HOST_PTR)))
      throw error(CL_INVALID_HOST_PTR);

   if (!size)
      throw error(CL_INVALID_BUFFER_SIZE);

   if (flags & ~(CL_MEM_WRITE_ONLY | CL_MEM_READ_ONLY |
                 CL_MEM_USE_HOST_PTR | CL_MEM_ALLOC_HOST_PTR |
                 CL_MEM_COPY_HOST_PTR))
      throw error(CL_INVALID_VALUE);

   ret_error(errcode_ret, CL_SUCCESS);
   return new root_buffer(*ctx, flags, size, host_ptr);

} catch (error &e) {
   ret_error(errcode_ret, e);
   return NULL;
}

PUBLIC cl_mem
clCreateSubBuffer(cl_mem obj, cl_mem_flags flags, cl_buffer_create_type op,
                  const void *op_info, cl_int *errcode_ret) try {
   root_buffer *parent = dynamic_cast<root_buffer *>(obj);

   if (!parent)
      throw error(CL_INVALID_MEM_OBJECT);

   if ((flags & (CL_MEM_USE_HOST_PTR |
                 CL_MEM_ALLOC_HOST_PTR |
                 CL_MEM_COPY_HOST_PTR)) ||
       (~flags & parent->flags() & (CL_MEM_READ_ONLY |
                                    CL_MEM_WRITE_ONLY)))
      throw error(CL_INVALID_VALUE);

   if (op == CL_BUFFER_CREATE_TYPE_REGION) {
      const cl_buffer_region *reg = (const cl_buffer_region *)op_info;

      if (!reg ||
          reg->origin > parent->size() ||
          reg->origin + reg->size > parent->size())
         throw error(CL_INVALID_VALUE);

      if (!reg->size)
         throw error(CL_INVALID_BUFFER_SIZE);

      ret_error(errcode_ret, CL_SUCCESS);
      return new sub_buffer(*parent, flags, reg->origin, reg->size);

   } else {
      throw error(CL_INVALID_VALUE);
   }

} catch (error &e) {
   ret_error(errcode_ret, e);
   return NULL;
}

PUBLIC cl_mem
clCreateImage2D(cl_context ctx, cl_mem_flags flags,
                const cl_image_format *format,
                size_t width, size_t height, size_t row_pitch,
                void *host_ptr, cl_int *errcode_ret) try {
   if (!ctx)
      throw error(CL_INVALID_CONTEXT);

   if (flags & ~(CL_MEM_WRITE_ONLY | CL_MEM_READ_ONLY |
                 CL_MEM_USE_HOST_PTR | CL_MEM_ALLOC_HOST_PTR |
                 CL_MEM_COPY_HOST_PTR))
      throw error(CL_INVALID_VALUE);

   if (!format)
      throw error(CL_INVALID_IMAGE_FORMAT_DESCRIPTOR);

   if (width < 1 || height < 1)
      throw error(CL_INVALID_IMAGE_SIZE);

   if (bool(host_ptr) != bool(flags & (CL_MEM_USE_HOST_PTR |
                                       CL_MEM_COPY_HOST_PTR)))
      throw error(CL_INVALID_HOST_PTR);

   if (!supported_formats(ctx, CL_MEM_OBJECT_IMAGE2D).count(*format))
      throw error(CL_IMAGE_FORMAT_NOT_SUPPORTED);

   ret_error(errcode_ret, CL_SUCCESS);
   return new image2d(*ctx, flags, format, width, height,
                      row_pitch, host_ptr);

} catch (error &e) {
   ret_error(errcode_ret, e);
   return NULL;
}

PUBLIC cl_mem
clCreateImage3D(cl_context ctx, cl_mem_flags flags,
                const cl_image_format *format,
                size_t width, size_t height, size_t depth,
                size_t row_pitch, size_t slice_pitch,
                void *host_ptr, cl_int *errcode_ret) try {
   if (!ctx)
      throw error(CL_INVALID_CONTEXT);

   if (flags & ~(CL_MEM_WRITE_ONLY | CL_MEM_READ_ONLY |
                 CL_MEM_USE_HOST_PTR | CL_MEM_ALLOC_HOST_PTR |
                 CL_MEM_COPY_HOST_PTR))
      throw error(CL_INVALID_VALUE);

   if (!format)
      throw error(CL_INVALID_IMAGE_FORMAT_DESCRIPTOR);

   if (width < 1 || height < 1 || depth < 2)
      throw error(CL_INVALID_IMAGE_SIZE);

   if (bool(host_ptr) != bool(flags & (CL_MEM_USE_HOST_PTR |
                                       CL_MEM_COPY_HOST_PTR)))
      throw error(CL_INVALID_HOST_PTR);

   if (!supported_formats(ctx, CL_MEM_OBJECT_IMAGE3D).count(*format))
      throw error(CL_IMAGE_FORMAT_NOT_SUPPORTED);

   ret_error(errcode_ret, CL_SUCCESS);
   return new image3d(*ctx, flags, format, width, height, depth,
                      row_pitch, slice_pitch, host_ptr);

} catch (error &e) {
   ret_error(errcode_ret, e);
   return NULL;
}

PUBLIC cl_int
clGetSupportedImageFormats(cl_context ctx, cl_mem_flags flags,
                           cl_mem_object_type type, cl_uint count,
                           cl_image_format *buf, cl_uint *count_ret) try {
   if (!ctx)
      throw error(CL_INVALID_CONTEXT);

   if (flags & ~(CL_MEM_WRITE_ONLY | CL_MEM_READ_ONLY |
                 CL_MEM_USE_HOST_PTR | CL_MEM_ALLOC_HOST_PTR |
                 CL_MEM_COPY_HOST_PTR))
      throw error(CL_INVALID_VALUE);

   if (!count && buf)
      throw error(CL_INVALID_VALUE);

   auto formats = supported_formats(ctx, type);

   if (buf)
      std::copy_n(formats.begin(), std::min((cl_uint)formats.size(), count),
                  buf);
   if (count_ret)
      *count_ret = formats.size();

   return CL_SUCCESS;

} catch (error &e) {
   return e.get();
}

PUBLIC cl_int
clGetMemObjectInfo(cl_mem obj, cl_mem_info param,
                   size_t size, void *buf, size_t *size_ret) {
   if (!obj)
      return CL_INVALID_MEM_OBJECT;

   switch (param) {
   case CL_MEM_TYPE:
      return scalar_property<cl_mem_object_type>(buf, size, size_ret,
                                                 obj->type());

   case CL_MEM_FLAGS:
      return scalar_property<cl_mem_flags>(buf, size, size_ret, obj->flags());

   case CL_MEM_SIZE:
      return scalar_property<size_t>(buf, size, size_ret, obj->size());

   case CL_MEM_HOST_PTR:
      return scalar_property<void *>(buf, size, size_ret, obj->host_ptr());

   case CL_MEM_MAP_COUNT:
      return scalar_property<cl_uint>(buf, size, size_ret, 0);

   case CL_MEM_REFERENCE_COUNT:
      return scalar_property<cl_uint>(buf, size, size_ret, obj->ref_count());

   case CL_MEM_CONTEXT:
      return scalar_property<cl_context>(buf, size, size_ret, &obj->ctx);

   case CL_MEM_ASSOCIATED_MEMOBJECT: {
      sub_buffer *sub = dynamic_cast<sub_buffer *>(obj);
      return scalar_property<cl_mem>(buf, size, size_ret,
                                     (sub ? &sub->parent : NULL));
   }
   case CL_MEM_OFFSET: {
      sub_buffer *sub = dynamic_cast<sub_buffer *>(obj);
      return scalar_property<size_t>(buf, size, size_ret,
                                     (sub ? sub->offset() : 0));
   }
   default:
      return CL_INVALID_VALUE;
   }
}

PUBLIC cl_int
clGetImageInfo(cl_mem obj, cl_image_info param,
               size_t size, void *buf, size_t *size_ret) {
   image *img = dynamic_cast<image *>(obj);
   if (!img)
      return CL_INVALID_MEM_OBJECT;

   switch (param) {
   case CL_IMAGE_FORMAT:
      return scalar_property<cl_image_format>(buf, size, size_ret,
                                              img->format());

   case CL_IMAGE_ELEMENT_SIZE:
      return scalar_property<size_t>(buf, size, size_ret, 0);

   case CL_IMAGE_ROW_PITCH:
      return scalar_property<size_t>(buf, size, size_ret, img->row_pitch());

   case CL_IMAGE_SLICE_PITCH:
      return scalar_property<size_t>(buf, size, size_ret, img->slice_pitch());

   case CL_IMAGE_WIDTH:
      return scalar_property<size_t>(buf, size, size_ret, img->width());

   case CL_IMAGE_HEIGHT:
      return scalar_property<size_t>(buf, size, size_ret, img->height());

   case CL_IMAGE_DEPTH:
      return scalar_property<size_t>(buf, size, size_ret, img->depth());

   default:
      return CL_INVALID_VALUE;
   }
}

PUBLIC cl_int
clRetainMemObject(cl_mem obj) {
   if (!obj)
      return CL_INVALID_MEM_OBJECT;

   obj->retain();
   return CL_SUCCESS;
}

PUBLIC cl_int
clReleaseMemObject(cl_mem obj) {
   if (!obj)
      return CL_INVALID_MEM_OBJECT;

   if (obj->release())
      delete obj;

   return CL_SUCCESS;
}

PUBLIC cl_int
clSetMemObjectDestructorCallback(cl_mem obj,
                                 void (CL_CALLBACK *pfn_notify)(cl_mem, void *),
                                 void *user_data) {
   if (!obj)
      return CL_INVALID_MEM_OBJECT;

   if (!pfn_notify)
      return CL_INVALID_VALUE;

   obj->destroy_notify([=]{ pfn_notify(obj, user_data); });

   return CL_SUCCESS;
}
<commit_msg>clover: Accept CL_MEM_READ_WRITE flag<commit_after>//
// Copyright 2012 Francisco Jerez
//
// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
// THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
// WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF
// OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//

#include "api/util.hpp"
#include "core/memory.hpp"
#include "core/format.hpp"

using namespace clover;

PUBLIC cl_mem
clCreateBuffer(cl_context ctx, cl_mem_flags flags, size_t size,
               void *host_ptr, cl_int *errcode_ret) try {
   if (!ctx)
      throw error(CL_INVALID_CONTEXT);

   if (bool(host_ptr) != bool(flags & (CL_MEM_USE_HOST_PTR |
                                       CL_MEM_COPY_HOST_PTR)))
      throw error(CL_INVALID_HOST_PTR);

   if (!size)
      throw error(CL_INVALID_BUFFER_SIZE);

   if (flags & ~(CL_MEM_READ_WRITE | CL_MEM_WRITE_ONLY | CL_MEM_READ_ONLY |
                 CL_MEM_USE_HOST_PTR | CL_MEM_ALLOC_HOST_PTR |
                 CL_MEM_COPY_HOST_PTR))
      throw error(CL_INVALID_VALUE);

   ret_error(errcode_ret, CL_SUCCESS);
   return new root_buffer(*ctx, flags, size, host_ptr);

} catch (error &e) {
   ret_error(errcode_ret, e);
   return NULL;
}

PUBLIC cl_mem
clCreateSubBuffer(cl_mem obj, cl_mem_flags flags, cl_buffer_create_type op,
                  const void *op_info, cl_int *errcode_ret) try {
   root_buffer *parent = dynamic_cast<root_buffer *>(obj);

   if (!parent)
      throw error(CL_INVALID_MEM_OBJECT);

   if ((flags & (CL_MEM_USE_HOST_PTR |
                 CL_MEM_ALLOC_HOST_PTR |
                 CL_MEM_COPY_HOST_PTR)) ||
       (~flags & parent->flags() & (CL_MEM_READ_ONLY |
                                    CL_MEM_WRITE_ONLY)))
      throw error(CL_INVALID_VALUE);

   if (op == CL_BUFFER_CREATE_TYPE_REGION) {
      const cl_buffer_region *reg = (const cl_buffer_region *)op_info;

      if (!reg ||
          reg->origin > parent->size() ||
          reg->origin + reg->size > parent->size())
         throw error(CL_INVALID_VALUE);

      if (!reg->size)
         throw error(CL_INVALID_BUFFER_SIZE);

      ret_error(errcode_ret, CL_SUCCESS);
      return new sub_buffer(*parent, flags, reg->origin, reg->size);

   } else {
      throw error(CL_INVALID_VALUE);
   }

} catch (error &e) {
   ret_error(errcode_ret, e);
   return NULL;
}

PUBLIC cl_mem
clCreateImage2D(cl_context ctx, cl_mem_flags flags,
                const cl_image_format *format,
                size_t width, size_t height, size_t row_pitch,
                void *host_ptr, cl_int *errcode_ret) try {
   if (!ctx)
      throw error(CL_INVALID_CONTEXT);

   if (flags & ~(CL_MEM_READ_WRITE | CL_MEM_WRITE_ONLY | CL_MEM_READ_ONLY |
                 CL_MEM_USE_HOST_PTR | CL_MEM_ALLOC_HOST_PTR |
                 CL_MEM_COPY_HOST_PTR))
      throw error(CL_INVALID_VALUE);

   if (!format)
      throw error(CL_INVALID_IMAGE_FORMAT_DESCRIPTOR);

   if (width < 1 || height < 1)
      throw error(CL_INVALID_IMAGE_SIZE);

   if (bool(host_ptr) != bool(flags & (CL_MEM_USE_HOST_PTR |
                                       CL_MEM_COPY_HOST_PTR)))
      throw error(CL_INVALID_HOST_PTR);

   if (!supported_formats(ctx, CL_MEM_OBJECT_IMAGE2D).count(*format))
      throw error(CL_IMAGE_FORMAT_NOT_SUPPORTED);

   ret_error(errcode_ret, CL_SUCCESS);
   return new image2d(*ctx, flags, format, width, height,
                      row_pitch, host_ptr);

} catch (error &e) {
   ret_error(errcode_ret, e);
   return NULL;
}

PUBLIC cl_mem
clCreateImage3D(cl_context ctx, cl_mem_flags flags,
                const cl_image_format *format,
                size_t width, size_t height, size_t depth,
                size_t row_pitch, size_t slice_pitch,
                void *host_ptr, cl_int *errcode_ret) try {
   if (!ctx)
      throw error(CL_INVALID_CONTEXT);

   if (flags & ~(CL_MEM_READ_WRITE | CL_MEM_WRITE_ONLY | CL_MEM_READ_ONLY |
                 CL_MEM_USE_HOST_PTR | CL_MEM_ALLOC_HOST_PTR |
                 CL_MEM_COPY_HOST_PTR))
      throw error(CL_INVALID_VALUE);

   if (!format)
      throw error(CL_INVALID_IMAGE_FORMAT_DESCRIPTOR);

   if (width < 1 || height < 1 || depth < 2)
      throw error(CL_INVALID_IMAGE_SIZE);

   if (bool(host_ptr) != bool(flags & (CL_MEM_USE_HOST_PTR |
                                       CL_MEM_COPY_HOST_PTR)))
      throw error(CL_INVALID_HOST_PTR);

   if (!supported_formats(ctx, CL_MEM_OBJECT_IMAGE3D).count(*format))
      throw error(CL_IMAGE_FORMAT_NOT_SUPPORTED);

   ret_error(errcode_ret, CL_SUCCESS);
   return new image3d(*ctx, flags, format, width, height, depth,
                      row_pitch, slice_pitch, host_ptr);

} catch (error &e) {
   ret_error(errcode_ret, e);
   return NULL;
}

PUBLIC cl_int
clGetSupportedImageFormats(cl_context ctx, cl_mem_flags flags,
                           cl_mem_object_type type, cl_uint count,
                           cl_image_format *buf, cl_uint *count_ret) try {
   if (!ctx)
      throw error(CL_INVALID_CONTEXT);

   if (flags & ~(CL_MEM_READ_WRITE | CL_MEM_WRITE_ONLY | CL_MEM_READ_ONLY |
                 CL_MEM_USE_HOST_PTR | CL_MEM_ALLOC_HOST_PTR |
                 CL_MEM_COPY_HOST_PTR))
      throw error(CL_INVALID_VALUE);

   if (!count && buf)
      throw error(CL_INVALID_VALUE);

   auto formats = supported_formats(ctx, type);

   if (buf)
      std::copy_n(formats.begin(), std::min((cl_uint)formats.size(), count),
                  buf);
   if (count_ret)
      *count_ret = formats.size();

   return CL_SUCCESS;

} catch (error &e) {
   return e.get();
}

PUBLIC cl_int
clGetMemObjectInfo(cl_mem obj, cl_mem_info param,
                   size_t size, void *buf, size_t *size_ret) {
   if (!obj)
      return CL_INVALID_MEM_OBJECT;

   switch (param) {
   case CL_MEM_TYPE:
      return scalar_property<cl_mem_object_type>(buf, size, size_ret,
                                                 obj->type());

   case CL_MEM_FLAGS:
      return scalar_property<cl_mem_flags>(buf, size, size_ret, obj->flags());

   case CL_MEM_SIZE:
      return scalar_property<size_t>(buf, size, size_ret, obj->size());

   case CL_MEM_HOST_PTR:
      return scalar_property<void *>(buf, size, size_ret, obj->host_ptr());

   case CL_MEM_MAP_COUNT:
      return scalar_property<cl_uint>(buf, size, size_ret, 0);

   case CL_MEM_REFERENCE_COUNT:
      return scalar_property<cl_uint>(buf, size, size_ret, obj->ref_count());

   case CL_MEM_CONTEXT:
      return scalar_property<cl_context>(buf, size, size_ret, &obj->ctx);

   case CL_MEM_ASSOCIATED_MEMOBJECT: {
      sub_buffer *sub = dynamic_cast<sub_buffer *>(obj);
      return scalar_property<cl_mem>(buf, size, size_ret,
                                     (sub ? &sub->parent : NULL));
   }
   case CL_MEM_OFFSET: {
      sub_buffer *sub = dynamic_cast<sub_buffer *>(obj);
      return scalar_property<size_t>(buf, size, size_ret,
                                     (sub ? sub->offset() : 0));
   }
   default:
      return CL_INVALID_VALUE;
   }
}

PUBLIC cl_int
clGetImageInfo(cl_mem obj, cl_image_info param,
               size_t size, void *buf, size_t *size_ret) {
   image *img = dynamic_cast<image *>(obj);
   if (!img)
      return CL_INVALID_MEM_OBJECT;

   switch (param) {
   case CL_IMAGE_FORMAT:
      return scalar_property<cl_image_format>(buf, size, size_ret,
                                              img->format());

   case CL_IMAGE_ELEMENT_SIZE:
      return scalar_property<size_t>(buf, size, size_ret, 0);

   case CL_IMAGE_ROW_PITCH:
      return scalar_property<size_t>(buf, size, size_ret, img->row_pitch());

   case CL_IMAGE_SLICE_PITCH:
      return scalar_property<size_t>(buf, size, size_ret, img->slice_pitch());

   case CL_IMAGE_WIDTH:
      return scalar_property<size_t>(buf, size, size_ret, img->width());

   case CL_IMAGE_HEIGHT:
      return scalar_property<size_t>(buf, size, size_ret, img->height());

   case CL_IMAGE_DEPTH:
      return scalar_property<size_t>(buf, size, size_ret, img->depth());

   default:
      return CL_INVALID_VALUE;
   }
}

PUBLIC cl_int
clRetainMemObject(cl_mem obj) {
   if (!obj)
      return CL_INVALID_MEM_OBJECT;

   obj->retain();
   return CL_SUCCESS;
}

PUBLIC cl_int
clReleaseMemObject(cl_mem obj) {
   if (!obj)
      return CL_INVALID_MEM_OBJECT;

   if (obj->release())
      delete obj;

   return CL_SUCCESS;
}

PUBLIC cl_int
clSetMemObjectDestructorCallback(cl_mem obj,
                                 void (CL_CALLBACK *pfn_notify)(cl_mem, void *),
                                 void *user_data) {
   if (!obj)
      return CL_INVALID_MEM_OBJECT;

   if (!pfn_notify)
      return CL_INVALID_VALUE;

   obj->destroy_notify([=]{ pfn_notify(obj, user_data); });

   return CL_SUCCESS;
}
<|endoftext|>