#include <iostream>
#include "traffic_light.hpp"
#include "MQTTClient.h"

std::vector<TrafficLight> ros_traffic_light;

// constexpr auto ADDRESS = "tcp://36.153.162.171:61883"; // 更改此处地址
constexpr auto ADDRESS = "tcp://192.168.3.241:61883";               // 更改此处地址
constexpr auto CLIENTID_SUB = "MINIBUS_CLIENT_SUB_TrafficLightMsg"; // 更改此处客户端ID

constexpr auto QOS = 1;
constexpr auto TIMEOUT = 10000L;

volatile MQTTClient_deliveryToken deliveredtoken;

void delivered(void *context, MQTTClient_deliveryToken dt)
{
    (void)context;
    printf("Message with token value %d delivery confirmed\n", dt);
    deliveredtoken = dt;
}

int msgarrvd(void *context, char *topicName, int topicLen, MQTTClient_message *message)
{
    (void)context;
    (void)topicLen;

    char sub_buff[1024];
    memset(sub_buff, 0, sizeof(sub_buff));
    memcpy(sub_buff, message->payload, message->payloadlen);

    std::string payload(sub_buff, message->payloadlen);

    json j;
    try
    {
        j = json::parse(payload);
    }
    catch (const json::parse_error &e)
    {
        std::cerr << "JSON parse error: " << e.what() << std::endl;
        return 1;
    }

    // 清除之前的数据
    ros_traffic_light.clear();

    try
    {
        for (const auto &item : j.at("TLM"))
        {
            TrafficLight tl;
            tl.color = item.at("color").get<int>();
            tl.id = item.at("id").get<uint32_t>();
            tl.type = item.at("type").get<int>();
            tl.confidence = item.at("confidence").get<double>();
            ros_traffic_light.push_back(tl);
        }
    }
    catch (const json::out_of_range &e)
    {
        std::cerr << "JSON out of range error: " << e.what() << std::endl;
        return 1;
    }

    // 验证
    // for (const auto &tl : ros_traffic_light)
    // {
    //     std::cout << "Color: " << static_cast<int>(tl.color) << ", "
    //               << "ID: " << tl.id << ", "
    //               << "Type: " << static_cast<int>(tl.type) << ", "
    //               << "Confidence: " << tl.confidence << std::endl;
    // }

    MQTTClient_freeMessage(&message);
    MQTTClient_free(topicName);
    return 1;
}

void connlost(void *context, char *cause)
{
    (void)context;
    printf("\nConnection lost\n");
    printf("cause: %s\n", cause);
}

void mqtt_sub_TrafficLightMsg(void)
{
    MQTTClient client;
    // MQTTClient_deliveryToken token_d_m;

    MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer;
    // MQTTClient_message pubmsg_d_m = MQTTClient_message_initializer;
    const char *SUB_TOPIC = "/perception/ros1/TrafficLightMsg";

    int rc;
    char *username = NULL;
    char *password = NULL;

    if ((rc = MQTTClient_create(&client, ADDRESS, CLIENTID_SUB, MQTTCLIENT_PERSISTENCE_NONE, NULL)) != MQTTCLIENT_SUCCESS)
    {
        printf("Failed to create client, return code %d\n", rc);
        MQTTClient_destroy(&client);
        return;
    }

    if ((rc = MQTTClient_setCallbacks(client, NULL, connlost, msgarrvd, delivered)) != MQTTCLIENT_SUCCESS)
    {
        printf("Failed to set callbacks, return code %d\n", rc);
        MQTTClient_destroy(&client);
        return;
    }

    conn_opts.keepAliveInterval = 20;
    conn_opts.cleansession = 1;
    conn_opts.username = username;
    conn_opts.password = password;

    if ((rc = MQTTClient_connect(client, &conn_opts)) != MQTTCLIENT_SUCCESS)
    {
        printf("Failed to connect, return code %d\n", rc);
        MQTTClient_destroy(&client);
        return;
    }

    printf("Subscribing to topic %s\nfor client %s using QoS%d\n\n", SUB_TOPIC, CLIENTID_SUB, QOS);
    if ((rc = MQTTClient_subscribe(client, SUB_TOPIC, QOS)) != MQTTCLIENT_SUCCESS)
    {
        printf("Failed to subscribe, return code %d\n", rc);
        MQTTClient_destroy(&client);
        return;
    }

    while (1)
    {
        usleep(200000);
    }

    if ((rc = MQTTClient_unsubscribe(client, SUB_TOPIC)) != MQTTCLIENT_SUCCESS)
    {
        printf("Failed to unsubscribe, return code %d\n", rc);
        MQTTClient_destroy(&client);
        return;
    }

    if ((rc = MQTTClient_disconnect(client, 10000)) != MQTTCLIENT_SUCCESS)
    {
        printf("Failed to disconnect, return code %d\n", rc);
        MQTTClient_destroy(&client);
        return;
    }

    MQTTClient_destroy(&client);
    return;
}
