#include "MQTTClient.h"
#include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include "json.h"
#include "remote_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>

namespace sweeperMsg = sweeper_interfaces::msg;

Json::Value root;
Json::Reader reader;
Json::Value data;
std::string command;
std::string remote_topic;
int time_cunt = 0;
// constexpr auto ADDRESS = "tcp://36.153.162.171:19583"; // 更改此处地址
constexpr auto ADDRESS = "tcp://192.168.4.196:11883";     // 更改此处地址
constexpr auto CLIENTID_REMOTE = "SWEEPER_CLIENT_REMOTE"; // 更改此处客户端ID

const char *getRemoteTopic()
{
    return remote_topic.c_str();
}

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

volatile MQTTClient_deliveryToken deliveredtoken;

char remote_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(remote_buff, 0, sizeof(remote_buff));
    memcpy(&remote_buff, (char *)message->payload, message->payloadlen);

    root.clear();

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

        data = root["data"];
        command = root["command"].asString();

        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("steering") == 0)
        {
            car_ctrl_mes.steering = root["value"].asFloat();
        }
        else if (command.compare("brake") == 0)
        {
            car_ctrl_mes.brake = root["value"].asFloat();
        }
        else if (command.compare("sweep") == 0)
        {
            car_ctrl_mes.sweep = 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_remote(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 *REMOTE_TOPIC = getRemoteTopic();

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

    if ((rc = MQTTClient_create(&client, ADDRESS, CLIENTID_REMOTE, 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", REMOTE_TOPIC, CLIENTID_REMOTE, QOS);
    if ((rc = MQTTClient_subscribe(client, REMOTE_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, REMOTE_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 remote_node : public rclcpp::Node
{
public:
    // 构造函数,有一个参数为节点名称
    remote_node(std::string name) : Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str());
        msg_publisher_ = this->create_publisher<sweeperMsg::McCtrl>("ctrl_command", 10);
        timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&remote_node::timer_callback, this));
    }

private:
    void timer_callback()
    {
        sweeperMsg::McCtrl message;
        message.gear = car_ctrl_mes.gear;
        if (car_ctrl_mes.gear == 0) // N档
            message.gear = 0;
        else if (car_ctrl_mes.gear == 1) // D档
            message.gear = 2;
        else if (car_ctrl_mes.gear == 2) // R档
            message.gear = 1;

        // 刹车
        message.brake = (car_ctrl_mes.brake > 0) ? true : false;

        // 油门 映射到 [0, 1000] 范围
        message.rpm = car_ctrl_mes.throttle / 65535.0f * 1000.0f;

        // 转向
        // 0~32200 -> -40~0  33200~65535 -> 0~40
        float steering = 0;
        if (car_ctrl_mes.steering < 32200)
        {
            const double originalWidth1 = 32200.0;
            const double targetWidth1 = 40.0;

            double unitLength1 = targetWidth1 / originalWidth1;

            steering = -40 + static_cast<float>(car_ctrl_mes.steering * unitLength1);
        }
        else if (car_ctrl_mes.steering > 33200)
        {
            const double originalWidth2 = 32355.0; // 注意这里是从 33200 到 65535，共计 32355 个数
            const double targetWidth2 = 40.0;

            double unitLength2 = targetWidth2 / originalWidth2;

            steering = static_cast<float>((car_ctrl_mes.steering - 32200) * unitLength2);
        }
        else
        {
            steering = 0.0; // 朝向前方
        }
        message.angle = steering;

        message.sweep = car_ctrl_mes.sweep;

        msg_publisher_->publish(message);
    }

    // 声名定时器指针
    rclcpp::TimerBase::SharedPtr timer_;
    // 声明话题发布者指针
    rclcpp::Publisher<sweeperMsg::McCtrl>::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))
    {
        remote_topic = root["mqtt"]["remote_topic"].asString();
    }
}

pthread_t mqtt_remote_thread_t;

int main(int argc, char **argv)
{
    init_main();
    memset(&car_ctrl_mes, 0, sizeof(car_ctrl_mes));
    pthread_create(&mqtt_remote_thread_t, NULL, mqtt_remote, NULL);

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

    return 0;
}