diff --git a/src/gsensor.c b/src/gsensor.c
index c019a74..c16938d 100644
--- a/src/gsensor.c
+++ b/src/gsensor.c
@@ -11,6 +11,7 @@
 // I2C drivers
 #include <ti/drivers/I2C.h>
 #include <ti/drivers/i2c/I2CCC32XX.h>
+#include <mqueue.h>
 
 extern void MQTTPublish(char * topic, char * payload);
 
@@ -19,11 +20,17 @@ extern void MQTTPublish(char * topic, char * payload);
 
 char mqtt_topic[] = "top_pik";
 
+extern mqd_t gsensorQueue;
 typedef struct {
     enum {Vooruit, Achteruit} richting;
     double timestamp;
     int8_t g;
-} pakketje;
+} gsensorMsgQueue;
+//typedef struct {
+//    enum {Vooruit, Achteruit} richting;
+//    double timestamp;
+//    int8_t g;
+//} pakketje;
 
 void * GSensorTask(void *args) {
 
@@ -53,16 +60,18 @@ void * GSensorTask(void *args) {
         // publish read into mailbox (defined in mqtt.c)
         timer += 0.1;
 
-        // pakketje pakket;
-        // pakket.richting = (buffer_rcv < 0) ? Achteruit : Vooruit;
-        // pakket.timestamp = timer;
-        // pakket.g = abs(buffer_rcv);
+        gsensorMsgQueue pakket;
+        pakket.richting = (buffer_rcv < 0) ? Achteruit : Vooruit;
+        pakket.timestamp = timer;
+        pakket.g = abs(buffer_rcv);
 
-        char payload[150];
+        mq_send(gsensorQueue, (const char*)&pakket, sizeof(gsensorMsgQueue), 0);
 
-        sprintf(&payload, "handshake (richting: %s; timestamp: %.1f)", (buffer_rcv < 0) ? "Achteruit" : "Vooruit", timer);
-
-        MQTTPublish(&mqtt_topic, payload);
+//        char payload[150];
+//
+//        sprintf(&payload, "handshake (richting: %s; timestamp: %.1f; g: %d)", (buffer_rcv < 0) ? "Achteruit" : "Vooruit", timer, abs(buffer_rcv));
+//
+//        MQTTPublish(&mqtt_topic, &payload);
         // MQTTPublish(pakket);
 
         usleep(100000U);
diff --git a/src/handshake.c b/src/handshake.c
new file mode 100644
index 0000000..684a102
--- /dev/null
+++ b/src/handshake.c
@@ -0,0 +1,104 @@
+/*
+ * handshake.c
+ *
+ *  Created on: 29 Oct 2020
+ *      Author: MReenen
+ */
+#include <mqueue.h>
+#include <ti/drivers/GPIO.h>
+#include "ti_drivers_config.h"
+
+#define TIME_BETWEEN_DIRECTION_CHANGE 1.0
+#define G_THUSHOLD 10
+#define TIMEOUT_DURATION 11
+
+const char yourName[] = "Mats";
+#define YOUR_STUDENTNUMBER "0964590"
+#define COLLIGE_STUDENTNUMBER "1234567"
+
+#define REPLY_TOPIC(n) "ems20/handshake/" n "/reply"
+#define REQUEST_TOPIC(n) "ems20/handshake/" n "/request"
+
+mqd_t gsensorQueue;
+bool handshakeRequestAcrive = false;
+
+struct {
+    char yourReply[] = REPLY_TOPIC(YOUR_STUDENTNUMBER);
+    char yourRequest[] = REQUEST_TOPIC(YOUR_STUDENTNUMBER);
+} topics;
+
+typedef struct {
+    enum {Vooruit, Achteruit} richting;
+    double timestamp;
+    int8_t g;
+} gsensorMsgQueue;
+
+double lastBack = -TIME_BETWEEN_DIRECTION_CHANGE - 1;
+
+extern void MQTTPublish(char * topic, char * payload);
+
+void MQTTCB_reply(char* topic, char* payload){
+    if(handshakeRequestAcrive){
+        //TODO: send playload + " heeft je hand geschud" via UDP
+    }
+}
+
+void MQTTCB_reply(char* topic, char* payload){
+    if(handshakeRequestAcrive){
+        //TODO: send playload + " heeft je hand geschud" via UDP
+    }
+}
+
+void handshake(){
+    handshakeRequestAcrive = true;
+    GPIO_write(CONFIG_LED_R, 1);
+    MQTTPublish(&topics.yourRequest, &yourName);
+    //TODO: "Handhake aangeboden" stuuren via UDP
+    //TODO: subscribe to your replay
+    //TODO: unsubscripe from colega request
+}
+
+void * handshakeTask(void *arg0){
+    // create massage queue
+    mq_attr attr;
+    attr.mq_maxmsg = 10;
+    attr.mq_msgsize = sizeof(gsensorMsgQueue);
+    gsensorQueue = mq_open("gsensorQueue", O_CREAT, 0, &attr);
+    if(((int)gsensorQueue) <= 0){
+        while(1);
+    }
+
+    GPIO_write(CONFIG_LED_R, 0);
+
+    gsensorMsgQueue queueElement;
+    double timeoutUntil = -1;
+
+    while(1){
+        mq_receive(gsensorQueue, (char*)&queueElement, sizeof(gsensorMsgQueue), NULL);
+
+        if(timeoutUntil >= queueElement.timestamp){
+            if(timeoutUntil <= queueElement.timestamp + 1){
+                handshakeRequestAcrive = false;
+                GPIO_write(CONFIG_LED_R, 0);
+                //TODO: unsubscribe from your replay
+            }
+            continue;
+        }
+
+        //TODO: subscripe to colega request
+        
+        if(queueElement.g > G_THUSHOLD){
+            if(queueElement.richting == Vooruit && queueElement.timestamp - lastBack < TIME_BETWEEN_DIRECTION_CHANGE){
+                timeoutUntil = queueElement.timestamp + TIMEOUT_DURATION;
+                handshake();
+            }
+            if(queueElement.richting == Achteruit){
+                lastBack = queueElement.timestamp;
+            }
+        }
+
+    }
+
+}
+
+
diff --git a/src/main_tirtos.c b/src/main_tirtos.c
index e502227..0192c27 100644
--- a/src/main_tirtos.c
+++ b/src/main_tirtos.c
@@ -51,6 +51,7 @@
 
 extern void * GSensorTask(void *arg0);
 extern void * MQTTTask(void *arg0);
+extern void * handshakeTask(void *arg0);
 
 /* Stack size in bytes */
 #define BIGOLTHREADSTACK 4096
@@ -67,6 +68,7 @@ int main(void)
     /* Call board init functions */
     Board_init();
     I2C_init();
+    GPIO_init();
 
     /* Initialize the attributes structure with default values */
     pthread_attr_init(&attrs);
@@ -107,6 +109,19 @@ int main(void)
         while(1);
     }
 
+
+    /* ==============================================
+       === tread for Handshake ======================
+       ==============================================
+    */
+    pthread_t Handshakehread;
+    retc = pthread_create(&Handshakehread, &attrs, handshakeTask, NULL);
+    if (retc != 0) {
+        /* pthread_create() failed */
+//        LOG_CRITICAL("main(): failed to create tread for MQTT");
+        while(1);
+    }
+
     BIOS_start();
 
     return (0);