#include "mqtt.h"
#include "config.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#ifndef BUFFER_SIZE
#define BUFFER_SIZE 64
#endif

#ifndef PUBLISH_INTERVAL_MS
#define PUBLISH_INTERVAL_MS 1000
#endif

#ifndef PUBLISHER_TASK_STACK_SIZE
#define PUBLISHER_TASK_STACK_SIZE 4096
#endif

#ifndef PUBLISHER_TASK_PRIORITY
#define PUBLISHER_TASK_PRIORITY 5
#endif

static esp_mqtt_client_handle_t client;

static void mqtt_event_handler(void *handler_args, esp_event_base_t base,
                               int32_t event_id, void *event_data) {
  esp_mqtt_event_handle_t event = event_data;

  switch (event->event_id) {
  case MQTT_EVENT_CONNECTED:
    printf("MQTT connected!\n");
    esp_mqtt_client_subscribe(event->client, SUB_TOPIC, 1);
    break;

  case MQTT_EVENT_DATA: {
    char topic[event->topic_len + 1];
    char data[event->data_len + 1];
    memcpy(topic, event->topic, event->topic_len);
    topic[event->topic_len] = '\0';
    memcpy(data, event->data, event->data_len);
    data[event->data_len] = '\0';

    printf("MQTT Msg received\nTopic: %s\nData: %s\n", topic, data);

    char *endptr;
    long long sent = strtoll(data, &endptr, 10);
    if (endptr != data) {
      long long now = esp_timer_get_time() / 1000ULL;
      long long rtt = now - sent;
      printf("RTT: %lld ms\n", rtt);
    }
    break;
  }

  default:
    break;
  }
}

static void publisher_task(void *pvParameters) {
  while (1) {
    char buf[BUFFER_SIZE];
    snprintf(buf, BUFFER_SIZE, "%lld",
             (long long)(esp_timer_get_time() / 1000ULL));
    esp_mqtt_client_publish(client, PUB_TOPIC, buf, 0, 1, 0);
    vTaskDelay(pdMS_TO_TICKS(PUBLISH_INTERVAL_MS));
  }
}

void mqtt_app_start(void) {
  esp_mqtt_client_config_t mqtt_cfg = {
      .broker.address.uri = MQTT_URI,
  };
  client = esp_mqtt_client_init(&mqtt_cfg);
  esp_mqtt_client_register_event(client, ESP_EVENT_ANY_ID, mqtt_event_handler,
                                 NULL);
  esp_mqtt_client_start(client);

  xTaskCreate(publisher_task, "publisher_task", PUBLISHER_TASK_STACK_SIZE, NULL,
              PUBLISHER_TASK_PRIORITY, NULL);
}