//
// Created by jedi on 25.06.21.
//

#include "mqtt.h"
#include "wifi.h"
#include "lux.h"

#include <stdio.h>
#include <string.h>

#include <espressif/esp_common.h>
#include <espressif/user_interface.h>

extern "C" {

#include <paho_mqtt_c/MQTTESP8266.h>
#include <paho_mqtt_c/MQTTClient.h>

}

#include <semphr.h>


/* You can use http://test.mosquitto.org/ to test mqtt_client instead
 * of setting up your own MQTT server */
//#define MQTT_HOST ("172.16.0.42")
//#define MQTT_HOST ("10.23.42.187")
#define MQTT_HOST ("172.16.1.53")
#define MQTT_PORT 1883

#define MQTT_USER NULL
#define MQTT_PASS NULL

bool is_on = true;

QueueHandle_t publish_queue;

static void publish_state() {
    char msg[PUB_MSG_LEN];
    snprintf(msg, PUB_MSG_LEN, R"({"state":"%s", "brightness":%d})", is_on ? "ON" : "OFF", light_value >> 4);
    if(xQueueSend(publish_queue, (void *) msg, 0) == pdFALSE) {
        printf("Publish queue overflow.\r\n");
    }
}

extern "C" void beat_task(void *pvParameters) {
    TickType_t xLastWakeTime = xTaskGetTickCount();
    while (true) {
        vTaskDelayUntil(&xLastWakeTime, 20000 / portTICK_PERIOD_MS);
        publish_state();
    }
}

template<typename T>
class optional {
        bool has_value_;
        T value_;
    public:
        optional() : has_value_(false) {}

        optional(T value) : has_value_(true), value_(value) {}

        bool has_value() const { return has_value_; }

        T value() const { return value_; }

        T value_or(T default_value) const {
            return has_value_ ? value_ : default_value;
        }

        T operator*() const { return value_; }

        T operator->() const { return value_; }

        operator bool() const { return has_value_; }

        optional<T> &operator=(T value) {
            has_value_ = true;
            value_ = value;
            return *this;
        }

        optional<T> &operator=(const optional<T> &other) {
            has_value_ = other.has_value_;
            value_ = other.value_;
            return *this;
        }

        optional<T> &operator=(optional<T> &&other) noexcept {
            has_value_ = other.has_value_;
            value_ = other.value_;
            return *this;
        }

        optional(const optional<T> &other) : has_value_(other.has_value_), value_(other.value_) {}

        optional(optional<T> &&other) noexcept: has_value_(other.has_value_), value_(other.value_) {}


};

class parser {
        char *head;
        const char *end;

    public:

        parser(char *begin, const char *end) : head(begin), end(end) {}

        bool consume_char(char c) {
            if(head < end && *head == c) {
                head++;
                return true;
            }
            return false;
        }

        bool consume_string(const char *str, size_t len) {
            if(head + len <= end && strncmp(head, str, len) == 0) {
                head += len;
                return true;
            }
            return false;
        }

        template<size_t N>
        bool consume_string(const char (&str)[N]) {
            return consume_string(str, N - 1);
        }

        optional<char> read_char() {
            if(head < end) {
                return {*head++};
            }
            return {};
        }

        optional<char> peek_char() {
            if(head < end) {
                return {*head};
            }
            return {};
        }

        optional<int> read_int() {
            int value = 0;
            bool negative = false;
            if(consume_char('-')) {
                negative = true;
            }
            auto peek = head;
            while (peek < end && *peek >= '0' && *peek <= '9') {
                value = value * 10 + (*peek - '0');
                peek++;
            }
            if(peek == head) {
                return {};
            }
            head = peek;
            return {negative ? -value : value};
        }
};

static void topic_received(mqtt_message_data_t *md) {
    mqtt_message_t *message = md->message;

    char *payload = (char *) message->payload;
    char *payload_end = payload + message->payloadlen;

    parser p(payload, payload_end);

    if(!p.consume_char('{')) {
        printf("expected '{' at start of payload\n");
        return;
    }
    while (true) {
        if(p.consume_string(R"("state":)")) {
            if(p.consume_string("\"ON\"")) {
                is_on = true;
                printf("ON\n");
            } else if(p.consume_string("\"OFF\"")) {
                is_on = false;
                printf("OFF\n");
            } else {
                printf("Invalid state\n");
                return;
            }
        } else if(p.consume_string(R"("brightness":)")) {
            auto brightness = p.read_int();
            if(!brightness) {
                printf("Invalid brightness\n");
                return;
            }
            light_value = *brightness * 0xFFF / 255;
            printf("Brightness: %d\n", *brightness);
        } else {
            printf("Invalid key\n");
            return;
        }
        if(p.consume_char(',')) {
            continue;
        }
        if(p.consume_char('}')) {
            break;
        }
        printf("expected ',' or '}'\n");
        return;
    }
    publish_state();
}

static const char *get_my_id() {
    // Use MAC address for Station as unique ID
    static char my_id[13];
    static bool my_id_done = false;
    int8_t i;
    uint8_t x;
    if(my_id_done)
        return my_id;
    if(!sdk_wifi_get_macaddr(STATION_IF, (uint8_t *) my_id))
        return nullptr;
    for (i = 5; i >= 0; --i) {
        x = my_id[i] & 0x0F;
        if(x > 9) x += 7;
        my_id[i * 2 + 1] = x + '0';
        x = my_id[i] >> 4;
        if(x > 9) x += 7;
        my_id[i * 2] = x + '0';
    }
    my_id[12] = '\0';
    my_id_done = true;
    return my_id;
}

const char disco_msg_fmt[] = R"json({ "name": "Fiatlux Test %s",
                                    "unique_id": "fiatlux_%s",
                                 "command_topic": "fiatlux/light/fiatlux_%s/set",
                                 "state_topic": "fiatlux/light/fiatlux_%s/state",
                                 "brightness": true, "schema": "json" })json";
const char disco_topic_fmt[] = "homeassistant/light/fiatlux_%s/config";
const char state_topic_fmt[] = "fiatlux/light/fiatlux_%s/state";
const char set_topic_fmt[] = "fiatlux/light/fiatlux_%s/set";

extern "C" void mqtt_task(void *pvParameters) {
    int ret = 0;
    mqtt_network network{};
    mqtt_client_t client = mqtt_client_default;
    char mqtt_client_id[20];
    uint8_t mqtt_buf[1000];
    uint8_t mqtt_readbuf[1000];
    mqtt_packet_connect_data_t data = mqtt_packet_connect_data_initializer;

    mqtt_network_new(&network);
    snprintf(mqtt_client_id, sizeof(mqtt_client_id), "fiatlux-%s", get_my_id());
    char disco_msg[sizeof(disco_msg_fmt) + 24];
    snprintf(disco_msg, sizeof(disco_msg), disco_msg_fmt, &get_my_id()[6], &get_my_id()[6], &get_my_id()[6],
             &get_my_id()[6]);
    char disco_topic[sizeof(disco_topic_fmt) + 6];
    snprintf(disco_topic, sizeof(disco_topic), disco_topic_fmt, &get_my_id()[6]);
    char state_topic[sizeof(state_topic_fmt) + 6];
    snprintf(state_topic, sizeof(state_topic), state_topic_fmt, &get_my_id()[6]);
    char set_topic[sizeof(set_topic_fmt) + 6];
    snprintf(set_topic, sizeof(set_topic), set_topic_fmt, &get_my_id()[6]);

    while (true) {
        xSemaphoreTake(wifi_alive, portMAX_DELAY);
        printf("%s: started\n\r", __func__);
        printf("%s: (Re)connecting to MQTT server %s ... ", __func__,
               MQTT_HOST);
        ret = mqtt_network_connect(&network, MQTT_HOST, MQTT_PORT);
        if(ret) {
            printf("error 1: %d\n\r", ret);
            vTaskDelay(1000 / portTICK_PERIOD_MS);
            continue;
        }
        printf("done\n\r");
        mqtt_client_new(&client, &network, 5000, mqtt_buf, 1000,
                        mqtt_readbuf, 1000);

        data.willFlag = 0;
        data.MQTTVersion = 3;
        data.clientID.cstring = mqtt_client_id;
        data.username.cstring = MQTT_USER;
        data.password.cstring = MQTT_PASS;
        data.keepAliveInterval = 10;
        data.cleansession = 0;
        printf("Send MQTT connect ... ");
        ret = mqtt_connect(&client, &data);
        if(ret) {
            printf("error 2: %d\n\r", ret);
            mqtt_network_disconnect(&network);
            vTaskDelay(1000 / portTICK_PERIOD_MS);
            continue;
        }
        printf("done\r\n");

        //Mqtt auto dicovery

        mqtt_subscribe(&client, set_topic, MQTT_QOS1, topic_received);
        //mqtt_subscribe(&client, "fiatlux/light/fiatlux_test/state", MQTT_QOS1, topic_received);
        xQueueReset(publish_queue);

        {
            mqtt_message_t message;
            message.payload = (void *) disco_msg;
            message.payloadlen = strlen(disco_msg);
            message.dup = 0;
            message.qos = MQTT_QOS1;
            message.retained = 0;
            ret = mqtt_publish(&client, disco_topic, &message);
            if(ret != MQTT_SUCCESS) {
                printf("error: %d %s\n", ret, disco_msg);
            }
        }

        while (true) {

            char msg[PUB_MSG_LEN - 1] = "\0";
            while (xQueueReceive(publish_queue, (void *) msg, 0) ==
                   pdTRUE) {
                mqtt_message_t message;
                message.payload = msg;
                message.payloadlen = strlen(msg);
                message.dup = 0;
                message.qos = MQTT_QOS1;
                message.retained = 0;
                ret = mqtt_publish(&client, state_topic, &message);
                if(ret != MQTT_SUCCESS) {
                    printf("error while publishing message: %d\n", ret);
                    break;
                }
            }

            ret = mqtt_yield(&client, 1000);
            if(ret == MQTT_DISCONNECTED)
                break;
        }
        printf("Connection dropped, request restart\n\r");
        mqtt_network_disconnect(&network);
        taskYIELD();
    }
}