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

#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_gps_topic;
constexpr auto ADDRESS = "tcp://36.153.162.171:61883"; // 更改此处地址
// constexpr auto ADDRESS = "tcp://192.168.3.241:61883"; // 更改此处地址
// constexpr auto ADDRESS = "tcp://vccs.jmbigdata.cn:1883";    // 荆门
constexpr auto CLIENTID_PUB_GPS = "MINIBUS_CLIENT_PUB_GPS_JM"; // 更改此处客户端ID

const char *getPubGPSTopic()
{
    return pub_gps_topic.c_str();
}

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

GPS gps_mes;

void *mqtt_pub_gps(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_GPS_TOPIC = getPubGPSTopic();

    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_GPS, 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

        std::ostringstream stream_3;
        stream_3 << std::fixed << std::setprecision(1) << gps_mes.course;
        root_st["course"] = stream_3.str(); // 1

        root_st["mode"] = gps_mes.mode;

        auto now = std::chrono::system_clock::now();
        auto duration = now.time_since_epoch();
        auto millis = std::chrono::duration_cast<std::chrono::milliseconds>(duration).count();
        root_st["timestamp"] = millis;

        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_GPS_TOPIC, &pubmsg_d_m, &token_d_m);
        printf("Published to %s\n", PUB_GPS_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_gps_node : public rclcpp::Node
{
public:
    // 构造函数,有一个参数为节点名称
    pub_gps_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_gps_node::command_callback, this, std::placeholders::_1));
    }

private:
    void command_callback(const interface::msg::RTK::SharedPtr msg)
    {
        gps_mes.lat = msg->lat;
        gps_mes.lng = msg->lon;
        gps_mes.course = msg->head;

        // 差分状态（定位）0：未定位 1：单点解 2：浮点解 3：固定解
        //          ^
        //          |
        // 定位解状态：0=未定位或无效解 1=单点定位 4=定位RTK固定解 5=定位RTK浮点解
        if (msg->p_quality == 5)
            gps_mes.mode = 2;
        else if (msg->p_quality == 4)
            gps_mes.mode = 3;
        else
            gps_mes.mode = msg->p_quality;
    }

    rclcpp::Subscription<interface::msg::RTK>::SharedPtr gps_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_gps_topic = root["mqtt"]["pub_gps_topic"].asString();
    }
}

pthread_t mqtt_pub_gps_thread_t;

int main(int argc, char **argv)
{
    init_main();
    // gps_mes.lat = 32.0316690680;
    // gps_mes.lng = 120.91536519100;
    // gps_mes.course = 86.898000;
    // gps_mes.mode = 4;

    pthread_create(&mqtt_pub_gps_thread_t, NULL, mqtt_pub_gps, NULL);

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

    return 0;
}