#include <rtt/RTT.hpp>
#include <rtt/types/StructTypeInfo.hpp>
#include <rtt/types/TemplateConstructor.hpp>
#include <rtt/internal/DataSource.hpp>
#include <rtt/Logger.hpp>
#include <rtt/os/main.h>

using namespace RTT;

struct A {
	int x;
	int y;
};

struct B {
	int x;
	int y;
};

std::ostream& operator<<(std::ostream& os, const A& a) { return os << "(" << a.x << ", " << a.y << ")"; }
std::ostream& operator<<(std::ostream& os, const B& b) { return os << "(" << b.x << ", " << b.y << ")"; }
std::istream& operator>>(std::istream& os, A& a) { return os; }
std::istream& operator>>(std::istream& os, B& b) { return os; }

namespace boost {
namespace serialization {
	template<class Archive>
	void serialize( Archive & a, A & v, unsigned int) {
		using boost::serialization::make_nvp;
		a & make_nvp("x", v.x);
		a & make_nvp("y", v.y);
	}

	template<class Archive>
	void serialize( Archive & a, B & v, unsigned int) {
		using boost::serialization::make_nvp;
		a & make_nvp("x", v.x);
		a & make_nvp("y", v.y);
	}
}
}

B convertAtoB(A a) {
	B b;
	b.x = a.x;
	b.y = a.y;
	return b;
}

int ORO_main(int argc, char** argv)
{
	Logger::log().setLogLevel(Logger::Debug);
	// Add types to RTT
	types::Types()->addType( new types::StructTypeInfo<A, true>("A") );
	types::Types()->addType( new types::StructTypeInfo<B, true>("B") );
	// Add type conversion
	types::Types()->type("B")->addConstructor( types::newConstructor(&convertAtoB) );
	// Ports
	OutputPort<A> oportA("oportA");
	InputPort<A> iportA("iportA");
	InputPort<B> iportB("iportB");
	// Port connection
	ConnPolicy policy = ConnPolicy::data();
	oportA.connectTo(&iportA, policy);
	oportA.connectTo(&iportB, policy);
	// Write output
	A a; a.x = 1; a.y = 2;
	oportA.write(a);
	log(Info) << "A written to oportA: " << a << endlog();
	// Read input of type A
	A a_;
	FlowStatus fsA = iportA.read(a_);
	log(Info) << "Reading iportA (" << fsA << "): A=" << a << endlog();
	// Read input of type B
	B b;
	FlowStatus fsB = iportB.read(b);
	log(Info) << "Reading iportB (" << fsB << "): B=" << b << endlog();
	return 0;
}