#include "rclcpp/rclcpp.hpp"
#include "pub_jingmen_node.hpp"
#include "interface/msg/rtk.hpp"
#include "interface/msg/pl.hpp"
#include "json.h"
#include "MQTTClient.h"

#include <fstream>
#include <iostream>
#include <string>
#include <chrono>
#include <ctime>
#include <unistd.h>
#include <string.h>
#include <time.h>
#include <sstream>
#include <iomanip>
#include <pthread.h>

std::string pub_jingmen_topic;
constexpr auto ADDRESS = "tcp://vccs.jmbigdata.cn:1883";    // 荆门
constexpr auto CLIENTID_PUB = "MINIBUS_CLIENT_PUB_jingmen"; // 更改此处客户端ID

// constexpr auto PUB_TOPIC = "/vehicle/info/V020001";
const char *getPubTopic()
{
    return pub_jingmen_topic.c_str();
}

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

GPS gps_mes;
std::string id_route = "e6f09885791749d8a26968c21a5d3905"; // 内圈
int id_station;

void *mqtt_pub_jingmen(void *arg)
{
    (void)arg;
    MQTTClient client;
    MQTTClient_deliveryToken token_d_m;

    MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer;
    MQTTClient_message pubmsg_d_m = MQTTClient_message_initializer;
    const char *PUB_jingmen_TOPIC = getPubTopic();

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

    unsigned char msg_st[200];
    memset(msg_st, 0, 200);

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

    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 NULL;
    }

    pubmsg_d_m.payload = msg_st;
    pubmsg_d_m.payloadlen = 1;
    pubmsg_d_m.qos = QOS;
    pubmsg_d_m.retained = 0;

    Json::Value root_st;
    std::string json_string;
    Json::FastWriter json_writer;

    while (1)
    {
        memset(msg_st, 0, 200);
        root_st.clear();

        std::ostringstream stream_1;
        stream_1 << std::fixed << std::setprecision(7) << gps_mes.lng;
        root_st["lng"] = stream_1.str(); // 7

        std::ostringstream stream_2;
        stream_2 << std::fixed << std::setprecision(7) << gps_mes.lat;
        root_st["lat"] = stream_2.str(); // 7

        // root_st["routeId"] = "e6f09885791749d8a26968c21a5d3905"; // 内圈
        root_st["routeId"] = id_route; // 内圈
        std::cout << "[ pub_jingmen ] : " << id_route << std::endl;

        std::ostringstream stream_4;
        stream_4 << id_station;
        root_st["stationId"] = stream_4.str(); //

        json_string = json_writer.write(root_st);
        const char *c_temp = json_string.c_str();
        memcpy(msg_st, c_temp, json_string.size());
        std::cout << json_string << std::endl;
        pubmsg_d_m.payloadlen = (int)json_string.size();

        MQTTClient_publishMessage(client, PUB_jingmen_TOPIC, &pubmsg_d_m, &token_d_m);
        printf("Published to %s\n", PUB_jingmen_TOPIC);
        rc = MQTTClient_waitForCompletion(client, token_d_m, TIMEOUT);

        usleep(500000); // 200ms
    }

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

    MQTTClient_destroy(&client);
    return NULL;
}

class pub_jingmen_node : public rclcpp::Node
{
public:
    // 构造函数,有一个参数为节点名称
    pub_jingmen_node(std::string name) : Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str());

        gps_subscribe_ = this->create_subscription<interface::msg::RTK>("rtk_message",
                                                                        10,
                                                                        std::bind(&pub_jingmen_node::command_callback, this, std::placeholders::_1));

        pl_subscribe_ = this->create_subscription<interface::msg::PL>("pl_message",
                                                                      10,
                                                                      std::bind(&pub_jingmen_node::command_callback_pl, this, std::placeholders::_1));
    }

private:
    void command_callback(const interface::msg::RTK::SharedPtr msg)
    {
        gps_mes.lat = msg->lat;
        gps_mes.lng = msg->lon;
    }
    void command_callback_pl(const interface::msg::PL::SharedPtr msg)
    {
        id_route = msg->id_route;
        id_station = msg->id_station;
    }

    rclcpp::Subscription<interface::msg::RTK>::SharedPtr gps_subscribe_;
    rclcpp::Subscription<interface::msg::PL>::SharedPtr pl_subscribe_;
};

void init_main()
{
    Json::Reader reader;
    Json::Value root;
    std::ifstream in("config.json", std::ios::binary);
    if (!in.is_open())
    {
        std::cout << "read config file error" << std::endl;
        return;
    }
    if (reader.parse(in, root))
    {
        pub_jingmen_topic = root["mqtt"]["pub_jingmen_topic"].asString();
    }
}

pthread_t mqtt_pub_jingmen_thread_t;

int main(int argc, char **argv)
{
    init_main();
    pthread_create(&mqtt_pub_jingmen_thread_t, NULL, mqtt_pub_jingmen, NULL);

    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<pub_jingmen_node>("pub_jingmen_node");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();

    return 0;
}