#include <FreeRTOS.h>
#include <task.h>
#include <timers.h>
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include "MQTTClient.h"
#include "MQTTFreertos.h"

static unsigned char sendBuf[1000];
static unsigned char readBuf[1000];

void messageArrived(MessageData* data)
{
	printf("Message arrived on topic %.*s: %.*s\n", data->topicName->lenstring.len, data->topicName->lenstring.data,
		data->message->payloadlen, data->message->payload);
}
static Network network;
static void MQTT_DemoTask(void *pvParameters)
{
	printf("Starting ...\n");
	int rc, count = 0;
	MQTTClient client;

	NetworkInit(&network);
	printf("NetworkConnect  ...\n");
begin:	
	NetworkConnect(&network, "10.0.0.42", 1883);
	printf("MQTTClientInit  ...\n");
	MQTTClientInit(&client, &network, 2000, sendBuf, sizeof(sendBuf), readBuf, sizeof(readBuf));

	MQTTString clientId = MQTTString_initializer;
	clientId.cstring = "bearpi";

	MQTTPacket_connectData data = MQTTPacket_connectData_initializer;
  	data.clientID          = clientId;
	data.willFlag          = 0;
	data.MQTTVersion       = 3;
	data.keepAliveInterval = 0;
	data.cleansession      = 1;

	printf("MQTTConnect  ...\n");
	rc = MQTTConnect(&client, &data);
	if (rc != 0) {
		printf("MQTTConnect: %d\n", rc);
		FreeRTOS_disconnect(&network);
		MQTTDisconnect(&client);
		vTaskDelay(200);
		goto begin;
	}

	printf("MQTTSubscribe  ...\n");
	rc = MQTTSubscribe(&client, "pubtopic", 2, messageArrived);
	if (rc != 0) {
		printf("MQTTSubscribe: %d\n", rc);
		vTaskDelay(200);
		goto begin;
	}
	while (++count)
	{
		MQTTMessage message;
		char payload[30];

		message.qos = 2;
		message.retained = 0;
		message.payload = payload;
		sprintf(payload, "message number %d", count);
		message.payloadlen = strlen(payload);

		if ((rc = MQTTPublish(&client, "pubtopic", &message)) != 0){
			printf("Return code from MQTT publish is %d\n", rc);
			FreeRTOS_disconnect(&network);
			MQTTDisconnect(&client);
			goto begin;
		}
		vTaskDelay(50);	
	}
}
void mqtt_main_entry(void)
{
    static StackType_t proc_main_stack[10240];
    static StaticTask_t proc_main_task;
    xTaskCreateStatic(MQTT_DemoTask, (char*)"MQTT_DemoTask", 10240, NULL, 25, proc_main_stack, &proc_main_task);
}
