diff --git a/src/handshake.c b/src/handshake.c
index 684a102..5904b4a 100644
--- a/src/handshake.c
+++ b/src/handshake.c
@@ -9,23 +9,27 @@
 #include "ti_drivers_config.h"
 
 #define TIME_BETWEEN_DIRECTION_CHANGE 1.0
+#define TIME_TO_RESPAN_ON_REQUEST 10.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;
+bool replyListen = false;
+bool requestListen = true;
 
-struct {
-    char yourReply[] = REPLY_TOPIC(YOUR_STUDENTNUMBER);
-    char yourRequest[] = REQUEST_TOPIC(YOUR_STUDENTNUMBER);
-} topics;
+double timestamp = 0;
+double requestTimeout = -TIME_TO_RESPAN_ON_REQUEST - 1;
+double lastBack = -TIME_BETWEEN_DIRECTION_CHANGE - 1;
+
+char requestFrom[6];
+
+char requestTopic[] = "ems20/handshake/0964590/request";
 
 typedef struct {
     enum {Vooruit, Achteruit} richting;
@@ -33,29 +37,60 @@ typedef struct {
     int8_t g;
 } gsensorMsgQueue;
 
-double lastBack = -TIME_BETWEEN_DIRECTION_CHANGE - 1;
 
 extern void MQTTPublish(char * topic, char * payload);
+extern void MQTTSubscribe(char* topic, void *cb(char* topic, char* payload));
 
 void MQTTCB_reply(char* topic, char* payload){
-    if(handshakeRequestAcrive){
-        //TODO: send playload + " heeft je hand geschud" via UDP
-    }
+    char msg[50];
+
+    if(!replyListen)
+        return;
+
+    //if(handshakeRequestAcrive){
+        sprintf(&msg, "%s heeft je hand geschud.", payload);
+        //udpSend(msg, strlen(msg));
+    //}
 }
 
-void MQTTCB_reply(char* topic, char* payload){
-    if(handshakeRequestAcrive){
-        //TODO: send playload + " heeft je hand geschud" via UDP
+void MQTTCB_request(char* topic, char* payload){
+    char msg[50];
+    uint8_t i;
+
+    if(!requestListen)
+        return;
+
+    // get student number from topic
+    if(strlen(topic) < 16+6){
+        return; //ERROR invalid topic
     }
+    for(i=0; i<6; i++){
+        requestFrom[i] = topic[i+16];
+    }
+
+    requestTimeout = timestamp + TIME_TO_RESPAN_ON_REQUEST;
+    sprintf(&msg, "%s bied je een handdruk aan.", payload);
+    //udpSend(msg, strlen(msg));
 }
 
 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
+    if(requestTimeout > timestamp){
+        // replyed to request
+        requestTimeout = 0;
+
+        replyListen = false;
+        requestListen = true; //?? direct luisteren naar requests na 
+        
+        char topic[128];
+        sprintf(&topic, "ems20/$s/reply", requestFrom);
+        MQTTPublish(&topic[0], &yourName[0]);
+    }else{ // send request
+        replyListen = true;
+        requestListen = false;
+        GPIO_write(CONFIG_LED_R, 1);
+        MQTTPublish(&requestTopic[0], &yourName[0]);
+    }
+    //udpSend("Handshake aangeboden", strlen("Handshake aangeboden"));
 }
 
 void * handshakeTask(void *arg0){
@@ -70,35 +105,42 @@ void * handshakeTask(void *arg0){
 
     GPIO_write(CONFIG_LED_R, 0);
 
+//    MQTTSubscribe("ems20/handshake/0964590/reply", &MQTTCB_reply);
+//    MQTTSubscribe("ems20/handshake/0123456/request", &MQTTCB_request);
+
     gsensorMsgQueue queueElement;
-    double timeoutUntil = -1;
+    double replyTimeout = -1;
 
     while(1){
         mq_receive(gsensorQueue, (char*)&queueElement, sizeof(gsensorMsgQueue), NULL);
 
-        if(timeoutUntil >= queueElement.timestamp){
-            if(timeoutUntil <= queueElement.timestamp + 1){
-                handshakeRequestAcrive = false;
+        timestamp = queueElement.timestamp;
+
+        if(replyTimeout >= timestamp){
+            if(replyTimeout <= timestamp + 1){
+                replyListen = false;
                 GPIO_write(CONFIG_LED_R, 0);
-                //TODO: unsubscribe from your replay
             }
+            requestListen = true;
             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;
-            }
+        if(requestTimeout > timestamp){
+            uint32_t a = timestamp*2;
+            GPIO_write(CONFIG_LED_R, a & 0x01);
+        }else{
+            GPIO_write(CONFIG_LED_R, 0);
         }
 
+        if(queueElement.g > G_THUSHOLD){
+            if(queueElement.richting == Vooruit && timestamp - lastBack < TIME_BETWEEN_DIRECTION_CHANGE){
+                replyTimeout = timestamp + TIMEOUT_DURATION;
+                handshake();
+            } else if(queueElement.richting == Achteruit){
+                lastBack = timestamp;
+            }
+        }
     }
-
 }
 
 
diff --git a/src/mqtt_client_app.c b/src/mqtt_client_app.c
index 1d70bd7..ad2283e 100644
--- a/src/mqtt_client_app.c
+++ b/src/mqtt_client_app.c
@@ -553,6 +553,10 @@ void MQTTPublish(char * topic, char * payload){
     int res = mq_send(appQueue, (const char*)&queueElement, sizeof(struct msgQueue), 0);
 }
 
+void MQTTSubscribe(char* topic, void *cb(char* topic, char* payload)){
+    uint8_t ret = MQTT_IF_Subscribe(0, topic, MQTT_QOS_2, cb);
+}
+
 void MQTTTask(void * args){
 
     int32_t ret;