#include "rclcpp/rclcpp.hpp"
#include "pub_chassis_node.hpp"
#include "interface/msg/mc.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_chassis_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 = "MINIBUS_CLIENT_PUB_CHASSIS_JM"; // 更改此处客户端ID

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

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

car_state car_st_mes;

void *mqtt_pub_chassis(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_CHASSIS_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();

        root_st["type"] = 1;
        root_st["mode"] = 1;
        root_st["gear"] = car_st_mes.gear;
        root_st["door"] = car_st_mes.door;
        root_st["warning"] = car_st_mes.warning;
        root_st["epb"] = car_st_mes.ebp;
        root_st["turnLight"] = car_st_mes.turn_light;
        root_st["headlamp"] = car_st_mes.headlamp;
        root_st["fogLight"] = car_st_mes.fog_light;
        root_st["acMode"] = car_st_mes.ac_mode;
        root_st["horn"] = car_st_mes.horn;

        int power = static_cast<int>(car_st_mes.power);
        root_st["power"] = power;

        int mileage = static_cast<int>(car_st_mes.mileage);
        root_st["mileage"] = mileage;

        int speed = static_cast<int>(car_st_mes.speed);
        root_st["speed"] = speed;

        // int steering = static_cast<int>(car_st_mes.steering_angle);
        root_st["steering"] = car_st_mes.steering_angle;

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

        root_st["radar"] = 1;
        root_st["uss"] = 1;
        root_st["lidar"] = 1;
        root_st["gateway"] = 1;
        root_st["rtk"] = 1;

        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_CHASSIS_TOPIC, &pubmsg_d_m, &token_d_m);
        printf("Published to %s\n", PUB_CHASSIS_TOPIC);
        rc = MQTTClient_waitForCompletion(client, token_d_m, TIMEOUT);

        usleep(500000);
    }

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

        st_subscribe_ = this->create_subscription<interface::msg::MC>("mc_message", 10, std::bind(&pub_chassis_node::command_callback, this, std::placeholders::_1));
    }

private:
    void command_callback(const interface::msg::MC::SharedPtr msg)
    {
        car_st_mes.ebp = msg->ebp;
        car_st_mes.gear = msg->gear;
        car_st_mes.door = msg->door;
        car_st_mes.warning = msg->warning;
        car_st_mes.turn_light = msg->turn_light;
        car_st_mes.headlamp = msg->headlamp;
        car_st_mes.fog_light = msg->fog_light;
        car_st_mes.ac_mode = msg->ac_mode;
        car_st_mes.horn = msg->horn;
        car_st_mes.power = msg->power;
        car_st_mes.mileage = msg->mileage;
        car_st_mes.speed = msg->speed;
        car_st_mes.steering_angle = msg->steering_angle;
    }

    rclcpp::Subscription<interface::msg::MC>::SharedPtr st_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_chassis_topic = root["mqtt"]["pub_chassis_topic"].asString();
    }
}

pthread_t mqtt_pub_chassis_thread_t;

int main(int argc, char **argv)
{
    init_main();
    pthread_create(&mqtt_pub_chassis_thread_t, NULL, mqtt_pub_chassis, NULL);

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

    return 0;
}