<commit_before>#ifndef _KUKA_FRI_
#define _KUKA_FRI_

#include <chrono>
#include <stdexcept>

#include <boost/units/physical_dimensions/torque.hpp>
#include <boost/units/physical_dimensions/plane_angle.hpp>
#include <boost/units/physical_dimensions/torque.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/asio.hpp>

// Kuka include files
#include "friClientIf.h"
#include "friClientData.h"

namespace KUKA {
	namespace LBRState {
		const int NUM_DOF = 7;
		const int LBRMONITORMESSAGEID = 0x245142;
	}
	namespace LBRCommand {
		// Following from Kuka friLBRCommand.cpp
		const int LBRCOMMANDMESSAGEID = 0x34001;
	}
}

namespace robone { namespace robot {

		struct state_tag{}; /// @todo consider moving to separate tag header
		struct command_tag{};
		struct external_state_tag{}; /// @todo consider eliding external and internal sensor tag
		struct interpolated_state_tag{};

	namespace arm {

		namespace kuka {
				// Following from Kuka example program
				const int default_port_id = 30200;

			namespace detail {
				template<typename OutputIterator>
				void copyJointState(tRepeatedDoubleArguments* values,OutputIterator it){
					std::copy(static_cast<double*>(values->value),static_cast<double*>(values->value)+KUKA::LBRState::NUM_DOF,it);
				}
			}
		}

	/// copy measured joint angle to output iterator
	template<typename OutputIterator>
	void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, boost::units::plane_angle_base_dimension, robone::robot::state_tag){

        if (monitoringMsg.monitorData.has_measuredJointPosition) {
           kuka::detail::copyJointState(static_cast<tRepeatedDoubleArguments*>(monitoringMsg.monitorData.measuredJointPosition.value.arg),it);
        }
	}

	/// copy commanded joint angle to output iterator
	template<typename OutputIterator>
	void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, boost::units::plane_angle_base_dimension, robone::robot::command_tag){

        if (monitoringMsg.monitorData.has_commandedJointPosition) {
           kuka::detail::copyJointState(static_cast<tRepeatedDoubleArguments*>(monitoringMsg.monitorData.commandedJointPosition.value.arg),it);
        }
	}

	/// copy measured joint torque to output iterator
	template<typename OutputIterator>
	void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, boost::units::torque_dimension, robone::robot::state_tag){
        if (monitoringMsg.monitorData.has_measuredTorque) {
           kuka::detail::copyJointState(static_cast<tRepeatedDoubleArguments*>(monitoringMsg.monitorData.measuredTorque.value.arg),it);
        }
	}

	/// copy measured external joint torque to output iterator
	template<typename OutputIterator>
	void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, boost::units::torque_dimension, external_state_tag){
        if (monitoringMsg.monitorData.has_externalTorque) {
           kuka::detail::copyJointState(static_cast<tRepeatedDoubleArguments*>(monitoringMsg.monitorData.externalTorque.value.arg),it);
        }
	}

	/// copy commanded  joint torque to output iterator
	template<typename OutputIterator>
	void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, boost::units::torque_dimension, command_tag){
        if (monitoringMsg.monitorData.has_commandedTorque) {
           kuka::detail::copyJointState(static_cast<tRepeatedDoubleArguments*>(monitoringMsg.monitorData.commandedTorque.value.arg),it);
        }
	}

	/// copy interpolated commanded joint angles
	template<typename OutputIterator>
	void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it,boost::units::plane_angle_base_dimension,interpolated_state_tag){
        if (monitoringMsg.ipoData.has_jointPosition) {
           kuka::detail::copyJointState(static_cast<tRepeatedDoubleArguments*>(monitoringMsg.ipoData.jointPosition.value.arg),it);
        }

	}

	/// @todo consider changing these get* functions to get(data,type_tag());
	KUKA::FRI::ESafetyState getSafetyState(const FRIMonitoringMessage& monitoringMsg) {
		KUKA::FRI::ESafetyState KukaSafetyState = KUKA::FRI::ESafetyState::NORMAL_OPERATION;
	    if (monitoringMsg.has_robotInfo) {
	        if (monitoringMsg.robotInfo.has_safetyState)
	            KukaSafetyState = static_cast<KUKA::FRI::ESafetyState>(monitoringMsg.robotInfo.safetyState);
	    }
		return KukaSafetyState;
	}

	/// @todo consider changing these get* functions to get(data,type_tag());
	KUKA::FRI::EOperationMode getOperationMode(const FRIMonitoringMessage& monitoringMsg) {
		KUKA::FRI::EOperationMode KukaSafetyState = KUKA::FRI::EOperationMode::TEST_MODE_1;
	    if (monitoringMsg.has_robotInfo) {
	        if (monitoringMsg.robotInfo.has_operationMode)
	            KukaSafetyState = static_cast<KUKA::FRI::EOperationMode>(monitoringMsg.robotInfo.operationMode);
	    }
		return KukaSafetyState;
	}

	/// @todo consider changing these get* functions to get(data,type_tag());
		/// @todo this one requires a callback... figure it out
	KUKA::FRI::EDriveState getDriveState(const FRIMonitoringMessage& monitoringMsg) {
		KUKA::FRI::EDriveState KukaSafetyState = KUKA::FRI::EDriveState::OFF;
	    if (monitoringMsg.has_robotInfo) {
	         //   KukaSafetyState = static_cast<KUKA::FRI::EDriveState>(monitoringMsg.robotInfo.driveState);
	    }
		return KukaSafetyState;
	}

#if 0 // original getConnectionInfo
	getConnectionInfo(const FRIMonitoringMessage& monitoringMsg){

	    if (monitoringMsg.has_connectionInfo) {
	        KukaSessionState = monitoringMsg.connectionInfo.sessionState;
	        KukaQuality = monitoringMsg.connectionInfo.quality;
	        if (monitoringMsg.connectionInfo.has_sendPeriod)
	            KukaSendPeriod = monitoringMsg.connectionInfo.sendPeriod;
	        if (monitoringMsg.connectionInfo.has_receiveMultiplier)
	            KukaReceiveMultiplier = monitoringMsg.connectionInfo.receiveMultiplier;
	    }
	}
#endif

	KUKA::FRI::ESessionState getSessionState(const FRIMonitoringMessage& monitoringMsg){
		KUKA::FRI::ESessionState KukaSessionState = KUKA::FRI::ESessionState::IDLE;
	    if (monitoringMsg.has_connectionInfo) {
	        KukaSessionState = static_cast<KUKA::FRI::ESessionState>(monitoringMsg.connectionInfo.sessionState);
	    }
		return KukaSessionState;
	}

	KUKA::FRI::EConnectionQuality getConnectionQuality(const FRIMonitoringMessage& monitoringMsg){
		KUKA::FRI::EConnectionQuality KukaQuality = KUKA::FRI::EConnectionQuality::POOR;
	    if (monitoringMsg.has_connectionInfo) {
	        KukaQuality = static_cast<KUKA::FRI::EConnectionQuality>(monitoringMsg.connectionInfo.quality);
	    }
		return KukaQuality;
	}

	uint32_t getSendPeriod(const FRIMonitoringMessage& monitoringMsg){
		uint32_t KukaSendPeriod = 0;
	    if (monitoringMsg.has_connectionInfo) {
	        if (monitoringMsg.connectionInfo.has_sendPeriod)
	            KukaSendPeriod = monitoringMsg.connectionInfo.sendPeriod;
	    }
		return KukaSendPeriod;
	}

	std::size_t getReceiveMultiplier(const FRIMonitoringMessage& monitoringMsg){
		std::size_t KukaReceiveMultiplier = 0;
	    if (monitoringMsg.has_connectionInfo) {
	        if (monitoringMsg.connectionInfo.has_receiveMultiplier)
	            KukaReceiveMultiplier = monitoringMsg.connectionInfo.receiveMultiplier;
	    }
		return KukaReceiveMultiplier;
	}

	std::chrono::time_point<std::chrono::high_resolution_clock> getTimeStamp(const FRIMonitoringMessage& monitoringMsg){
		// defaults to the epoch
		std::chrono::time_point<std::chrono::high_resolution_clock> timestamp;
        if (monitoringMsg.monitorData.has_timestamp) {
            timestamp += std::chrono::seconds(monitoringMsg.monitorData.timestamp.sec) +
				         std::chrono::nanoseconds(monitoringMsg.monitorData.timestamp.nanosec);
        }
		return timestamp;
	}

	/// @todo replace with something generic
	struct KukaState {
		typedef boost::array<double,KUKA::LBRState::NUM_DOF> joint_state;
		joint_state position;
		joint_state torque;
		joint_state commandedPosition;
		joint_state commandedTorque;
		joint_state ipoJointPosition;
		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;
	};

	    // Decode message buffer (using nanopb decoder)
	void decode(KUKA::FRI::ClientData& friData){
	/// @todo FRI_MONITOR_MSG_MAX_SIZE may not be the right size... probably need the actual size received
	    if (!friData.decoder.decode(friData.receiveBuffer, KUKA::FRI::FRI_MONITOR_MSG_MAX_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;
	    }
	}

	/// encode data in the class into the send buffer
	/// @todo update the statements in here to run on the actual data types available
	void encode(KUKA::FRI::ClientData& friData,KukaState& state){
	    // Check whether to send a response
	    friData.lastSendCounter = 0;

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

	    // copy current joint position to commanded position
	    friData.commandMsg.has_commandData = true;
	    friData.commandMsg.commandData.has_jointPosition = true;
	    tRepeatedDoubleArguments *dest  = (tRepeatedDoubleArguments*)friData.commandMsg.commandData.jointPosition.value.arg;
		if ((state.sessionState == KUKA::FRI::COMMANDING_WAIT) || (state.sessionState == KUKA::FRI::COMMANDING_ACTIVE))
		    std::copy(state.ipoJointPosition.begin(),state.ipoJointPosition.end(),dest->value); /// @todo is this the right thing to copy?
	    else
		    std::copy(state.commandedPosition.begin(),state.commandedPosition.end(),dest->value);  /// @todo is this the right thing to copy?

		int buffersize = KUKA::FRI::FRI_COMMAND_MSG_MAX_SIZE;
	        if (!friData.encoder.encode(friData.sendBuffer, buffersize))
	            return;
	}

	void copy(const FRIMonitoringMessage& monitoringMsg, KukaState& state ){
		copy(monitoringMsg,state.position.begin(),boost::units::plane_angle_base_dimension(),robone::robot::state_tag());
		copy(monitoringMsg,state.torque.begin(),boost::units::torque_dimension(),robone::robot::state_tag());
		copy(monitoringMsg,state.commandedPosition.begin(),boost::units::plane_angle_base_dimension(),robone::robot::command_tag());
		copy(monitoringMsg,state.commandedTorque.begin(),boost::units::torque_dimension(),robone::robot::command_tag());
		copy(monitoringMsg,state.ipoJointPosition.begin(),boost::units::plane_angle_base_dimension(),robone::robot::interpolated_state_tag());
		state.sessionState = getSessionState(monitoringMsg);
		state.connectionQuality = getConnectionQuality(monitoringMsg);
		state.safetyState = getSafetyState(monitoringMsg);
		state.operationMode = getOperationMode(monitoringMsg);
		/// @todo this one requires a callback... figure it out
		//state.driveState = getDriveState(monitoringMsg);

		/// @todo fill out missing state update steps

	}

	/// @todo implment async version of this, probably in a small class
	/// @todo implement sending state
	void update_state(boost::asio::ip::udp::socket& socket, KUKA::FRI::ClientData& friData, KukaState& state){
		std::size_t buf_size = socket.receive(boost::asio::buffer(friData.receiveBuffer,KUKA::FRI::FRI_MONITOR_MSG_MAX_SIZE));
		copy(friData.monitoringMsg,state);

	    friData.lastSendCounter++;
	    // Check whether to send a response
	    if (friData.lastSendCounter >= friData.monitoringMsg.connectionInfo.receiveMultiplier){
	    	encode(friData,state);
			socket.send(boost::asio::buffer(friData.sendBuffer,KUKA::FRI::FRI_MONITOR_MSG_MAX_SIZE));
	    }
	}

}}} /// namespace robone::robot::arm

#endif

<commit_msg>#12 INTERMEDIATE CODE: correct buffer sizes now passed around<commit_after>#ifndef _KUKA_FRI_
#define _KUKA_FRI_

#include <chrono>
#include <stdexcept>

#include <boost/units/physical_dimensions/torque.hpp>
#include <boost/units/physical_dimensions/plane_angle.hpp>
#include <boost/units/physical_dimensions/torque.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/asio.hpp>

// Kuka include files
#include "friClientIf.h"
#include "friClientData.h"

namespace KUKA {
	namespace LBRState {
		const int NUM_DOF = 7;
		const int LBRMONITORMESSAGEID = 0x245142;
	}
	namespace LBRCommand {
		// Following from Kuka friLBRCommand.cpp
		const int LBRCOMMANDMESSAGEID = 0x34001;
	}
}

namespace robone { namespace robot {

		struct state_tag{}; /// @todo consider moving to separate tag header
		struct command_tag{};
		struct external_state_tag{}; /// @todo consider eliding external and internal sensor tag
		struct interpolated_state_tag{};

	namespace arm {

		namespace kuka {
				// Following from Kuka example program
				const int default_port_id = 30200;

			namespace detail {
				template<typename OutputIterator>
				void copyJointState(tRepeatedDoubleArguments* values,OutputIterator it){
					std::copy(static_cast<double*>(values->value),static_cast<double*>(values->value)+KUKA::LBRState::NUM_DOF,it);
				}
			}
		}

	/// copy measured joint angle to output iterator
	template<typename OutputIterator>
	void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, boost::units::plane_angle_base_dimension, robone::robot::state_tag){

        if (monitoringMsg.monitorData.has_measuredJointPosition) {
           kuka::detail::copyJointState(static_cast<tRepeatedDoubleArguments*>(monitoringMsg.monitorData.measuredJointPosition.value.arg),it);
        }
	}

	/// copy commanded joint angle to output iterator
	template<typename OutputIterator>
	void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, boost::units::plane_angle_base_dimension, robone::robot::command_tag){

        if (monitoringMsg.monitorData.has_commandedJointPosition) {
           kuka::detail::copyJointState(static_cast<tRepeatedDoubleArguments*>(monitoringMsg.monitorData.commandedJointPosition.value.arg),it);
        }
	}

	/// copy measured joint torque to output iterator
	template<typename OutputIterator>
	void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, boost::units::torque_dimension, robone::robot::state_tag){
        if (monitoringMsg.monitorData.has_measuredTorque) {
           kuka::detail::copyJointState(static_cast<tRepeatedDoubleArguments*>(monitoringMsg.monitorData.measuredTorque.value.arg),it);
        }
	}

	/// copy measured external joint torque to output iterator
	template<typename OutputIterator>
	void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, boost::units::torque_dimension, external_state_tag){
        if (monitoringMsg.monitorData.has_externalTorque) {
           kuka::detail::copyJointState(static_cast<tRepeatedDoubleArguments*>(monitoringMsg.monitorData.externalTorque.value.arg),it);
        }
	}

	/// copy commanded  joint torque to output iterator
	template<typename OutputIterator>
	void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, boost::units::torque_dimension, command_tag){
        if (monitoringMsg.monitorData.has_commandedTorque) {
           kuka::detail::copyJointState(static_cast<tRepeatedDoubleArguments*>(monitoringMsg.monitorData.commandedTorque.value.arg),it);
        }
	}

	/// copy interpolated commanded joint angles
	template<typename OutputIterator>
	void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it,boost::units::plane_angle_base_dimension,interpolated_state_tag){
        if (monitoringMsg.ipoData.has_jointPosition) {
           kuka::detail::copyJointState(static_cast<tRepeatedDoubleArguments*>(monitoringMsg.ipoData.jointPosition.value.arg),it);
        }

	}

	/// @todo consider changing these get* functions to get(data,type_tag());
	KUKA::FRI::ESafetyState getSafetyState(const FRIMonitoringMessage& monitoringMsg) {
		KUKA::FRI::ESafetyState KukaSafetyState = KUKA::FRI::ESafetyState::NORMAL_OPERATION;
	    if (monitoringMsg.has_robotInfo) {
	        if (monitoringMsg.robotInfo.has_safetyState)
	            KukaSafetyState = static_cast<KUKA::FRI::ESafetyState>(monitoringMsg.robotInfo.safetyState);
	    }
		return KukaSafetyState;
	}

	/// @todo consider changing these get* functions to get(data,type_tag());
	KUKA::FRI::EOperationMode getOperationMode(const FRIMonitoringMessage& monitoringMsg) {
		KUKA::FRI::EOperationMode KukaSafetyState = KUKA::FRI::EOperationMode::TEST_MODE_1;
	    if (monitoringMsg.has_robotInfo) {
	        if (monitoringMsg.robotInfo.has_operationMode)
	            KukaSafetyState = static_cast<KUKA::FRI::EOperationMode>(monitoringMsg.robotInfo.operationMode);
	    }
		return KukaSafetyState;
	}

	/// @todo consider changing these get* functions to get(data,type_tag());
		/// @todo this one requires a callback... figure it out
	KUKA::FRI::EDriveState getDriveState(const FRIMonitoringMessage& monitoringMsg) {
		KUKA::FRI::EDriveState KukaSafetyState = KUKA::FRI::EDriveState::OFF;
	    if (monitoringMsg.has_robotInfo) {
	         //   KukaSafetyState = static_cast<KUKA::FRI::EDriveState>(monitoringMsg.robotInfo.driveState);
	    }
		return KukaSafetyState;
	}

#if 0 // original getConnectionInfo
	getConnectionInfo(const FRIMonitoringMessage& monitoringMsg){

	    if (monitoringMsg.has_connectionInfo) {
	        KukaSessionState = monitoringMsg.connectionInfo.sessionState;
	        KukaQuality = monitoringMsg.connectionInfo.quality;
	        if (monitoringMsg.connectionInfo.has_sendPeriod)
	            KukaSendPeriod = monitoringMsg.connectionInfo.sendPeriod;
	        if (monitoringMsg.connectionInfo.has_receiveMultiplier)
	            KukaReceiveMultiplier = monitoringMsg.connectionInfo.receiveMultiplier;
	    }
	}
#endif

	KUKA::FRI::ESessionState getSessionState(const FRIMonitoringMessage& monitoringMsg){
		KUKA::FRI::ESessionState KukaSessionState = KUKA::FRI::ESessionState::IDLE;
	    if (monitoringMsg.has_connectionInfo) {
	        KukaSessionState = static_cast<KUKA::FRI::ESessionState>(monitoringMsg.connectionInfo.sessionState);
	    }
		return KukaSessionState;
	}

	KUKA::FRI::EConnectionQuality getConnectionQuality(const FRIMonitoringMessage& monitoringMsg){
		KUKA::FRI::EConnectionQuality KukaQuality = KUKA::FRI::EConnectionQuality::POOR;
	    if (monitoringMsg.has_connectionInfo) {
	        KukaQuality = static_cast<KUKA::FRI::EConnectionQuality>(monitoringMsg.connectionInfo.quality);
	    }
		return KukaQuality;
	}

	uint32_t getSendPeriod(const FRIMonitoringMessage& monitoringMsg){
		uint32_t KukaSendPeriod = 0;
	    if (monitoringMsg.has_connectionInfo) {
	        if (monitoringMsg.connectionInfo.has_sendPeriod)
	            KukaSendPeriod = monitoringMsg.connectionInfo.sendPeriod;
	    }
		return KukaSendPeriod;
	}

	std::size_t getReceiveMultiplier(const FRIMonitoringMessage& monitoringMsg){
		std::size_t KukaReceiveMultiplier = 0;
	    if (monitoringMsg.has_connectionInfo) {
	        if (monitoringMsg.connectionInfo.has_receiveMultiplier)
	            KukaReceiveMultiplier = monitoringMsg.connectionInfo.receiveMultiplier;
	    }
		return KukaReceiveMultiplier;
	}

	std::chrono::time_point<std::chrono::high_resolution_clock> getTimeStamp(const FRIMonitoringMessage& monitoringMsg){
		// defaults to the epoch
		std::chrono::time_point<std::chrono::high_resolution_clock> timestamp;
        if (monitoringMsg.monitorData.has_timestamp) {
            timestamp += std::chrono::seconds(monitoringMsg.monitorData.timestamp.sec) +
				         std::chrono::nanoseconds(monitoringMsg.monitorData.timestamp.nanosec);
        }
		return timestamp;
	}

	/// @todo replace with something generic
	struct KukaState {
		typedef boost::array<double,KUKA::LBRState::NUM_DOF> joint_state;
		joint_state position;
		joint_state torque;
		joint_state commandedPosition;
		joint_state commandedTorque;
		joint_state ipoJointPosition;
		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;
	};

	    // Decode message buffer (using nanopb decoder)
	void decode(KUKA::FRI::ClientData& friData, std::size_t msg_size){
	/// @todo FRI_MONITOR_MSG_MAX_SIZE may not be the right size... probably need the actual size received
	    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;
	    }
	}

	/// encode data in the class into the send buffer
	/// @todo update the statements in here to run on the actual data types available
	std::size_t encode(KUKA::FRI::ClientData& friData,KukaState& state){
	    // Check whether to send a response
	    friData.lastSendCounter = 0;

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

	    // copy current joint position to commanded position
	    friData.commandMsg.has_commandData = true;
	    friData.commandMsg.commandData.has_jointPosition = true;
	    tRepeatedDoubleArguments *dest  = (tRepeatedDoubleArguments*)friData.commandMsg.commandData.jointPosition.value.arg;
		if ((state.sessionState == KUKA::FRI::COMMANDING_WAIT) || (state.sessionState == KUKA::FRI::COMMANDING_ACTIVE))
		    std::copy(state.ipoJointPosition.begin(),state.ipoJointPosition.end(),dest->value); /// @todo is this the right thing to copy?
	    else
		    std::copy(state.commandedPosition.begin(),state.commandedPosition.end(),dest->value);  /// @todo is this the right thing to copy?

		int buffersize = KUKA::FRI::FRI_COMMAND_MSG_MAX_SIZE;
	        if (!friData.encoder.encode(friData.sendBuffer, buffersize))
	            return 0;

		return buffersize;
	}

	void copy(const FRIMonitoringMessage& monitoringMsg, KukaState& state ){
		copy(monitoringMsg,state.position.begin(),boost::units::plane_angle_base_dimension(),robone::robot::state_tag());
		copy(monitoringMsg,state.torque.begin(),boost::units::torque_dimension(),robone::robot::state_tag());
		copy(monitoringMsg,state.commandedPosition.begin(),boost::units::plane_angle_base_dimension(),robone::robot::command_tag());
		copy(monitoringMsg,state.commandedTorque.begin(),boost::units::torque_dimension(),robone::robot::command_tag());
		copy(monitoringMsg,state.ipoJointPosition.begin(),boost::units::plane_angle_base_dimension(),robone::robot::interpolated_state_tag());
		state.sessionState = getSessionState(monitoringMsg);
		state.connectionQuality = getConnectionQuality(monitoringMsg);
		state.safetyState = getSafetyState(monitoringMsg);
		state.operationMode = getOperationMode(monitoringMsg);
		/// @todo this one requires a callback... figure it out
		//state.driveState = getDriveState(monitoringMsg);

		/// @todo fill out missing state update steps

	}

	/// @todo implment async version of this, probably in a small class
	/// @todo implement sending state
	void update_state(boost::asio::ip::udp::socket& socket, KUKA::FRI::ClientData& friData, KukaState& state){
		std::size_t buf_size = socket.receive(boost::asio::buffer(friData.receiveBuffer,KUKA::FRI::FRI_MONITOR_MSG_MAX_SIZE));
		decode(friData,buf_size);
		copy(friData.monitoringMsg,state);

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

}}} /// namespace robone::robot::arm

#endif

<|endoftext|>


<commit_before>// Copyright (c) 2011, Christian Rorvik
// Distributed under the Simplified BSD License (See accompanying file LICENSE.txt)

#ifndef CRUNCH_CONCURRENCY_PLATFORM_WIN32_ATOMIC_OPS_X86_HPP
#define CRUNCH_CONCURRENCY_PLATFORM_WIN32_ATOMIC_OPS_X86_HPP

#include "crunch/base/assert.hpp"
#include "crunch/concurrency/fence.hpp"
#include "crunch/concurrency/memory_order.hpp"

#include <intrin.h>

namespace Crunch { namespace Concurrency { namespace Platform {

namespace Detail
{
    inline char AtomicLoad(char volatile const& src) { return src; }
    inline short AtomicLoad(short volatile const& src) { return src; }
    inline long AtomicLoad(long volatile const& src) { return src; }

    inline __int64 AtomicLoad(__int64 volatile const& src)
    {
#if defined (CRUNCH_ARCH_X86_64)
        return src;
#else
        __int64 result;
        __asm
        {
            mov eax, dword ptr [src]
            fild qword ptr [eax]
            fistp qword ptr [result]
        }
        return result;
#endif
    }

    inline __m128i AtomicLoad(__m128i volatile const& src)
    {
        return _mm_load_si128(const_cast<__m128i const*>(&src));;
    }

    inline void AtomicStore(char volatile& dst, char src) { dst = src; }
    inline void AtomicStore(short volatile& dst, short src) { dst = src; }
    inline void AtomicStore(long volatile& dst, long src) { dst = src; }

    inline void AtomicStore(__int64 volatile& dst, __int64 src)
    {
#if defined (CRUNCH_ARCH_X86_64)
        dst = src;
#else
        __asm
        {
            mov eax, dword ptr [dst]
            fild qword ptr [src]
            fistp qword ptr [eax]
        }
#endif
    };

    inline void AtomicStore(__m128i volatile& dst, __m128i src)
    {
        _mm_store_si128(const_cast<__m128i*>(&dst), src);
    }

    inline void AtomicStoreSeqCst(char volatile& dst, char src)
    {
        CRUNCH_MEMORY_FENCE();
        AtomicStore(dst, src);
        CRUNCH_MEMORY_FENCE();
    }

    inline void AtomicStoreSeqCst(short volatile& dst, short src)
    {
        CRUNCH_MEMORY_FENCE();
        AtomicStore(dst, src);
        CRUNCH_MEMORY_FENCE();
    }

    inline void AtomicStoreSeqCst(long volatile& dst, long src)
    {
        _InterlockedExchange(&dst, src);
    }

    inline void AtomicStoreSeqCst(__int64 volatile& dst, __int64 src)
    {
#if defined (CRUNCH_ARCH_X86_64)
        _InterlockedExchange64(&dst, src);
#else
        CRUNCH_MEMORY_FENCE();
        AtomicStore(dst, src);
        CRUNCH_MEMORY_FENCE();
#endif
    }

    inline void AtomicStoreSeqCst(__m128i volatile& dst, __m128i src)
    {
        CRUNCH_MEMORY_FENCE();
        AtomicStore(dst, src);
        CRUNCH_MEMORY_FENCE();
    }

#if defined (CRUNCH_ARCH_X86_32)
    template<typename Op>
    inline __int64 FallbackAtomicOp(__int64 volatile& value, Op op)
    {
        __int64 oldValue = value;
        for (;;)
        {
            __int64 const newValue = op(oldValue);
            __int64 currentValue = _InterlockedCompareExchange64(&value, newValue, oldValue);
            if (currentValue == oldValue)
                return oldValue;

            _mm_pause();
            oldValue = currentValue;
        }
    }
#endif
}

template<typename T>
inline T AtomicLoad(T volatile& src, MemoryOrder ordering = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&src, sizeof(T));

    if (ordering == MEMORY_ORDER_SEQ_CST)
        CRUNCH_COMPILER_FENCE();

    T result = Detail::AtomicLoad(src);

    if (ordering & MEMORY_ORDER_ACQUIRE)
        CRUNCH_COMPILER_FENCE();

    return result;
}

template<typename T>
inline void AtomicStore(T volatile& dst, T src, MemoryOrder ordering = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&dst, sizeof(T));

    if (ordering == MEMORY_ORDER_SEQ_CST)
    {
        Detail::AtomicStoreSeqCst(dst, src);
    }
    else
    {
        if (ordering & MEMORY_ORDER_RELEASE)
            CRUNCH_COMPILER_FENCE();

        Detail::AtomicStore(dst, src);
    }
}

inline long AtomicSwap(long volatile& dst, long src, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&dst, 4);
    return _InterlockedExchange(&dst, src);
}

#if defined (CRUNCH_ARCH_X86_64)
inline __int64 AtomicSwap(__int64 volatile& dst, __int64 src, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&dst, 8);
    return _InterlockedExchange64(&dst, src);
}
#endif

inline bool AtomicCompareAndSwap(short volatile& dst, short src, short& cmp, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&dst, 2);
    short cmp_ = cmp;
    cmp = _InterlockedCompareExchange16(&dst, src, cmp);
    return cmp_ == cmp;
}

inline bool AtomicCompareAndSwap(long volatile& dst, long src, long& cmp, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&dst, 4);
    long cmp_ = cmp;
    cmp = _InterlockedCompareExchange(&dst, src, cmp);
    return cmp_ == cmp;
}

inline bool AtomicCompareAndSwap(__int64 volatile& dst, __int64 src, __int64& cmp, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&dst, 8);
    __int64 cmp_ = cmp;
    cmp = _InterlockedCompareExchange64(&dst, src, cmp);
    return cmp_ == cmp;
}

#if defined (CRUNCH_ARCH_X86_64)
inline bool AtomicCompareAndSwap(__m128i volatile& dst, __m128i src, __m128i& cmp, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&dst, 16);

    return _InterlockedCompareExchange128(
        dst.m128i_i64,
        src.m128i_i64[1],
        src.m128i_i64[0],
        cmp.m128i_i64) != 0;
}
#endif

inline short AtomicIncrement(short volatile& addend, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 2);
    return _InterlockedIncrement16(&addend) - 1;
}

inline long AtomicIncrement(long volatile& addend, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 4);
    return _InterlockedIncrement(&addend) - 1;
}

inline __int64 AtomicIncrement(__int64 volatile& addend, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 8);
#if defined (CRUNCH_ARCH_X86_64)
    return _InterlockedIncrement64(&addend) - 1;
#else
    return Detail::FallbackAtomicOp(addend, [] (__int64 x) { return x + 1; });
#endif
}

inline short AtomicDecrement(short volatile& addend, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 2);
    return _InterlockedDecrement16(&addend) + 1;
}

inline long AtomicDecrement(long volatile& addend, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 4);
    return _InterlockedDecrement(&addend) + 1;
}

inline __int64 AtomicDecrement(__int64 volatile& addend, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 8);
#if defined (CRUNCH_ARCH_X86_64)
    return _InterlockedDecrement64(&addend) + 1;
#else
    return Detail::FallbackAtomicOp(addend, [] (__int64 x) { return x - 1; });
#endif
}

inline long AtomicAdd(long volatile& addend, long value, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 4);
    return _InterlockedExchangeAdd(&addend, value);
}

inline __int64 AtomicAdd(__int64 volatile& addend, __int64 value, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 8);
#if defined (CRUNCH_ARCH_X86_64)
    return _InterlockedExchangeAdd64(&addend, value);
#else
    return Detail::FallbackAtomicOp(addend, [=] (__int64 x) { return x + value; });
#endif
}

inline long AtomicSub(long volatile& addend, long value, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 4);
    return _InterlockedExchangeAdd(&addend, -value);
}

inline __int64 AtomicSub(__int64 volatile& addend, __int64 value, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 8);
#if defined (CRUNCH_ARCH_X86_64)
    return _InterlockedExchangeAdd64(&addend, -value);
#else
    return Detail::FallbackAtomicOp(addend, [=] (__int64 x) { return x - value; });
#endif
}

inline char AtomicAnd(char volatile& value, char mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    return _InterlockedAnd8(&value, mask);
}

inline short AtomicAnd(short volatile& value, short mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 2);
    return _InterlockedAnd16(&value, mask);
}

inline long AtomicAnd(long volatile& value, long mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 4);
    return _InterlockedAnd(&value, mask);
}

inline __int64 AtomicAnd(__int64 volatile& value, __int64 mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 8);
#if defined (CRUNCH_ARCH_X86_64)
    return _InterlockedAnd64(&value, mask);
#else
    return Detail::FallbackAtomicOp(value, [=] (__int64 x) { return x & mask; });
#endif
}

inline char AtomicOr(char volatile& value, char mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    return _InterlockedOr8(&value, mask);
}

inline short AtomicOr(short volatile& value, short mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 2);
    return _InterlockedOr16(&value, mask);
}

inline long AtomicOr(long volatile& value, long mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 4);
    return _InterlockedOr(&value, mask);
}

inline __int64 AtomicOr(__int64 volatile& value, __int64 mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 8);
#if defined (CRUNCH_ARCH_X86_64)
    return _InterlockedOr64(&value, mask);
#else
    return Detail::FallbackAtomicOp(value, [=] (__int64 x) { return x | mask; });
#endif
}

inline char AtomicXor(char volatile& value, char mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    return _InterlockedXor8(&value, mask);
}

inline short AtomicXor(short volatile& value, short mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 2);
    return _InterlockedXor16(&value, mask);
}

inline long AtomicXor(long volatile& value, long mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 4);
    return _InterlockedXor(&value, mask);
}

inline __int64 AtomicXor(__int64 volatile& value, __int64 mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 8);
#if defined (CRUNCH_ARCH_X86_64)
    return _InterlockedXor64(&value, mask);
#else
    return Detail::FallbackAtomicOp(value, [=] (__int64 x) { return x ^ mask; });
#endif
}

}}}

#endif
<commit_msg>crunch_concurrency - Added CAS fallback for Swap on x86<commit_after>// Copyright (c) 2011, Christian Rorvik
// Distributed under the Simplified BSD License (See accompanying file LICENSE.txt)

#ifndef CRUNCH_CONCURRENCY_PLATFORM_WIN32_ATOMIC_OPS_X86_HPP
#define CRUNCH_CONCURRENCY_PLATFORM_WIN32_ATOMIC_OPS_X86_HPP

#include "crunch/base/assert.hpp"
#include "crunch/concurrency/fence.hpp"
#include "crunch/concurrency/memory_order.hpp"

#include <intrin.h>

namespace Crunch { namespace Concurrency { namespace Platform {

namespace Detail
{
    inline char AtomicLoad(char volatile const& src) { return src; }
    inline short AtomicLoad(short volatile const& src) { return src; }
    inline long AtomicLoad(long volatile const& src) { return src; }

    inline __int64 AtomicLoad(__int64 volatile const& src)
    {
#if defined (CRUNCH_ARCH_X86_64)
        return src;
#else
        __int64 result;
        __asm
        {
            mov eax, dword ptr [src]
            fild qword ptr [eax]
            fistp qword ptr [result]
        }
        return result;
#endif
    }

    inline __m128i AtomicLoad(__m128i volatile const& src)
    {
        return _mm_load_si128(const_cast<__m128i const*>(&src));;
    }

    inline void AtomicStore(char volatile& dst, char src) { dst = src; }
    inline void AtomicStore(short volatile& dst, short src) { dst = src; }
    inline void AtomicStore(long volatile& dst, long src) { dst = src; }

    inline void AtomicStore(__int64 volatile& dst, __int64 src)
    {
#if defined (CRUNCH_ARCH_X86_64)
        dst = src;
#else
        __asm
        {
            mov eax, dword ptr [dst]
            fild qword ptr [src]
            fistp qword ptr [eax]
        }
#endif
    };

    inline void AtomicStore(__m128i volatile& dst, __m128i src)
    {
        _mm_store_si128(const_cast<__m128i*>(&dst), src);
    }

    inline void AtomicStoreSeqCst(char volatile& dst, char src)
    {
        CRUNCH_MEMORY_FENCE();
        AtomicStore(dst, src);
        CRUNCH_MEMORY_FENCE();
    }

    inline void AtomicStoreSeqCst(short volatile& dst, short src)
    {
        CRUNCH_MEMORY_FENCE();
        AtomicStore(dst, src);
        CRUNCH_MEMORY_FENCE();
    }

    inline void AtomicStoreSeqCst(long volatile& dst, long src)
    {
        _InterlockedExchange(&dst, src);
    }

    inline void AtomicStoreSeqCst(__int64 volatile& dst, __int64 src)
    {
#if defined (CRUNCH_ARCH_X86_64)
        _InterlockedExchange64(&dst, src);
#else
        CRUNCH_MEMORY_FENCE();
        AtomicStore(dst, src);
        CRUNCH_MEMORY_FENCE();
#endif
    }

    inline void AtomicStoreSeqCst(__m128i volatile& dst, __m128i src)
    {
        CRUNCH_MEMORY_FENCE();
        AtomicStore(dst, src);
        CRUNCH_MEMORY_FENCE();
    }

#if defined (CRUNCH_ARCH_X86_32)
    template<typename Op>
    inline __int64 FallbackAtomicOp(__int64 volatile& value, Op op)
    {
        __int64 oldValue = value;
        for (;;)
        {
            __int64 const newValue = op(oldValue);
            __int64 currentValue = _InterlockedCompareExchange64(&value, newValue, oldValue);
            if (currentValue == oldValue)
                return oldValue;

            _mm_pause();
            oldValue = currentValue;
        }
    }
#endif
}

template<typename T>
inline T AtomicLoad(T volatile& src, MemoryOrder ordering = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&src, sizeof(T));

    if (ordering == MEMORY_ORDER_SEQ_CST)
        CRUNCH_COMPILER_FENCE();

    T result = Detail::AtomicLoad(src);

    if (ordering & MEMORY_ORDER_ACQUIRE)
        CRUNCH_COMPILER_FENCE();

    return result;
}

template<typename T>
inline void AtomicStore(T volatile& dst, T src, MemoryOrder ordering = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&dst, sizeof(T));

    if (ordering == MEMORY_ORDER_SEQ_CST)
    {
        Detail::AtomicStoreSeqCst(dst, src);
    }
    else
    {
        if (ordering & MEMORY_ORDER_RELEASE)
            CRUNCH_COMPILER_FENCE();

        Detail::AtomicStore(dst, src);
    }
}

inline long AtomicSwap(long volatile& dst, long src, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&dst, 4);
    return _InterlockedExchange(&dst, src);
}

inline __int64 AtomicSwap(__int64 volatile& dst, __int64 src, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&dst, 8);
#if defined (CRUNCH_ARCH_X86_64)
    return _InterlockedExchange64(&dst, src);
#else
    return Detail::FallbackAtomicOp(dst, [=] (__int64) { return src; });
#endif
}

inline bool AtomicCompareAndSwap(short volatile& dst, short src, short& cmp, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&dst, 2);
    short cmp_ = cmp;
    cmp = _InterlockedCompareExchange16(&dst, src, cmp);
    return cmp_ == cmp;
}

inline bool AtomicCompareAndSwap(long volatile& dst, long src, long& cmp, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&dst, 4);
    long cmp_ = cmp;
    cmp = _InterlockedCompareExchange(&dst, src, cmp);
    return cmp_ == cmp;
}

inline bool AtomicCompareAndSwap(__int64 volatile& dst, __int64 src, __int64& cmp, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&dst, 8);
    __int64 cmp_ = cmp;
    cmp = _InterlockedCompareExchange64(&dst, src, cmp);
    return cmp_ == cmp;
}

#if defined (CRUNCH_ARCH_X86_64)
inline bool AtomicCompareAndSwap(__m128i volatile& dst, __m128i src, __m128i& cmp, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&dst, 16);

    return _InterlockedCompareExchange128(
        dst.m128i_i64,
        src.m128i_i64[1],
        src.m128i_i64[0],
        cmp.m128i_i64) != 0;
}
#endif

inline short AtomicIncrement(short volatile& addend, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 2);
    return _InterlockedIncrement16(&addend) - 1;
}

inline long AtomicIncrement(long volatile& addend, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 4);
    return _InterlockedIncrement(&addend) - 1;
}

inline __int64 AtomicIncrement(__int64 volatile& addend, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 8);
#if defined (CRUNCH_ARCH_X86_64)
    return _InterlockedIncrement64(&addend) - 1;
#else
    return Detail::FallbackAtomicOp(addend, [] (__int64 x) { return x + 1; });
#endif
}

inline short AtomicDecrement(short volatile& addend, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 2);
    return _InterlockedDecrement16(&addend) + 1;
}

inline long AtomicDecrement(long volatile& addend, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 4);
    return _InterlockedDecrement(&addend) + 1;
}

inline __int64 AtomicDecrement(__int64 volatile& addend, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 8);
#if defined (CRUNCH_ARCH_X86_64)
    return _InterlockedDecrement64(&addend) + 1;
#else
    return Detail::FallbackAtomicOp(addend, [] (__int64 x) { return x - 1; });
#endif
}

inline long AtomicAdd(long volatile& addend, long value, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 4);
    return _InterlockedExchangeAdd(&addend, value);
}

inline __int64 AtomicAdd(__int64 volatile& addend, __int64 value, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 8);
#if defined (CRUNCH_ARCH_X86_64)
    return _InterlockedExchangeAdd64(&addend, value);
#else
    return Detail::FallbackAtomicOp(addend, [=] (__int64 x) { return x + value; });
#endif
}

inline long AtomicSub(long volatile& addend, long value, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 4);
    return _InterlockedExchangeAdd(&addend, -value);
}

inline __int64 AtomicSub(__int64 volatile& addend, __int64 value, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&addend, 8);
#if defined (CRUNCH_ARCH_X86_64)
    return _InterlockedExchangeAdd64(&addend, -value);
#else
    return Detail::FallbackAtomicOp(addend, [=] (__int64 x) { return x - value; });
#endif
}

inline char AtomicAnd(char volatile& value, char mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    return _InterlockedAnd8(&value, mask);
}

inline short AtomicAnd(short volatile& value, short mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 2);
    return _InterlockedAnd16(&value, mask);
}

inline long AtomicAnd(long volatile& value, long mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 4);
    return _InterlockedAnd(&value, mask);
}

inline __int64 AtomicAnd(__int64 volatile& value, __int64 mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 8);
#if defined (CRUNCH_ARCH_X86_64)
    return _InterlockedAnd64(&value, mask);
#else
    return Detail::FallbackAtomicOp(value, [=] (__int64 x) { return x & mask; });
#endif
}

inline char AtomicOr(char volatile& value, char mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    return _InterlockedOr8(&value, mask);
}

inline short AtomicOr(short volatile& value, short mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 2);
    return _InterlockedOr16(&value, mask);
}

inline long AtomicOr(long volatile& value, long mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 4);
    return _InterlockedOr(&value, mask);
}

inline __int64 AtomicOr(__int64 volatile& value, __int64 mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 8);
#if defined (CRUNCH_ARCH_X86_64)
    return _InterlockedOr64(&value, mask);
#else
    return Detail::FallbackAtomicOp(value, [=] (__int64 x) { return x | mask; });
#endif
}

inline char AtomicXor(char volatile& value, char mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    return _InterlockedXor8(&value, mask);
}

inline short AtomicXor(short volatile& value, short mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 2);
    return _InterlockedXor16(&value, mask);
}

inline long AtomicXor(long volatile& value, long mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 4);
    return _InterlockedXor(&value, mask);
}

inline __int64 AtomicXor(__int64 volatile& value, __int64 mask, MemoryOrder = MEMORY_ORDER_SEQ_CST)
{
    CRUNCH_ASSERT_ALIGNMENT(&value, 8);
#if defined (CRUNCH_ARCH_X86_64)
    return _InterlockedXor64(&value, mask);
#else
    return Detail::FallbackAtomicOp(value, [=] (__int64 x) { return x ^ mask; });
#endif
}

}}}

#endif
<|endoftext|>


<commit_before>/*
 * ClutterMozHeadless; A headless Mozilla renderer
 * Copyright (c) 2009, Intel Corporation.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU Lesser General Public License,
 * version 2.1, as published by the Free Software Foundation.
 *
 * This program is distributed in the hope it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
 * License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
 *
 * Authored by Chris Lord <chris@linux.intel.com>
 */

#include <nsIGlobalHistory2.h>
#include <nsWeakReference.h>
#include <nsIGenericFactory.h>
#include <nsIIOService.h>
#include <nsIObserverService.h>
#include <nsIURI.h>
#include <nsNetUtil.h>
#include <nsStringGlue.h>

#include "clutter-mozheadless.h"
#include "clutter-mozheadless-history.h"
#include <moz-headless.h>
#include <mhs/mhs.h>

class HeadlessGlobalHistory : public nsIGlobalHistory2,
                              public nsSupportsWeakReference
{
 public:
  HeadlessGlobalHistory();
  virtual ~HeadlessGlobalHistory();

  static HeadlessGlobalHistory *GetSingleton(void);
  static HeadlessGlobalHistory *sHeadlessGlobalHistory;

  NS_DECL_ISUPPORTS
  NS_DECL_NSIGLOBALHISTORY2

  void SendLinkVisitedEvent (nsIURI *aURI);

 private:
  MhsHistory                   *mMhsHistory;
};

static void
_link_visited_cb (MhsHistory *history,
                  const gchar *uri,
                  HeadlessGlobalHistory *global_history)
{
  nsresult rv;
  nsCOMPtr<nsIIOService> io_service =
    do_GetService (NS_IOSERVICE_CONTRACTID, &rv);

  if (!NS_FAILED (rv))
    {
      nsDependentCString uri_cstring (uri);

      nsCOMPtr<nsIURI> ns_uri;
      rv = io_service->NewURI (uri_cstring, nsnull, nsnull, getter_AddRefs (ns_uri));

      if (!NS_FAILED (rv))
        global_history->SendLinkVisitedEvent (ns_uri);
    }
}

HeadlessGlobalHistory::HeadlessGlobalHistory(void)
{
  mMhsHistory = mhs_history_new ();

  g_signal_connect (mMhsHistory, "link-visited",
                    G_CALLBACK (_link_visited_cb), this);
}

HeadlessGlobalHistory::~HeadlessGlobalHistory()
{
  if (mMhsHistory) {
    g_signal_handlers_disconnect_by_func (mMhsHistory,
                                          (gpointer)_link_visited_cb,
                                          this);
    g_object_unref (mMhsHistory);
    mMhsHistory = NULL;
  }

  if (sHeadlessGlobalHistory == this)
    sHeadlessGlobalHistory = nsnull;
}

HeadlessGlobalHistory *HeadlessGlobalHistory::sHeadlessGlobalHistory = nsnull;

HeadlessGlobalHistory *
HeadlessGlobalHistory::GetSingleton(void)
{
  if (!sHeadlessGlobalHistory) {
    sHeadlessGlobalHistory = new HeadlessGlobalHistory ();
  }

  return sHeadlessGlobalHistory;
}

NS_IMETHODIMP_(nsrefcnt)
HeadlessGlobalHistory::AddRef ()
{
  return 1;
}

NS_IMETHODIMP_(nsrefcnt)
HeadlessGlobalHistory::Release ()
{
  return 1;
}

NS_IMPL_QUERY_INTERFACE2(HeadlessGlobalHistory,
                         nsIGlobalHistory2,
                         nsISupportsWeakReference)

NS_IMETHODIMP
HeadlessGlobalHistory::AddURI(nsIURI *aURI,
                              PRBool  aRedirect,
                              PRBool  aToplevel,
                              nsIURI *aReferrer)
{
  nsCAutoString uriString, refString;
  nsresult rv = aURI->GetSpec(uriString);
  if (NS_FAILED(rv))
    return rv;

  const char *referrer = NULL;
  if (aReferrer) {
    rv = aReferrer->GetSpec(refString);
    if (NS_FAILED(rv))
      return rv;
    referrer = refString.get();
  }

  GError *error = NULL;
  gboolean result =
    mhs_history_add_uri (mMhsHistory,
                         (const gchar *)uriString.get(),
                         (gboolean)aRedirect,
                         (gboolean)aToplevel,
                         (const gchar *)referrer,
                         &error);
  if (!result)
    {
      g_warning ("Error adding URI: %s", error->message);
      g_error_free (error);
      return NS_ERROR_UNEXPECTED;
    }

  return NS_OK;
}

NS_IMETHODIMP
HeadlessGlobalHistory::IsVisited(nsIURI *aURI, PRBool *_retval)
{
  nsCAutoString uriString;
  nsresult rv = aURI->GetSpec(uriString);
  if (NS_FAILED(rv))
    return rv;

  GError *error = NULL;
  gboolean is_visited = FALSE;
  gboolean result =
    mhs_history_is_visited (mMhsHistory,
                            (const gchar *)uriString.get(),
                            &is_visited,
                            &error);
  if (!result)
    {
      g_warning ("Error checking is-visited: %s", error->message);
      g_error_free (error);
      return NS_ERROR_UNEXPECTED;
    }
  else
    *_retval = is_visited;

  return NS_OK;
}

NS_IMETHODIMP
HeadlessGlobalHistory::SetPageTitle(nsIURI *aURI, const nsAString &aTitle)
{
  nsCAutoString uriString;
  nsresult rv = aURI->GetSpec(uriString);
  if (NS_FAILED(rv))
    return rv;

  char *title_utf8 = ToNewUTF8String(aTitle);

  GError *error = NULL;
  gboolean result =
    mhs_history_set_page_title (mMhsHistory,
                                (const gchar *)uriString.get(),
                                (const gchar *)title_utf8,
                                &error);
  if (!result)
    {
      g_warning ("Error setting page title: %s", error->message);
      g_error_free (error);
      rv = NS_ERROR_UNEXPECTED;
    }
  else
    rv = NS_OK;

  NS_Free (title_utf8);

  return rv;
}

void
HeadlessGlobalHistory::SendLinkVisitedEvent (nsIURI *aURI)
{
  nsCOMPtr<nsIObserverService> obsService =
    do_GetService("@mozilla.org/observer-service;1");
  if (obsService)
    obsService->NotifyObservers(aURI, NS_LINK_VISITED_EVENT_TOPIC, nsnull);
}

#define HEADLESS_GLOBALHISTORY_CID \
  {0x45f4a193, 0xe8ec, 0x4687, {0xa9, 0x29, 0xf8, 0x5c, 0x92, 0x49, 0x40, 0x31}}
NS_GENERIC_FACTORY_SINGLETON_CONSTRUCTOR(HeadlessGlobalHistory, HeadlessGlobalHistory::GetSingleton)

static const nsModuleComponentInfo historyServiceComp = {
  "Global history",
  HEADLESS_GLOBALHISTORY_CID,
  "@mozilla.org/browser/global-history;2",
  HeadlessGlobalHistoryConstructor
};

void
clutter_mozheadless_history_init ()
{
  static gboolean comp_is_registered = FALSE;

  if (!comp_is_registered)
    {
      moz_headless_register_component ((gpointer)&historyServiceComp);
      comp_is_registered = TRUE;
    }
}

void
clutter_mozheadless_history_deinit ()
{
  if (HeadlessGlobalHistory::sHeadlessGlobalHistory)
    delete HeadlessGlobalHistory::sHeadlessGlobalHistory;
}

<commit_msg>[history] Use the nsresult from the GError<commit_after>/*
 * ClutterMozHeadless; A headless Mozilla renderer
 * Copyright (c) 2009, Intel Corporation.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU Lesser General Public License,
 * version 2.1, as published by the Free Software Foundation.
 *
 * This program is distributed in the hope it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
 * License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
 *
 * Authored by Chris Lord <chris@linux.intel.com>
 */

#include <nsIGlobalHistory2.h>
#include <nsWeakReference.h>
#include <nsIGenericFactory.h>
#include <nsIIOService.h>
#include <nsIObserverService.h>
#include <nsIURI.h>
#include <nsNetUtil.h>
#include <nsStringGlue.h>

#include "clutter-mozheadless.h"
#include "clutter-mozheadless-history.h"
#include <moz-headless.h>
#include <mhs/mhs.h>

class HeadlessGlobalHistory : public nsIGlobalHistory2,
                              public nsSupportsWeakReference
{
 public:
  HeadlessGlobalHistory();
  virtual ~HeadlessGlobalHistory();

  static HeadlessGlobalHistory *GetSingleton(void);
  static HeadlessGlobalHistory *sHeadlessGlobalHistory;

  NS_DECL_ISUPPORTS
  NS_DECL_NSIGLOBALHISTORY2

  void SendLinkVisitedEvent (nsIURI *aURI);

 private:
  MhsHistory                   *mMhsHistory;
};

static void
_link_visited_cb (MhsHistory *history,
                  const gchar *uri,
                  HeadlessGlobalHistory *global_history)
{
  nsresult rv;
  nsCOMPtr<nsIIOService> io_service =
    do_GetService (NS_IOSERVICE_CONTRACTID, &rv);

  if (!NS_FAILED (rv))
    {
      nsDependentCString uri_cstring (uri);

      nsCOMPtr<nsIURI> ns_uri;
      rv = io_service->NewURI (uri_cstring, nsnull, nsnull, getter_AddRefs (ns_uri));

      if (!NS_FAILED (rv))
        global_history->SendLinkVisitedEvent (ns_uri);
    }
}

HeadlessGlobalHistory::HeadlessGlobalHistory(void)
{
  mMhsHistory = mhs_history_new ();

  g_signal_connect (mMhsHistory, "link-visited",
                    G_CALLBACK (_link_visited_cb), this);
}

HeadlessGlobalHistory::~HeadlessGlobalHistory()
{
  if (mMhsHistory) {
    g_signal_handlers_disconnect_by_func (mMhsHistory,
                                          (gpointer)_link_visited_cb,
                                          this);
    g_object_unref (mMhsHistory);
    mMhsHistory = NULL;
  }

  if (sHeadlessGlobalHistory == this)
    sHeadlessGlobalHistory = nsnull;
}

HeadlessGlobalHistory *HeadlessGlobalHistory::sHeadlessGlobalHistory = nsnull;

HeadlessGlobalHistory *
HeadlessGlobalHistory::GetSingleton(void)
{
  if (!sHeadlessGlobalHistory) {
    sHeadlessGlobalHistory = new HeadlessGlobalHistory ();
  }

  return sHeadlessGlobalHistory;
}

NS_IMETHODIMP_(nsrefcnt)
HeadlessGlobalHistory::AddRef ()
{
  return 1;
}

NS_IMETHODIMP_(nsrefcnt)
HeadlessGlobalHistory::Release ()
{
  return 1;
}

NS_IMPL_QUERY_INTERFACE2(HeadlessGlobalHistory,
                         nsIGlobalHistory2,
                         nsISupportsWeakReference)

NS_IMETHODIMP
HeadlessGlobalHistory::AddURI(nsIURI *aURI,
                              PRBool  aRedirect,
                              PRBool  aToplevel,
                              nsIURI *aReferrer)
{
  nsCAutoString uriString, refString;
  nsresult rv = aURI->GetSpec(uriString);
  if (NS_FAILED(rv))
    return rv;

  const char *referrer = NULL;
  if (aReferrer) {
    rv = aReferrer->GetSpec(refString);
    if (NS_FAILED(rv))
      return rv;
    referrer = refString.get();
  }

  GError *error = NULL;
  gboolean result =
    mhs_history_add_uri (mMhsHistory,
                         (const gchar *)uriString.get(),
                         (gboolean)aRedirect,
                         (gboolean)aToplevel,
                         (const gchar *)referrer,
                         &error);
  if (!result)
    {
      nsresult rv = mhs_error_to_nsresult (error);
      g_warning ("Error adding URI: %s", error->message);
      g_error_free (error);
      return rv;
    }

  return NS_OK;
}

NS_IMETHODIMP
HeadlessGlobalHistory::IsVisited(nsIURI *aURI, PRBool *_retval)
{
  nsCAutoString uriString;
  nsresult rv = aURI->GetSpec(uriString);
  if (NS_FAILED(rv))
    return rv;

  GError *error = NULL;
  gboolean is_visited = FALSE;
  gboolean result =
    mhs_history_is_visited (mMhsHistory,
                            (const gchar *)uriString.get(),
                            &is_visited,
                            &error);
  if (!result)
    {
      nsresult rv = mhs_error_to_nsresult (error);
      g_warning ("Error checking is-visited: %s", error->message);
      g_error_free (error);
      return rv;
    }
  else
    *_retval = is_visited;

  return NS_OK;
}

NS_IMETHODIMP
HeadlessGlobalHistory::SetPageTitle(nsIURI *aURI, const nsAString &aTitle)
{
  nsCAutoString uriString;
  nsresult rv = aURI->GetSpec(uriString);
  if (NS_FAILED(rv))
    return rv;

  char *title_utf8 = ToNewUTF8String(aTitle);

  GError *error = NULL;
  gboolean result =
    mhs_history_set_page_title (mMhsHistory,
                                (const gchar *)uriString.get(),
                                (const gchar *)title_utf8,
                                &error);
  if (!result)
    {
      rv = mhs_error_to_nsresult (error);
      g_warning ("Error setting page title: %s", error->message);
      g_error_free (error);
    }
  else
    rv = NS_OK;

  NS_Free (title_utf8);

  return rv;
}

void
HeadlessGlobalHistory::SendLinkVisitedEvent (nsIURI *aURI)
{
  nsCOMPtr<nsIObserverService> obsService =
    do_GetService("@mozilla.org/observer-service;1");
  if (obsService)
    obsService->NotifyObservers(aURI, NS_LINK_VISITED_EVENT_TOPIC, nsnull);
}

#define HEADLESS_GLOBALHISTORY_CID \
  {0x45f4a193, 0xe8ec, 0x4687, {0xa9, 0x29, 0xf8, 0x5c, 0x92, 0x49, 0x40, 0x31}}
NS_GENERIC_FACTORY_SINGLETON_CONSTRUCTOR(HeadlessGlobalHistory, HeadlessGlobalHistory::GetSingleton)

static const nsModuleComponentInfo historyServiceComp = {
  "Global history",
  HEADLESS_GLOBALHISTORY_CID,
  "@mozilla.org/browser/global-history;2",
  HeadlessGlobalHistoryConstructor
};

void
clutter_mozheadless_history_init ()
{
  static gboolean comp_is_registered = FALSE;

  if (!comp_is_registered)
    {
      moz_headless_register_component ((gpointer)&historyServiceComp);
      comp_is_registered = TRUE;
    }
}

void
clutter_mozheadless_history_deinit ()
{
  if (HeadlessGlobalHistory::sHeadlessGlobalHistory)
    delete HeadlessGlobalHistory::sHeadlessGlobalHistory;
}

<|endoftext|>


<commit_before>#pragma once

#include <cstddef>
#include <memory>
#include <functional>
#include <ratio>
#include <iterator>
#include <initializer_list>

#include <circular_iterator.hpp>

namespace helene
{
template <class T,
          std::size_t Size,
          template <class, class, std::size_t> class StoragePolicy>
class sliding_window
    : public StoragePolicy<sliding_window<T, Size, StoragePolicy>, T, Size>
{
    typedef StoragePolicy<sliding_window<T, Size, StoragePolicy>, T, Size>
        storage_policy;

public:
    static const std::size_t size = Size;

    typedef T value_type;
    typedef T& reference;
    typedef const T& const_reference;
    typedef std::size_t size_type;
    typedef std::ptrdiff_t difference_type;
    typedef circular_iterator<T*> iterator;
    typedef circular_iterator<const T*> const_iterator;

public:
    sliding_window()
        : storage_policy(),
          head_(storage_policy::begin(),
                storage_policy::end(),
                storage_policy::begin())
    {
    }

    template <class Iterator>
    sliding_window(Iterator first, Iterator last) : sliding_window()
    {
        for(; first != last; ++first, ++head_)
        {
            *head_ = *first;
        }
    }

    sliding_window(std::initializer_list<T> il)
        : sliding_window(il.begin(), il.end())
    {
    }

    sliding_window(const sliding_window& other)
        : storage_policy(other),
          head_(storage_policy::begin(),
                storage_policy::end(),
                storage_policy::begin() + other.head_.underlying_position())
    {
    }

    sliding_window&
    operator=(const sliding_window& other)
    {
        std::copy(other.cbegin(), other.cend(), begin());
        return *this;
    }

public:
    void
    push_back(const T& value)
    {
        *head_ = value;
        ++head_;
    }

    void
    push_back(const T& value, size_type n)
    {
        std::fill_n(head_, n, value);
        head_ += n;
    }

    void
    push_front(const T& value)
    {
        --head_;
        *head_ = value;
    }

    void
    push_front(const T& value, size_type n)
    {
        std::fill_n(std::make_reverse_iterator(head_), n, value);
        head_ -= n;
    }

    reference
    front()
    {
        return *head_;
    }

    const_reference
    front() const
    {
        return *head_;
    }

    reference
    back()
    {
        return *(head_ + Size - 1);
    }

    const_reference
    back() const
    {
        return *(head_ + Size - 1);
    }

    reference operator[](size_type n)
    {
        return head_[n];
    }

    const_reference operator[](size_type n) const
    {
        return head_[n];
    }

    iterator
    begin()
    {
        return head_;
    }

    iterator
    end()
    {
        return head_ + Size;
    }

    const_iterator
    cbegin() const
    {
        return head_;
    }

    const_iterator
    cend() const
    {
        return head_ + Size;
    }

    bool
    operator==(const sliding_window& other) const
    {
        return std::equal(cbegin(), cend(), other.cbegin());
    }

    bool
    operator!=(const sliding_window& other) const
    {
        return !operator==(other);
    }

private:
    iterator head_;
};

template <class Derived, class T, std::size_t Size>
class stack_storage
{
public:
    stack_storage() : buffer_{}
    {
    }

    stack_storage(const stack_storage& other)
    {
        std::copy(std::begin(other.buffer_),
                  std::end(other.buffer_),
                  std::begin(buffer_));
    }

    stack_storage&
    operator=(const stack_storage& other)
    {
        std::copy(std::begin(other.buffer_),
                  std::end(other.buffer_),
                  std::begin(buffer_));

        return *this;
    }

    void
    swap(stack_storage& other)
    {
        std::swap(buffer_, other.buffer_);
    }

public:
    T*
    begin()
    {
        return buffer_;
    }

    T*
    end()
    {
        return buffer_ + Size;
    }

protected:
    T buffer_[Size];
};

template <class Derived, class T, std::size_t Size>
class static_heap_storage
{
public:
    static_heap_storage() : buffer_(new T[Size]{})
    {
    }

    static_heap_storage(const static_heap_storage& other)
        : static_heap_storage()
    {
        std::copy(
            other.buffer_.get(), other.buffer_.get() + Size, buffer_.get());
    }

    static_heap_storage(static_heap_storage&&) = default;

    static_heap_storage&
    operator=(const static_heap_storage& other)
    {
        auto temp = other;
        swap(temp);
        return *this;
    }

    static_heap_storage& operator=(static_heap_storage&&) = default;

    void
    swap(static_heap_storage& other)
    {
        std::swap(buffer_, other.buffer_);
    }

public:
    T*
    begin()
    {
        return buffer_.get();
    }

    T*
    end()
    {
        return buffer_.get() + Size;
    }

protected:
    std::unique_ptr<T[]> buffer_;
};

template <class T, std::size_t Size>
using stack_sliding_window = sliding_window<T, Size, stack_storage>;

template <class T, std::size_t Size>
using static_heap_sliding_window = sliding_window<T, Size, static_heap_storage>;

namespace detail
{
struct runtime_ratio
{
    runtime_ratio() : num(1), den(1)
    {
    }

    template <std::intmax_t Num, std::intmax_t Denom>
    constexpr runtime_ratio(std::ratio<Num, Denom>) : num(Num), den(Denom)
    {
    }

    std::intmax_t num;
    std::intmax_t den;
};

template <class Numeric>
Numeric operator*(const runtime_ratio& lhs, Numeric rhs)
{
    return rhs * static_cast<Numeric>(lhs.num) / static_cast<Numeric>(lhs.den);
}

template <class Numeric>
Numeric operator*(Numeric lhs, const runtime_ratio& rhs)
{
    return rhs * lhs;
}

template <class Numeric>
Numeric
operator/(Numeric lhs, const runtime_ratio& rhs)
{
    return lhs * static_cast<Numeric>(rhs.den) / static_cast<Numeric>(rhs.num);
}
} // namespace detail

template <class KeyType,
          class ValueType,
          std::size_t Size,
          class Compare = std::less<KeyType>,
          class SlidingWindowType = static_heap_sliding_window<ValueType, Size>>
class sliding_window_map
{
public:
    typedef ValueType value_type;
    typedef ValueType& reference;
    typedef const ValueType& const_reference;
    typedef typename SlidingWindowType::iterator iterator;
    typedef typename SlidingWindowType::const_iterator const_iterator;
    typedef typename SlidingWindowType::difference_type difference_type;
    typedef typename SlidingWindowType::size_type size_type;

public:
    sliding_window_map() : sliding_buffer_(), precision_(), origin_()
    {
    }

    template <std::intmax_t Num, std::intmax_t Denom>
    sliding_window_map(std::ratio<Num, Denom>)
        : sliding_buffer_(), precision_(std::ratio<Num, Denom>()), origin_()
    {
    }

    sliding_window_map(KeyType origin)
        : sliding_buffer_(), precision_(), origin_(origin / precision_)
    {
    }

    template <std::intmax_t Num, std::intmax_t Denom>
    sliding_window_map(KeyType origin, std::ratio<Num, Denom>)
        : sliding_buffer_(),
          precision_(std::ratio<Num, Denom>()),
          origin_(origin / precision_)
    {
    }

    std::pair<KeyType, KeyType>
    window() const
    {
        return std::make_pair(static_cast<KeyType>(origin_) * precision_,
                              static_cast<KeyType>(origin_ + Size) *
                                  precision_);
    }

    ValueType&
    at(KeyType k)
    {
        const auto index = index_of_key(k);
        const Compare c;
        if(c(index, 0) || (!c(index, Size)))
        {
            throw std::out_of_range("Key outside of current window");
        }

        return sliding_buffer_[index];
    }

    ValueType& operator[](KeyType k)
    {
        return sliding_buffer_[k / precision_ - origin_];
    }

    void
    insert_or_assign(KeyType k, const ValueType& v)
    {
        const auto index = index_of_key(k);
        const Compare c;

        if(c(index, 0))
        {
            // below current window
            sliding_buffer_.push_front(ValueType(), -index);
            sliding_buffer_.front() = v;
            origin_ += index * precision_;
        }
        else if(!c(index, Size))
        {
            // above current window
            sliding_buffer_.push_back(ValueType(), index - Size + 1);
            sliding_buffer_.back() = v;
            origin_ += (index - Size + 1) * precision_;
        }
        else
        {
            // within current window
            sliding_buffer_[index] = v;
        }
    }

    iterator
    begin()
    {
        return sliding_buffer_.begin();
    }

    iterator
    end()
    {
        return sliding_buffer_.end();
    }

    const_iterator
    cbegin()
    {
        return sliding_buffer_.cbegin();
    }

    const_iterator
    cend()
    {
        return sliding_buffer_.cend();
    }

private:
    difference_type
    index_of_key(KeyType k)
    {
        return (k - origin_ * precision_) / precision_;
    }

private:
    SlidingWindowType sliding_buffer_;
    detail::runtime_ratio precision_;
    difference_type origin_;
};
} // namespace helene

namespace std
{
template <class Derived, class T, std::size_t Size>
void
swap(helene::stack_storage<Derived, T, Size>& lhs,
     helene::stack_storage<Derived, T, Size>& rhs)
{
    lhs.swap(rhs);
}

template <class Derived, class T, std::size_t Size>
void
swap(helene::static_heap_storage<Derived, T, Size>& lhs,
     helene::static_heap_storage<Derived, T, Size>& rhs)
{
    lhs.swap(rhs);
}
} // namespace std
<commit_msg>Const qualify cbegin and cend. 	modified:   include/sliding_window.hpp<commit_after>#pragma once

#include <cstddef>
#include <memory>
#include <functional>
#include <ratio>
#include <iterator>
#include <initializer_list>

#include <circular_iterator.hpp>

namespace helene
{
template <class T,
          std::size_t Size,
          template <class, class, std::size_t> class StoragePolicy>
class sliding_window
    : public StoragePolicy<sliding_window<T, Size, StoragePolicy>, T, Size>
{
    typedef StoragePolicy<sliding_window<T, Size, StoragePolicy>, T, Size>
        storage_policy;

public:
    static const std::size_t size = Size;

    typedef T value_type;
    typedef T& reference;
    typedef const T& const_reference;
    typedef std::size_t size_type;
    typedef std::ptrdiff_t difference_type;
    typedef circular_iterator<T*> iterator;
    typedef circular_iterator<const T*> const_iterator;

public:
    sliding_window()
        : storage_policy(),
          head_(storage_policy::begin(),
                storage_policy::end(),
                storage_policy::begin())
    {
    }

    template <class Iterator>
    sliding_window(Iterator first, Iterator last) : sliding_window()
    {
        for(; first != last; ++first, ++head_)
        {
            *head_ = *first;
        }
    }

    sliding_window(std::initializer_list<T> il)
        : sliding_window(il.begin(), il.end())
    {
    }

    sliding_window(const sliding_window& other)
        : storage_policy(other),
          head_(storage_policy::begin(),
                storage_policy::end(),
                storage_policy::begin() + other.head_.underlying_position())
    {
    }

    sliding_window&
    operator=(const sliding_window& other)
    {
        std::copy(other.cbegin(), other.cend(), begin());
        return *this;
    }

public:
    void
    push_back(const T& value)
    {
        *head_ = value;
        ++head_;
    }

    void
    push_back(const T& value, size_type n)
    {
        std::fill_n(head_, n, value);
        head_ += n;
    }

    void
    push_front(const T& value)
    {
        --head_;
        *head_ = value;
    }

    void
    push_front(const T& value, size_type n)
    {
        std::fill_n(std::make_reverse_iterator(head_), n, value);
        head_ -= n;
    }

    reference
    front()
    {
        return *head_;
    }

    const_reference
    front() const
    {
        return *head_;
    }

    reference
    back()
    {
        return *(head_ + Size - 1);
    }

    const_reference
    back() const
    {
        return *(head_ + Size - 1);
    }

    reference operator[](size_type n)
    {
        return head_[n];
    }

    const_reference operator[](size_type n) const
    {
        return head_[n];
    }

    iterator
    begin()
    {
        return head_;
    }

    iterator
    end()
    {
        return head_ + Size;
    }

    const_iterator
    cbegin() const
    {
        return head_;
    }

    const_iterator
    cend() const
    {
        return head_ + Size;
    }

    bool
    operator==(const sliding_window& other) const
    {
        return std::equal(cbegin(), cend(), other.cbegin());
    }

    bool
    operator!=(const sliding_window& other) const
    {
        return !operator==(other);
    }

private:
    iterator head_;
};

template <class Derived, class T, std::size_t Size>
class stack_storage
{
public:
    stack_storage() : buffer_{}
    {
    }

    stack_storage(const stack_storage& other)
    {
        std::copy(std::begin(other.buffer_),
                  std::end(other.buffer_),
                  std::begin(buffer_));
    }

    stack_storage&
    operator=(const stack_storage& other)
    {
        std::copy(std::begin(other.buffer_),
                  std::end(other.buffer_),
                  std::begin(buffer_));

        return *this;
    }

    void
    swap(stack_storage& other)
    {
        std::swap(buffer_, other.buffer_);
    }

public:
    T*
    begin()
    {
        return buffer_;
    }

    T*
    end()
    {
        return buffer_ + Size;
    }

protected:
    T buffer_[Size];
};

template <class Derived, class T, std::size_t Size>
class static_heap_storage
{
public:
    static_heap_storage() : buffer_(new T[Size]{})
    {
    }

    static_heap_storage(const static_heap_storage& other)
        : static_heap_storage()
    {
        std::copy(
            other.buffer_.get(), other.buffer_.get() + Size, buffer_.get());
    }

    static_heap_storage(static_heap_storage&&) = default;

    static_heap_storage&
    operator=(const static_heap_storage& other)
    {
        auto temp = other;
        swap(temp);
        return *this;
    }

    static_heap_storage& operator=(static_heap_storage&&) = default;

    void
    swap(static_heap_storage& other)
    {
        std::swap(buffer_, other.buffer_);
    }

public:
    T*
    begin()
    {
        return buffer_.get();
    }

    T*
    end()
    {
        return buffer_.get() + Size;
    }

protected:
    std::unique_ptr<T[]> buffer_;
};

template <class T, std::size_t Size>
using stack_sliding_window = sliding_window<T, Size, stack_storage>;

template <class T, std::size_t Size>
using static_heap_sliding_window = sliding_window<T, Size, static_heap_storage>;

namespace detail
{
struct runtime_ratio
{
    runtime_ratio() : num(1), den(1)
    {
    }

    template <std::intmax_t Num, std::intmax_t Denom>
    constexpr runtime_ratio(std::ratio<Num, Denom>) : num(Num), den(Denom)
    {
    }

    std::intmax_t num;
    std::intmax_t den;
};

template <class Numeric>
Numeric operator*(const runtime_ratio& lhs, Numeric rhs)
{
    return rhs * static_cast<Numeric>(lhs.num) / static_cast<Numeric>(lhs.den);
}

template <class Numeric>
Numeric operator*(Numeric lhs, const runtime_ratio& rhs)
{
    return rhs * lhs;
}

template <class Numeric>
Numeric
operator/(Numeric lhs, const runtime_ratio& rhs)
{
    return lhs * static_cast<Numeric>(rhs.den) / static_cast<Numeric>(rhs.num);
}
} // namespace detail

template <class KeyType,
          class ValueType,
          std::size_t Size,
          class Compare = std::less<KeyType>,
          class SlidingWindowType = static_heap_sliding_window<ValueType, Size>>
class sliding_window_map
{
public:
    typedef ValueType value_type;
    typedef ValueType& reference;
    typedef const ValueType& const_reference;
    typedef typename SlidingWindowType::iterator iterator;
    typedef typename SlidingWindowType::const_iterator const_iterator;
    typedef typename SlidingWindowType::difference_type difference_type;
    typedef typename SlidingWindowType::size_type size_type;

public:
    sliding_window_map() : sliding_buffer_(), precision_(), origin_()
    {
    }

    template <std::intmax_t Num, std::intmax_t Denom>
    sliding_window_map(std::ratio<Num, Denom>)
        : sliding_buffer_(), precision_(std::ratio<Num, Denom>()), origin_()
    {
    }

    sliding_window_map(KeyType origin)
        : sliding_buffer_(), precision_(), origin_(origin / precision_)
    {
    }

    template <std::intmax_t Num, std::intmax_t Denom>
    sliding_window_map(KeyType origin, std::ratio<Num, Denom>)
        : sliding_buffer_(),
          precision_(std::ratio<Num, Denom>()),
          origin_(origin / precision_)
    {
    }

    std::pair<KeyType, KeyType>
    window() const
    {
        return std::make_pair(static_cast<KeyType>(origin_) * precision_,
                              static_cast<KeyType>(origin_ + Size) *
                                  precision_);
    }

    ValueType&
    at(KeyType k)
    {
        const auto index = index_of_key(k);
        const Compare c;
        if(c(index, 0) || (!c(index, Size)))
        {
            throw std::out_of_range("Key outside of current window");
        }

        return sliding_buffer_[index];
    }

    ValueType& operator[](KeyType k)
    {
        return sliding_buffer_[k / precision_ - origin_];
    }

    void
    insert_or_assign(KeyType k, const ValueType& v)
    {
        const auto index = index_of_key(k);
        const Compare c;

        if(c(index, 0))
        {
            // below current window
            sliding_buffer_.push_front(ValueType(), -index);
            sliding_buffer_.front() = v;
            origin_ += index * precision_;
        }
        else if(!c(index, Size))
        {
            // above current window
            sliding_buffer_.push_back(ValueType(), index - Size + 1);
            sliding_buffer_.back() = v;
            origin_ += (index - Size + 1) * precision_;
        }
        else
        {
            // within current window
            sliding_buffer_[index] = v;
        }
    }

    iterator
    begin()
    {
        return sliding_buffer_.begin();
    }

    iterator
    end()
    {
        return sliding_buffer_.end();
    }

    const_iterator
    cbegin() const
    {
        return sliding_buffer_.cbegin();
    }

    const_iterator
    cend() const
    {
        return sliding_buffer_.cend();
    }

private:
    difference_type
    index_of_key(KeyType k)
    {
        return (k - origin_ * precision_) / precision_;
    }

private:
    SlidingWindowType sliding_buffer_;
    detail::runtime_ratio precision_;
    difference_type origin_;
};
} // namespace helene

namespace std
{
template <class Derived, class T, std::size_t Size>
void
swap(helene::stack_storage<Derived, T, Size>& lhs,
     helene::stack_storage<Derived, T, Size>& rhs)
{
    lhs.swap(rhs);
}

template <class Derived, class T, std::size_t Size>
void
swap(helene::static_heap_storage<Derived, T, Size>& lhs,
     helene::static_heap_storage<Derived, T, Size>& rhs)
{
    lhs.swap(rhs);
}
} // namespace std
<|endoftext|>


<commit_before>#define BOOST_TEST_MODULE forward_models sphere
#include <boost/test/included/unit_test.hpp>
#include "algebra.h"
#include "map.h"
#include "optimization.h"
#include "forward_models/sphere.h"
#include "utility.h"
#include "test_types.h"
#include <iostream>

constexpr double EPS = 1e-9;
constexpr int ntests = 100;

using namespace invlib;

// Use the sphere function forward model to test the equivalence of the
// standard, n-form and m-form when using the Gauss-Newton optimizer.
template
<
typename T
>
void sphere_test(unsigned int n)
{
    using RealType   = typename T::RealType;
    using VectorType = typename T::VectorType;
    using MatrixType = typename T::MatrixType;
    using Model      = Sphere<MatrixType>;

    MatrixType Se = random_positive_definite<MatrixType>(1);
    MatrixType Sa = random_positive_definite<MatrixType>(n);
    VectorType xa = random<VectorType>(n);
    VectorType y  = random<VectorType>(1);

    Model F(n);
    MAP<Model, RealType, VectorType, MatrixType,
        MatrixType, MatrixType, Formulation::STANDARD> std(F, xa, Sa, Se);
    MAP<Model, RealType, VectorType, MatrixType,
        MatrixType, MatrixType, Formulation::NFORM>    nform(F, xa, Sa, Se);
    MAP<Model, RealType, VectorType, MatrixType,
        MatrixType, MatrixType, Formulation::MFORM>    mform(F, xa, Sa, Se);

    GaussNewton<RealType> GN{};
    GN.tolerance() = 1e-15; GN.maximum_iterations() = 1000;

    VectorType x_std, x_n, x_m;
    std.compute(x_std, y, GN);
    nform.compute(x_n, y, GN);
    mform.compute(x_m, y, GN);

    RealType e1, e2;
    e1 = maximum_error(x_std, x_m);
    e2 = maximum_error(x_std, x_n);

    BOOST_TEST((e1 < EPS), "Error STD - NFORM = " << e1);
    BOOST_TEST((e2 < EPS), "Error STD - MFORM =" << e2);

    // Test inversion using CG solver.

    ConjugateGradient cg(1e-12);
    GaussNewton<RealType, ConjugateGradient> GN_CG(cg);
    GN_CG.tolerance() = 1e-15; GN_CG.maximum_iterations() = 1000;

    std.compute(x_std, y, GN_CG);
    nform.compute(x_n, y, GN_CG);
    mform.compute(x_m, y, GN_CG);

    e1 = maximum_error(x_std, x_m);
    e2 = maximum_error(x_std, x_n);

    BOOST_TEST((e1 < EPS), "Error STD - NFORM CG = " << e1);
    BOOST_TEST((e2 < EPS), "Error STD - MFORM CG = " << e2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(sphere, T, matrix_types)
{
    srand(time(NULL));
    for (unsigned int i = 0; i < ntests; i++)
    {
        unsigned int n = 10;
        sphere_test<T>(n);
    }
}
<commit_msg>Adapted test to new structure of source tree.<commit_after>#define BOOST_TEST_MODULE forward_models sphere
#include <boost/test/included/unit_test.hpp>
#include <iostream>

#include "invlib/algebra.h"
#include "invlib/map.h"
#include "invlib/optimization.h"

#include "forward_models/sphere.h"
#include "utility.h"
#include "test_types.h"

constexpr double EPS = 1e-9;
constexpr int ntests = 100;

using namespace invlib;

// Use the sphere function forward model to test the equivalence of the
// standard, n-form and m-form when using the Gauss-Newton optimizer.
template
<
typename T
>
void sphere_test(unsigned int n)
{
    using RealType   = typename T::RealType;
    using VectorType = typename T::VectorType;
    using MatrixType = typename T::MatrixType;
    using Model      = Sphere<MatrixType>;

    MatrixType Se = random_positive_definite<MatrixType>(1);
    MatrixType Sa = random_positive_definite<MatrixType>(n);
    VectorType xa = random<VectorType>(n);
    VectorType y  = random<VectorType>(1);

    Model F(n);
    MAP<Model, RealType, VectorType, MatrixType,
        MatrixType, MatrixType, Formulation::STANDARD> std(F, xa, Sa, Se);
    MAP<Model, RealType, VectorType, MatrixType,
        MatrixType, MatrixType, Formulation::NFORM>    nform(F, xa, Sa, Se);
    MAP<Model, RealType, VectorType, MatrixType,
        MatrixType, MatrixType, Formulation::MFORM>    mform(F, xa, Sa, Se);

    GaussNewton<RealType> GN{};
    GN.tolerance() = 1e-15; GN.maximum_iterations() = 1000;

    VectorType x_std, x_n, x_m;
    std.compute(x_std, y, GN);
    nform.compute(x_n, y, GN);
    mform.compute(x_m, y, GN);

    RealType e1, e2;
    e1 = maximum_error(x_std, x_m);
    e2 = maximum_error(x_std, x_n);

    BOOST_TEST((e1 < EPS), "Error STD - NFORM = " << e1);
    BOOST_TEST((e2 < EPS), "Error STD - MFORM =" << e2);

    // Test inversion using CG solver.

    ConjugateGradient cg(1e-12);
    GaussNewton<RealType, ConjugateGradient> GN_CG(cg);
    GN_CG.tolerance() = 1e-15; GN_CG.maximum_iterations() = 1000;

    std.compute(x_std, y, GN_CG);
    nform.compute(x_n, y, GN_CG);
    mform.compute(x_m, y, GN_CG);

    e1 = maximum_error(x_std, x_m);
    e2 = maximum_error(x_std, x_n);

    BOOST_TEST((e1 < EPS), "Error STD - NFORM CG = " << e1);
    BOOST_TEST((e2 < EPS), "Error STD - MFORM CG = " << e2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(sphere, T, matrix_types)
{
    srand(time(NULL));
    for (unsigned int i = 0; i < ntests; i++)
    {
        unsigned int n = 10;
        sphere_test<T>(n);
    }
}
<|endoftext|>


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

#include "selxSuperElastixFilterCustomComponents.h"
#include "selxItkDisplacementFieldSourceComponent.h"
#include "selxItkDisplacementFieldSinkComponent.h"
#include "selxItkMeshSourceComponent.h"
#include "selxItkMeshSinkComponent.h"
#include "itkMeshFileReader.h"
#include "itkMeshFileWriter.h"
#include "selxDisplacementFieldMeshWarperComponent.h"

#include "gtest/gtest.h"
#include "selxDataManager.h"

namespace selx {

class DisplacementFieldMeshWarperComponentTest : public ::testing::Test {
public:

  typedef ItkDisplacementFieldMeshWarperComponent< 2, float, float >::ItkDisplacementFieldType DisplacementFieldType;
  typedef DisplacementFieldType::Pointer DisplacementFieldPointer;
  typedef itk::ImageFileReader< DisplacementFieldType > DisplacementFieldReaderType;
  typedef DisplacementFieldReaderType::Pointer DisplacementFieldReaderPointer;
  typedef itk::ImageFileWriter< DisplacementFieldType > DisplacementFieldWriterType;
  typedef DisplacementFieldWriterType::Pointer DisplacementFieldWriterPointer;

  typedef itk::Mesh< float, 2 > MeshType;
  typedef MeshType::Pointer MeshPointer;
  typedef itk::MeshFileReader< MeshType > MeshReaderType;
  typedef MeshReaderType::Pointer MeshReaderPointer;
  typedef itk::MeshFileWriter< MeshType > MeshWriterType;
  typedef MeshWriterType::Pointer MeshWriterPointer;

  typedef Blueprint::Pointer BlueprintPointer;

  typedef TypeList<
    ItkDisplacementFieldSourceComponent< 2, float >,
    ItkDisplacementFieldSinkComponent< 2, float >,
    ItkMeshSourceComponent< 2, float >,
    ItkMeshSinkComponent< 2, float >,
    ItkDisplacementFieldMeshWarperComponent< 2, float, float > > TestComponents;

  typedef SuperElastixFilterCustomComponents< TestComponents > SuperElastixFilterType;
  typedef SuperElastixFilterType::Pointer SuperElastixFilterPointer;

  DataManager::Pointer dataManager = DataManager::New();
};

TEST_F( DisplacementFieldMeshWarperComponentTest, SinkAndSource )
{

  DisplacementFieldPointer displacementField = DisplacementFieldType::New();
  displacementField->SetRegions(DisplacementFieldType::Superclass::SizeType({{3, 3}}));
  displacementField->Allocate();

  DisplacementFieldType::Superclass::IndexType index00, index01, index02,
                                               index10, index11, index12,
                                               index20, index21, index22;

  index00[0] = 0; index00[1] = 0;
  index01[0] = 0; index01[1] = 1;
  index02[0] = 0; index02[1] = 2;
  index10[0] = 1; index10[1] = 0;
  index11[0] = 1; index11[1] = 1;
  index12[0] = 1; index12[1] = 2;
  index20[0] = 2; index20[1] = 0;
  index21[0] = 2; index21[1] = 1;
  index22[0] = 2; index22[1] = 2;

  typedef itk::Vector<float, 2> VectorType;
  VectorType displacement00, displacement01, displacement10, displacement11, displacementminus11;
  displacement00[0] = 0.;
  displacement00[1] = 0.;

  displacement01[0] = 0.;
  displacement01[1] = 1.;

  displacement10[0] = 1.;
  displacement10[1] = 0.;

  displacement11[0] = 1.;
  displacement11[1] = 1.;

  displacementminus11[0] = -1.;
  displacementminus11[1] = -1.;

  displacementField->SetPixel(index00, displacement01);
  displacementField->SetPixel(index01, displacement10);
  displacementField->SetPixel(index02, displacement00);
  displacementField->SetPixel(index10, displacement10);
  displacementField->SetPixel(index11, displacement11);
  displacementField->SetPixel(index12, displacement00);
  displacementField->SetPixel(index20, displacement00);
  displacementField->SetPixel(index21, displacement00);
  displacementField->SetPixel(index22, displacementminus11);

  Logger::Pointer logger = Logger::New();
  logger->AddStream( "cout", std::cout );
  logger->SetLogLevel( LogLevel::TRC );

  BlueprintPointer blueprint = Blueprint::New();
  using ParameterMapType = Blueprint::ParameterMapType;

  ParameterMapType displacementFieldSourceParameters;
  displacementFieldSourceParameters[ "NameOfClass" ]    = { "ItkDisplacementFieldSourceComponent" };
  displacementFieldSourceParameters[ "Dimensionality" ] = { "2" };
  blueprint->SetComponent( "DisplacementFieldSource", displacementFieldSourceParameters );

  ParameterMapType meshSourceParameters;
  meshSourceParameters[ "NameOfClass" ]    = { "ItkMeshSourceComponent" };
  meshSourceParameters[ "Dimensionality" ] = { "2" };
  blueprint->SetComponent( "MeshSource", meshSourceParameters );

  ParameterMapType meshSinkParameters;
  meshSinkParameters[ "NameOfClass" ]    = { "ItkMeshSinkComponent" };
  meshSinkParameters[ "Dimensionality" ] = { "2" };
  blueprint->SetComponent( "MeshSink", meshSinkParameters );

  ParameterMapType displacementFieldMeshWarperParameters;
  displacementFieldMeshWarperParameters[ "NameOfClass" ]    = { "ItkDisplacementFieldMeshWarperComponent" };
  displacementFieldMeshWarperParameters[ "Dimensionality" ] = { "2" };
  blueprint->SetComponent( "DisplacementFieldMeshWarper", displacementFieldMeshWarperParameters );

  ParameterMapType connection0;
  connection0[ "NameOfInterface" ] = { "itkMeshInterface" };
  blueprint->SetConnection( "MeshSource", "ItkDisplacementFieldMeshWarperComponent", connection0 );

  ParameterMapType connection1;
  connection0[ "NameOfInterface" ] = { "itkDisplacementFieldInterface" };
  blueprint->SetConnection( "DisplacementFieldSource", "ItkDisplacementFieldMeshWarperComponent", connection1 );

  ParameterMapType connection2;
  connection0[ "NameOfInterface" ] = { "itkMeshInterface" };
  blueprint->SetConnection( "ItkDisplacementFieldMeshWarperComponent", "MeshSink", connection2 );

  SuperElastixFilterPointer superElastixFilter = SuperElastixFilterType::New();
  superElastixFilter->SetBlueprint(blueprint);
  superElastixFilter->SetLogger(logger);

  MeshReaderPointer meshReader = MeshReaderType::New();
  meshReader->SetFileName( this->dataManager->GetInputFile( "2dSquare.vtk" ) );

  superElastixFilter->SetInput( "DisplacementFieldSource", displacementField );
  superElastixFilter->SetInput( "MeshSource", meshReader->GetOutput() );

  MeshWriterPointer meshWriter = MeshWriterType::New();
  meshWriter->SetFileName( this->dataManager->GetOutputFile( "DisplacementFieldMeshWarperComponentTest.SinkAndSource.WarpedMesh.vtk" ) );
  meshWriter->SetInput( superElastixFilter->GetOutput< MeshType >( "MeshSink" ) );
  meshWriter->Update();
}

} // namespace selx<commit_msg>BUG: Blueprint used wrong component identifiers at connections<commit_after>/*=========================================================================
 *
 *  Copyright Leiden University Medical Center, Erasmus University Medical
 *  Center and contributors
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *        http://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/

#include "selxSuperElastixFilterCustomComponents.h"
#include "selxItkDisplacementFieldSourceComponent.h"
#include "selxItkDisplacementFieldSinkComponent.h"
#include "selxItkMeshSourceComponent.h"
#include "selxItkMeshSinkComponent.h"
#include "itkMeshFileReader.h"
#include "itkMeshFileWriter.h"
#include "selxDisplacementFieldMeshWarperComponent.h"

#include "gtest/gtest.h"
#include "selxDataManager.h"

namespace selx {

class DisplacementFieldMeshWarperComponentTest : public ::testing::Test {
public:

  typedef ItkDisplacementFieldMeshWarperComponent< 2, float, float >::ItkDisplacementFieldType DisplacementFieldType;
  typedef DisplacementFieldType::Pointer DisplacementFieldPointer;
  typedef itk::ImageFileReader< DisplacementFieldType > DisplacementFieldReaderType;
  typedef DisplacementFieldReaderType::Pointer DisplacementFieldReaderPointer;
  typedef itk::ImageFileWriter< DisplacementFieldType > DisplacementFieldWriterType;
  typedef DisplacementFieldWriterType::Pointer DisplacementFieldWriterPointer;

  typedef itk::Mesh< float, 2 > MeshType;
  typedef MeshType::Pointer MeshPointer;
  typedef itk::MeshFileReader< MeshType > MeshReaderType;
  typedef MeshReaderType::Pointer MeshReaderPointer;
  typedef itk::MeshFileWriter< MeshType > MeshWriterType;
  typedef MeshWriterType::Pointer MeshWriterPointer;

  typedef Blueprint::Pointer BlueprintPointer;

  typedef TypeList<
    ItkDisplacementFieldSourceComponent< 2, float >,
    ItkDisplacementFieldSinkComponent< 2, float >,
    ItkMeshSourceComponent< 2, float >,
    ItkMeshSinkComponent< 2, float >,
    ItkDisplacementFieldMeshWarperComponent< 2, float, float > > TestComponents;

  typedef SuperElastixFilterCustomComponents< TestComponents > SuperElastixFilterType;
  typedef SuperElastixFilterType::Pointer SuperElastixFilterPointer;

  DataManager::Pointer dataManager = DataManager::New();
};

TEST_F( DisplacementFieldMeshWarperComponentTest, SinkAndSource )
{

  DisplacementFieldPointer displacementField = DisplacementFieldType::New();
  displacementField->SetRegions(DisplacementFieldType::Superclass::SizeType({{3, 3}}));
  displacementField->Allocate();

  DisplacementFieldType::Superclass::IndexType index00, index01, index02,
                                               index10, index11, index12,
                                               index20, index21, index22;

  index00[0] = 0; index00[1] = 0;
  index01[0] = 0; index01[1] = 1;
  index02[0] = 0; index02[1] = 2;
  index10[0] = 1; index10[1] = 0;
  index11[0] = 1; index11[1] = 1;
  index12[0] = 1; index12[1] = 2;
  index20[0] = 2; index20[1] = 0;
  index21[0] = 2; index21[1] = 1;
  index22[0] = 2; index22[1] = 2;

  typedef itk::Vector<float, 2> VectorType;
  VectorType displacement00, displacement01, displacement10, displacement11, displacementminus11;
  displacement00[0] = 0.;
  displacement00[1] = 0.;

  displacement01[0] = 0.;
  displacement01[1] = 1.;

  displacement10[0] = 1.;
  displacement10[1] = 0.;

  displacement11[0] = 1.;
  displacement11[1] = 1.;

  displacementminus11[0] = -1.;
  displacementminus11[1] = -1.;

  displacementField->SetPixel(index00, displacement01);
  displacementField->SetPixel(index01, displacement10);
  displacementField->SetPixel(index02, displacement00);
  displacementField->SetPixel(index10, displacement10);
  displacementField->SetPixel(index11, displacement11);
  displacementField->SetPixel(index12, displacement00);
  displacementField->SetPixel(index20, displacement00);
  displacementField->SetPixel(index21, displacement00);
  displacementField->SetPixel(index22, displacementminus11);

  Logger::Pointer logger = Logger::New();
  logger->AddStream( "cout", std::cout );
  logger->SetLogLevel( LogLevel::TRC );

  BlueprintPointer blueprint = Blueprint::New();
  using ParameterMapType = Blueprint::ParameterMapType;

  ParameterMapType displacementFieldSourceParameters;
  displacementFieldSourceParameters[ "NameOfClass" ]    = { "ItkDisplacementFieldSourceComponent" };
  displacementFieldSourceParameters[ "Dimensionality" ] = { "2" };
  blueprint->SetComponent( "DisplacementFieldSource", displacementFieldSourceParameters );

  ParameterMapType meshSourceParameters;
  meshSourceParameters[ "NameOfClass" ]    = { "ItkMeshSourceComponent" };
  meshSourceParameters[ "Dimensionality" ] = { "2" };
  blueprint->SetComponent( "MeshSource", meshSourceParameters );

  ParameterMapType meshSinkParameters;
  meshSinkParameters[ "NameOfClass" ]    = { "ItkMeshSinkComponent" };
  meshSinkParameters[ "Dimensionality" ] = { "2" };
  blueprint->SetComponent( "MeshSink", meshSinkParameters );

  ParameterMapType displacementFieldMeshWarperParameters;
  displacementFieldMeshWarperParameters[ "NameOfClass" ]    = { "ItkDisplacementFieldMeshWarperComponent" };
  displacementFieldMeshWarperParameters[ "Dimensionality" ] = { "2" };
  blueprint->SetComponent( "DisplacementFieldMeshWarper", displacementFieldMeshWarperParameters );

  ParameterMapType connection0;
  connection0[ "NameOfInterface" ] = { "itkMeshInterface" };
  blueprint->SetConnection( "MeshSource", "DisplacementFieldMeshWarper", connection0 );

  ParameterMapType connection1;
  connection0[ "NameOfInterface" ] = { "itkDisplacementFieldInterface" };
  blueprint->SetConnection( "DisplacementFieldSource", "DisplacementFieldMeshWarper", connection1 );

  ParameterMapType connection2;
  connection0[ "NameOfInterface" ] = { "itkMeshInterface" };
  blueprint->SetConnection( "DisplacementFieldMeshWarper", "MeshSink", connection2 );

  SuperElastixFilterPointer superElastixFilter = SuperElastixFilterType::New();
  superElastixFilter->SetBlueprint(blueprint);
  superElastixFilter->SetLogger(logger);

  MeshReaderPointer meshReader = MeshReaderType::New();
  meshReader->SetFileName( this->dataManager->GetInputFile( "2dSquare.vtk" ) );

  superElastixFilter->SetInput( "DisplacementFieldSource", displacementField );
  superElastixFilter->SetInput( "MeshSource", meshReader->GetOutput() );

  MeshWriterPointer meshWriter = MeshWriterType::New();
  meshWriter->SetFileName( this->dataManager->GetOutputFile( "DisplacementFieldMeshWarperComponentTest.SinkAndSource.WarpedMesh.vtk" ) );
  meshWriter->SetInput( superElastixFilter->GetOutput< MeshType >( "MeshSink" ) );
  meshWriter->Update();
}

} // namespace selx<|endoftext|>