diff --git a/ati_netbox2_lc.lc b/ati_netbox2_lc.lc
new file mode 100644
index 0000000000000000000000000000000000000000..2a4226b0269bd8a7ae5a8f126bc19e47504e2402
--- /dev/null
+++ b/ati_netbox2_lc.lc
@@ -0,0 +1,4 @@
+sample struct {
+	long  info[3]; 
+	float forcesTorques[6];
+} ati2rob_2;
diff --git a/ati_netbox_lc.lc b/ati_netbox_lc.lc
new file mode 100644
index 0000000000000000000000000000000000000000..6211ade0b9f3b2d7e694731b2c98ca8fc938d6ad
--- /dev/null
+++ b/ati_netbox_lc.lc
@@ -0,0 +1,4 @@
+sample struct {
+	long  info[3]; 
+	float forcesTorques[6];
+} ati2rob;
diff --git a/coupling.lc b/coupling.lc
new file mode 100644
index 0000000000000000000000000000000000000000..a15f23644c1f1e52655e3317567b146d19913571
--- /dev/null
+++ b/coupling.lc
@@ -0,0 +1,8 @@
+//robot to cpp
+sample float yaFromRobot[28];
+
+
+//cpp to robot
+sample float yarddToRobot[14];
+sample float optsToRobot[100];
+
diff --git a/cpp/Makefile b/cpp/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..1ef37769997a6b6b5f016bf88f91c2db1e08d63f
--- /dev/null
+++ b/cpp/Makefile
@@ -0,0 +1,36 @@
+CC=g++
+CFLAGS=-I. -Wall -I/opt/robot/labcomm -I/opt/robot/orca \
+-fpermissive -lpthread -std=c++11 -I/home/martinka/miscellaneous/armadillo-6.400.3/include -DARMA_DONT_USE_WRAPPER -llapack -lblas 
+LFLAGS=-lrt
+LIBS=-lm  -llapack -lblas
+VPATH=/opt/robot/orca /opt/robot/labcomm
+OBJSTATIC = orca_client.o orca_messages.o labcomm.o coupling.o traj2dmp.o \
+dmp.o armmoving.o state2yardd.o record_ya.o twod_controller.o write_norms_to_file.o
+
+all: main
+%: %.cc
+	$(CC) -o $@ $(CFLAGS) $^ $(LIBS)
+
+
+%.o: %.cc $(DEPS)
+	$(CC) $(CFLAGS) -c -o $@ $<
+
+%.h %.c:        %.lc
+	labcomm -C $<
+
+
+main: $(OBJSTATIC) main.cc
+	$(CC) $(CFLAGS) $(LFLAGS) $(OBJSTATIC) main.cc -o main
+	
+sim: $(OBJSTATIC) sim.cc
+	$(CC) $(CFLAGS) $(LFLAGS) $(OBJSTATIC) sim.cc -o sim	
+
+static: $(OBJSTATIC)
+	ar rcs libcmt.a $^
+
+clean:
+	rm -f main
+	rm -f *.o \
+	rm -f libcmt.a
+	rm -f libcmt.so
+
diff --git a/cpp/armmoving.cc b/cpp/armmoving.cc
new file mode 100644
index 0000000000000000000000000000000000000000..44f26f7506ca016a2ead2b48ec28fcd7bf7b74a4
--- /dev/null
+++ b/cpp/armmoving.cc
@@ -0,0 +1,35 @@
+#include "armmoving.h"
+#include <iostream>
+#include <armadillo>
+
+using namespace arma;
+
+
+
+bool armMoving(const mat& jointPos) {
+	static bool ismoving = false;
+	const double th = 0.002;
+	
+	
+	static int counter = 0;
+	static mat jointPosOld = zeros<mat>(1,dof);
+	
+	if (counter == 0) { // first run
+		jointPosOld = jointPos;
+	}
+	
+	
+	if (counter == 50) {
+		counter = 1;
+		if ( all(vectorise(sum(abs(jointPosOld - jointPos),1) > th)))  {
+			ismoving = true;
+		} else {
+			ismoving = false;
+		}
+		jointPosOld = jointPos;
+	}
+		
+		
+	++counter;
+	return ismoving;
+}
diff --git a/cpp/armmoving.h b/cpp/armmoving.h
new file mode 100644
index 0000000000000000000000000000000000000000..9721884b9473f670c55a72cc6971460f848cb97a
--- /dev/null
+++ b/cpp/armmoving.h
@@ -0,0 +1,10 @@
+#include <armadillo>
+#include "dmp.h"
+using namespace arma;
+#ifndef ARMMOVING_H
+#define ARMMOVING_H
+
+bool armMoving(const mat& jointPos);
+
+
+#endif
diff --git a/cpp/coupling.c b/cpp/coupling.c
new file mode 100644
index 0000000000000000000000000000000000000000..ff9997fb423ef1736dc2859d93a1061febc85381
--- /dev/null
+++ b/cpp/coupling.c
@@ -0,0 +1,265 @@
+#include "labcomm.h"
+#include "labcomm_private.h"
+#include "coupling.h"
+
+static unsigned char signature_bytes_yaFromRobot[] = {
+// array [28]
+0, 0, 0, 16, 
+  0, 0, 0, 1, 
+  0, 0, 0, 28, 
+  0, 0, 0, 37, 
+// }
+};
+labcomm_signature_t labcomm_signature_coupling_yaFromRobot = {
+  LABCOMM_SAMPLE, "yaFromRobot",
+  (int (*)(void *))labcomm_sizeof_coupling_yaFromRobot,
+  sizeof(signature_bytes_yaFromRobot),
+  signature_bytes_yaFromRobot
+ };
+static unsigned char signature_bytes_yarddToRobot[] = {
+// array [14]
+0, 0, 0, 16, 
+  0, 0, 0, 1, 
+  0, 0, 0, 14, 
+  0, 0, 0, 37, 
+// }
+};
+labcomm_signature_t labcomm_signature_coupling_yarddToRobot = {
+  LABCOMM_SAMPLE, "yarddToRobot",
+  (int (*)(void *))labcomm_sizeof_coupling_yarddToRobot,
+  sizeof(signature_bytes_yarddToRobot),
+  signature_bytes_yarddToRobot
+ };
+static unsigned char signature_bytes_optsToRobot[] = {
+// array [100]
+0, 0, 0, 16, 
+  0, 0, 0, 1, 
+  0, 0, 0, 100, 
+  0, 0, 0, 37, 
+// }
+};
+labcomm_signature_t labcomm_signature_coupling_optsToRobot = {
+  LABCOMM_SAMPLE, "optsToRobot",
+  (int (*)(void *))labcomm_sizeof_coupling_optsToRobot,
+  sizeof(signature_bytes_optsToRobot),
+  signature_bytes_optsToRobot
+ };
+static void decode_yaFromRobot(
+  labcomm_decoder_t *d,
+  void (*handle)(
+    coupling_yaFromRobot *v,
+    void *context
+  ),
+  void *context
+)
+{
+  coupling_yaFromRobot v;
+  {
+    int i_0_0;
+    for (i_0_0 = 0 ; i_0_0 < 28 ; i_0_0++) {
+      v.a[i_0_0] = labcomm_decode_float(d);
+    }
+  }
+  handle(&v, context);
+}
+void labcomm_decoder_register_coupling_yaFromRobot(
+  struct labcomm_decoder *d,
+  void (*handler)(
+    coupling_yaFromRobot *v,
+    void *context
+  ),
+  void *context
+)
+{
+  labcomm_internal_decoder_register(
+    d,
+    &labcomm_signature_coupling_yaFromRobot,
+    (labcomm_decoder_typecast_t)decode_yaFromRobot,
+    (labcomm_handler_typecast_t)handler,
+    context
+  );
+}
+static void encode_yaFromRobot(
+  labcomm_encoder_t *e,
+  coupling_yaFromRobot *v
+)
+{
+  e->writer.write(&e->writer, labcomm_writer_start);
+  labcomm_encode_type_index(e, &labcomm_signature_coupling_yaFromRobot);
+  {
+    {
+      int i_0_0;
+      for (i_0_0 = 0 ; i_0_0 < 28 ; i_0_0++) {
+        labcomm_encode_float(e, (*v).a[i_0_0]);
+      }
+    }
+  }
+  e->writer.write(&e->writer, labcomm_writer_end);
+}
+void labcomm_encode_coupling_yaFromRobot(
+labcomm_encoder_t *e,
+coupling_yaFromRobot *v
+)
+{
+labcomm_internal_encode(e, &labcomm_signature_coupling_yaFromRobot, v);
+}
+void labcomm_encoder_register_coupling_yaFromRobot(
+  struct labcomm_encoder *e
+)
+{
+  labcomm_internal_encoder_register(
+    e,
+    &labcomm_signature_coupling_yaFromRobot,
+    (labcomm_encode_typecast_t)encode_yaFromRobot
+  );
+}
+int labcomm_sizeof_coupling_yaFromRobot(coupling_yaFromRobot *v)
+{
+  return 116;
+}
+static void decode_yarddToRobot(
+  labcomm_decoder_t *d,
+  void (*handle)(
+    coupling_yarddToRobot *v,
+    void *context
+  ),
+  void *context
+)
+{
+  coupling_yarddToRobot v;
+  {
+    int i_0_0;
+    for (i_0_0 = 0 ; i_0_0 < 14 ; i_0_0++) {
+      v.a[i_0_0] = labcomm_decode_float(d);
+    }
+  }
+  handle(&v, context);
+}
+void labcomm_decoder_register_coupling_yarddToRobot(
+  struct labcomm_decoder *d,
+  void (*handler)(
+    coupling_yarddToRobot *v,
+    void *context
+  ),
+  void *context
+)
+{
+  labcomm_internal_decoder_register(
+    d,
+    &labcomm_signature_coupling_yarddToRobot,
+    (labcomm_decoder_typecast_t)decode_yarddToRobot,
+    (labcomm_handler_typecast_t)handler,
+    context
+  );
+}
+static void encode_yarddToRobot(
+  labcomm_encoder_t *e,
+  coupling_yarddToRobot *v
+)
+{
+  e->writer.write(&e->writer, labcomm_writer_start);
+  labcomm_encode_type_index(e, &labcomm_signature_coupling_yarddToRobot);
+  {
+    {
+      int i_0_0;
+      for (i_0_0 = 0 ; i_0_0 < 14 ; i_0_0++) {
+        labcomm_encode_float(e, (*v).a[i_0_0]);
+      }
+    }
+  }
+  e->writer.write(&e->writer, labcomm_writer_end);
+}
+void labcomm_encode_coupling_yarddToRobot(
+labcomm_encoder_t *e,
+coupling_yarddToRobot *v
+)
+{
+labcomm_internal_encode(e, &labcomm_signature_coupling_yarddToRobot, v);
+}
+void labcomm_encoder_register_coupling_yarddToRobot(
+  struct labcomm_encoder *e
+)
+{
+  labcomm_internal_encoder_register(
+    e,
+    &labcomm_signature_coupling_yarddToRobot,
+    (labcomm_encode_typecast_t)encode_yarddToRobot
+  );
+}
+int labcomm_sizeof_coupling_yarddToRobot(coupling_yarddToRobot *v)
+{
+  return 60;
+}
+static void decode_optsToRobot(
+  labcomm_decoder_t *d,
+  void (*handle)(
+    coupling_optsToRobot *v,
+    void *context
+  ),
+  void *context
+)
+{
+  coupling_optsToRobot v;
+  {
+    int i_0_0;
+    for (i_0_0 = 0 ; i_0_0 < 100 ; i_0_0++) {
+      v.a[i_0_0] = labcomm_decode_float(d);
+    }
+  }
+  handle(&v, context);
+}
+void labcomm_decoder_register_coupling_optsToRobot(
+  struct labcomm_decoder *d,
+  void (*handler)(
+    coupling_optsToRobot *v,
+    void *context
+  ),
+  void *context
+)
+{
+  labcomm_internal_decoder_register(
+    d,
+    &labcomm_signature_coupling_optsToRobot,
+    (labcomm_decoder_typecast_t)decode_optsToRobot,
+    (labcomm_handler_typecast_t)handler,
+    context
+  );
+}
+static void encode_optsToRobot(
+  labcomm_encoder_t *e,
+  coupling_optsToRobot *v
+)
+{
+  e->writer.write(&e->writer, labcomm_writer_start);
+  labcomm_encode_type_index(e, &labcomm_signature_coupling_optsToRobot);
+  {
+    {
+      int i_0_0;
+      for (i_0_0 = 0 ; i_0_0 < 100 ; i_0_0++) {
+        labcomm_encode_float(e, (*v).a[i_0_0]);
+      }
+    }
+  }
+  e->writer.write(&e->writer, labcomm_writer_end);
+}
+void labcomm_encode_coupling_optsToRobot(
+labcomm_encoder_t *e,
+coupling_optsToRobot *v
+)
+{
+labcomm_internal_encode(e, &labcomm_signature_coupling_optsToRobot, v);
+}
+void labcomm_encoder_register_coupling_optsToRobot(
+  struct labcomm_encoder *e
+)
+{
+  labcomm_internal_encoder_register(
+    e,
+    &labcomm_signature_coupling_optsToRobot,
+    (labcomm_encode_typecast_t)encode_optsToRobot
+  );
+}
+int labcomm_sizeof_coupling_optsToRobot(coupling_optsToRobot *v)
+{
+  return 404;
+}
diff --git a/cpp/coupling.h b/cpp/coupling.h
new file mode 100644
index 0000000000000000000000000000000000000000..e0eeac5e0483832e2c650f20558dd40feff4923b
--- /dev/null
+++ b/cpp/coupling.h
@@ -0,0 +1,80 @@
+/* LabComm declarations:
+sample float yaFromRobot[28];
+sample float yarddToRobot[14];
+sample float optsToRobot[100];
+*/
+
+
+#ifndef __LABCOMM_coupling_H__
+#define __LABCOMM_coupling_H__
+
+#include "labcomm.h"
+
+#ifndef PREDEFINED_coupling_yaFromRobot
+typedef struct {
+  float a[28];
+} coupling_yaFromRobot;
+#endif
+void labcomm_decoder_register_coupling_yaFromRobot(
+  struct labcomm_decoder *d,
+  void (*handler)(
+    coupling_yaFromRobot *v,
+    void *context
+  ),
+  void *context
+);
+void labcomm_encoder_register_coupling_yaFromRobot(
+  struct labcomm_encoder *e);
+void labcomm_encode_coupling_yaFromRobot(
+  struct labcomm_encoder *e,
+  coupling_yaFromRobot *v
+);
+extern int labcomm_sizeof_coupling_yaFromRobot(coupling_yaFromRobot *v);
+
+#ifndef PREDEFINED_coupling_yarddToRobot
+typedef struct {
+  float a[14];
+} coupling_yarddToRobot;
+#endif
+void labcomm_decoder_register_coupling_yarddToRobot(
+  struct labcomm_decoder *d,
+  void (*handler)(
+    coupling_yarddToRobot *v,
+    void *context
+  ),
+  void *context
+);
+void labcomm_encoder_register_coupling_yarddToRobot(
+  struct labcomm_encoder *e);
+void labcomm_encode_coupling_yarddToRobot(
+  struct labcomm_encoder *e,
+  coupling_yarddToRobot *v
+);
+extern int labcomm_sizeof_coupling_yarddToRobot(coupling_yarddToRobot *v);
+
+#ifndef PREDEFINED_coupling_optsToRobot
+typedef struct {
+  float a[100];
+} coupling_optsToRobot;
+#endif
+void labcomm_decoder_register_coupling_optsToRobot(
+  struct labcomm_decoder *d,
+  void (*handler)(
+    coupling_optsToRobot *v,
+    void *context
+  ),
+  void *context
+);
+void labcomm_encoder_register_coupling_optsToRobot(
+  struct labcomm_encoder *e);
+void labcomm_encode_coupling_optsToRobot(
+  struct labcomm_encoder *e,
+  coupling_optsToRobot *v
+);
+extern int labcomm_sizeof_coupling_optsToRobot(coupling_optsToRobot *v);
+
+#define LABCOMM_FORALL_SAMPLES_coupling(func, sep) \
+  func(yaFromRobot, coupling_yaFromRobot) sep \
+  func(yarddToRobot, coupling_yarddToRobot) sep \
+  func(optsToRobot, coupling_optsToRobot)
+#endif
diff --git a/cpp/dmp.cc b/cpp/dmp.cc
new file mode 100644
index 0000000000000000000000000000000000000000..270fc0584c337452d49c43d5e73667cddb486bb6
--- /dev/null
+++ b/cpp/dmp.cc
@@ -0,0 +1,49 @@
+#include "dmp.h"
+#include <iostream>
+#include <armadillo>
+using namespace arma;
+using namespace std;
+
+
+
+
+Dmp::Dmp(mat w, mat g, double t) : weights(w), goal(g), tau(t) {}
+
+mat Dmp::getW() const {
+	return weights;
+}
+
+mat Dmp::getG() const {
+	return goal;
+}
+
+double Dmp::getTau() const {
+	return tau;
+}
+
+void Dmp::setG(mat g) {
+	goal = g;
+}
+
+void Dmp::setTau(double t) {
+	tau = t;
+}
+
+Dmp& Dmp::doubleSpeed() {
+	tau = tau/2;
+	return *this;
+}
+
+Dmp& Dmp::speedupTimes(const double& x) {
+	tau = tau/x;
+	return *this;
+}
+
+
+ostream& operator<<(ostream& os, const Dmp& dmp) {
+	os << "Weights = " << dmp.weights << endl;
+	os << "Goal = " << dmp.goal << endl;
+	os << "Tau = " << dmp.tau << endl;
+	
+	return os;
+}
diff --git a/cpp/dmp.h b/cpp/dmp.h
new file mode 100644
index 0000000000000000000000000000000000000000..e9f3d5ef8cd56046c8bf98cfbc3e776613cbabbe
--- /dev/null
+++ b/cpp/dmp.h
@@ -0,0 +1,63 @@
+#include <armadillo>
+using namespace arma;
+using namespace std;
+
+#ifndef DMP_H
+#define DMP_H
+
+const int dof = 14;
+const double offs = pow(10,-10);
+
+const double alpha_z = 10; // Velocity gain
+const double beta_z = alpha_z / 4;
+const int n_kernel = 50;
+const mat ct = linspace<mat>(0, 1, n_kernel);
+const mat cx = exp(-3*ct);
+const mat c = cx;
+
+class Dmp {
+	friend ostream& operator<<(ostream&, const Dmp&);
+public:
+	Dmp(mat w, mat g, double t);
+	mat getW() const;
+	mat getG() const;
+	double getTau() const;
+	
+	void setG(mat g);
+	void setTau(double t);
+	Dmp& doubleSpeed();
+	Dmp& speedupTimes(const double& x);
+	
+	
+	
+	
+
+private:
+	mat weights;
+	mat goal; 
+	double tau;
+	
+	//new, to be integrated:
+	string ID;
+	string GroupID;
+	string log_url;
+	string name;
+	string comment;
+	string purpose;
+	string tool;
+	string robot;
+	string timestamp;
+	string controllertype; //e.g. extctrl
+	string robotcell;
+	string movementType;
+	string startTolerance;
+	string endTolerance;
+	string goalObject;
+};
+
+ostream& operator<<(ostream& os, const Dmp& dmp);
+
+
+
+
+#endif
diff --git a/cpp/k600_driver_3leds_comet.c b/cpp/k600_driver_3leds_comet.c
new file mode 100644
index 0000000000000000000000000000000000000000..6917cb6dc56d6f0012ce0039559130cf390c99dc
--- /dev/null
+++ b/cpp/k600_driver_3leds_comet.c
@@ -0,0 +1,390 @@
+// Example: 
+//     ./k600_driver_frames ip_or_name_of_your_extctrl_computer 2000 nbrfr
+//
+//   Start measurement at K600 
+// telnet fair-02 50505
+
+
+#define _POSIX_C_SOURCE 199309 
+#include <time.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <orca_client.h>
+#include <pthread.h>
+#include "k600data.h"
+
+#include <sys/select.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <unistd.h>
+
+#include <sys/time.h>
+
+
+#include <netdb.h>
+#include <string.h>
+#include <sys/times.h>
+
+#define K600SERVER 	"fair-02"
+#define TCPPORT 	50506	/* shared client/server tcp port number */
+#define N_LEDS 3
+#define N_FRAMES 0
+#define BUFSIZE         4+8*7*N_FRAMES+8*4*N_LEDS   /*size of buffer sent */
+/*     See  Nikon_doc/JS014_K-Link_TCP_Interface.doc    */
+
+/* This function which continously receives data from irc5
+ * must be started from a thread because the function will 
+ * not end until the connection to irc5 is shut down. */
+
+void* handler_thread(void *arg) {
+  orca_client_channel_t* signalsFrRob = (orca_client_channel_t*) arg;
+  labcomm_decoder_run(signalsFrRob->decoder);
+  return(0);
+}
+//-------------------------------------------------------------
+
+
+ /*
+  * initSocket
+  *	create a tcp socket
+  *	fill in the server structure with
+  *		protocol family, remote address, tcp port
+  * CALLS:
+  *	gethostbyname - map hostname to host addr
+  */
+// __ //  static
+int initSocket(server, remote)
+ struct sockaddr_in *server; 	/* server structure for connect */
+ char *remote;		        /* remote hostname */
+ {
+ 	struct hostent *gethostbyname();
+ 	struct hostent *h;
+ 	int sock;
+ 
+ 	/* create INTERNET,TCP socket
+ 	*/
+ 	sock = socket(AF_INET, SOCK_STREAM, 0); 
+ 
+ 	if ( sock < 0 ) {
+ 		perror("socket");
+ 		exit(1);
+ 	}
+ 
+ 	/* fill in the server structure for the connect call
+ 	 *
+ 	 * family: INET meaning tcp/ip/ethernet.
+ 	 * addr: string that represents INET number for remote system.
+ 	 * port: string that represents tcp server number.
+ 	 */ 
+ 	server->sin_family = AF_INET;
+ 	h = 0;
+ 
+ 	/* read local inet name of REMOTE machine into 
+ 	 * hostent structure. This buffer must be copied into the
+ 	 * sockaddr_in server structure.
+ 	 */
+ 	h = gethostbyname(remote);
+ 	if ( h == 0 ) {
+ 		fprintf(stderr,"gethostbyname: cannot find host %s\n",remote);
+ 		exit(2);
+ 	}
+
+ 	/*
+ 	bcopy(h->h_addr, &server->sin_addr, h->h_length);
+ 	*/
+	// CHECK THIS!!! 	memcpy(&server->sin_addr, h->h_addr, h->h_length);
+ 	memcpy(&server->sin_addr, h-> h_addr_list[0], h->h_length);
+ 	/* convert host short to network byteorder short
+ 	*/
+ 	server->sin_port = htons(TCPPORT);
+ 	return(sock);
+ }
+ 
+
+
+
+//-------------------------------------------------------------
+
+ /* read from socket. TCP read call issued to kernel
+  * may return with less data than you expected. This routine
+  * must check that and force a complete read.
+  */
+int  doRead(sock, buf,amountNeeded)
+     int sock;
+ char *buf;
+ int amountNeeded;
+ {
+   // 	register int i;
+ 	int rc;
+ 	char *bpt;
+ 	int count = amountNeeded;
+ 	int amtread;
+
+ 	bpt = buf;
+ 	amtread = 0;
+  
+ again:
+ 	if ( (rc = read(sock,bpt,count)) <= 0 ) {
+ 		perror("doRead: reading socket stream");
+ 		exit(1);
+ 	}
+ 	if (rc !=BUFSIZE) {
+	       printf("=============== rc= %d,\n",rc);
+ 	}
+
+ 	amtread += rc;
+ 
+  	if ( amtread < amountNeeded ) {
+  		count = count - rc;	
+  		bpt = bpt + rc;
+  		goto again;
+  	}
+	return(amountNeeded);
+ }
+ 
+// __ // /* 
+// __ //  * dump a buffer in hex
+// __ //  */
+// __ // printBuf(buf,size)
+// __ // char *buf;
+// __ // {
+// __ // 	int i;
+// __ // 	i = 16;
+// __ // 	while(size--) {
+// __ // 		printf("%x", (int)(*buf++&0xff));
+// __ // 		if ( i == 0 ) {
+// __ // 			i = 16;
+// __ // 			printf("\n");
+// __ // 		}
+// __ // 		else
+// __ // 			i--;
+// __ // 	}
+// __ // }
+// __ // 
+
+
+
+//------------------------------------------------------------------
+
+pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
+static  int sock;
+static char buf[BUFSIZE];
+
+
+/* Is called when robotvec is received from orca/robot. */
+void handle_robotvec(k600data_robotvec* v, void* context) {
+static int my_counter = 0;
+  if(my_counter++ > 10){
+    my_counter=0;
+    //printf("robotvec[0] : %f \n", v->a[0] ); 
+  }
+}
+
+/* Is called when robotvec is received from orca/robot. */
+void *nikon_reader(void *arg) {
+
+  orca_client_channel_t* signalsToRobot = (orca_client_channel_t*) arg;
+  int ok;
+  int  nbrofdata,j;
+  k600data_nikon store_k600vec;
+
+ for (j = 0 ; j < 12 ; j++) {
+   store_k600vec.a[j].x=(double)1.10101;
+   store_k600vec.a[j].y=(double)1.20101;
+   store_k600vec.a[j].z=(double)1.30101;	 
+   store_k600vec.a[j].roll  =(double)1.40101;
+   store_k600vec.a[j].pitch =(double)1.50101;
+   store_k600vec.a[j].yaw   =(double)1.60101;
+   store_k600vec.a[j].status=100*j+j;
+ }
+ 
+int n_counter = 0;
+
+  while(1){
+
+    ok=doRead(sock, buf, BUFSIZE);
+    if (ok){
+      nbrofdata=*((int*)(buf));
+      //printf("nbr_of_data %d \n", nbrofdata );
+
+      //printf("time_stamp %d \n", *((int*)(buf)) );
+      //pthread_mutex_lock(&mutex);
+      
+      j=0  ;  store_k600vec.a[0].x     = *((double*)(buf+4+8*(j)));
+      j=1  ;  store_k600vec.a[0].y     = *((double*)(buf+4+8*(j)));	    
+      j=2  ;  store_k600vec.a[0].z     = *((double*)(buf+4+8*(j)));
+      j=3  ;  store_k600vec.a[0].status= *((int*)(buf+4+8*(j)));
+      
+      j=4  ;  store_k600vec.a[1].x     = *((double*)(buf+4+8*(j)));
+      j=5  ;  store_k600vec.a[1].y     = *((double*)(buf+4+8*(j)));	    
+      j=6  ;  store_k600vec.a[1].z     = *((double*)(buf+4+8*(j)));
+      j=7  ;  store_k600vec.a[1].status= *((int*)(buf+4+8*(j)));
+      
+      j=8  ;  store_k600vec.a[2].x     = *((double*)(buf+4+8*(j)));
+      j=9  ;  store_k600vec.a[2].y     = *((double*)(buf+4+8*(j)));	    
+      j=10 ;  store_k600vec.a[2].z     = *((double*)(buf+4+8*(j)));
+      j=11 ;  store_k600vec.a[2].status= *((int*)(buf+4+8*(j)));
+      
+      
+      
+      
+      
+      
+      if(n_counter++ > 1000){
+        n_counter = 0;
+        printf("nbr_of_data %d \n", nbrofdata );
+        printf("store_k600vec.a[0].x %f \n",  store_k600vec.a[0].x);
+        printf("store_k600vec.a[0].y %f \n",  store_k600vec.a[0].y);
+        printf("store_k600vec.a[0].z %f \n",  store_k600vec.a[0].z);
+        printf("store_k600vec.a[0].roll %f \n",  store_k600vec.a[0].roll);
+        printf("store_k600vec.a[0].pitch %f \n",  store_k600vec.a[0].pitch);
+        printf("store_k600vec.a[0].yaw %f \n",  store_k600vec.a[0].yaw);
+        printf("store_k600vec.a[0].status %d \n",  store_k600vec.a[0].status);
+        
+        printf("store_k600vec.a[5].x %f \n",  store_k600vec.a[5].x);
+        printf("store_k600vec.a[5].y %f \n",  store_k600vec.a[5].y);	 
+        printf("store_k600vec.a[5].z %f \n",  store_k600vec.a[5].z);
+        printf("store_k600vec.a[5].status %d \n",  store_k600vec.a[5].status);
+        
+        }
+      
+      
+	labcomm_encode_k600data_nikon(signalsToRobot->encoder, &store_k600vec);
+	//}
+      //   pthread_mutex_unlock(&mutex);
+    }
+  }
+  return(0); 
+}
+
+//-------------------------------------------------------------
+
+int main(int argc, char *argv[])
+{ 
+
+  struct orca_client *orca;
+  orca_client_channel_t *signals2RobNikon; 
+  orca_client_channel_t *signals2RobMyvec; 
+  orca_client_channel_t *signalsFrRob;
+  pthread_t thread, thread_nikon;
+  int iret1,iret2;
+ 
+  struct sockaddr_in server;
+  char *remote= K600SERVER;
+
+  struct timespec delay;	
+  delay.tv_sec = 0; 
+  delay.tv_nsec = 4000000; //4 ms 
+
+  
+  if (argc<2){
+    printf("Example: \n");
+    printf("./k600_driver  ip_or_name_of_your_extctrl_computer 2000 \n");
+  }
+  else{  
+    printf("Start k600_driver\n");
+    orca = orca_client_new_tcp(argv[1], atoi(argv[2]));
+    printf("BB\n");
+    if (!orca) {
+      printf("Connection to %s:%d failed\n", argv[1], atoi(argv[1]));
+      return 1;
+    }
+
+    {
+	sock = initSocket(&server, remote);	
+	/* connect to remote server	*/
+	if (connect(sock, (struct sockaddr*)(&server), sizeof(server)) < 0 ) {
+		perror("connecting stream socket");
+		exit(1);
+	}
+     
+    }
+
+    {
+      orca_client_selection_t selection;
+      char *channel[1];
+
+      channel[0] = "nikon";  // see lc-file
+      selection.n_0 = 1;
+      selection.a = channel;
+      signals2RobNikon = orca_client_select_input_tcp(orca, &selection);
+      labcomm_encoder_register_k600data_nikon(signals2RobNikon->encoder);
+    }
+    {
+      orca_client_selection_t selection;
+      char *channel[1];
+
+      channel[0] = "myvec";  // see lc-file
+      selection.n_0 = 1;
+      selection.a = channel;
+      signals2RobMyvec = orca_client_select_input_tcp(orca, &selection);
+      labcomm_encoder_register_k600data_myvec(signals2RobMyvec->encoder);
+    }
+
+    {
+      orca_client_selection_t selectionFrRob;
+      char *channelFrRob= "robotvec";
+      char **channel_pFrRob = &channelFrRob;
+      selectionFrRob.n_0 = 1;
+      selectionFrRob.a = channel_pFrRob;
+      signalsFrRob = orca_client_select_output_tcp(orca, &selectionFrRob);
+      labcomm_decoder_register_k600data_robotvec(signalsFrRob->decoder,handle_robotvec,0);
+    }
+    //------------------------------------------------------------------------
+
+    // Create an independent thread that deals with receiving from the robot
+    iret1 = pthread_create(&thread, NULL, handler_thread, (void*) signalsFrRob);
+    if (iret1){
+      printf("Could not create handler_thread.\n");
+      exit(1);
+    }
+    iret2 = pthread_create(&thread_nikon, NULL, nikon_reader, (void*) signals2RobNikon);
+    if (iret2){
+      printf("Could not create nikon_thread.\n");
+      exit(1);
+    }
+
+    //------------------------------------------------------------------------
+    if (1) {
+
+      int  j;
+      k600data_myvec vec13;
+
+
+      printf("CC\n");
+
+      while (1) {
+
+	//        printf("\r\nAngle: ");
+	//        scanf("%f", &angle);
+        //        vec13.a[1] = angle; 
+
+	for (j = 0 ; j < 13 ; j++) {
+ 	  vec13.a[j] = (float)j+1.00012345;
+	}
+
+// __// 	pthread_mutex_lock(&mutex);
+// __// 	pthread_mutex_unlock(&mutex);
+
+
+	labcomm_encode_k600data_myvec(signals2RobMyvec->encoder, &vec13);
+
+#if 0
+	{
+	  int my_counter2 = 0;
+
+	  if(my_counter2++ > 100){
+	    my_counter2=0;
+	    printf("qwerty   %f [ms]\n",  vec13.a[1]);
+	  }	
+	}
+#endif 
+	  nanosleep(&delay, 0);
+      }
+    }
+  }
+  return(0);
+}
+  
diff --git a/cpp/k600data.lc b/cpp/k600data.lc
new file mode 100644
index 0000000000000000000000000000000000000000..20a1811a4025fb291aac9727ed2605213992dc88
--- /dev/null
+++ b/cpp/k600data.lc
@@ -0,0 +1,13 @@
+sample float myvec[13];
+sample float robotvec[20];
+
+typedef struct {
+double x;
+double y;
+double z;
+double roll;
+double pitch;
+double yaw;
+int status;
+} k600_package;
+sample k600_package nikon[12];
diff --git a/cpp/main.cc b/cpp/main.cc
new file mode 100644
index 0000000000000000000000000000000000000000..eaef22f4a3efdeb80e4755a5e783e43482d4f4da
--- /dev/null
+++ b/cpp/main.cc
@@ -0,0 +1,294 @@
+#include "dmp.h"
+#include <time.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include "orca_client.h"
+#include <pthread.h>
+#include "coupling.h"
+
+
+#include <sys/select.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <unistd.h>
+
+#include <sys/time.h>
+#include <vector>
+
+#include <netdb.h>
+#include <string.h>
+#include <sys/times.h>
+#include <iostream>
+
+
+#include <unistd.h>
+#include <mutex> 
+#include <thread> 
+#include <time.h>
+#include <armadillo>
+#include "armmoving.h"
+#include "record_ya.h"
+#include "state2yardd.h"
+#include "write_norms_to_file.h"
+
+
+
+
+using namespace std;
+
+mutex mtx;
+// Some global variables. Sry. To be changed. But perhaps the dmpVector should be kept global.
+
+vector<Dmp> dmpVec;
+mat yardd(1,dof);
+mat yclog(1,dof);
+mat ycddlog(1,dof);
+
+// Log convergence states
+mat ya_minus_yc(1,dof);
+mat yad_minus_ycd(1,dof);
+double e_norm_log;
+double xlog;
+mat yc_minus_g(1,dof);
+mat zlog(1,dof);
+
+
+
+
+
+//Global constants:
+const double dt = 0.004;
+const int takeEvery = 50;
+const double sparse_dt = takeEvery*dt;
+
+
+
+// Some global variable settings. Sorry about that. Will be changed.
+int DMP_NBR = 0;
+int RECORD_DMP = 0;
+int PLAY_DMP = 0;
+
+
+
+/* This function which continously receives data from irc5
+ * must be started from a thread because the function will 
+ * not end until the connection to irc5 is shut down. */
+
+void* handler_thread(void *arg) {
+  orca_client_channel_t* signalsFromRobot = (orca_client_channel_t*) arg;
+  labcomm_decoder_run(signalsFromRobot->decoder);
+  return(0);
+}
+
+
+
+//------------------------------------------------------------------
+/* Is called when trajectory is received from orca/robot. */
+void handle_trajectory(coupling_yaFromRobot* v, void* context) {
+
+	
+	mat ya(1,dof);
+	for (int i = 0; i < dof; ++i) {
+		ya(i) = v->a[i];
+	}
+	mat yad(1,dof);
+	for (int i = dof; i < 2*dof; ++i) {
+		yad(i-dof) = v->a[i];
+	}
+	
+	
+
+	
+	mtx.lock();
+		record_ya(ya, dmpVec, dt, sparse_dt, takeEvery, RECORD_DMP, PLAY_DMP);
+	mtx.unlock();	
+	
+	mtx.lock();
+		state2yardd(ya,yad,yardd, PLAY_DMP, DMP_NBR, dmpVec, yclog, ycddlog, ya_minus_yc, yad_minus_ycd, e_norm_log, xlog, yc_minus_g, zlog);
+	mtx.unlock();
+	
+	mtx.lock();
+		write_norms_to_file(ya_minus_yc, yad_minus_ycd, e_norm_log, xlog, yc_minus_g, zlog, PLAY_DMP);
+	mtx.unlock();
+	
+
+	
+}
+//-------------------------------------------------------------
+
+void runMenu() {
+	
+	cout << "OPTIONS: " << endl;
+	cout << "Record demonstrations on/off - type r" << endl;
+	cout << "Play next dmp - type p" << endl;
+	cout << "Stop playing DMP - type s" << endl;
+	cout << "Choose DMP to play - type a number" << endl;
+	
+	cout << "Choose an option:     ";
+	char option;
+	while (cin >> option) {
+		if (option == 'p') { //play next dmp
+			int dmpNbr = DMP_NBR+1;
+			if (dmpNbr > 0 && dmpNbr < dmpVec.size()+1) {
+				DMP_NBR = dmpNbr;
+				PLAY_DMP = 1;
+				RECORD_DMP = 0;
+			} else {
+				cout << "Sorry, no such dmp available" << endl;
+			}
+		} else if (option == 'r') { //change value of RECORD_DMP
+			mtx.lock();
+			RECORD_DMP = !RECORD_DMP;
+			if (RECORD_DMP) {
+				cout << "Recording" << endl;
+			} else {
+				cout << "Not recording" << endl;
+			}
+			mtx.unlock();
+		} else if (option == 's') { //stop playing dmp
+			mtx.lock();
+			RECORD_DMP = 0;
+			PLAY_DMP = 0;
+			mtx.unlock();
+		} else if (option -48 >= 0 && option - 48 < 10) { // This is ugly and should be changed.
+			int dmpNbr = option-48;
+			mtx.lock();
+			if (dmpNbr > 0 && dmpNbr < dmpVec.size()+1) {
+				DMP_NBR = dmpNbr;
+				PLAY_DMP = 1;
+				RECORD_DMP = 0;
+			} else {
+				cout << "Sorry, no such dmp available" << endl;
+			}
+			mtx.unlock();
+		}
+		
+		
+		
+		cout << endl;
+		usleep(100000);
+		mtx.lock();
+		if (dmpVec.size() == 0) {
+			cout << "We have 0 dmp:s" << endl;
+		} else {
+			cout << "We have 1..." << dmpVec.size() << " dmp:s" << endl;
+		}
+		mtx.unlock();
+	}
+}
+
+
+//-------------------------------------------------------------
+
+int main(int argc, char *argv[]) { 
+  struct orca_client *orca;
+  orca_client_channel_t *signalsFromRobotYa;
+  orca_client_channel_t *signalsFromRobotYad;
+  orca_client_channel_t *signals2RobYardd;
+  orca_client_channel_t *signals2RobOpts;
+
+  pthread_t thread_robot;
+  int iret1;
+  
+  if (argc<2){
+    printf("Example: \n");
+    printf("./fit_dmp_labcomm_driver  ip_or_name_of_your_extctrl_computer 2000 \n");
+  }
+  else{  
+    printf("Start fit_dmp_labcomm_driver\n");
+    orca = orca_client_new_tcp(argv[1], atoi(argv[2]));
+    printf("BB\n");
+  }
+    if (!orca) {
+      printf("Connection to %s:%d failed\n", argv[1], atoi(argv[1]));
+      return 1;
+    }
+
+
+
+  // Register to robot:
+    {
+      orca_client_selection_t selection;
+      char *channel[1];
+
+      channel[0] = "yarddToRobot";  // see lc-file
+      selection.n_0 = 1;
+      selection.a = channel;
+      signals2RobYardd = orca_client_select_input_tcp(orca, &selection);
+      labcomm_encoder_register_coupling_yarddToRobot(signals2RobYardd->encoder);
+    }
+    {
+      orca_client_selection_t selection;
+      char *channel[1];
+
+      channel[0] = "optsToRobot";  // see lc-file
+      selection.n_0 = 1;
+      selection.a = channel;
+      signals2RobOpts = orca_client_select_input_tcp(orca, &selection);
+      labcomm_encoder_register_coupling_optsToRobot(signals2RobOpts->encoder);
+    }
+    
+	// Register from robot:
+    {
+      orca_client_selection_t selectionFrRob;
+      char *channelFrRob= "yaFromRobot";
+      char **channel_pFrRob = &channelFrRob;
+      selectionFrRob.n_0 = 1;
+      selectionFrRob.a = channel_pFrRob;
+      signalsFromRobotYa = orca_client_select_output_tcp(orca, &selectionFrRob);
+      labcomm_decoder_register_coupling_yaFromRobot(signalsFromRobotYa->decoder,handle_trajectory,0);
+    }
+  
+    // Create an independent thread that deals with receiving from the robot
+    iret1 = pthread_create(&thread_robot, NULL, handler_thread, (void*) signalsFromRobotYa);
+    if (iret1){
+      printf("Could not create handler_thread.\n");
+      exit(1);
+    }
+
+    coupling_optsToRobot opts_store;
+    coupling_yarddToRobot yardd_store;
+    thread menuThread(runMenu);
+    menuThread.detach();
+
+    //timeval tv;
+
+    while(1){
+		//gettimeofday(&tv, 0);
+		//int start = tv.tv_usec;
+        
+        usleep(4000);
+		
+        
+        
+		mtx.lock();
+		for (int i = 0 ; i < dof ; i++) {
+			yardd_store.a[i] = yardd(i);
+		}
+		for (int i = 0 ; i < 100 ; i++) {
+			opts_store.a[i] = 0;
+		}
+		opts_store.a[0] = PLAY_DMP;
+		opts_store.a[99] = RECORD_DMP;
+		
+		labcomm_encode_coupling_yarddToRobot(signals2RobYardd->encoder, &yardd_store);
+		labcomm_encode_coupling_optsToRobot(signals2RobOpts->encoder, &opts_store);
+		mtx.unlock();
+		static int count = 1;
+		if (PLAY_DMP && count == 250) {
+			cout << yardd << endl;
+			count = 1;
+		}
+		if (PLAY_DMP) {
+			++count;
+		}
+		//gettimeofday(&tv, 0);
+		//cout << tv.tv_usec - start << endl;
+    }
+
+
+  printf("Shutting down\n");
+  return(0);
+} 
diff --git a/cpp/orca_messages.c b/cpp/orca_messages.c
new file mode 100644
index 0000000000000000000000000000000000000000..cdcc077ccbec4e3b343a5113fd43f27f6fb01c95
--- /dev/null
+++ b/cpp/orca_messages.c
@@ -0,0 +1,880 @@
+#include "labcomm.h"
+#include "labcomm_private.h"
+#include "orca_messages.h"
+
+static unsigned char signature_bytes_directory[] = {
+// struct { 4 fields
+0, 0, 0, 17, 
+  0, 0, 0, 4, 
+  // connection_list_t 'input'
+  0, 0, 0, 5, 
+  105, 110, 112, 117, 116, 
+  // array [_]
+  0, 0, 0, 16, 
+    0, 0, 0, 1, 
+    0, 0, 0, 0, 
+    // struct { 3 fields
+    0, 0, 0, 17, 
+      0, 0, 0, 3, 
+      // int 'index'
+      0, 0, 0, 5, 
+      105, 110, 100, 101, 120, 
+      0, 0, 0, 35, 
+      // string 'name'
+      0, 0, 0, 4, 
+      110, 97, 109, 101, 
+      0, 0, 0, 39, 
+      // array [_] 'signature'
+      0, 0, 0, 9, 
+      115, 105, 103, 110, 97, 116, 117, 114, 101, 
+      // array [_]
+      0, 0, 0, 16, 
+        0, 0, 0, 1, 
+        0, 0, 0, 0, 
+        0, 0, 0, 33, 
+      // }
+    // }
+  // }
+  // connection_list_t 'output'
+  0, 0, 0, 6, 
+  111, 117, 116, 112, 117, 116, 
+  // array [_]
+  0, 0, 0, 16, 
+    0, 0, 0, 1, 
+    0, 0, 0, 0, 
+    // struct { 3 fields
+    0, 0, 0, 17, 
+      0, 0, 0, 3, 
+      // int 'index'
+      0, 0, 0, 5, 
+      105, 110, 100, 101, 120, 
+      0, 0, 0, 35, 
+      // string 'name'
+      0, 0, 0, 4, 
+      110, 97, 109, 101, 
+      0, 0, 0, 39, 
+      // array [_] 'signature'
+      0, 0, 0, 9, 
+      115, 105, 103, 110, 97, 116, 117, 114, 101, 
+      // array [_]
+      0, 0, 0, 16, 
+        0, 0, 0, 1, 
+        0, 0, 0, 0, 
+        0, 0, 0, 33, 
+      // }
+    // }
+  // }
+  // connection_list_t 'parameter'
+  0, 0, 0, 9, 
+  112, 97, 114, 97, 109, 101, 116, 101, 114, 
+  // array [_]
+  0, 0, 0, 16, 
+    0, 0, 0, 1, 
+    0, 0, 0, 0, 
+    // struct { 3 fields
+    0, 0, 0, 17, 
+      0, 0, 0, 3, 
+      // int 'index'
+      0, 0, 0, 5, 
+      105, 110, 100, 101, 120, 
+      0, 0, 0, 35, 
+      // string 'name'
+      0, 0, 0, 4, 
+      110, 97, 109, 101, 
+      0, 0, 0, 39, 
+      // array [_] 'signature'
+      0, 0, 0, 9, 
+      115, 105, 103, 110, 97, 116, 117, 114, 101, 
+      // array [_]
+      0, 0, 0, 16, 
+        0, 0, 0, 1, 
+        0, 0, 0, 0, 
+        0, 0, 0, 33, 
+      // }
+    // }
+  // }
+  // connection_list_t 'log'
+  0, 0, 0, 3, 
+  108, 111, 103, 
+  // array [_]
+  0, 0, 0, 16, 
+    0, 0, 0, 1, 
+    0, 0, 0, 0, 
+    // struct { 3 fields
+    0, 0, 0, 17, 
+      0, 0, 0, 3, 
+      // int 'index'
+      0, 0, 0, 5, 
+      105, 110, 100, 101, 120, 
+      0, 0, 0, 35, 
+      // string 'name'
+      0, 0, 0, 4, 
+      110, 97, 109, 101, 
+      0, 0, 0, 39, 
+      // array [_] 'signature'
+      0, 0, 0, 9, 
+      115, 105, 103, 110, 97, 116, 117, 114, 101, 
+      // array [_]
+      0, 0, 0, 16, 
+        0, 0, 0, 1, 
+        0, 0, 0, 0, 
+        0, 0, 0, 33, 
+      // }
+    // }
+  // }
+// }
+};
+labcomm_signature_t labcomm_signature_orca_messages_directory = {
+  LABCOMM_SAMPLE, "directory",
+  (int (*)(void *))labcomm_sizeof_orca_messages_directory,
+  sizeof(signature_bytes_directory),
+  signature_bytes_directory
+ };
+static unsigned char signature_bytes_select_input[] = {
+// struct { 3 fields
+0, 0, 0, 17, 
+  0, 0, 0, 3, 
+  // int 'port'
+  0, 0, 0, 4, 
+  112, 111, 114, 116, 
+  0, 0, 0, 35, 
+  // int 'decimation'
+  0, 0, 0, 10, 
+  100, 101, 99, 105, 109, 97, 116, 105, 111, 110, 
+  0, 0, 0, 35, 
+  // array [_] 'list'
+  0, 0, 0, 4, 
+  108, 105, 115, 116, 
+  // array [_]
+  0, 0, 0, 16, 
+    0, 0, 0, 1, 
+    0, 0, 0, 0, 
+    0, 0, 0, 35, 
+  // }
+// }
+};
+labcomm_signature_t labcomm_signature_orca_messages_select_input = {
+  LABCOMM_SAMPLE, "select_input",
+  (int (*)(void *))labcomm_sizeof_orca_messages_select_input,
+  sizeof(signature_bytes_select_input),
+  signature_bytes_select_input
+ };
+static unsigned char signature_bytes_select_output[] = {
+// struct { 3 fields
+0, 0, 0, 17, 
+  0, 0, 0, 3, 
+  // int 'port'
+  0, 0, 0, 4, 
+  112, 111, 114, 116, 
+  0, 0, 0, 35, 
+  // int 'decimation'
+  0, 0, 0, 10, 
+  100, 101, 99, 105, 109, 97, 116, 105, 111, 110, 
+  0, 0, 0, 35, 
+  // array [_] 'list'
+  0, 0, 0, 4, 
+  108, 105, 115, 116, 
+  // array [_]
+  0, 0, 0, 16, 
+    0, 0, 0, 1, 
+    0, 0, 0, 0, 
+    0, 0, 0, 35, 
+  // }
+// }
+};
+labcomm_signature_t labcomm_signature_orca_messages_select_output = {
+  LABCOMM_SAMPLE, "select_output",
+  (int (*)(void *))labcomm_sizeof_orca_messages_select_output,
+  sizeof(signature_bytes_select_output),
+  signature_bytes_select_output
+ };
+static unsigned char signature_bytes_select_parameter[] = {
+// struct { 3 fields
+0, 0, 0, 17, 
+  0, 0, 0, 3, 
+  // int 'port'
+  0, 0, 0, 4, 
+  112, 111, 114, 116, 
+  0, 0, 0, 35, 
+  // int 'decimation'
+  0, 0, 0, 10, 
+  100, 101, 99, 105, 109, 97, 116, 105, 111, 110, 
+  0, 0, 0, 35, 
+  // array [_] 'list'
+  0, 0, 0, 4, 
+  108, 105, 115, 116, 
+  // array [_]
+  0, 0, 0, 16, 
+    0, 0, 0, 1, 
+    0, 0, 0, 0, 
+    0, 0, 0, 35, 
+  // }
+// }
+};
+labcomm_signature_t labcomm_signature_orca_messages_select_parameter = {
+  LABCOMM_SAMPLE, "select_parameter",
+  (int (*)(void *))labcomm_sizeof_orca_messages_select_parameter,
+  sizeof(signature_bytes_select_parameter),
+  signature_bytes_select_parameter
+ };
+static unsigned char signature_bytes_select_log[] = {
+// struct { 3 fields
+0, 0, 0, 17, 
+  0, 0, 0, 3, 
+  // int 'port'
+  0, 0, 0, 4, 
+  112, 111, 114, 116, 
+  0, 0, 0, 35, 
+  // int 'decimation'
+  0, 0, 0, 10, 
+  100, 101, 99, 105, 109, 97, 116, 105, 111, 110, 
+  0, 0, 0, 35, 
+  // array [_] 'list'
+  0, 0, 0, 4, 
+  108, 105, 115, 116, 
+  // array [_]
+  0, 0, 0, 16, 
+    0, 0, 0, 1, 
+    0, 0, 0, 0, 
+    0, 0, 0, 35, 
+  // }
+// }
+};
+labcomm_signature_t labcomm_signature_orca_messages_select_log = {
+  LABCOMM_SAMPLE, "select_log",
+  (int (*)(void *))labcomm_sizeof_orca_messages_select_log,
+  sizeof(signature_bytes_select_log),
+  signature_bytes_select_log
+ };
+static void decode_directory(
+  labcomm_decoder_t *d,
+  void (*handle)(
+    orca_messages_directory *v,
+    void *context
+  ),
+  void *context
+)
+{
+  orca_messages_directory v;
+  v.input.n_0 = labcomm_decode_int(d);
+  v.input.a = malloc(sizeof(v.input.a[0]) * v.input.n_0);
+  {
+    int i_0_0;
+    for (i_0_0 = 0 ; i_0_0 < v.input.n_0 ; i_0_0++) {
+      int i_0 = i_0_0;
+      v.input.a[i_0].index = labcomm_decode_int(d);
+      v.input.a[i_0].name = labcomm_decode_string(d);
+      v.input.a[i_0].signature.n_0 = labcomm_decode_int(d);
+      v.input.a[i_0].signature.a = malloc(sizeof(v.input.a[i_0].signature.a[0]) * v.input.a[i_0].signature.n_0);
+      {
+        int i_1_0;
+        for (i_1_0 = 0 ; i_1_0 < v.input.a[i_0].signature.n_0 ; i_1_0++) {
+          int i_1 = i_1_0;
+          v.input.a[i_0].signature.a[i_1] = labcomm_decode_byte(d);
+        }
+      }
+    }
+  }
+  v.output.n_0 = labcomm_decode_int(d);
+  v.output.a = malloc(sizeof(v.output.a[0]) * v.output.n_0);
+  {
+    int i_0_0;
+    for (i_0_0 = 0 ; i_0_0 < v.output.n_0 ; i_0_0++) {
+      int i_0 = i_0_0;
+      v.output.a[i_0].index = labcomm_decode_int(d);
+      v.output.a[i_0].name = labcomm_decode_string(d);
+      v.output.a[i_0].signature.n_0 = labcomm_decode_int(d);
+      v.output.a[i_0].signature.a = malloc(sizeof(v.output.a[i_0].signature.a[0]) * v.output.a[i_0].signature.n_0);
+      {
+        int i_1_0;
+        for (i_1_0 = 0 ; i_1_0 < v.output.a[i_0].signature.n_0 ; i_1_0++) {
+          int i_1 = i_1_0;
+          v.output.a[i_0].signature.a[i_1] = labcomm_decode_byte(d);
+        }
+      }
+    }
+  }
+  v.parameter.n_0 = labcomm_decode_int(d);
+  v.parameter.a = malloc(sizeof(v.parameter.a[0]) * v.parameter.n_0);
+  {
+    int i_0_0;
+    for (i_0_0 = 0 ; i_0_0 < v.parameter.n_0 ; i_0_0++) {
+      int i_0 = i_0_0;
+      v.parameter.a[i_0].index = labcomm_decode_int(d);
+      v.parameter.a[i_0].name = labcomm_decode_string(d);
+      v.parameter.a[i_0].signature.n_0 = labcomm_decode_int(d);
+      v.parameter.a[i_0].signature.a = malloc(sizeof(v.parameter.a[i_0].signature.a[0]) * v.parameter.a[i_0].signature.n_0);
+      {
+        int i_1_0;
+        for (i_1_0 = 0 ; i_1_0 < v.parameter.a[i_0].signature.n_0 ; i_1_0++) {
+          int i_1 = i_1_0;
+          v.parameter.a[i_0].signature.a[i_1] = labcomm_decode_byte(d);
+        }
+      }
+    }
+  }
+  v.log.n_0 = labcomm_decode_int(d);
+  v.log.a = malloc(sizeof(v.log.a[0]) * v.log.n_0);
+  {
+    int i_0_0;
+    for (i_0_0 = 0 ; i_0_0 < v.log.n_0 ; i_0_0++) {
+      int i_0 = i_0_0;
+      v.log.a[i_0].index = labcomm_decode_int(d);
+      v.log.a[i_0].name = labcomm_decode_string(d);
+      v.log.a[i_0].signature.n_0 = labcomm_decode_int(d);
+      v.log.a[i_0].signature.a = malloc(sizeof(v.log.a[i_0].signature.a[0]) * v.log.a[i_0].signature.n_0);
+      {
+        int i_1_0;
+        for (i_1_0 = 0 ; i_1_0 < v.log.a[i_0].signature.n_0 ; i_1_0++) {
+          int i_1 = i_1_0;
+          v.log.a[i_0].signature.a[i_1] = labcomm_decode_byte(d);
+        }
+      }
+    }
+  }
+  handle(&v, context);
+  {
+    {
+      int i_0_0;
+      for (i_0_0 = 0 ; i_0_0 < v.input.n_0 ; i_0_0++) {
+        int i_0 = i_0_0;
+        free(v.input.a[i_0].name);
+        free(v.input.a[i_0].signature.a);
+      }
+    }
+    free(v.input.a);
+    {
+      int i_0_0;
+      for (i_0_0 = 0 ; i_0_0 < v.output.n_0 ; i_0_0++) {
+        int i_0 = i_0_0;
+        free(v.output.a[i_0].name);
+        free(v.output.a[i_0].signature.a);
+      }
+    }
+    free(v.output.a);
+    {
+      int i_0_0;
+      for (i_0_0 = 0 ; i_0_0 < v.parameter.n_0 ; i_0_0++) {
+        int i_0 = i_0_0;
+        free(v.parameter.a[i_0].name);
+        free(v.parameter.a[i_0].signature.a);
+      }
+    }
+    free(v.parameter.a);
+    {
+      int i_0_0;
+      for (i_0_0 = 0 ; i_0_0 < v.log.n_0 ; i_0_0++) {
+        int i_0 = i_0_0;
+        free(v.log.a[i_0].name);
+        free(v.log.a[i_0].signature.a);
+      }
+    }
+    free(v.log.a);
+  }
+}
+void labcomm_decoder_register_orca_messages_directory(
+  struct labcomm_decoder *d,
+  void (*handler)(
+    orca_messages_directory *v,
+    void *context
+  ),
+  void *context
+)
+{
+  labcomm_internal_decoder_register(
+    d,
+    &labcomm_signature_orca_messages_directory,
+    (labcomm_decoder_typecast_t)decode_directory,
+    (labcomm_handler_typecast_t)handler,
+    context
+  );
+}
+static void encode_directory(
+  labcomm_encoder_t *e,
+  orca_messages_directory *v
+)
+{
+  e->writer.write(&e->writer, labcomm_writer_start);
+  labcomm_encode_type_index(e, &labcomm_signature_orca_messages_directory);
+  {
+    labcomm_encode_int(e, (*v).input.n_0);
+    {
+      int i_0_0;
+      for (i_0_0 = 0 ; i_0_0 < (*v).input.n_0 ; i_0_0++) {
+        int i_0 = i_0_0;
+        labcomm_encode_int(e, (*v).input.a[i_0].index);
+        labcomm_encode_string(e, (*v).input.a[i_0].name);
+        labcomm_encode_int(e, (*v).input.a[i_0].signature.n_0);
+        {
+          int i_1_0;
+          for (i_1_0 = 0 ; i_1_0 < (*v).input.a[i_0].signature.n_0 ; i_1_0++) {
+            int i_1 = i_1_0;
+            labcomm_encode_byte(e, (*v).input.a[i_0].signature.a[i_1]);
+          }
+        }
+      }
+    }
+    labcomm_encode_int(e, (*v).output.n_0);
+    {
+      int i_0_0;
+      for (i_0_0 = 0 ; i_0_0 < (*v).output.n_0 ; i_0_0++) {
+        int i_0 = i_0_0;
+        labcomm_encode_int(e, (*v).output.a[i_0].index);
+        labcomm_encode_string(e, (*v).output.a[i_0].name);
+        labcomm_encode_int(e, (*v).output.a[i_0].signature.n_0);
+        {
+          int i_1_0;
+          for (i_1_0 = 0 ; i_1_0 < (*v).output.a[i_0].signature.n_0 ; i_1_0++) {
+            int i_1 = i_1_0;
+            labcomm_encode_byte(e, (*v).output.a[i_0].signature.a[i_1]);
+          }
+        }
+      }
+    }
+    labcomm_encode_int(e, (*v).parameter.n_0);
+    {
+      int i_0_0;
+      for (i_0_0 = 0 ; i_0_0 < (*v).parameter.n_0 ; i_0_0++) {
+        int i_0 = i_0_0;
+        labcomm_encode_int(e, (*v).parameter.a[i_0].index);
+        labcomm_encode_string(e, (*v).parameter.a[i_0].name);
+        labcomm_encode_int(e, (*v).parameter.a[i_0].signature.n_0);
+        {
+          int i_1_0;
+          for (i_1_0 = 0 ; i_1_0 < (*v).parameter.a[i_0].signature.n_0 ; i_1_0++) {
+            int i_1 = i_1_0;
+            labcomm_encode_byte(e, (*v).parameter.a[i_0].signature.a[i_1]);
+          }
+        }
+      }
+    }
+    labcomm_encode_int(e, (*v).log.n_0);
+    {
+      int i_0_0;
+      for (i_0_0 = 0 ; i_0_0 < (*v).log.n_0 ; i_0_0++) {
+        int i_0 = i_0_0;
+        labcomm_encode_int(e, (*v).log.a[i_0].index);
+        labcomm_encode_string(e, (*v).log.a[i_0].name);
+        labcomm_encode_int(e, (*v).log.a[i_0].signature.n_0);
+        {
+          int i_1_0;
+          for (i_1_0 = 0 ; i_1_0 < (*v).log.a[i_0].signature.n_0 ; i_1_0++) {
+            int i_1 = i_1_0;
+            labcomm_encode_byte(e, (*v).log.a[i_0].signature.a[i_1]);
+          }
+        }
+      }
+    }
+  }
+  e->writer.write(&e->writer, labcomm_writer_end);
+}
+void labcomm_encode_orca_messages_directory(
+labcomm_encoder_t *e,
+orca_messages_directory *v
+)
+{
+labcomm_internal_encode(e, &labcomm_signature_orca_messages_directory, v);
+}
+void labcomm_encoder_register_orca_messages_directory(
+  struct labcomm_encoder *e
+)
+{
+  labcomm_internal_encoder_register(
+    e,
+    &labcomm_signature_orca_messages_directory,
+    (labcomm_encode_typecast_t)encode_directory
+  );
+}
+int labcomm_sizeof_orca_messages_directory(orca_messages_directory *v)
+{
+  int result = 4;
+  {
+    int i_0_0;
+    for (i_0_0 = 0 ; i_0_0 < (*v).input.n_0 ; i_0_0++) {
+      int i_0 = i_0_0;
+      result += 4 + strlen((*v).input.a[i_0].name);
+      result += 1 * (*v).input.a[i_0].signature.n_0;
+      result += 4;
+    }
+  }
+  {
+    int i_0_0;
+    for (i_0_0 = 0 ; i_0_0 < (*v).output.n_0 ; i_0_0++) {
+      int i_0 = i_0_0;
+      result += 4 + strlen((*v).output.a[i_0].name);
+      result += 1 * (*v).output.a[i_0].signature.n_0;
+      result += 4;
+    }
+  }
+  {
+    int i_0_0;
+    for (i_0_0 = 0 ; i_0_0 < (*v).parameter.n_0 ; i_0_0++) {
+      int i_0 = i_0_0;
+      result += 4 + strlen((*v).parameter.a[i_0].name);
+      result += 1 * (*v).parameter.a[i_0].signature.n_0;
+      result += 4;
+    }
+  }
+  {
+    int i_0_0;
+    for (i_0_0 = 0 ; i_0_0 < (*v).log.n_0 ; i_0_0++) {
+      int i_0 = i_0_0;
+      result += 4 + strlen((*v).log.a[i_0].name);
+      result += 1 * (*v).log.a[i_0].signature.n_0;
+      result += 4;
+    }
+  }
+  return result;
+}
+static void decode_select_input(
+  labcomm_decoder_t *d,
+  void (*handle)(
+    orca_messages_select_input *v,
+    void *context
+  ),
+  void *context
+)
+{
+  orca_messages_select_input v;
+  v.port = labcomm_decode_int(d);
+  v.decimation = labcomm_decode_int(d);
+  v.list.n_0 = labcomm_decode_int(d);
+  v.list.a = malloc(sizeof(v.list.a[0]) * v.list.n_0);
+  {
+    int i_0_0;
+    for (i_0_0 = 0 ; i_0_0 < v.list.n_0 ; i_0_0++) {
+      int i_0 = i_0_0;
+      v.list.a[i_0] = labcomm_decode_int(d);
+    }
+  }
+  handle(&v, context);
+  {
+    free(v.list.a);
+  }
+}
+void labcomm_decoder_register_orca_messages_select_input(
+  struct labcomm_decoder *d,
+  void (*handler)(
+    orca_messages_select_input *v,
+    void *context
+  ),
+  void *context
+)
+{
+  labcomm_internal_decoder_register(
+    d,
+    &labcomm_signature_orca_messages_select_input,
+    (labcomm_decoder_typecast_t)decode_select_input,
+    (labcomm_handler_typecast_t)handler,
+    context
+  );
+}
+static void encode_select_input(
+  labcomm_encoder_t *e,
+  orca_messages_select_input *v
+)
+{
+  e->writer.write(&e->writer, labcomm_writer_start);
+  labcomm_encode_type_index(e, &labcomm_signature_orca_messages_select_input);
+  {
+    labcomm_encode_int(e, (*v).port);
+    labcomm_encode_int(e, (*v).decimation);
+    labcomm_encode_int(e, (*v).list.n_0);
+    {
+      int i_0_0;
+      for (i_0_0 = 0 ; i_0_0 < (*v).list.n_0 ; i_0_0++) {
+        int i_0 = i_0_0;
+        labcomm_encode_int(e, (*v).list.a[i_0]);
+      }
+    }
+  }
+  e->writer.write(&e->writer, labcomm_writer_end);
+}
+void labcomm_encode_orca_messages_select_input(
+labcomm_encoder_t *e,
+orca_messages_select_input *v
+)
+{
+labcomm_internal_encode(e, &labcomm_signature_orca_messages_select_input, v);
+}
+void labcomm_encoder_register_orca_messages_select_input(
+  struct labcomm_encoder *e
+)
+{
+  labcomm_internal_encoder_register(
+    e,
+    &labcomm_signature_orca_messages_select_input,
+    (labcomm_encode_typecast_t)encode_select_input
+  );
+}
+int labcomm_sizeof_orca_messages_select_input(orca_messages_select_input *v)
+{
+  int result = 4;
+  result += 4 * (*v).list.n_0;
+  result += 8;
+  return result;
+}
+static void decode_select_output(
+  labcomm_decoder_t *d,
+  void (*handle)(
+    orca_messages_select_output *v,
+    void *context
+  ),
+  void *context
+)
+{
+  orca_messages_select_output v;
+  v.port = labcomm_decode_int(d);
+  v.decimation = labcomm_decode_int(d);
+  v.list.n_0 = labcomm_decode_int(d);
+  v.list.a = malloc(sizeof(v.list.a[0]) * v.list.n_0);
+  {
+    int i_0_0;
+    for (i_0_0 = 0 ; i_0_0 < v.list.n_0 ; i_0_0++) {
+      int i_0 = i_0_0;
+      v.list.a[i_0] = labcomm_decode_int(d);
+    }
+  }
+  handle(&v, context);
+  {
+    free(v.list.a);
+  }
+}
+void labcomm_decoder_register_orca_messages_select_output(
+  struct labcomm_decoder *d,
+  void (*handler)(
+    orca_messages_select_output *v,
+    void *context
+  ),
+  void *context
+)
+{
+  labcomm_internal_decoder_register(
+    d,
+    &labcomm_signature_orca_messages_select_output,
+    (labcomm_decoder_typecast_t)decode_select_output,
+    (labcomm_handler_typecast_t)handler,
+    context
+  );
+}
+static void encode_select_output(
+  labcomm_encoder_t *e,
+  orca_messages_select_output *v
+)
+{
+  e->writer.write(&e->writer, labcomm_writer_start);
+  labcomm_encode_type_index(e, &labcomm_signature_orca_messages_select_output);
+  {
+    labcomm_encode_int(e, (*v).port);
+    labcomm_encode_int(e, (*v).decimation);
+    labcomm_encode_int(e, (*v).list.n_0);
+    {
+      int i_0_0;
+      for (i_0_0 = 0 ; i_0_0 < (*v).list.n_0 ; i_0_0++) {
+        int i_0 = i_0_0;
+        labcomm_encode_int(e, (*v).list.a[i_0]);
+      }
+    }
+  }
+  e->writer.write(&e->writer, labcomm_writer_end);
+}
+void labcomm_encode_orca_messages_select_output(
+labcomm_encoder_t *e,
+orca_messages_select_output *v
+)
+{
+labcomm_internal_encode(e, &labcomm_signature_orca_messages_select_output, v);
+}
+void labcomm_encoder_register_orca_messages_select_output(
+  struct labcomm_encoder *e
+)
+{
+  labcomm_internal_encoder_register(
+    e,
+    &labcomm_signature_orca_messages_select_output,
+    (labcomm_encode_typecast_t)encode_select_output
+  );
+}
+int labcomm_sizeof_orca_messages_select_output(orca_messages_select_output *v)
+{
+  int result = 4;
+  result += 4 * (*v).list.n_0;
+  result += 8;
+  return result;
+}
+static void decode_select_parameter(
+  labcomm_decoder_t *d,
+  void (*handle)(
+    orca_messages_select_parameter *v,
+    void *context
+  ),
+  void *context
+)
+{
+  orca_messages_select_parameter v;
+  v.port = labcomm_decode_int(d);
+  v.decimation = labcomm_decode_int(d);
+  v.list.n_0 = labcomm_decode_int(d);
+  v.list.a = malloc(sizeof(v.list.a[0]) * v.list.n_0);
+  {
+    int i_0_0;
+    for (i_0_0 = 0 ; i_0_0 < v.list.n_0 ; i_0_0++) {
+      int i_0 = i_0_0;
+      v.list.a[i_0] = labcomm_decode_int(d);
+    }
+  }
+  handle(&v, context);
+  {
+    free(v.list.a);
+  }
+}
+void labcomm_decoder_register_orca_messages_select_parameter(
+  struct labcomm_decoder *d,
+  void (*handler)(
+    orca_messages_select_parameter *v,
+    void *context
+  ),
+  void *context
+)
+{
+  labcomm_internal_decoder_register(
+    d,
+    &labcomm_signature_orca_messages_select_parameter,
+    (labcomm_decoder_typecast_t)decode_select_parameter,
+    (labcomm_handler_typecast_t)handler,
+    context
+  );
+}
+static void encode_select_parameter(
+  labcomm_encoder_t *e,
+  orca_messages_select_parameter *v
+)
+{
+  e->writer.write(&e->writer, labcomm_writer_start);
+  labcomm_encode_type_index(e, &labcomm_signature_orca_messages_select_parameter);
+  {
+    labcomm_encode_int(e, (*v).port);
+    labcomm_encode_int(e, (*v).decimation);
+    labcomm_encode_int(e, (*v).list.n_0);
+    {
+      int i_0_0;
+      for (i_0_0 = 0 ; i_0_0 < (*v).list.n_0 ; i_0_0++) {
+        int i_0 = i_0_0;
+        labcomm_encode_int(e, (*v).list.a[i_0]);
+      }
+    }
+  }
+  e->writer.write(&e->writer, labcomm_writer_end);
+}
+void labcomm_encode_orca_messages_select_parameter(
+labcomm_encoder_t *e,
+orca_messages_select_parameter *v
+)
+{
+labcomm_internal_encode(e, &labcomm_signature_orca_messages_select_parameter, v);
+}
+void labcomm_encoder_register_orca_messages_select_parameter(
+  struct labcomm_encoder *e
+)
+{
+  labcomm_internal_encoder_register(
+    e,
+    &labcomm_signature_orca_messages_select_parameter,
+    (labcomm_encode_typecast_t)encode_select_parameter
+  );
+}
+int labcomm_sizeof_orca_messages_select_parameter(orca_messages_select_parameter *v)
+{
+  int result = 4;
+  result += 4 * (*v).list.n_0;
+  result += 8;
+  return result;
+}
+static void decode_select_log(
+  labcomm_decoder_t *d,
+  void (*handle)(
+    orca_messages_select_log *v,
+    void *context
+  ),
+  void *context
+)
+{
+  orca_messages_select_log v;
+  v.port = labcomm_decode_int(d);
+  v.decimation = labcomm_decode_int(d);
+  v.list.n_0 = labcomm_decode_int(d);
+  v.list.a = malloc(sizeof(v.list.a[0]) * v.list.n_0);
+  {
+    int i_0_0;
+    for (i_0_0 = 0 ; i_0_0 < v.list.n_0 ; i_0_0++) {
+      int i_0 = i_0_0;
+      v.list.a[i_0] = labcomm_decode_int(d);
+    }
+  }
+  handle(&v, context);
+  {
+    free(v.list.a);
+  }
+}
+void labcomm_decoder_register_orca_messages_select_log(
+  struct labcomm_decoder *d,
+  void (*handler)(
+    orca_messages_select_log *v,
+    void *context
+  ),
+  void *context
+)
+{
+  labcomm_internal_decoder_register(
+    d,
+    &labcomm_signature_orca_messages_select_log,
+    (labcomm_decoder_typecast_t)decode_select_log,
+    (labcomm_handler_typecast_t)handler,
+    context
+  );
+}
+static void encode_select_log(
+  labcomm_encoder_t *e,
+  orca_messages_select_log *v
+)
+{
+  e->writer.write(&e->writer, labcomm_writer_start);
+  labcomm_encode_type_index(e, &labcomm_signature_orca_messages_select_log);
+  {
+    labcomm_encode_int(e, (*v).port);
+    labcomm_encode_int(e, (*v).decimation);
+    labcomm_encode_int(e, (*v).list.n_0);
+    {
+      int i_0_0;
+      for (i_0_0 = 0 ; i_0_0 < (*v).list.n_0 ; i_0_0++) {
+        int i_0 = i_0_0;
+        labcomm_encode_int(e, (*v).list.a[i_0]);
+      }
+    }
+  }
+  e->writer.write(&e->writer, labcomm_writer_end);
+}
+void labcomm_encode_orca_messages_select_log(
+labcomm_encoder_t *e,
+orca_messages_select_log *v
+)
+{
+labcomm_internal_encode(e, &labcomm_signature_orca_messages_select_log, v);
+}
+void labcomm_encoder_register_orca_messages_select_log(
+  struct labcomm_encoder *e
+)
+{
+  labcomm_internal_encoder_register(
+    e,
+    &labcomm_signature_orca_messages_select_log,
+    (labcomm_encode_typecast_t)encode_select_log
+  );
+}
+int labcomm_sizeof_orca_messages_select_log(orca_messages_select_log *v)
+{
+  int result = 4;
+  result += 4 * (*v).list.n_0;
+  result += 8;
+  return result;
+}
diff --git a/cpp/orca_messages.h b/cpp/orca_messages.h
new file mode 100644
index 0000000000000000000000000000000000000000..7bf38e08b698f96e0a8101666c7780b59f24678e
--- /dev/null
+++ b/cpp/orca_messages.h
@@ -0,0 +1,161 @@
+/* LabComm declarations:
+typedef struct {
+  int index;
+  string name;
+  byte signature[_];
+} connection_list_t[_];
+typedef struct {
+  int port;
+  int decimation;
+  int list[_];
+} select_t;
+sample struct {
+  connection_list_t input;
+  connection_list_t output;
+  connection_list_t parameter;
+  connection_list_t log;
+} directory;
+sample select_t select_input;
+sample select_t select_output;
+sample select_t select_parameter;
+sample select_t select_log;
+*/
+
+
+#ifndef __LABCOMM_orca_messages_H__
+#define __LABCOMM_orca_messages_H__
+
+#include "labcomm.h"
+
+#ifndef PREDEFINED_orca_messages_connection_list_t
+typedef struct {
+  int n_0;
+  struct {
+    int index;
+    char* name;
+    struct {
+      int n_0;
+      unsigned char *a;
+    } signature;
+  } *a;
+} orca_messages_connection_list_t;
+#endif
+
+#ifndef PREDEFINED_orca_messages_select_t
+typedef struct {
+  int port;
+  int decimation;
+  struct {
+    int n_0;
+    int *a;
+  } list;
+} orca_messages_select_t;
+#endif
+
+#ifndef PREDEFINED_orca_messages_directory
+typedef struct {
+  orca_messages_connection_list_t input;
+  orca_messages_connection_list_t output;
+  orca_messages_connection_list_t parameter;
+  orca_messages_connection_list_t log;
+} orca_messages_directory;
+#endif
+void labcomm_decoder_register_orca_messages_directory(
+  struct labcomm_decoder *d,
+  void (*handler)(
+    orca_messages_directory *v,
+    void *context
+  ),
+  void *context
+);
+void labcomm_encoder_register_orca_messages_directory(
+  struct labcomm_encoder *e);
+void labcomm_encode_orca_messages_directory(
+  struct labcomm_encoder *e,
+  orca_messages_directory *v
+);
+extern int labcomm_sizeof_orca_messages_directory(orca_messages_directory *v);
+
+#ifndef PREDEFINED_orca_messages_select_input
+typedef orca_messages_select_t orca_messages_select_input;
+#endif
+void labcomm_decoder_register_orca_messages_select_input(
+  struct labcomm_decoder *d,
+  void (*handler)(
+    orca_messages_select_input *v,
+    void *context
+  ),
+  void *context
+);
+void labcomm_encoder_register_orca_messages_select_input(
+  struct labcomm_encoder *e);
+void labcomm_encode_orca_messages_select_input(
+  struct labcomm_encoder *e,
+  orca_messages_select_input *v
+);
+extern int labcomm_sizeof_orca_messages_select_input(orca_messages_select_input *v);
+
+#ifndef PREDEFINED_orca_messages_select_output
+typedef orca_messages_select_t orca_messages_select_output;
+#endif
+void labcomm_decoder_register_orca_messages_select_output(
+  struct labcomm_decoder *d,
+  void (*handler)(
+    orca_messages_select_output *v,
+    void *context
+  ),
+  void *context
+);
+void labcomm_encoder_register_orca_messages_select_output(
+  struct labcomm_encoder *e);
+void labcomm_encode_orca_messages_select_output(
+  struct labcomm_encoder *e,
+  orca_messages_select_output *v
+);
+extern int labcomm_sizeof_orca_messages_select_output(orca_messages_select_output *v);
+
+#ifndef PREDEFINED_orca_messages_select_parameter
+typedef orca_messages_select_t orca_messages_select_parameter;
+#endif
+void labcomm_decoder_register_orca_messages_select_parameter(
+  struct labcomm_decoder *d,
+  void (*handler)(
+    orca_messages_select_parameter *v,
+    void *context
+  ),
+  void *context
+);
+void labcomm_encoder_register_orca_messages_select_parameter(
+  struct labcomm_encoder *e);
+void labcomm_encode_orca_messages_select_parameter(
+  struct labcomm_encoder *e,
+  orca_messages_select_parameter *v
+);
+extern int labcomm_sizeof_orca_messages_select_parameter(orca_messages_select_parameter *v);
+
+#ifndef PREDEFINED_orca_messages_select_log
+typedef orca_messages_select_t orca_messages_select_log;
+#endif
+void labcomm_decoder_register_orca_messages_select_log(
+  struct labcomm_decoder *d,
+  void (*handler)(
+    orca_messages_select_log *v,
+    void *context
+  ),
+  void *context
+);
+void labcomm_encoder_register_orca_messages_select_log(
+  struct labcomm_encoder *e);
+void labcomm_encode_orca_messages_select_log(
+  struct labcomm_encoder *e,
+  orca_messages_select_log *v
+);
+extern int labcomm_sizeof_orca_messages_select_log(orca_messages_select_log *v);
+
+#define LABCOMM_FORALL_SAMPLES_orca_messages(func, sep) \
+  func(directory, orca_messages_directory) sep \
+  func(select_input, orca_messages_select_input) sep \
+  func(select_output, orca_messages_select_output) sep \
+  func(select_parameter, orca_messages_select_parameter) sep \
+  func(select_log, orca_messages_select_log)
+#endif
diff --git a/cpp/record_ya.cc b/cpp/record_ya.cc
new file mode 100644
index 0000000000000000000000000000000000000000..95680af540b7fdf1ab8ee43f9b8df6d3a951da68
--- /dev/null
+++ b/cpp/record_ya.cc
@@ -0,0 +1,61 @@
+#include "record_ya.h"
+
+void record_ya(const mat& ya,vector<Dmp>& dmpVec,const double& dt,const double& sparse_dt,const int& takeEvery,const int& RECORD_DMP, const int& PLAY_DMP) {
+	
+	
+	static int sample = 0;
+	static const int dof = 14;
+	static int recordedBefore = 0;
+	static mat demoTraj = zeros<mat>(1,dof);
+	static int stillCount = 0;
+	static int sampleRecordTot = 0;
+	
+	if (!armMoving(ya) && RECORD_DMP) {
+		++stillCount;
+	} else if (armMoving(ya)) {
+		stillCount = 0;
+	}
+	
+	// Stop/Start recording condition:
+	int record_dmp = ((RECORD_DMP) && stillCount < 250 && sampleRecordTot > 500) || (PLAY_DMP && RECORD_DMP);
+
+	
+	if (record_dmp && !recordedBefore) {
+		
+		for (int i = 0 ; i < dof ; i++) {
+			demoTraj(0,i) = ya(i);
+		}
+	}
+	
+	if (record_dmp) {
+		++sample;
+		if (sample == takeEvery) {
+			mat tempMat = zeros<mat>(1,dof);
+			for (int i = 0 ; i < dof ; i++) {
+				tempMat(0,i) = ya(i);
+			}
+			demoTraj = join_vert(demoTraj, tempMat);
+			sample = 0;
+		}
+	}
+	
+	
+	
+	if (!record_dmp && recordedBefore) {
+		//cout << "Running traj2dmp" << endl;
+		dmpVec.push_back(traj2dmp(demoTraj, sparse_dt).speedupTimes(1.0));
+		int maxDmpVec = dmpVec.size();
+		cout << "We have 1..." << maxDmpVec << " dmp:s" << endl;
+		demoTraj = zeros<mat>(1,dof);
+		//cout << dmpVec.back() << endl;
+	}
+    recordedBefore = record_dmp;
+    if (RECORD_DMP) {
+		++sampleRecordTot;
+	} else {
+		sampleRecordTot = 0;
+	}
+	
+	
+	
+}
diff --git a/cpp/record_ya.h b/cpp/record_ya.h
new file mode 100644
index 0000000000000000000000000000000000000000..f84a694dd0012ff4b1ed4cc480c4512bc81ff1a2
--- /dev/null
+++ b/cpp/record_ya.h
@@ -0,0 +1,19 @@
+#include <iostream>
+#include <armadillo>
+#include "dmp.h"
+#include "armmoving.h"
+#include "traj2dmp.h"
+
+
+
+using namespace arma;
+using namespace std;
+
+#ifndef RECORD_YA_H
+#define RECORD_YA_H
+
+
+
+void record_ya(const mat& ya,vector<Dmp>& dmpVec,const double& dt,const double& sparse_dt,const int& takeEvery,const int& RECORD_DMPm, const int& PLAY_DMP);
+
+#endif
diff --git a/cpp/state2yardd.cc b/cpp/state2yardd.cc
new file mode 100644
index 0000000000000000000000000000000000000000..463b32423fc28c20297cae5bd81dc1014a868369
--- /dev/null
+++ b/cpp/state2yardd.cc
@@ -0,0 +1,121 @@
+#include "state2yardd.h"
+#include "twod_controller.h"
+
+
+void state2yardd(const mat& ya, const mat& yad, mat& yardd, int& PLAY_DMP, const int& DMP_NBR, const vector<Dmp>& dmpVec, mat& yclog, mat& ycddlog, mat& ya_minus_yc, mat& yad_minus_ycd, double& e_norm_log, double& xlog, mat& yc_minus_g, mat& zlog) {
+	
+	
+	// declare:
+	static const int dof = 14;
+	static mat z(dof,1);
+	static double x;	
+	static mat w;
+	static double tau;
+	static mat g;
+	static const double offs = pow(10,-10);
+	static mat y0(dof,1);
+	static mat yc = zeros<mat>(dof,1);
+	static double dt = 0.004;
+	static const double kc = 10000;
+	static double x_dot;
+	static double dmp_nbr_old = DMP_NBR;
+	static double e = 0;
+	static double alpha_e = 5;
+	static const double eps_stop = 0.010;
+	static const double x_th = 0.10;
+	mat D  = pow((diff(c)*0.55),2);
+	mat Dlast(1,1);
+	Dlast = D(D.n_rows - 1);
+	D  = 1/join_vert(D, Dlast);
+	
+	
+	
+	// Init:	
+	static int play_dmp_old = 0;
+	if (PLAY_DMP && !play_dmp_old || DMP_NBR != dmp_nbr_old) {
+		
+		z.zeros();
+		Dmp currentDmp = dmpVec.at(DMP_NBR-1);
+		w = currentDmp.getW();
+		tau = currentDmp.getTau();
+		g = currentDmp.getG();
+		y0 = ya;
+		yc = ya;
+		x = 1;
+		z.zeros();
+		e = 0;
+		
+	}
+	
+	if (PLAY_DMP) {
+		double f;
+		double z_dot;
+		double e_dot = alpha_e*(norm(ya-yc) - e);
+		e = e + e_dot*dt;
+		//cout << e << endl;
+		double tau_adapt = tau*(1 + kc*pow(e,2));
+		//cout << (1 + kc*pow(e,2)) << endl;
+		mat ycdot(1,dof);
+		mat ycddot(1,dof);
+		for (int j = 0; j < dof; ++j) {
+			mat psi = exp(-0.5*pow(x-c,2)%D);
+			//f = as_scalar(sum(w.t()*psi)) / as_scalar(sum(psi + offs)) *x * (g(j) - y0(j));
+			f = as_scalar(w.col(j).t()*psi) / as_scalar(sum(psi + offs)) * x;
+
+			//f = 0; //////////////////// Test row
+			z_dot = 1/ tau_adapt*(alpha_z*(beta_z*(g(j)-yc(j)) - z(j)) + f);
+			z(j) = z(j) + dt * z_dot;
+			
+			
+
+			ycdot(j) = z(j)/tau_adapt;
+			ycddot(j) = (z_dot*tau_adapt-2*tau*kc*z(j)*e*e_dot)/pow(tau_adapt,2);
+			yc(j) = yc(j) + ycdot(j)*dt;
+			// Convergence states:
+			ya_minus_yc(j) = ya(j) - yc(j);
+			yad_minus_ycd(j) = yad(j) - ycdot(j);
+			yc_minus_g(j) = yc(j) - g(j);
+			zlog(j) = z(j);
+			
+		}
+		x_dot = -x/tau_adapt;
+		x = x + x_dot*dt;
+		
+		twod_controller(ya, yad, yc, ycdot, ycddot, yardd);
+		yclog = yc;
+		ycddlog = ycddot;
+		
+		//cout << norm(ya-g) << endl;
+		
+		// Stop at goal:
+		
+		if ( (norm(ya-g) < eps_stop) && (x < x_th) ) {
+			PLAY_DMP = 0; //Optional stopping condition
+		}
+		// Convergence states
+		e_norm_log = e;
+		xlog = x;
+		
+		
+	} else {
+		yardd.zeros();
+		yclog = yc;
+		ycddlog.zeros();
+		
+		// Convergence states:
+		ya_minus_yc.zeros();
+		yad_minus_ycd.zeros();
+		e_norm_log = 0;
+		xlog = 0;
+		yc_minus_g.zeros();
+		zlog.zeros();
+		
+	}
+	
+	play_dmp_old = PLAY_DMP;	
+	dmp_nbr_old = DMP_NBR;
+	
+	
+	
+}
+
diff --git a/cpp/state2yardd.h b/cpp/state2yardd.h
new file mode 100644
index 0000000000000000000000000000000000000000..323cd4a5f3f7907545136242d1b8d43df784040c
--- /dev/null
+++ b/cpp/state2yardd.h
@@ -0,0 +1,19 @@
+#include "traj2dmp.h"
+#include <iostream>
+#include <armadillo>
+#include <math.h>
+
+
+
+
+using namespace arma;
+
+
+#ifndef STATE2YARDD_H
+#define STATE2YARDD_H
+
+
+
+void state2yardd(const mat& ya, const mat& yad, mat& yardd, int& PLAY_DMP, const int& DMP_NBR, const vector<Dmp>& dmpVec, mat& yclog, mat& ycddlog, mat& ya_minus_yc, mat& yad_minus_ycd, double& e_norm_log, double& xlog, mat& yc_minus_g, mat& zlog);
+
+#endif
diff --git a/cpp/traj2dmp.cc b/cpp/traj2dmp.cc
new file mode 100644
index 0000000000000000000000000000000000000000..6446762a4b04aa8fa2c0c5ee5184296a9de719f5
--- /dev/null
+++ b/cpp/traj2dmp.cc
@@ -0,0 +1,98 @@
+#include "traj2dmp.h"
+#include <iostream>
+#include <armadillo>
+
+using namespace arma;
+
+mat smartDiff(const mat& inMat) { //now supports col vecs only
+	mat temp(1,1);
+	temp << 0 << endr;
+	mat outMat = (join_vert(temp, diff(inMat)) + join_vert(diff(inMat), temp)) / 2.0;
+	return outMat;
+	
+}
+
+Dmp traj2dmp(const mat& traj, const double& sparse_dt) {
+	
+	
+	
+	mat D  = pow((diff(c)*0.55),2);
+	mat Dlast(1,1);
+    Dlast = D(D.n_rows - 1);
+    D  = 1/join_vert(D, Dlast);
+
+
+	
+	mat g(1,dof);
+	mat y0(1,dof);
+	mat w = zeros<mat>(n_kernel,dof);
+
+	int P = traj.n_rows;
+	double tau = P*sparse_dt/3;
+	mat x(P,1);
+	x(0) = 1;
+	double x_dot;
+	for (int t = 1; t < P; ++t) {
+		x_dot = -x(t-1)/tau;
+		x(t) = x(t-1) + x_dot*sparse_dt;
+	}
+	mat psi(P,n_kernel);
+	//cout << c.n_rows << endl;
+	//cout << D.n_rows << endl;
+	for (int t = 0; t<P; ++t) {
+		psi.row(t) = exp(-0.5*pow(x(t)-c,2)%D).t();
+	}
+	//cout << psi << endl;
+	
+	
+	
+	
+	//cout << psi << endl;
+	for (int j = 0; j < traj.n_cols; ++j) {
+		mat T = traj.col(j);
+		mat TD = smartDiff(T)/sparse_dt;
+		mat TDD = smartDiff(TD)/sparse_dt;
+		g(j) = T(T.n_rows-1);
+		y0(j) = T(0);
+		
+
+		mat f_target = pow(tau,2)*TDD -alpha_z*(beta_z*(g(j)-T) -tau*TD);
+		//cout << "f_target:" << endl;
+
+		//cout << f_target << endl;
+		//mat s = x*(g(j) - y0(j));
+		mat s = x;
+		mat Gamma_i(P,P);
+		Gamma_i.zeros();
+		//cout << psi << endl;
+		for (int i  = 0; i < n_kernel; ++i) {
+			Gamma_i.diag() = psi.col(i);
+			//cout << Gamma_i.max() << endl;
+			//cout << Gamma_i.diag() << endl;
+			//cout << "Gamma_i: " << endl;
+			//cout << Gamma_i << endl;
+			//cout << "psi: " <<  endl;
+			//cout << psi.col(i) << endl;
+			double num = as_scalar(s.t()*Gamma_i*f_target);
+			double den = as_scalar(s.t()*Gamma_i*s);
+			
+			w(i,j) = num / den;
+			//cout << s.t()*Gamma_i*s << endl;
+			//cout << den << endl;
+				
+			
+			/*cout << "fit:" << endl;
+			for (int t = 0; t < P; ++t) {
+				
+				double fit = as_scalar(w.col(j).t()*psi.row(t).t()) / as_scalar(sum(psi.row(t) + offs)) * x(t);
+				cout << fit << endl;
+			}*/
+				//cout << " " << endl;
+			//cout << " " << endl;
+			//cout << " " << endl;
+		}
+	}
+	//cout << w << endl;
+	return Dmp(w, g, tau);
+}
+		
diff --git a/cpp/traj2dmp.h b/cpp/traj2dmp.h
new file mode 100644
index 0000000000000000000000000000000000000000..918a5f84d5e6932525dcc21a68e66ec246d07807
--- /dev/null
+++ b/cpp/traj2dmp.h
@@ -0,0 +1,11 @@
+#include "dmp.h"
+#include <armadillo>
+using namespace arma;
+#ifndef TRAJ2DMP_H
+#define TRAJ2DMP_H
+
+
+
+mat smartDiff(const mat& inMat);
+Dmp traj2dmp(const mat& trajectory, const double& dt);
+#endif
diff --git a/cpp/twod_controller.cc b/cpp/twod_controller.cc
new file mode 100644
index 0000000000000000000000000000000000000000..43f91fc03a8742ac81e7f45b076cb6d5abec7162
--- /dev/null
+++ b/cpp/twod_controller.cc
@@ -0,0 +1,19 @@
+#include "twod_controller.h"
+
+
+void twod_controller(const mat& ya, const mat& yad, const mat& yc, const mat& ycdot, const mat& ycddot, mat& yardd) {
+
+	
+	//5, 6.25 well balanced for no feed-forward at least
+	//static const double kv = 10;
+	//static const double kp = 25;
+	
+	//static const double kv = 0;
+	//static const double kp = 0;
+	
+	static const double kv = 10;
+	static const double kp = kv*kv/4;	
+	
+	//yardd = kp*(yc - ya) + kv*(ycdot - yad);
+	yardd = kp*(yc - ya) + kv*(ycdot - yad) + ycddot;
+}
diff --git a/cpp/twod_controller.h b/cpp/twod_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..4f33fba4c37d163e5f9d79639c84c2bab0105816
--- /dev/null
+++ b/cpp/twod_controller.h
@@ -0,0 +1,26 @@
+#include <armadillo>
+#include <math.h>
+
+
+
+
+using namespace arma;
+
+
+#ifndef TWOD_CONTROLLER_H
+#define TWOD_CONTROLLER_H
+
+
+
+void twod_controller(const mat& ya, const mat& yad, const mat& yc, const mat& ycdot, const mat& ycddot, mat& yardd);
+
+#endif
+
+
+
+
+
+
+
+
+
diff --git a/cpp/write_norms_to_file.cc b/cpp/write_norms_to_file.cc
new file mode 100644
index 0000000000000000000000000000000000000000..8687328cbc722cace9820bc501ddd4c2aee09b2d
--- /dev/null
+++ b/cpp/write_norms_to_file.cc
@@ -0,0 +1,31 @@
+#include "write_norms_to_file.h"
+
+void write_norms_to_file(const mat& ya_minus_yc,const mat& yad_minus_ycd,const double& e_norm_log,const double& xlog,const mat& yc_minus_g,const mat& zlog,const int& PLAY_DMP) {
+	static ofstream logfile;
+	static int has_played = 0;
+
+	if (PLAY_DMP && !has_played) {
+		logfile.open("data/logfile1.txt");
+		has_played = 1;
+	}
+
+	if (PLAY_DMP) {
+		for (int j = 0; j < dof; ++j) {
+			logfile << ya_minus_yc(j) << " ";
+		}
+		for (int j = 0; j < dof; ++j) {
+			logfile << yad_minus_ycd(j) << " ";
+		}
+		logfile << e_norm_log << " ";
+		logfile << xlog << " ";
+		for (int j = 0; j < dof; ++j) {
+			logfile <<  yc_minus_g(j) << " ";
+		}
+		for (int j = 0; j < dof; ++j) {
+			logfile << zlog(j) << " ";
+		}
+		logfile << endl;
+	}
+	
+
+}
diff --git a/cpp/write_norms_to_file.h b/cpp/write_norms_to_file.h
new file mode 100644
index 0000000000000000000000000000000000000000..ff36d99add6f37c2378f4a7805d6a461514d8ad8
--- /dev/null
+++ b/cpp/write_norms_to_file.h
@@ -0,0 +1,14 @@
+#include <iostream>
+#include <armadillo>
+#include <math.h>
+#include <fstream>
+#include "dmp.h"
+using namespace std;
+using namespace arma;
+
+#ifndef WRITE_NORMS_TO_FILE_H
+#define WRITE_NORMS_TO_FILE_H
+
+void write_norms_to_file(const mat& ya_minus_yc,const mat& yad_minus_ycd,const double& e_norm_log,const double& xlog,const mat& yc_minus_g,const mat& zlog, const int& PLAY_DMP);
+
+#endif