<commit_before>#ifndef _KUKA_FRI_CLIENT_DATA
#define _KUKA_FRI_CLIENT_DATA

#include <boost/range/algorithm/copy.hpp>
#include <boost/range/algorithm/transform.hpp>
#include "friClientData.h"
#include "grl/KukaFRI.hpp"
#include "grl/KukaFRIThreadSeparator.hpp"
#include <boost/log/trivial.hpp>

/// @todo move back to KukaFRIClientDataTest.cpp
template<typename T,size_t N>
std::ostream& operator<< (std::ostream& out, const boost::container::static_vector<T,N>& v) {
    out << "[";
    size_t last = v.size() - 1;
    for(size_t i = 0; i < v.size(); ++i) {
        out << v[i];
        if (i != last)
            out << ", ";
    }
    out << "]";
    return out;
}

/// @todo move back to KukaFRIClientDataTest.cpp
template<typename T,std::size_t U>
inline boost::log::formatting_ostream& operator<<(boost::log::formatting_ostream& out,  boost::container::static_vector<T,U>& v)
{
    out << "[";
    size_t last = v.size() - 1;
    for(size_t i = 0; i < v.size(); ++i) {
        out << v[i];
        if (i != last)
            out << ", ";
    }
    out << "]";
    return out;
}

namespace grl { namespace robot { namespace arm {

    // Decode message buffer (using nanopb decoder)
void decode(KUKA::FRI::ClientData& friData, std::size_t msg_size){
    // The decoder was given a pointer to the monitoringMessage at initialization
    if (!friData.decoder.decode(friData.receiveBuffer, msg_size)) {
        throw std::runtime_error( "Error decoding received data");
    }

    // check message type
    if (friData.expectedMonitorMsgID != friData.monitoringMsg.header.messageIdentifier)
    {
		throw std::invalid_argument(std::string("KukaFRI.hpp: Problem reading buffer, id code: ") +
			                         boost::lexical_cast<std::string>(static_cast<int>(friData.monitoringMsg.header.messageIdentifier)) +
									std::string(" does not match expected id code: ") +
				                    boost::lexical_cast<std::string>(static_cast<int>(friData.expectedMonitorMsgID)) + std::string("\n")
									);
        return;
    }

    friData.lastState = grl::robot::arm::get(friData.monitoringMsg,KUKA::FRI::ESessionState());
}

/////////////////////////////////
//  moving out to user code
//    // copy current joint position to commanded position
//	if ((state.ipoJointPosition.size() == KUKA::LBRState::NUM_DOF) &&
//        (((friData.sessionState == KUKA::FRI::COMMANDING_WAIT)) || (state.sessionState == KUKA::FRI::COMMANDING_ACTIVE))
//       ) {
//
//         KukaState::joint_state jointStateToCommand;
//        // vector addition between ipoJointPosition and ipoJointPositionOffsets, copying the result into jointStateToCommand
//        boost::transform ( state.ipoJointPosition, state.ipoJointPositionOffsets, std::back_inserter(jointStateToCommand), std::plus<KukaState::joint_state::value_type>());
//
//        // copy the commanded position into the command message
//        set(friData.commandMsg, jointStateToCommand, grl::revolute_joint_angle_open_chain_command_tag());
//
//		//BOOST_LOG_TRIVIAL(trace) << "ENCODE position: " << state.position << " connectionQuality: " << state.connectionQuality << " operationMode: " << state.operationMode << " sessionState: " << state.sessionState << " driveState: " << state.driveState << " ipoJointPosition: " << state.ipoJointPosition << " ipoJointPositionOffsets: " << state.ipoJointPositionOffsets << " jointStateToCommand: " << jointStateToCommand << "\n";
//    }else {
//        set(friData.commandMsg, state.commandedPosition, grl::revolute_joint_angle_open_chain_command_tag());
//    }

/// encode data in the class into the send buffer
std::size_t encode(KUKA::FRI::ClientData& friData,boost::system::error_code& ec){
    // reset send counter
    friData.lastSendCounter = 0;

    // set sequence counters
    friData.commandMsg.header.sequenceCounter = friData.sequenceCounter++;
    friData.commandMsg.header.reflectedSequenceCounter = friData.monitoringMsg.header.sequenceCounter;

	int buffersize = KUKA::FRI::FRI_COMMAND_MSG_MAX_SIZE;
    if (!friData.encoder.encode(friData.sendBuffer, buffersize)){
        // @todo figure out PB_GET_ERROR
        ec = boost::system::errc::make_error_code(boost::system::errc::bad_message);
        return 0;
    }

	return buffersize;
}

/// @pre socket must already have the endpoint resolved and "connected". While udp is technically stateless the asio socket supports the connection api components for convenience.
void update_state(boost::asio::ip::udp::socket& socket, KUKA::FRI::ClientData& friData, boost::system::error_code& receive_ec,std::size_t& receive_bytes_transferred, boost::system::error_code& send_ec, std::size_t& send_bytes_transferred){

    int message_flags = 0;
	receive_bytes_transferred = socket.receive(boost::asio::buffer(friData.receiveBuffer,KUKA::FRI::FRI_MONITOR_MSG_MAX_SIZE),message_flags,receive_ec);
	decode(friData,receive_bytes_transferred);

    friData.lastSendCounter++;
    // Check whether to send a response
    if (friData.lastSendCounter >= friData.monitoringMsg.connectionInfo.receiveMultiplier){
    	send_bytes_transferred = encode(friData,send_ec);
        if(send_ec) return;
		socket.send(boost::asio::buffer(friData.sendBuffer,send_bytes_transferred), message_flags, send_ec);
    }
}

class KukaFRIClientDataDriver : public std::enable_shared_from_this<KukaFRIClientDataDriver>, public KukaFRI {

public:

    using KukaFRI::ParamIndex;
    using KukaFRI::ThreadingRunMode;
    using KukaFRI::Params;
    using KukaFRI::defaultParams;

	KukaFRIClientDataDriver(boost::asio::io_service& ios, Params params = defaultParams())
        :
        isConnectionEstablished_(false),
        io_service_(ios)
    {
      construct(params);
	}

	KukaFRIClientDataDriver(Params params = defaultParams())
        :
        isConnectionEstablished_(false),
        optional_internal_io_service_P(new boost::asio::io_service),
        io_service_(*optional_internal_io_service_P)
    {
        construct(params);
	}

    /// Call this to initialize the object
    void construct(Params params = defaultParams())
    {

        try
        {
            connect(params, io_service_);
            // start up the driver thread since the io_service_ is internal only
            if(std::get<is_running_automatically>(params))
            {
              driver_threadP_.reset(new std::thread([&]{ update(); }));
            }

        } catch( boost::exception &e) {
            add_details_to_connection_error(e,params);
            throw;
        }

    }

    /// @brief blocking call to communicate with the robot continuously
    /// @pre construct() should
    void run(){
      update();
    }

    /// @brief Updates the passed friData shared pointer to a pointer to newly updated data, plus any errors that occurred.
    ///
    /// @note This function is not thread safe cannot be called simultaneously from multiple threads.
    ///
    /// @param[in,out] friData the pointer to update with new state and optional input state.
    ///
    /// @pre If friData!=nullptr it is assumed valid for use and this class will take control of the object.
    void update_state(std::shared_ptr<KUKA::FRI::ClientData>& friData, boost::system::error_code& receive_ec,std::size_t& receive_bytes_transferred, boost::system::error_code& send_ec, std::size_t& send_bytes_transferred){

        // ensure we have valid data for future updates
        std::shared_ptr<KUKA::FRI::ClientData> tempFriData;
        if(friData.get() == nullptr)
        {
          tempFriData = std::make_shared<KUKA::FRI::ClientData>(KUKA::LBRState::NUM_DOF);
        }
        else
        {
          tempFriData = friData;
        }
        std::atomic_exchange(&userThreadStateP_,latestStateP_);

        if(userThreadStateP_.get()!=nullptr)
        {
          std::tie(friData,receive_ec,receive_bytes_transferred,send_ec,send_bytes_transferred) = *userThreadStateP_;
          std::atomic_exchange(&std::get<latest_receive_monitor_state>(*userThreadStateP_),tempFriData);
        }
        else
        {
          std::tie(friData,receive_ec,receive_bytes_transferred,send_ec,send_bytes_transferred) = LatestState();
        }
    }

    void destruct(){
       m_shouldStop = true;
       if(driver_threadP_){
         driver_threadP_->join();
       }
    }

    ~KukaFRIClientDataDriver(){
       destruct();
    }

    /// Is everything ok?
    /// @return true if the kuka fri connection is actively running without any issues
    /// @todo consider expanding to support real error codes
    bool is_active()
    {
      return !exceptionPtr && isConnectionEstablished_;
    }

private:
    /// Reads data off of the real kuka fri device in a separate thread
    void update() {
       try {
            // initialize all of the states
            latestStateP_ = make_shared_valid_LatestState();
            nextStateP_ = make_shared_valid_LatestState();
            userThreadStateP_ = make_shared_valid_LatestState();

            // run the primary update loop in a separate thread
            while (!m_shouldStop)
            {
                grl::robot::arm::update_state  (
                                                *socketP_,
                                                *std::get<latest_receive_monitor_state>(*nextStateP_),
                                                std::get<latest_receive_ec>(*nextStateP_),
                                                std::get<latest_receive_bytes_transferred>(*nextStateP_),
                                                std::get<latest_send_ec>(*nextStateP_),
                                                std::get<latest_send_bytes_transferred>(*nextStateP_)
                                               );
                isConnectionEstablished_ = true;
                std::atomic_exchange(&latestStateP_,nextStateP_);
            }

       } catch(...) {
            // transport the exception to the main thread in a safe manner
            exceptionPtr = std::current_exception();
            m_shouldStop = true;
       }

       isConnectionEstablished_ = false;
    }

    enum LatestStateIndex{
     latest_receive_monitor_state,
     latest_receive_ec,
     latest_receive_bytes_transferred,
     latest_send_ec,
     latest_send_bytes_transferred
    };

    typedef std::tuple<std::shared_ptr<KUKA::FRI::ClientData>,boost::system::error_code, std::size_t,boost::system::error_code, std::size_t>
    LatestState;

    /// @todo num_dof should not be a magic number
    /// @note the clientData will never be null
    static LatestState make_LatestState(std::shared_ptr<KUKA::FRI::ClientData> clientData = std::make_shared<KUKA::FRI::ClientData>(KUKA::LBRState::NUM_DOF)){
      return std::make_tuple(clientData,boost::system::error_code(), std::size_t(),boost::system::error_code(), std::size_t());
    }

    static std::shared_ptr<LatestState> make_shared_valid_LatestState(std::shared_ptr<KUKA::FRI::ClientData> friData = std::make_shared<KUKA::FRI::ClientData>(KUKA::LBRState::NUM_DOF)){

        std::shared_ptr<LatestState> tempLatestStateP;

        if(friData.get()==nullptr)
        {
          tempLatestStateP = std::make_shared<LatestState>(make_LatestState());
        }
        else
        {
          tempLatestStateP = std::make_shared<LatestState>(make_LatestState(friData));
        }

        return tempLatestStateP;
    }

    /// @todo replace with unique_ptr
    /// the latest state we have available
    std::shared_ptr<LatestState> latestStateP_;
    std::shared_ptr<LatestState> nextStateP_;
    std::shared_ptr<LatestState> userThreadStateP_;
    std::atomic<bool> m_shouldStop;
    std::exception_ptr exceptionPtr;
    std::atomic<bool> isConnectionEstablished_;

    /// may be null, allows the user to choose if they want to provide an io_service
    std::unique_ptr<boost::asio::io_service> optional_internal_io_service_P;

	// other things to do somewhere:
	// - get list of control points
	// - get the control point in the arm base coordinate system
	// - load up a configuration file with ip address to send to, etc.
	boost::asio::io_service& io_service_;
    std::unique_ptr<boost::asio::ip::udp::socket> socketP_;
    std::unique_ptr<std::thread> driver_threadP_;
};

/// @brief don't use this
/// @todo replace with something generic
/// @deprecated
struct KukaState {
	typedef boost::container::static_vector<double,KUKA::LBRState::NUM_DOF> joint_state;
    typedef boost::container::static_vector<double,6> cartesian_state;

	joint_state     position;
	joint_state     torque;
	joint_state     commandedPosition;
    cartesian_state commandedCartesianWrenchFeedForward;
	joint_state     commandedTorque;

	joint_state     ipoJointPosition;
    joint_state     ipoJointPositionOffsets;
	KUKA::FRI::ESessionState      sessionState;
	KUKA::FRI::EConnectionQuality connectionQuality;
	KUKA::FRI::ESafetyState       safetyState;
	KUKA::FRI::EOperationMode     operationMode;
	KUKA::FRI::EDriveState        driveState;

	std::chrono::time_point<std::chrono::high_resolution_clock> timestamp;

    void clear(){
      position.clear();
      torque.clear();
      commandedPosition.clear();
      commandedTorque.clear();
      ipoJointPosition.clear();
    }
};

/// @brief don't use this
/// @deprecated
void copy(const FRIMonitoringMessage& monitoringMsg, KukaState& state ){
    state.clear();
	copy(monitoringMsg,std::back_inserter(state.position),revolute_joint_angle_open_chain_state_tag());
	copy(monitoringMsg,std::back_inserter(state.torque),revolute_joint_torque_open_chain_state_tag());
	copy(monitoringMsg,std::back_inserter(state.commandedPosition),revolute_joint_angle_open_chain_command_tag());
	copy(monitoringMsg,std::back_inserter(state.commandedTorque),revolute_joint_torque_open_chain_command_tag());
	copy(monitoringMsg,std::back_inserter(state.ipoJointPosition),revolute_joint_angle_interpolated_open_chain_state_tag());
	state.sessionState = get(monitoringMsg,KUKA::FRI::ESessionState());
	state.connectionQuality = get(monitoringMsg,KUKA::FRI::EConnectionQuality());
	state.safetyState = get(monitoringMsg,KUKA::FRI::ESafetyState());
	state.operationMode = get(monitoringMsg,KUKA::FRI::EOperationMode());
	state.driveState = get(monitoringMsg,KUKA::FRI::EDriveState());

	/// @todo fill out missing state update steps

}

}}} // namespace grl::robot::arm

#endif<commit_msg>KukaFriClientData minor cleanup and documentation<commit_after>#ifndef _KUKA_FRI_CLIENT_DATA
#define _KUKA_FRI_CLIENT_DATA

#include <boost/range/algorithm/copy.hpp>
#include <boost/range/algorithm/transform.hpp>
#include "friClientData.h"
#include "grl/KukaFRI.hpp"
#include "grl/KukaFRIThreadSeparator.hpp"
#include <boost/log/trivial.hpp>

/// @todo move back to KukaFRIClientDataTest.cpp
template<typename T,size_t N>
std::ostream& operator<< (std::ostream& out, const boost::container::static_vector<T,N>& v) {
    out << "[";
    size_t last = v.size() - 1;
    for(size_t i = 0; i < v.size(); ++i) {
        out << v[i];
        if (i != last)
            out << ", ";
    }
    out << "]";
    return out;
}

/// @todo move back to KukaFRIClientDataTest.cpp
template<typename T,std::size_t U>
inline boost::log::formatting_ostream& operator<<(boost::log::formatting_ostream& out,  boost::container::static_vector<T,U>& v)
{
    out << "[";
    size_t last = v.size() - 1;
    for(size_t i = 0; i < v.size(); ++i) {
        out << v[i];
        if (i != last)
            out << ", ";
    }
    out << "]";
    return out;
}

namespace grl { namespace robot { namespace arm {

    // Decode message buffer (using nanopb decoder)
void decode(KUKA::FRI::ClientData& friData, std::size_t msg_size){
    // The decoder was given a pointer to the monitoringMessage at initialization
    if (!friData.decoder.decode(friData.receiveBuffer, msg_size)) {
        throw std::runtime_error( "Error decoding received data");
    }

    // check message type
    if (friData.expectedMonitorMsgID != friData.monitoringMsg.header.messageIdentifier)
    {
		throw std::invalid_argument(std::string("KukaFRI.hpp: Problem reading buffer, id code: ") +
			                         boost::lexical_cast<std::string>(static_cast<int>(friData.monitoringMsg.header.messageIdentifier)) +
									std::string(" does not match expected id code: ") +
				                    boost::lexical_cast<std::string>(static_cast<int>(friData.expectedMonitorMsgID)) + std::string("\n")
									);
        return;
    }

    friData.lastState = grl::robot::arm::get(friData.monitoringMsg,KUKA::FRI::ESessionState());
}

/////////////////////////////////
//  moving out to user code
//    // copy current joint position to commanded position
//	if ((state.ipoJointPosition.size() == KUKA::LBRState::NUM_DOF) &&
//        (((friData.sessionState == KUKA::FRI::COMMANDING_WAIT)) || (state.sessionState == KUKA::FRI::COMMANDING_ACTIVE))
//       ) {
//
//         KukaState::joint_state jointStateToCommand;
//        // vector addition between ipoJointPosition and ipoJointPositionOffsets, copying the result into jointStateToCommand
//        boost::transform ( state.ipoJointPosition, state.ipoJointPositionOffsets, std::back_inserter(jointStateToCommand), std::plus<KukaState::joint_state::value_type>());
//
//        // copy the commanded position into the command message
//        set(friData.commandMsg, jointStateToCommand, grl::revolute_joint_angle_open_chain_command_tag());
//
//		//BOOST_LOG_TRIVIAL(trace) << "ENCODE position: " << state.position << " connectionQuality: " << state.connectionQuality << " operationMode: " << state.operationMode << " sessionState: " << state.sessionState << " driveState: " << state.driveState << " ipoJointPosition: " << state.ipoJointPosition << " ipoJointPositionOffsets: " << state.ipoJointPositionOffsets << " jointStateToCommand: " << jointStateToCommand << "\n";
//    }else {
//        set(friData.commandMsg, state.commandedPosition, grl::revolute_joint_angle_open_chain_command_tag());
//    }

/// encode data in the class into the send buffer
std::size_t encode(KUKA::FRI::ClientData& friData,boost::system::error_code& ec){
    // reset send counter
    friData.lastSendCounter = 0;

    // set sequence counters
    friData.commandMsg.header.sequenceCounter = friData.sequenceCounter++;
    friData.commandMsg.header.reflectedSequenceCounter = friData.monitoringMsg.header.sequenceCounter;

	int buffersize = KUKA::FRI::FRI_COMMAND_MSG_MAX_SIZE;
    if (!friData.encoder.encode(friData.sendBuffer, buffersize)){
        // @todo figure out PB_GET_ERROR
        ec = boost::system::errc::make_error_code(boost::system::errc::bad_message);
        return 0;
    }

	return buffersize;
}

/// @pre socket must already have the endpoint resolved and "connected". While udp is technically stateless the asio socket supports the connection api components for convenience.
void update_state(boost::asio::ip::udp::socket& socket, KUKA::FRI::ClientData& friData, boost::system::error_code& receive_ec,std::size_t& receive_bytes_transferred, boost::system::error_code& send_ec, std::size_t& send_bytes_transferred){

    int message_flags = 0;
	receive_bytes_transferred = socket.receive(boost::asio::buffer(friData.receiveBuffer,KUKA::FRI::FRI_MONITOR_MSG_MAX_SIZE),message_flags,receive_ec);
	decode(friData,receive_bytes_transferred);

    friData.lastSendCounter++;
    // Check whether to send a response
    if (friData.lastSendCounter >= friData.monitoringMsg.connectionInfo.receiveMultiplier){
    	send_bytes_transferred = encode(friData,send_ec);
        if(send_ec) return;
		socket.send(boost::asio::buffer(friData.sendBuffer,send_bytes_transferred), message_flags, send_ec);
    }
}

class KukaFRIClientDataDriver : public std::enable_shared_from_this<KukaFRIClientDataDriver>, public KukaFRI {

public:

    using KukaFRI::ParamIndex;
    using KukaFRI::ThreadingRunMode;
    using KukaFRI::Params;
    using KukaFRI::defaultParams;

	KukaFRIClientDataDriver(boost::asio::io_service& ios, Params params = defaultParams())
        :
        isConnectionEstablished_(false),
        io_service_(ios)
    {
      construct(params);
	}

	KukaFRIClientDataDriver(Params params = defaultParams())
        :
        isConnectionEstablished_(false),
        optional_internal_io_service_P(new boost::asio::io_service),
        io_service_(*optional_internal_io_service_P)
    {
        construct(params);
	}

    /// Call this to initialize the object
    void construct(Params params = defaultParams())
    {

        try
        {
            connect(params, io_service_);
            // start up the driver thread since the io_service_ is internal only
            if(std::get<is_running_automatically>(params))
            {
              driver_threadP_.reset(new std::thread([&]{ update(); }));
            }

        } catch( boost::exception &e) {
            add_details_to_connection_error(e,params);
            throw;
        }

    }

    /// @brief blocking call to communicate with the robot continuously
    /// @pre construct() should
    void run(){
      update();
    }

    /// @brief Updates the passed friData shared pointer to a pointer to newly updated data, plus any errors that occurred.
    ///
    /// @note This function is not thread safe cannot be called simultaneously from multiple threads.
    ///
    /// @param[in,out] friData the pointer to update with new state and optional input state.
    ///
    /// @pre If friData!=nullptr it is assumed valid for use and this class will take control of the object.
    void update_state(std::shared_ptr<KUKA::FRI::ClientData>& friData, boost::system::error_code& receive_ec,std::size_t& receive_bytes_transferred, boost::system::error_code& send_ec, std::size_t& send_bytes_transferred){

        // ensure we have valid data for future updates
        std::shared_ptr<KUKA::FRI::ClientData> tempFriData;
        if(friData.get() == nullptr)
        {
          tempFriData = std::make_shared<KUKA::FRI::ClientData>(KUKA::LBRState::NUM_DOF);
        }
        else
        {
          tempFriData = friData;
        }
        std::atomic_exchange(&userThreadStateP_,latestStateP_);

        if(userThreadStateP_.get()!=nullptr)
        {
          std::tie(friData,receive_ec,receive_bytes_transferred,send_ec,send_bytes_transferred) = *userThreadStateP_;
          std::atomic_exchange(&std::get<latest_receive_monitor_state>(*userThreadStateP_),tempFriData);
        }
        else
        {
          std::tie(friData,receive_ec,receive_bytes_transferred,send_ec,send_bytes_transferred) = LatestState();
        }
    }

    void destruct(){
       m_shouldStop = true;
       if(driver_threadP_){
         driver_threadP_->join();
       }
    }

    ~KukaFRIClientDataDriver(){
       destruct();
    }

    /// Is everything ok?
    /// @return true if the kuka fri connection is actively running without any issues
    /// @todo consider expanding to support real error codes
    bool is_active()
    {
      return !exceptionPtr && isConnectionEstablished_;
    }

private:
    /// Reads data off of the real kuka fri device in a separate thread
    void update() {
       try {
            // initialize all of the states
            latestStateP_     = make_shared_valid_LatestState();
            nextStateP_       = make_shared_valid_LatestState();
            userThreadStateP_ = make_shared_valid_LatestState();

            // run the primary update loop in a separate thread
            while (!m_shouldStop)
            {
                grl::robot::arm::update_state  (
                                                *socketP_,
                                                *std::get<latest_receive_monitor_state>(*nextStateP_),
                                                std::get<latest_receive_ec>(*nextStateP_),
                                                std::get<latest_receive_bytes_transferred>(*nextStateP_),
                                                std::get<latest_send_ec>(*nextStateP_),
                                                std::get<latest_send_bytes_transferred>(*nextStateP_)
                                               );
                isConnectionEstablished_ = true;
                std::atomic_exchange(&latestStateP_,nextStateP_);
            }

       } catch(...) {
            // transport the exception to the main thread in a safe manner
            exceptionPtr = std::current_exception();
            m_shouldStop = true;
       }

       isConnectionEstablished_ = false;
    }

    enum LatestStateIndex{
     latest_receive_monitor_state,
     latest_receive_ec,
     latest_receive_bytes_transferred,
     latest_send_ec,
     latest_send_bytes_transferred
    };

    typedef std::tuple<std::shared_ptr<KUKA::FRI::ClientData>,boost::system::error_code, std::size_t,boost::system::error_code, std::size_t>
    LatestState;

    /// Creates a default LatestState Object
    /// @todo num_dof should not be a magic number
    /// @note the clientData will never be null
    static LatestState make_LatestState(std::shared_ptr<KUKA::FRI::ClientData> clientData){
      return std::make_tuple(clientData,boost::system::error_code(), std::size_t(),boost::system::error_code(), std::size_t());
    }

    /// Initialize valid shared ptr to LatestState object with a valid allocated friData
    static std::shared_ptr<LatestState> make_shared_valid_LatestState(std::shared_ptr<KUKA::FRI::ClientData> friData = std::make_shared<KUKA::FRI::ClientData>(KUKA::LBRState::NUM_DOF)){

        std::shared_ptr<LatestState> tempLatestStateP;

        if(friData.get()==nullptr)
        {
          tempLatestStateP = std::make_shared<LatestState>(make_LatestState(std::make_shared<KUKA::FRI::ClientData>(KUKA::LBRState::NUM_DOF)));
        }
        else
        {
          tempLatestStateP = std::make_shared<LatestState>(make_LatestState(friData));
        }

        return tempLatestStateP;
    }

    /// @todo replace with unique_ptr
    /// the latest state we have available
    std::shared_ptr<LatestState> latestStateP_;
    std::shared_ptr<LatestState> nextStateP_;
    std::shared_ptr<LatestState> userThreadStateP_;
    std::atomic<bool> m_shouldStop;
    std::exception_ptr exceptionPtr;
    std::atomic<bool> isConnectionEstablished_;

    /// may be null, allows the user to choose if they want to provide an io_service
    std::unique_ptr<boost::asio::io_service> optional_internal_io_service_P;

	// other things to do somewhere:
	// - get list of control points
	// - get the control point in the arm base coordinate system
	// - load up a configuration file with ip address to send to, etc.
	boost::asio::io_service& io_service_;
    std::unique_ptr<boost::asio::ip::udp::socket> socketP_;
    std::unique_ptr<std::thread> driver_threadP_;
};

/// @brief don't use this
/// @todo replace with something generic
/// @deprecated
struct KukaState {
	typedef boost::container::static_vector<double,KUKA::LBRState::NUM_DOF> joint_state;
    typedef boost::container::static_vector<double,6> cartesian_state;

	joint_state     position;
	joint_state     torque;
	joint_state     commandedPosition;
    cartesian_state commandedCartesianWrenchFeedForward;
	joint_state     commandedTorque;

	joint_state     ipoJointPosition;
    joint_state     ipoJointPositionOffsets;
	KUKA::FRI::ESessionState      sessionState;
	KUKA::FRI::EConnectionQuality connectionQuality;
	KUKA::FRI::ESafetyState       safetyState;
	KUKA::FRI::EOperationMode     operationMode;
	KUKA::FRI::EDriveState        driveState;

	std::chrono::time_point<std::chrono::high_resolution_clock> timestamp;

    void clear(){
      position.clear();
      torque.clear();
      commandedPosition.clear();
      commandedTorque.clear();
      ipoJointPosition.clear();
    }
};

/// @brief don't use this
/// @deprecated
void copy(const FRIMonitoringMessage& monitoringMsg, KukaState& state ){
    state.clear();
	copy(monitoringMsg,std::back_inserter(state.position),revolute_joint_angle_open_chain_state_tag());
	copy(monitoringMsg,std::back_inserter(state.torque),revolute_joint_torque_open_chain_state_tag());
	copy(monitoringMsg,std::back_inserter(state.commandedPosition),revolute_joint_angle_open_chain_command_tag());
	copy(monitoringMsg,std::back_inserter(state.commandedTorque),revolute_joint_torque_open_chain_command_tag());
	copy(monitoringMsg,std::back_inserter(state.ipoJointPosition),revolute_joint_angle_interpolated_open_chain_state_tag());
	state.sessionState = get(monitoringMsg,KUKA::FRI::ESessionState());
	state.connectionQuality = get(monitoringMsg,KUKA::FRI::EConnectionQuality());
	state.safetyState = get(monitoringMsg,KUKA::FRI::ESafetyState());
	state.operationMode = get(monitoringMsg,KUKA::FRI::EOperationMode());
	state.driveState = get(monitoringMsg,KUKA::FRI::EDriveState());

	/// @todo fill out missing state update steps

}

}}} // namespace grl::robot::arm

#endif<|endoftext|>


<commit_before>/*
This file is part of Bohrium and copyright (c) 2012 the Bohrium
team <http://www.bh107.org>.

Bohrium is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation, either version 3
of the License, or (at your option) any later version.

Bohrium is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the
GNU Lesser General Public License along with Bohrium.

If not, see <http://www.gnu.org/licenses/>.
*/

void slide_views(BhIR *bhir) {
    // Iterate through all instructions and slide the relevant views
    for (bh_instruction &instr : bhir->instr_list) {
        for (bh_view &view : instr.operand) {
            if (not view.slide_strides.empty()) {
                // The relevant dimension in the view is updated by the given stride
                for (size_t i = 0; i < view.slide_strides.size(); i++) {
                    size_t off_dim    = view.slide_dimensions.at(i);
                    size_t off_stride = view.slide_strides.at(i);
                    view.start += view.stride[off_dim] * off_stride;
                }
            }
        }
    }
}
<commit_msg>Changed from size_t to int to handle decreasing slides<commit_after>/*
This file is part of Bohrium and copyright (c) 2012 the Bohrium
team <http://www.bh107.org>.

Bohrium is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation, either version 3
of the License, or (at your option) any later version.

Bohrium is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the
GNU Lesser General Public License along with Bohrium.

If not, see <http://www.gnu.org/licenses/>.
*/

void slide_views(BhIR *bhir) {
    // Iterate through all instructions and slide the relevant views
    for (bh_instruction &instr : bhir->instr_list) {
        for (bh_view &view : instr.operand) {
            if (not view.slide_strides.empty()) {
                // The relevant dimension in the view is updated by the given stride
                for (size_t i = 0; i < view.slide_strides.size(); i++) {
                    size_t off_dim    = view.slide_dimensions.at(i);
                    int off_stride = (int) view.slide_strides.at(i);
                    view.start += view.stride[off_dim] * off_stride;
                }
            }
        }
    }
}
<|endoftext|>


<commit_before>/** Definition of the pqxx::basic_connection class template.
 *
 * Instantiations of basic_connection bring connections and policies together.
 *
 * DO NOT INCLUDE THIS FILE DIRECTLY; include pqxx/basic_connection instead.
 *
 * Copyright (c) 2006-2017, Jeroen T. Vermeulen.
 *
 * See COPYING for copyright license.  If you did not receive a file called
 * COPYING with this source code, please notify the distributor of this mistake,
 * or contact the author.
 */
#ifndef PQXX_H_BASIC_CONNECTION
#define PQXX_H_BASIC_CONNECTION

#include "pqxx/compiler-public.hxx"
#include "pqxx/compiler-internal-pre.hxx"

#include <cstddef>
#include <memory>
#include <string>

#include "pqxx/connection_base"

namespace pqxx
{

/// Base-class template for all libpqxx connection types.
/** Combines connection_base (the highly complex class implementing essentially
 * all connection-related functionality) with a connection policy (a simpler
 * helper class determining the rules that govern the process of setting up the
 * underlying connection to the backend).
 *
 * The pattern used to combine these classes is the same as for
 * basic_transaction.  Through use of the template mechanism, the policy object
 * is embedded in the basic_connection object so that it does not need to be
 * allocated separately.  This also avoids the need for virtual functions in
 * this class.
 */
template<typename CONNECTPOLICY> class basic_connection :
  public connection_base
{
public:
  basic_connection() :
    connection_base(m_policy),
    m_options(std::string()),
    m_policy(m_options)
	{ init(); }

  /// The parsing of options is the same as libpq's PQConnect.
  /// See: https://www.postgresql.org/docs/10/static/libpq-connect.html
  explicit basic_connection(const std::string &opt) :
    connection_base(m_policy),
    m_options(opt),
    m_policy(m_options)
	{init();}

  /// See: basic_connection(const std::string &oput)
  explicit basic_connection(const char opt[]) :
    basic_connection(opt ? std::string(opt) : std::string()) {}

  explicit basic_connection(std::nullptr_t) : basic_connection() {}

  ~basic_connection() noexcept
	{ close(); }

  const std::string &options() const noexcept				//[t01]
	{return m_policy.options();}

private:
  /// Connect string.  @warn Must be initialized before the connector!
  std::string m_options;
  /// Connection policy.  @warn Must be initialized after the connect string!
  CONNECTPOLICY m_policy;
};

} // namespace

#include "pqxx/compiler-internal-post.hxx"

#endif
<commit_msg>Typo.<commit_after>/** Definition of the pqxx::basic_connection class template.
 *
 * Instantiations of basic_connection bring connections and policies together.
 *
 * DO NOT INCLUDE THIS FILE DIRECTLY; include pqxx/basic_connection instead.
 *
 * Copyright (c) 2006-2017, Jeroen T. Vermeulen.
 *
 * See COPYING for copyright license.  If you did not receive a file called
 * COPYING with this source code, please notify the distributor of this mistake,
 * or contact the author.
 */
#ifndef PQXX_H_BASIC_CONNECTION
#define PQXX_H_BASIC_CONNECTION

#include "pqxx/compiler-public.hxx"
#include "pqxx/compiler-internal-pre.hxx"

#include <cstddef>
#include <memory>
#include <string>

#include "pqxx/connection_base"

namespace pqxx
{

/// Base-class template for all libpqxx connection types.
/** Combines connection_base (the highly complex class implementing essentially
 * all connection-related functionality) with a connection policy (a simpler
 * helper class determining the rules that govern the process of setting up the
 * underlying connection to the backend).
 *
 * The pattern used to combine these classes is the same as for
 * basic_transaction.  Through use of the template mechanism, the policy object
 * is embedded in the basic_connection object so that it does not need to be
 * allocated separately.  This also avoids the need for virtual functions in
 * this class.
 */
template<typename CONNECTPOLICY> class basic_connection :
  public connection_base
{
public:
  basic_connection() :
    connection_base(m_policy),
    m_options(std::string()),
    m_policy(m_options)
	{ init(); }

  /// The parsing of options is the same as libpq's PQconnect.
  /// See: https://www.postgresql.org/docs/10/static/libpq-connect.html
  explicit basic_connection(const std::string &opt) :
    connection_base(m_policy),
    m_options(opt),
    m_policy(m_options)
	{init();}

  /// See: @c basic_connection(const std::string &opt)
  explicit basic_connection(const char opt[]) :
    basic_connection(opt ? std::string(opt) : std::string()) {}

  explicit basic_connection(std::nullptr_t) : basic_connection() {}

  ~basic_connection() noexcept
	{ close(); }

  const std::string &options() const noexcept				//[t01]
	{return m_policy.options();}

private:
  /// Connect string.  @warn Must be initialized before the connector!
  std::string m_options;
  /// Connection policy.  @warn Must be initialized after the connect string!
  CONNECTPOLICY m_policy;
};

} // namespace

#include "pqxx/compiler-internal-post.hxx"

#endif
<|endoftext|>


<commit_before>//
// Copyright (c) 2013-2014 Christoph Malek
// See LICENSE for more information.
//

#ifndef RJ_CORE_RENDER_HPP
#define RJ_CORE_RENDER_HPP

#include <SFML/Graphics.hpp>

namespace rj
{
	class game;

	namespace rndr
	{
		template<typename Game>
		void ro(Game& g, const sf::Drawable& drawable)
		{g.render_object(drawable);}
	}
}

#endif // RJ_CORE_RENDER_HPP
<commit_msg>added rj::rndr::rmo<T, T...>(...): render multiple objects in one fnc<commit_after>//
// Copyright (c) 2013-2014 Christoph Malek
// See LICENSE for more information.
//

#ifndef RJ_CORE_RENDER_HPP
#define RJ_CORE_RENDER_HPP

#include <SFML/Graphics.hpp>

namespace rj
{
	class game;

	namespace rndr
	{
		template<typename Game>
		void ro(Game& g, const sf::Drawable& drawable)
		{g.render_object(drawable);}

		template<typename Game>
		void rmo_impl(Game& g)
		{}

		template<typename Game, typename Head, typename... Tail>
		void rmo_impl(Game& g, const Head& head, Tail&&... tail)
		{
			ro(g, head);
			rmo_impl(g, std::forward<Tail>(tail)...);
		}

		template<typename Game, typename... Args>
		void rmo(Game& g, Args&&... args)
		{rmo_impl(g, std::forward<Args>(args)...);}
	}
}

#endif // RJ_CORE_RENDER_HPP
<|endoftext|>


<commit_before>/* This is free and unencumbered software released into the public domain. */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include "local_socket.h"

#include "error.h"
#include "pathname.h"

#include <array>        /* for std::array */
#include <cassert>      /* for assert() */
#include <cerrno>       /* for errno */
#include <cstdint>      /* for std::uint8_t */
#include <cstring>      /* for std::memset(), std::strcpy(), std::strlen() */
#include <fcntl.h>      /* for F_*, fcntl() */
#include <stdexcept>    /* for std::length_error, std::logic_error */
#include <sys/socket.h> /* for AF_LOCAL, CMSG_*, accept(), connect(), recvmsg(), sendmsg(), socket(), socketpair() */
#include <sys/un.h>     /* for struct sockaddr_un */

using namespace posix;

local_socket::local_socket() : socket() {
  int flags = 0;
#ifdef SOCK_CLOEXEC
  /* Nonstandard Linux extension to set O_CLOEXEC atomically: */
  flags |= SOCK_CLOEXEC;
#endif

  int sockfd;
  if ((sockfd = ::socket(AF_LOCAL, SOCK_STREAM | flags, 0)) == -1) {
    throw_error("socket(AF_LOCAL, SOCK_STREAM)");
  }

  assign(sockfd);
#ifndef SOCK_CLOEXEC
  cloexec(true);
#endif
}

std::pair<local_socket, local_socket>
local_socket::pair() {
  int flags = 0;
#ifdef SOCK_CLOEXEC
  /* Nonstandard Linux extension to set O_CLOEXEC atomically: */
  flags |= SOCK_CLOEXEC;
#endif

  int fds[2] = {0, 0};
  if (::socketpair(AF_LOCAL, SOCK_STREAM | flags, 0, fds) == -1) {
    throw_error("socketpair(AF_LOCAL, SOCK_STREAM)");
  }

  std::pair<local_socket, local_socket> pair{local_socket(fds[0]), local_socket(fds[1])};
#ifndef SOCK_CLOEXEC
  pair.first.cloexec(true);
  pair.second.cloexec(true);
#endif

  return pair;
}

local_socket
local_socket::bind(const pathname& pathname) {
  assert(!pathname.empty());

  local_socket socket = local_socket();

  struct sockaddr_un addr;
  addr.sun_family = AF_LOCAL;

  if (pathname.size() >= sizeof(addr.sun_path)) {
    throw std::length_error{"socket pathname exceeds maximum length"};
  }
  std::strcpy(addr.sun_path, pathname.c_str());

  const socklen_t addrlen = sizeof(addr.sun_family) + std::strlen(addr.sun_path);

retry:
  if (::bind(socket.fd(), reinterpret_cast<struct sockaddr*>(&addr), addrlen) == -1) {
    switch (errno) {
      case EINTR:  /* Interrupted system call */
        goto retry;
      default:
        throw_error("bind");
    }
  }

  return socket;
}

local_socket
local_socket::connect(const pathname& pathname) {
  assert(!pathname.empty());

  local_socket socket = local_socket();

  struct sockaddr_un addr;
  addr.sun_family = AF_LOCAL;

  if (pathname.size() >= sizeof(addr.sun_path)) {
    throw std::length_error{"socket pathname exceeds maximum length"};
  }
  std::strcpy(addr.sun_path, pathname.c_str());

  const socklen_t addrlen = sizeof(addr.sun_family) + std::strlen(addr.sun_path);

retry:
  if (::connect(socket.fd(), reinterpret_cast<struct sockaddr*>(&addr), addrlen) == -1) {
    switch (errno) {
      case EINTR:  /* Interrupted system call */
        goto retry;
      default:
        throw_error("connect");
    }
  }

  return socket;
}

local_socket
local_socket::accept() {
  int connfd;
retry:
#if defined(HAVE_ACCEPT4) && defined(SOCK_CLOEXEC)
  /* Nonstandard Linux extension to set O_CLOEXEC atomically: */
  if ((connfd = ::accept4(fd(), nullptr, nullptr, SOCK_CLOEXEC)) == -1) {
#else
  if ((connfd = ::accept(fd(), nullptr, nullptr)) == -1) {
#endif
    switch (errno) {
      case EINTR:   /* Interrupted system call */
        goto retry;
      default:
        throw_error("accept");
    }
  }

  local_socket connection(connfd);
#if !(defined(HAVE_ACCEPT4) && defined(SOCK_CLOEXEC))
  connection.cloexec(true);
#endif

  return connection;
}

void
local_socket::send_descriptor(const descriptor& descriptor) {
#ifdef __linux__
  /* The control buffer must be large enough to hold a single frame of
   * cmsghdr ancillary data: */
  std::array<std::uint8_t, CMSG_SPACE(sizeof(int))> cmsg_buffer;
  static_assert(sizeof(struct cmsghdr) <= cmsg_buffer.size(),
    "sizeof(struct cmsghdr) > cmsg_buffer.size()");
  static_assert(CMSG_SPACE(sizeof(int)) <= cmsg_buffer.size(),
    "CMSG_SPACE(sizeof(int)) > cmsg_buffer.size()");
  static_assert(CMSG_LEN(sizeof(int)) <= cmsg_buffer.size(),
    "CMSG_LEN(sizeof(int)) > cmsg_buffer.size()");

  /* Linux and Solaris require there to be at least 1 byte of actual data: */
  std::array<std::uint8_t, 1> data_buffer = {{'\0'}};

  struct iovec iov;
  std::memset(&iov, 0, sizeof(iov));
  iov.iov_base = data_buffer.data();
  iov.iov_len  = data_buffer.size();

  struct msghdr msg;
  std::memset(&msg, 0, sizeof(msghdr));
  msg.msg_iov = &iov;
  msg.msg_iovlen = 1;
  msg.msg_control = cmsg_buffer.data();
  msg.msg_controllen = cmsg_buffer.size();

  struct cmsghdr* const cmsg = CMSG_FIRSTHDR(&msg);
  cmsg->cmsg_len   = CMSG_LEN(sizeof(int));
  cmsg->cmsg_level = SOL_SOCKET;
  cmsg->cmsg_type  = SCM_RIGHTS;
  *reinterpret_cast<int*>(CMSG_DATA(cmsg)) = descriptor.fd();

retry:
  const ssize_t rc = ::sendmsg(fd(), &msg, 0);
  if (rc == -1) {
    switch (errno) {
      case EINTR:  /* Interrupted system call */
        goto retry;
      default:
        throw_error("sendmsg");
    }
  }
#else /* __linux__ */
  (void)descriptor; /* not used */
  throw_error(ENOSYS); /* Function not implemented */
#endif /* __linux__ */
}

descriptor
local_socket::recv_descriptor() {
#ifdef __linux__
  descriptor result;

  /* The control buffer must be large enough to hold a single frame of
   * cmsghdr ancillary data: */
  std::array<std::uint8_t, CMSG_SPACE(sizeof(int))> cmsg_buffer;
  static_assert(sizeof(struct cmsghdr) <= cmsg_buffer.size(),
    "sizeof(struct cmsghdr) > cmsg_buffer.size()");
  static_assert(CMSG_SPACE(sizeof(int)) <= cmsg_buffer.size(),
    "CMSG_SPACE(sizeof(int)) > cmsg_buffer.size()");
  static_assert(CMSG_LEN(sizeof(int)) <= cmsg_buffer.size(),
    "CMSG_LEN(sizeof(int)) > cmsg_buffer.size()");

  /* Linux and Solaris require there to be at least 1 byte of actual data: */
  std::array<std::uint8_t, 1> data_buffer;

  struct iovec iov;
  std::memset(&iov, 0, sizeof(iov));
  iov.iov_base = data_buffer.data();
  iov.iov_len  = data_buffer.size();

  struct msghdr msg;
  std::memset(&msg, 0, sizeof(msghdr));
  msg.msg_iov = &iov;
  msg.msg_iovlen = 1;
  msg.msg_control = cmsg_buffer.data();
  msg.msg_controllen = cmsg_buffer.size();

  int flags = 0;
#ifdef MSG_CMSG_CLOEXEC
  flags |= MSG_CMSG_CLOEXEC; /* Linux only */
#endif

retry:
  const ssize_t rc = ::recvmsg(fd(), &msg, flags);
  if (rc == -1) {
    switch (errno) {
      case EINTR:  /* Interrupted system call */
        goto retry;
      default:
        throw_error("recvmsg");
    }
  }

  const struct cmsghdr* const cmsg = CMSG_FIRSTHDR(&msg);
  if (cmsg && cmsg->cmsg_len == CMSG_LEN(sizeof(int))) {
    if (cmsg->cmsg_level != SOL_SOCKET) {
      throw std::logic_error("invalid cmsg_level");
    }
    if (cmsg->cmsg_type != SCM_RIGHTS) {
      throw std::logic_error("invalid cmsg_type");
    }
    result.assign(*reinterpret_cast<const int*>(CMSG_DATA(cmsg)));
  }

#ifndef MSG_CMSG_CLOEXEC
  result.cloexec(true);
#endif

  return result;
#else /* __linux__ */
  throw_error(ENOSYS); /* Function not implemented */
#endif /* __linux__ */
}
<commit_msg>Reformulated two strcpy() calls to resolve Coverity defect reports.<commit_after>/* This is free and unencumbered software released into the public domain. */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include "local_socket.h"

#include "error.h"
#include "pathname.h"

#include <array>        /* for std::array */
#include <cassert>      /* for assert() */
#include <cerrno>       /* for errno */
#include <cstdint>      /* for std::uint8_t */
#include <cstring>      /* for std::memset(), std::strcpy(), std::strlen() */
#include <fcntl.h>      /* for F_*, fcntl() */
#include <stdexcept>    /* for std::length_error, std::logic_error */
#include <sys/socket.h> /* for AF_LOCAL, CMSG_*, accept(), connect(), recvmsg(), sendmsg(), socket(), socketpair() */
#include <sys/un.h>     /* for struct sockaddr_un */

using namespace posix;

local_socket::local_socket() : socket() {
  int flags = 0;
#ifdef SOCK_CLOEXEC
  /* Nonstandard Linux extension to set O_CLOEXEC atomically: */
  flags |= SOCK_CLOEXEC;
#endif

  int sockfd;
  if ((sockfd = ::socket(AF_LOCAL, SOCK_STREAM | flags, 0)) == -1) {
    throw_error("socket(AF_LOCAL, SOCK_STREAM)");
  }

  assign(sockfd);
#ifndef SOCK_CLOEXEC
  cloexec(true);
#endif
}

std::pair<local_socket, local_socket>
local_socket::pair() {
  int flags = 0;
#ifdef SOCK_CLOEXEC
  /* Nonstandard Linux extension to set O_CLOEXEC atomically: */
  flags |= SOCK_CLOEXEC;
#endif

  int fds[2] = {0, 0};
  if (::socketpair(AF_LOCAL, SOCK_STREAM | flags, 0, fds) == -1) {
    throw_error("socketpair(AF_LOCAL, SOCK_STREAM)");
  }

  std::pair<local_socket, local_socket> pair{local_socket(fds[0]), local_socket(fds[1])};
#ifndef SOCK_CLOEXEC
  pair.first.cloexec(true);
  pair.second.cloexec(true);
#endif

  return pair;
}

local_socket
local_socket::bind(const pathname& pathname) {
  assert(!pathname.empty());

  local_socket socket = local_socket();

  struct sockaddr_un addr;
  addr.sun_family = AF_LOCAL;

  if (pathname.size() >= sizeof(addr.sun_path)) {
    throw std::length_error{"socket pathname exceeds maximum length"};
  }
  std::strncpy(addr.sun_path, pathname.c_str(), sizeof(addr.sun_path) - 1);
  addr.sun_path[sizeof(addr.sun_path) - 1] = '\0';

  const socklen_t addrlen = sizeof(addr.sun_family) + std::strlen(addr.sun_path);

retry:
  if (::bind(socket.fd(), reinterpret_cast<struct sockaddr*>(&addr), addrlen) == -1) {
    switch (errno) {
      case EINTR:  /* Interrupted system call */
        goto retry;
      default:
        throw_error("bind");
    }
  }

  return socket;
}

local_socket
local_socket::connect(const pathname& pathname) {
  assert(!pathname.empty());

  local_socket socket = local_socket();

  struct sockaddr_un addr;
  addr.sun_family = AF_LOCAL;

  if (pathname.size() >= sizeof(addr.sun_path)) {
    throw std::length_error{"socket pathname exceeds maximum length"};
  }
  std::strncpy(addr.sun_path, pathname.c_str(), sizeof(addr.sun_path) - 1);
  addr.sun_path[sizeof(addr.sun_path) - 1] = '\0';

  const socklen_t addrlen = sizeof(addr.sun_family) + std::strlen(addr.sun_path);

retry:
  if (::connect(socket.fd(), reinterpret_cast<struct sockaddr*>(&addr), addrlen) == -1) {
    switch (errno) {
      case EINTR:  /* Interrupted system call */
        goto retry;
      default:
        throw_error("connect");
    }
  }

  return socket;
}

local_socket
local_socket::accept() {
  int connfd;
retry:
#if defined(HAVE_ACCEPT4) && defined(SOCK_CLOEXEC)
  /* Nonstandard Linux extension to set O_CLOEXEC atomically: */
  if ((connfd = ::accept4(fd(), nullptr, nullptr, SOCK_CLOEXEC)) == -1) {
#else
  if ((connfd = ::accept(fd(), nullptr, nullptr)) == -1) {
#endif
    switch (errno) {
      case EINTR:   /* Interrupted system call */
        goto retry;
      default:
        throw_error("accept");
    }
  }

  local_socket connection(connfd);
#if !(defined(HAVE_ACCEPT4) && defined(SOCK_CLOEXEC))
  connection.cloexec(true);
#endif

  return connection;
}

void
local_socket::send_descriptor(const descriptor& descriptor) {
#ifdef __linux__
  /* The control buffer must be large enough to hold a single frame of
   * cmsghdr ancillary data: */
  std::array<std::uint8_t, CMSG_SPACE(sizeof(int))> cmsg_buffer;
  static_assert(sizeof(struct cmsghdr) <= cmsg_buffer.size(),
    "sizeof(struct cmsghdr) > cmsg_buffer.size()");
  static_assert(CMSG_SPACE(sizeof(int)) <= cmsg_buffer.size(),
    "CMSG_SPACE(sizeof(int)) > cmsg_buffer.size()");
  static_assert(CMSG_LEN(sizeof(int)) <= cmsg_buffer.size(),
    "CMSG_LEN(sizeof(int)) > cmsg_buffer.size()");

  /* Linux and Solaris require there to be at least 1 byte of actual data: */
  std::array<std::uint8_t, 1> data_buffer = {{'\0'}};

  struct iovec iov;
  std::memset(&iov, 0, sizeof(iov));
  iov.iov_base = data_buffer.data();
  iov.iov_len  = data_buffer.size();

  struct msghdr msg;
  std::memset(&msg, 0, sizeof(msghdr));
  msg.msg_iov = &iov;
  msg.msg_iovlen = 1;
  msg.msg_control = cmsg_buffer.data();
  msg.msg_controllen = cmsg_buffer.size();

  struct cmsghdr* const cmsg = CMSG_FIRSTHDR(&msg);
  cmsg->cmsg_len   = CMSG_LEN(sizeof(int));
  cmsg->cmsg_level = SOL_SOCKET;
  cmsg->cmsg_type  = SCM_RIGHTS;
  *reinterpret_cast<int*>(CMSG_DATA(cmsg)) = descriptor.fd();

retry:
  const ssize_t rc = ::sendmsg(fd(), &msg, 0);
  if (rc == -1) {
    switch (errno) {
      case EINTR:  /* Interrupted system call */
        goto retry;
      default:
        throw_error("sendmsg");
    }
  }
#else /* __linux__ */
  (void)descriptor; /* not used */
  throw_error(ENOSYS); /* Function not implemented */
#endif /* __linux__ */
}

descriptor
local_socket::recv_descriptor() {
#ifdef __linux__
  descriptor result;

  /* The control buffer must be large enough to hold a single frame of
   * cmsghdr ancillary data: */
  std::array<std::uint8_t, CMSG_SPACE(sizeof(int))> cmsg_buffer;
  static_assert(sizeof(struct cmsghdr) <= cmsg_buffer.size(),
    "sizeof(struct cmsghdr) > cmsg_buffer.size()");
  static_assert(CMSG_SPACE(sizeof(int)) <= cmsg_buffer.size(),
    "CMSG_SPACE(sizeof(int)) > cmsg_buffer.size()");
  static_assert(CMSG_LEN(sizeof(int)) <= cmsg_buffer.size(),
    "CMSG_LEN(sizeof(int)) > cmsg_buffer.size()");

  /* Linux and Solaris require there to be at least 1 byte of actual data: */
  std::array<std::uint8_t, 1> data_buffer;

  struct iovec iov;
  std::memset(&iov, 0, sizeof(iov));
  iov.iov_base = data_buffer.data();
  iov.iov_len  = data_buffer.size();

  struct msghdr msg;
  std::memset(&msg, 0, sizeof(msghdr));
  msg.msg_iov = &iov;
  msg.msg_iovlen = 1;
  msg.msg_control = cmsg_buffer.data();
  msg.msg_controllen = cmsg_buffer.size();

  int flags = 0;
#ifdef MSG_CMSG_CLOEXEC
  flags |= MSG_CMSG_CLOEXEC; /* Linux only */
#endif

retry:
  const ssize_t rc = ::recvmsg(fd(), &msg, flags);
  if (rc == -1) {
    switch (errno) {
      case EINTR:  /* Interrupted system call */
        goto retry;
      default:
        throw_error("recvmsg");
    }
  }

  const struct cmsghdr* const cmsg = CMSG_FIRSTHDR(&msg);
  if (cmsg && cmsg->cmsg_len == CMSG_LEN(sizeof(int))) {
    if (cmsg->cmsg_level != SOL_SOCKET) {
      throw std::logic_error("invalid cmsg_level");
    }
    if (cmsg->cmsg_type != SCM_RIGHTS) {
      throw std::logic_error("invalid cmsg_type");
    }
    result.assign(*reinterpret_cast<const int*>(CMSG_DATA(cmsg)));
  }

#ifndef MSG_CMSG_CLOEXEC
  result.cloexec(true);
#endif

  return result;
#else /* __linux__ */
  throw_error(ENOSYS); /* Function not implemented */
#endif /* __linux__ */
}
<|endoftext|>


<commit_before>#ifndef TWISTEDCPP_PROTOCOL_CORE
#define TWISTEDCPP_PROTOCOL_CORE

#include "detail/sockets.hpp"

#include <boost/asio/ip/tcp.hpp>
#include <boost/asio/steady_timer.hpp>
#include <boost/optional.hpp>
#include <boost/asio/spawn.hpp>
#include <boost/asio/write.hpp>

namespace twisted {

template <typename ChildProtocol, typename BufferType>
class protocol_core : public std::enable_shared_from_this<ChildProtocol> {
public:
    typedef detail::socket_base socket_type;
    typedef boost::asio::steady_timer timer_type;
    typedef boost::asio::io_service::strand strand_type;
    typedef BufferType buffer_type;
    typedef typename buffer_type::iterator buffer_iterator;
    typedef typename buffer_type::const_iterator const_buffer_iterator;

    protocol_core() = default;
    protocol_core(protocol_core&&) = default;
    protocol_core& operator=(protocol_core&&) = default;

    void set_socket(std::unique_ptr<socket_type> socket) {
        _socket = std::move(socket);
        _strand = boost::in_place(boost::ref(_socket->get_io_service()));
    }

    void run_protocol() {
        auto self = this_protocol().shared_from_this();
        boost::asio::spawn(*_strand,
                           [this, self](boost::asio::yield_context yield) {
            _yield = boost::in_place(yield);

            try {
                _socket->do_handshake(*_yield);

                while (_socket->is_open()) {
                    auto bytes_read =
                        _socket->async_read_some(asio_buffer(), yield);
                    checked_on_message(buffer_begin(),
                                       std::next(buffer_begin(), bytes_read));
                }
            }
            catch (boost::system::system_error& connection_error) { // network
                                                                    // errors
                print_connection_error(connection_error);
                this_protocol().on_disconnect();
            }
            catch (const std::exception& excep) { // errors from user protocols
                print_exception_what(excep);
            }
        });
    }

    template <typename Iter>
    void send_message(Iter begin, Iter end) {
        _socket->async_write(
            boost::asio::buffer(&*begin, std::distance(begin, end)), *_yield);
    }

    void send_buffers(const std::array<boost::asio::const_buffer, 2>& buffers) {
        _socket->async_write(buffers, *_yield);
    }

    void on_disconnect() {}

    void on_error(std::exception_ptr eptr) { std::rethrow_exception(eptr); }

    void lose_connection() { _socket->close(); }

    /*
     * @brief CRTP wrapper for derived class access
     */
    ChildProtocol& this_protocol() {
        return *static_cast<ChildProtocol*>(this);
    }

    /*
     * @brief CRTP wrapper for derived class access
     */
    const ChildProtocol& this_protocol() const {
        return *static_cast<ChildProtocol*>(this);
    }

private:
    void print_connection_error(
        const boost::system::system_error& connection_error) const {
        std::cerr << "Client disconnected with code " << connection_error.what()
                  << std::endl;
    }

    void print_exception_what(const std::exception& excep) {
        std::cerr << "Killing connection, exception in client handler: "
                  << excep.what() << std::endl;
    }

    void checked_on_message(const_buffer_iterator begin,
                            const_buffer_iterator end) {
        try {
            this_protocol().on_message(begin, end);
        }
        catch (...) {
            this_protocol().on_error(std::current_exception());
        }
    }

    buffer_iterator buffer_begin() {
        return this_protocol().read_buffer().begin();
    }

    buffer_iterator buffer_begin() const {
        return this_protocol().read_buffer().begin();
    }

    buffer_iterator buffer_end() { return this_protocol().read_buffer().end(); }

    buffer_iterator buffer_end() const {
        return this_protocol().read_buffer().end();
    }

    boost::asio::mutable_buffers_1 asio_buffer() {
        return boost::asio::buffer(&*buffer_begin(),
                                   std::distance(buffer_begin(), buffer_end()));
    }

private:
    boost::optional<boost::asio::yield_context> _yield;
    std::unique_ptr<socket_type> _socket;
    boost::optional<strand_type> _strand;
};

} // namespace twisted

#endif
<commit_msg>dc output only in debug mode<commit_after>#ifndef TWISTEDCPP_PROTOCOL_CORE
#define TWISTEDCPP_PROTOCOL_CORE

#include "detail/sockets.hpp"

#include <boost/asio/ip/tcp.hpp>
#include <boost/asio/steady_timer.hpp>
#include <boost/optional.hpp>
#include <boost/asio/spawn.hpp>
#include <boost/asio/write.hpp>

namespace twisted {

template <typename ChildProtocol, typename BufferType>
class protocol_core : public std::enable_shared_from_this<ChildProtocol> {
public:
    typedef detail::socket_base socket_type;
    typedef boost::asio::steady_timer timer_type;
    typedef boost::asio::io_service::strand strand_type;
    typedef BufferType buffer_type;
    typedef typename buffer_type::iterator buffer_iterator;
    typedef typename buffer_type::const_iterator const_buffer_iterator;

    protocol_core() = default;
    protocol_core(protocol_core&&) = default;
    protocol_core& operator=(protocol_core&&) = default;

    void set_socket(std::unique_ptr<socket_type> socket) {
        _socket = std::move(socket);
        _strand = boost::in_place(boost::ref(_socket->get_io_service()));
    }

    void run_protocol() {
        auto self = this_protocol().shared_from_this();
        boost::asio::spawn(*_strand,
                           [this, self](boost::asio::yield_context yield) {
            _yield = boost::in_place(yield);

            try {
                _socket->do_handshake(*_yield);

                while (_socket->is_open()) {
                    auto bytes_read =
                        _socket->async_read_some(asio_buffer(), yield);
                    checked_on_message(buffer_begin(),
                                       std::next(buffer_begin(), bytes_read));
                }
            }
            catch (boost::system::system_error& connection_error) { // network
                                                                    // errors
                print_connection_error(connection_error);
                this_protocol().on_disconnect();
            }
            catch (const std::exception& excep) { // errors from user protocols
                print_exception_what(excep);
            }
        });
    }

    template <typename Iter>
    void send_message(Iter begin, Iter end) {
        _socket->async_write(
            boost::asio::buffer(&*begin, std::distance(begin, end)), *_yield);
    }

    void send_buffers(const std::array<boost::asio::const_buffer, 2>& buffers) {
        _socket->async_write(buffers, *_yield);
    }

    void on_disconnect() {}

    void on_error(std::exception_ptr eptr) { std::rethrow_exception(eptr); }

    void lose_connection() { _socket->close(); }

    /*
     * @brief CRTP wrapper for derived class access
     */
    ChildProtocol& this_protocol() {
        return *static_cast<ChildProtocol*>(this);
    }

    /*
     * @brief CRTP wrapper for derived class access
     */
    const ChildProtocol& this_protocol() const {
        return *static_cast<ChildProtocol*>(this);
    }

private:
    void print_connection_error(

#ifdef NDEBUG
        const boost::system::system_error& /*connection_error*/) const {
#else
        const boost::system::system_error& connection_error) const {
        std::cerr << "Client disconnected with code " << connection_error.what()
                  << std::endl;
#endif
    }

#ifdef NDEBUG
    void print_exception_what(const std::exception& /*excep*/) {
#else
    void print_exception_what(const std::exception& excep) {
        std::cerr << "Killing connection, exception in client handler: "
                  << excep.what() << std::endl;
#endif
    }

    void checked_on_message(const_buffer_iterator begin,
                            const_buffer_iterator end) {
        try {
            this_protocol().on_message(begin, end);
        }
        catch (...) {
            this_protocol().on_error(std::current_exception());
        }
    }

    buffer_iterator buffer_begin() {
        return this_protocol().read_buffer().begin();
    }

    buffer_iterator buffer_begin() const {
        return this_protocol().read_buffer().begin();
    }

    buffer_iterator buffer_end() { return this_protocol().read_buffer().end(); }

    buffer_iterator buffer_end() const {
        return this_protocol().read_buffer().end();
    }

    boost::asio::mutable_buffers_1 asio_buffer() {
        return boost::asio::buffer(&*buffer_begin(),
                                   std::distance(buffer_begin(), buffer_end()));
    }

private:
    boost::optional<boost::asio::yield_context> _yield;
    std::unique_ptr<socket_type> _socket;
    boost::optional<strand_type> _strand;
};

} // namespace twisted

#endif
<|endoftext|>


<commit_before>#pragma once

#include <cstdint>
#include <memory>
#include <sstream>
#include <string>
#include <string_view>

#if !defined(TADPOLE_UNUSED)
# define TADPOLE_UNUSED(x) ((void)x)
#endif

namespace tadpole {

using nil_t   = std::nullptr_t;
using byte_t  = std::uint8_t;
using i8_t    = std::int8_t;
using u8_t    = std::uint8_t;
using i16_t   = std::int16_t;
using u16_t   = std::uint16_t;
using i32_t   = std::int32_t;
using u32_t   = std::uint32_t;
using i64_t   = std::int64_t;
using u64_t   = std::uint64_t;
using sz_t    = std::size_t;
using str_t   = std::string;
using strv_t  = std::string_view;
using ss_t    = std::stringstream;

class Copyable {
protected:
  Copyable() noexcept = default;
  ~Copyable() noexcept = default;
  Copyable(const Copyable&) noexcept = default;
  Copyable(Copyable&&) noexcept = default;
  Copyable& operator=(const Copyable&) noexcept = default;
  Copyable& operator=(Copyable&&) noexcept = default;
};

class UnCopyable {
  UnCopyable(const UnCopyable&) noexcept = delete;
  UnCopyable(UnCopyable&&) noexcept = delete;
  UnCopyable& operator=(const UnCopyable&) noexcept = delete;
  UnCopyable& operator=(UnCopyable&&) noexcept = delete;
protected:
  UnCopyable() noexcept = default;
  ~UnCopyable() noexcept = default;
};

template <typename T, typename S> inline T as_type(S x) noexcept {
  return static_cast<T>(x);
}

template <typename T, typename S> inline T* as_down(S* x) noexcept {
  return dynamic_cast<T*>(x);
}

template <typename T>
inline T* get_rawptr(const std::unique_ptr<T>& p) noexcept {
  return p.get();
}

template <typename T>
inline T* get_rawptr(const std::shared_ptr<T>& p) noexcept {
  return p.get();
}

inline str_t as_string(double d) noexcept {
  ss_t ss;
  ss << d;
  return ss.str();
}

template <typename T, typename... Args>
inline str_t as_string(T&& x, Args&&... args) noexcept {
  ss_t ss;

  ss << std::forward<T>(x);
  ((ss << std::forward<Args>(args)), ...);

  return ss.str();
}

template <typename... Args>
inline str_t string_format(strv_t fmt, const Args&... args) {
  sz_t sz = std::snprintf(nullptr, 0, fmt.data(), args...) + 1;
  std::unique_ptr<char[]> buf{ new char[sz] };
  std::snprintf(buf.get(), sz, fmt.data(), args...);
  return str_t(buf.get(), buf.get() + sz - 1);
}

}<commit_msg>:construction: chore(string-format): updated the implementation of string format<commit_after>#pragma once

#include <cstdint>
#include <memory>
#include <sstream>
#include <string>
#include <string_view>

#if !defined(TADPOLE_UNUSED)
# define TADPOLE_UNUSED(x) ((void)x)
#endif

namespace tadpole {

using nil_t   = std::nullptr_t;
using byte_t  = std::uint8_t;
using i8_t    = std::int8_t;
using u8_t    = std::uint8_t;
using i16_t   = std::int16_t;
using u16_t   = std::uint16_t;
using i32_t   = std::int32_t;
using u32_t   = std::uint32_t;
using i64_t   = std::int64_t;
using u64_t   = std::uint64_t;
using sz_t    = std::size_t;
using str_t   = std::string;
using strv_t  = std::string_view;
using ss_t    = std::stringstream;

class Copyable {
protected:
  Copyable() noexcept = default;
  ~Copyable() noexcept = default;
  Copyable(const Copyable&) noexcept = default;
  Copyable(Copyable&&) noexcept = default;
  Copyable& operator=(const Copyable&) noexcept = default;
  Copyable& operator=(Copyable&&) noexcept = default;
};

class UnCopyable {
  UnCopyable(const UnCopyable&) noexcept = delete;
  UnCopyable(UnCopyable&&) noexcept = delete;
  UnCopyable& operator=(const UnCopyable&) noexcept = delete;
  UnCopyable& operator=(UnCopyable&&) noexcept = delete;
protected:
  UnCopyable() noexcept = default;
  ~UnCopyable() noexcept = default;
};

template <typename T, typename S> inline T as_type(S x) noexcept {
  return static_cast<T>(x);
}

template <typename T, typename S> inline T* as_down(S* x) noexcept {
  return dynamic_cast<T*>(x);
}

template <typename T>
inline T* get_rawptr(const std::unique_ptr<T>& p) noexcept {
  return p.get();
}

template <typename T>
inline T* get_rawptr(const std::shared_ptr<T>& p) noexcept {
  return p.get();
}

inline str_t as_string(double d) noexcept {
  ss_t ss;
  ss << d;
  return ss.str();
}

template <typename T, typename... Args>
inline str_t as_string(T&& x, Args&&... args) noexcept {
  ss_t ss;

  ss << std::forward<T>(x);
  ((ss << std::forward<Args>(args)), ...);

  return ss.str();
}

template <typename... Args>
inline str_t string_format(strv_t fmt, const Args&... args) {
  int sz = std::snprintf(nullptr, 0, fmt.data(), args...) + 1;
  std::unique_ptr<char[]> buf{ new char[sz] };
  std::snprintf(buf.get(), sz, fmt.data(), args...);
  return str_t(buf.get(), buf.get() + sz - 1);
}

}<|endoftext|>


<commit_before>#include <cassert>
#include <chrono>
#include <deque>
#include <iostream>
#include <set>
#include <unordered_set>
#include <utility> // pair

#include <boost/dynamic_bitset.hpp>

#include "mc/units/concurrent_units.hh"
#include "mc/units/nopost_live.hh"
#include "mc/units/post.hh"
#include "mc/units/post_live.hh"
#include "mc/units/pre.hh"
#include "mc/units/sdd.hh"
#include "mc/units/worker.hh"

namespace pnmc { namespace mc { namespace units {

namespace chrono = std::chrono;

/*------------------------------------------------------------------------------------------------*/

order
mk_order(const pn::net& net)
{
  std::vector<unsigned int> units;
  units.reserve(net.units().size());

  std::unordered_set<unsigned int> units_added;
  units_added.reserve(net.units().size());

  // Iteration on places because we want to put units in the same order as the places they
  // contain.
  for (const auto& p : net.places())
  {
    if (not net.places_of_unit(p.unit).empty()) // Don't add empty units.
    {
      if (units_added.emplace(p.unit).second) // Check if units has already been added.
      {
        units.push_back(p.unit);
      }
    }
  }

  return {order_builder(units.begin(), units.end())};
}

/*------------------------------------------------------------------------------------------------*/

SDD
initial_state(const order& o, const pn::net& net)
{
  using flat_set = sdd::values::flat_set<unsigned int>;
  return SDD(o, [&](unsigned int unit) -> flat_set
                   {
                     std::vector<unsigned int> marked_places;
                     for (const auto& p : net.places_of_unit(unit))
                     {
                       /// @todo Put an 'initial' flag in place to avoid linear search.
                       const auto search = std::find( net.initial_places().cbegin()
                                                    , net.initial_places().cend()
                                                    , p.get().id);
                       if (search != net.initial_places().cend())
                       {
                         marked_places.push_back(p.get().id);
                       }
                     }
                     if (marked_places.size() == 0)
                     {
                       return flat_set({0});
                     }
                     else if (marked_places.size() == 1)
                     {
                       return flat_set({marked_places[0] + 1});
                     }
                     else
                     {
                       throw std::runtime_error("More that one initial place in an unit.");
                     }
                   }
            );
}

/*------------------------------------------------------------------------------------------------*/

homomorphism
transition_relation( const conf::pnmc_configuration& conf, const order& o
                   , const pn::net& net, boost::dynamic_bitset<>& transitions_bitset)
{
  chrono::time_point<chrono::system_clock> start;
  chrono::time_point<chrono::system_clock> end;
  std::size_t elapsed;

  start = chrono::system_clock::now();
  std::set<homomorphism> operands;
  operands.insert(id<sdd_conf>());

  // When computing dead transitions, we need to start numbering them from 0 as we use this
  // value as an index in the bitset.
  unsigned int tindex = 0;

  for (const auto& transition : net.transitions())
  {
    homomorphism h_t = id<sdd_conf>();

    if (transition.post.empty() and transition.pre.empty() and conf.compute_dead_transitions)
    {
      // Empty transition, but we need to increment the transition counter.
      ++tindex;
    }
    else if (not transition.post.empty())
    {
      // post actions.
      auto arc_cit = transition.post.begin();
      if (conf.compute_dead_transitions)
      {
        const auto f
          = function( o, net.unit_of_place(arc_cit->first)
                                    , post_live(arc_cit->first, tindex++, transitions_bitset));
        h_t = composition(h_t, sdd::carrier(o, net.unit_of_place(arc_cit->first), f));
      }
      else
      {
        homomorphism f
          = function(o, net.unit_of_place(arc_cit->first), post(arc_cit->first));
        h_t = composition(h_t, sdd::carrier(o, net.unit_of_place(arc_cit->first), f));
      }
      for (++arc_cit; arc_cit != transition.post.end(); ++arc_cit)
      {
        homomorphism f
          = function(o, net.unit_of_place(arc_cit->first), post(arc_cit->first));
        h_t = composition(h_t, sdd::carrier(o, net.unit_of_place(arc_cit->first), f));
      }
    }
    else if (conf.compute_dead_transitions)
    {
      // Target the same variable as the pre that is fired just before this fake post.
      const auto unit = transition.pre.begin()->first;

      const auto f = function(o, unit, nopost_live(tindex++, transitions_bitset));
      h_t = composition(h_t, sdd::carrier(o, unit, f));
    }

    // pre actions.
    for (const auto& arc : transition.pre)
    {
      homomorphism f = function(o, net.unit_of_place(arc.first), pre(arc.first));
      h_t = composition(h_t, sdd::carrier(o, net.unit_of_place(arc.first), f));
    }

    operands.insert(h_t);
  }
  end = chrono::system_clock::now();
  elapsed = chrono::duration_cast<chrono::seconds>(end-start).count();

  if (conf.show_time)
  {
    std::cout << "Transition relation time: " << elapsed << "s" << std::endl;
  }

  start = chrono::system_clock::now();
  const auto res = sdd::rewrite(o, fixpoint(sum(o, operands.cbegin(), operands.cend())));
  end = chrono::system_clock::now();
  elapsed = chrono::duration_cast<chrono::seconds>(end-start).count();

  if (conf.show_time)
  {
    std::cout << "Rewrite time: " << elapsed << "s" << std::endl;
  }

  return res;
}

/*------------------------------------------------------------------------------------------------*/

SDD
state_space( const conf::pnmc_configuration& conf, const order& o, SDD m
           , homomorphism h)
{
  chrono::time_point<chrono::system_clock> start = chrono::system_clock::now();
  const auto res = h(o, m);
  chrono::time_point<chrono::system_clock> end = chrono::system_clock::now();
  const std::size_t elapsed = chrono::duration_cast<chrono::seconds>(end-start).count();
  if (conf.show_time)
  {
    std::cout << "State space computation time: " << elapsed << "s" << std::endl;
  }
  return res;
}

/*------------------------------------------------------------------------------------------------*/

worker::worker(const conf::pnmc_configuration& c)
  : conf(c)
{}

/*------------------------------------------------------------------------------------------------*/

void
worker::operator()(const pn::net& net)
const
{
  auto manager = sdd::manager<sdd_conf>::init();
  boost::dynamic_bitset<> transitions_bitset(net.transitions().size());

  const auto o = mk_order(net);
  assert(not o.empty() && "Empty order");

  if (conf.order_show)
  {
    std::cout << o << std::endl;
  }

  const SDD m0 = initial_state(o, net);
  const homomorphism h = transition_relation(conf, o, net, transitions_bitset);

  if (conf.show_relation)
  {
    std::cout << h << std::endl;
  }

  const SDD m = state_space(conf, o, m0, h);

  if (conf.show_nb_states)
  {
    std::cout << m.size().template convert_to<long double>() << " states" << std::endl;
  }

  if (conf.compute_dead_transitions)
  {
    for (std::size_t i = 0; i < net.transitions().size(); ++i)
    {
      std::cout << (!transitions_bitset[i]) << "\n";
    }
  }

  if (conf.compute_concurrent_units)
  {
    compute_concurrent_units(conf, net, o, m);
  }

  if (conf.show_hash_tables_stats)
  {
//    std::cout << manager << std::endl;
  }
}

/*------------------------------------------------------------------------------------------------*/

}}} // namespace pnmc::mc::units
<commit_msg>Try FORCE.<commit_after>#include <cassert>
#include <chrono>
#include <deque>
#include <iostream>
#include <set>
#include <unordered_set>
#include <utility> // pair

#include <boost/dynamic_bitset.hpp>
#include <sdd/order/strategies/force.hh>

#include "mc/units/concurrent_units.hh"
#include "mc/units/nopost_live.hh"
#include "mc/units/post.hh"
#include "mc/units/post_live.hh"
#include "mc/units/pre.hh"
#include "mc/units/sdd.hh"
#include "mc/units/worker.hh"

namespace pnmc { namespace mc { namespace units {

namespace chrono = std::chrono;

/*------------------------------------------------------------------------------------------------*/

order
mk_order(const pn::net& net)
{
//  std::vector<unsigned int> units;
//  units.reserve(net.units().size());
//
//  std::unordered_set<unsigned int> units_added;
//  units_added.reserve(net.units().size());
//
//  // Iteration on places because we want to put units in the same order as the places they
//  // contain.
//  for (const auto& p : net.places())
//  {
//    if (not net.places_of_unit(p.unit).empty()) // Don't add empty units.
//    {
//      if (units_added.emplace(p.unit).second) // Check if unit has already been added.
//      {
//        units.push_back(p.unit);
//      }
//    }
//  }
//
//  return {order_builder(units.begin(), units.end())};

  std::vector<unsigned int> units;
  units.reserve(net.units().size());

  std::unordered_set<unsigned int> units_added;
  units_added.reserve(net.units().size());

  // Iteration on places because we want to put units in the same order as the places they
  // contain.
  for (const auto& p : net.places())
  {
    if (not net.places_of_unit(p.unit).empty()) // Don't add empty units.
    {
      if (units_added.emplace(p.unit).second) // Check if unit has already been added.
      {
        units.push_back(p.unit);
      }
    }
  }

  // The hypergraph that stores connections between the places of the Petri net.
  sdd::force::hypergraph<sdd_conf> graph(units.cbegin(), units.cend());

  std::vector<unsigned int> identifiers;
  std::unordered_set<unsigned int> identifiers_added;
  for (const auto& transition : net.transitions())
  {
    for (const auto& arc : transition.pre)
    {
//      identifiers.emplace_back(arc.first);
      const auto u = net.unit_of_place(arc.first);
      if (identifiers_added.emplace(arc.first).second)
      {
        identifiers.push_back(u);
      }
    }

    for (const auto& arc : transition.post)
    {
      const auto u = net.unit_of_place(arc.first);
      if (identifiers_added.emplace(arc.first).second)
      {
        identifiers.push_back(u);
      }
    }
    graph.add_hyperedge(identifiers.cbegin(), identifiers.cend());

    // We use this container again in the next loop.
    identifiers.clear();
    identifiers_added.clear();
  }
  // Apply the FORCE ordering strategy.
  auto force = sdd::force::worker<sdd_conf>(graph);
  const auto o = force();

  // Dump the hypergraph to a DOT file if required by the configuration.
  return sdd::order<sdd_conf>(o);
}

/*------------------------------------------------------------------------------------------------*/

SDD
initial_state(const order& o, const pn::net& net)
{
  using flat_set = sdd::values::flat_set<unsigned int>;
  return SDD(o, [&](unsigned int unit) -> flat_set
                   {
                     std::vector<unsigned int> marked_places;
                     for (const auto& p : net.places_of_unit(unit))
                     {
                       /// @todo Put an 'initial' flag in place to avoid linear search.
                       const auto search = std::find( net.initial_places().cbegin()
                                                    , net.initial_places().cend()
                                                    , p.get().id);
                       if (search != net.initial_places().cend())
                       {
                         marked_places.push_back(p.get().id);
                       }
                     }
                     if (marked_places.size() == 0)
                     {
                       return flat_set({0});
                     }
                     else if (marked_places.size() == 1)
                     {
                       return flat_set({marked_places[0] + 1});
                     }
                     else
                     {
                       throw std::runtime_error("More that one initial place in an unit.");
                     }
                   }
            );
}

/*------------------------------------------------------------------------------------------------*/

homomorphism
transition_relation( const conf::pnmc_configuration& conf, const order& o
                   , const pn::net& net, boost::dynamic_bitset<>& transitions_bitset)
{
  chrono::time_point<chrono::system_clock> start;
  chrono::time_point<chrono::system_clock> end;
  std::size_t elapsed;

  start = chrono::system_clock::now();
  std::set<homomorphism> operands;
  operands.insert(id<sdd_conf>());

  // When computing dead transitions, we need to start numbering them from 0 as we use this
  // value as an index in the bitset.
  unsigned int tindex = 0;

  for (const auto& transition : net.transitions())
  {
    homomorphism h_t = id<sdd_conf>();

    if (transition.post.empty() and transition.pre.empty() and conf.compute_dead_transitions)
    {
      // Empty transition, but we need to increment the transition counter.
      ++tindex;
    }
    else if (not transition.post.empty())
    {
      // post actions.
      auto arc_cit = transition.post.begin();
      if (conf.compute_dead_transitions)
      {
        const auto f
          = function( o, net.unit_of_place(arc_cit->first)
                                    , post_live(arc_cit->first, tindex++, transitions_bitset));
        h_t = composition(h_t, sdd::carrier(o, net.unit_of_place(arc_cit->first), f));
      }
      else
      {
        homomorphism f
          = function(o, net.unit_of_place(arc_cit->first), post(arc_cit->first));
        h_t = composition(h_t, sdd::carrier(o, net.unit_of_place(arc_cit->first), f));
      }
      for (++arc_cit; arc_cit != transition.post.end(); ++arc_cit)
      {
        homomorphism f
          = function(o, net.unit_of_place(arc_cit->first), post(arc_cit->first));
        h_t = composition(h_t, sdd::carrier(o, net.unit_of_place(arc_cit->first), f));
      }
    }
    else if (conf.compute_dead_transitions)
    {
      // Target the same variable as the pre that is fired just before this fake post.
      const auto unit = transition.pre.begin()->first;

      const auto f = function(o, unit, nopost_live(tindex++, transitions_bitset));
      h_t = composition(h_t, sdd::carrier(o, unit, f));
    }

    // pre actions.
    for (const auto& arc : transition.pre)
    {
      homomorphism f = function(o, net.unit_of_place(arc.first), pre(arc.first));
      h_t = composition(h_t, sdd::carrier(o, net.unit_of_place(arc.first), f));
    }

    operands.insert(h_t);
  }
  end = chrono::system_clock::now();
  elapsed = chrono::duration_cast<chrono::seconds>(end-start).count();

  if (conf.show_time)
  {
    std::cout << "Transition relation time: " << elapsed << "s" << std::endl;
  }

  start = chrono::system_clock::now();
  const auto res = sdd::rewrite(o, fixpoint(sum(o, operands.cbegin(), operands.cend())));
  end = chrono::system_clock::now();
  elapsed = chrono::duration_cast<chrono::seconds>(end-start).count();

  if (conf.show_time)
  {
    std::cout << "Rewrite time: " << elapsed << "s" << std::endl;
  }

  return res;
}

/*------------------------------------------------------------------------------------------------*/

SDD
state_space( const conf::pnmc_configuration& conf, const order& o, SDD m
           , homomorphism h)
{
  chrono::time_point<chrono::system_clock> start = chrono::system_clock::now();
  const auto res = h(o, m);
  chrono::time_point<chrono::system_clock> end = chrono::system_clock::now();
  const std::size_t elapsed = chrono::duration_cast<chrono::seconds>(end-start).count();
  if (conf.show_time)
  {
    std::cout << "State space computation time: " << elapsed << "s" << std::endl;
  }
  return res;
}

/*------------------------------------------------------------------------------------------------*/

worker::worker(const conf::pnmc_configuration& c)
  : conf(c)
{}

/*------------------------------------------------------------------------------------------------*/

void
worker::operator()(const pn::net& net)
const
{
  auto manager = sdd::manager<sdd_conf>::init();
  boost::dynamic_bitset<> transitions_bitset(net.transitions().size());

  const auto o = mk_order(net);
  assert(not o.empty() && "Empty order");

  if (conf.order_show)
  {
    std::cout << o << std::endl;
  }

  const SDD m0 = initial_state(o, net);
  const homomorphism h = transition_relation(conf, o, net, transitions_bitset);

  if (conf.show_relation)
  {
    std::cout << h << std::endl;
  }

  const SDD m = state_space(conf, o, m0, h);

  if (conf.show_nb_states)
  {
    std::cout << m.size().template convert_to<long double>() << " states" << std::endl;
  }

  if (conf.compute_dead_transitions)
  {
    for (std::size_t i = 0; i < net.transitions().size(); ++i)
    {
      std::cout << (!transitions_bitset[i]) << "\n";
    }
  }

  if (conf.compute_concurrent_units)
  {
    compute_concurrent_units(conf, net, o, m);
  }

  if (conf.show_hash_tables_stats)
  {
//    std::cout << manager << std::endl;
  }
}

/*------------------------------------------------------------------------------------------------*/

}}} // namespace pnmc::mc::units
<|endoftext|>