/*
 * 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;
            }
        }

    }

}