#include "MQTTClient.h"
#include "rclcpp/rclcpp.hpp"
#include "interface/msg/ctrl.hpp"
#include "json.h"
#include "sub_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>

Json::Value root;
Json::Reader reader;
Json::Value data;
std::string command;
std::string sub_topic;
int time_cunt = 0;
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_JM"; // 更改此处客户端ID

// constexpr auto SUB_TOPIC = "/vehicle/ctrl/V020001";
const char *getSubTopic()
{
    return sub_topic.c_str();
}

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

volatile MQTTClient_deliveryToken deliveredtoken;

char sub_buff[500];

car_ctrl car_ctrl_mes;

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;
    time_cunt = 0;
    printf("Message arrived\n");
    printf("topic: %s\n", topicName);
    printf("message: %.*s\n", message->payloadlen, (char *)message->payload);

    memset(sub_buff, 0, sizeof(sub_buff));
    memcpy(&sub_buff, (char *)message->payload, message->payloadlen);

    root.clear();

    if (!reader.parse(sub_buff, root))
    {
        printf("recv json fail\n");
        return 1;
    }
    else
    {
        std::cout << root << std::endl;

        data = root["data"];
        command = root["command"].asString();
        std::cout << command << std::endl;
        // if (command.isNull())
        // {
        //     MQTTClient_freeMessage(&message);
        //     MQTTClient_free(topicName);
        //     return 1;
        // }

        if (command.compare("mode") == 0)
        {
            car_ctrl_mes.mode = root["value"].asInt();
        }
        else if (command.compare("gear") == 0)
        {
            car_ctrl_mes.gear = root["value"].asInt();
        }
        else if (command.compare("throttle") == 0)
        {
            car_ctrl_mes.throttle = root["value"].asFloat();
        }
        else if (command.compare("brake") == 0)
        {
            car_ctrl_mes.brake = root["value"].asFloat();
        }
        else if (command.compare("steering") == 0)
        {
            car_ctrl_mes.steering = root["value"].asFloat();
            // std::cout << car_ctrl_mes.steering << std::endl;
        }
        else if (command.compare("epb") == 0)
        {
            car_ctrl_mes.epb = root["value"].asInt();
        }
        else if (command.compare("door") == 0)
        {
            car_ctrl_mes.door = root["value"].asInt();
        }
        else if (command.compare("turnLight") == 0)
        {
            car_ctrl_mes.turn_light = root["value"].asInt();
        }
        else if (command.compare("warning") == 0)
        {
            car_ctrl_mes.warning = root["value"].asInt();
        }
        else if (command.compare("headlamp") == 0)
        {
            car_ctrl_mes.headlamp = root["value"].asInt();
        }
        else if (command.compare("fogLight") == 0)
        {
            car_ctrl_mes.fog_light = root["value"].asInt();
        }
        else if (command.compare("acMode") == 0)
        {
            car_ctrl_mes.ac_mode = root["value"].asInt();
        }
    }

    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(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 *SUB_TOPIC = getSubTopic();

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

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

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

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

    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 sub_node : public rclcpp::Node
{
public:
    // 构造函数,有一个参数为节点名称
    sub_node(std::string name) : Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str());
        msg_publisher_ = this->create_publisher<interface::msg::CTRL>("ctrl_command", 10);
        timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&sub_node::timer_callback, this));
    }

private:
    void timer_callback()
    {
        time_cunt++;
        interface::msg::CTRL message;
        // if (time_cunt < 10) // 1s
        message.mode = car_ctrl_mes.mode;
        // else
        //     message.mode = 0;
        // std::cout << "message.mode:" << message.mode << std::endl;
        message.gear = car_ctrl_mes.gear;
        message.brake = car_ctrl_mes.brake;
        message.throttle = car_ctrl_mes.throttle;
        // std::cout << "message.throttle:" << message.throttle << std::endl;
        message.steering = car_ctrl_mes.steering;
        // std::cout << "message.steering:" << message.steering << std::endl;
        // std::cout << "car_ctrl_mes.steering:" << static_cast<int>(message.steering) << std::endl;
        message.epb = car_ctrl_mes.epb;
        message.door = car_ctrl_mes.door;
        message.headlamp = car_ctrl_mes.headlamp;
        message.turn_light = car_ctrl_mes.turn_light;
        // std::cout << "===============sub warning:" << car_ctrl_mes.warning << std::endl;
        message.warning = car_ctrl_mes.warning;
        // std::cout << "=================msg warning:" << static_cast<int>(message.warning) << std::endl;

        message.fog_light = car_ctrl_mes.fog_light;
        message.ac_mode = car_ctrl_mes.ac_mode;

        msg_publisher_->publish(message);
    }

    // 声名定时器指针
    rclcpp::TimerBase::SharedPtr timer_;
    // 声明话题发布者指针
    rclcpp::Publisher<interface::msg::CTRL>::SharedPtr msg_publisher_;
};

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))
    {
        sub_topic = root["mqtt"]["sub_topic"].asString();
    }
}

pthread_t mqtt_sub_thread_t;

int main(int argc, char **argv)
{
    init_main();
    memset(&car_ctrl_mes, 0, sizeof(car_ctrl_mes));
    car_ctrl_mes.steering = 32767.0;
    pthread_create(&mqtt_sub_thread_t, NULL, mqtt_sub, NULL);

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

    return 0;
}