#define logMessage printf

#include "easy-connect.h"
#include "MQTTNetwork.h"
#include "MQTTmbed.h"
#include "MQTTClient.h"

EventQueue event_queue;
Thread event_thread;

char* topic = "mbed-sample";
float version = 0.6;

DigitalOut led(LED1);

int arrivedcount = 0;

void messageArrived(MQTT::MessageData& md)
{
    MQTT::Message &message = md.message;
    logMessage("Message arrived: qos %d, retained %d, dup %d, packetid %d\r\n", message.qos, message.retained, message.dup, message.id);
    logMessage("Payload %.*s\r\n", message.payloadlen, (char*)message.payload);
    ++arrivedcount;
}

unsigned int msg_id = 0;

void send_qos0(MQTT::Client<MQTTNetwork, Countdown>* client) {
    printf("send_qos0\r\n");

    MQTT::Message message;

    // QoS 0
    char buf[100];
    sprintf(buf, "Hello World!  QoS 0 message from app version %f\r\n", version);

    message.qos = MQTT::QOS0;
    message.retained = false;
    message.dup = false;
    message.payload = (void*)buf;
    message.payloadlen = strlen(buf)+1;
    message.id = ++msg_id;

    int old_ac = arrivedcount;

    int rc = client->publish(topic, message);
    // printf("publish for id %d returned %d\r\n", message.id, rc);

    while (old_ac == arrivedcount) {
        int y = client->yield(100);
        if (y == -1) {
            printf("yield failed...\r\n");
            break;
        }
        // printf("yield returned %d\r\n", y);
    }
    printf("retrieved message, exit'ing send_qos0\r\n");
}

void blink() {
    led = !led;
}

int main(int argc, char* argv[])
{
    InterruptIn sw2(SW2);

    event_thread.start(callback(&event_queue, &EventQueue::dispatch_forever));

    Ticker t;
    t.attach(&blink, 0.5f);

    logMessage("HelloMQTT: version is %.2f\r\n", version);

    NetworkInterface* network = easy_connect(true);
    if (!network) {
        return -1;
    }

    MQTTNetwork mqttNetwork(network);
    MQTT::Client<MQTTNetwork, Countdown> client(mqttNetwork);

    const char* hostname = "m2m.eclipse.org";
    int port = 1883;
    logMessage("Connecting to %s:%d\r\n", hostname, port);
    int rc = mqttNetwork.connect(hostname, port);
    logMessage("mqttNetwork.connect returned %d\r\n", rc);

    MQTTPacket_connectData data = MQTTPacket_connectData_initializer;
    data.MQTTVersion = 3;
    data.clientID.cstring = "mbed-sample";
    data.username.cstring = "testuser";
    data.password.cstring = "testpassword";
    logMessage("MQTT connect...\r\n");
    rc = client.connect(data);
    logMessage("rc from MQTT connect is %d\r\n", rc);

    logMessage("MQTT subscribe...\r\n");
    rc = client.subscribe(topic, MQTT::QOS2, messageArrived);
    logMessage("rc from MQTT subscribe is %d\r\n", rc);

    sw2.fall(event_queue.event(&send_qos0, &client));
    logMessage("press SW2 to send message\r\n");

    wait_ms(osWaitForever);
}