#include "rclcpp/rclcpp.hpp"
#include "interface/msg/rtk.hpp"
#include "serial_read.hpp"

#include <string.h>
#include <stdlib.h>
#include <queue>

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

        lat = 0.0;
        lon = 0.0;
        head = 0.0;
        speed = 0.0;

        // 初始化串口读取
        boost_serial = new Boost_serial();
        boost_serial->init("/dev/ttyUSB0");

        // 创建发布者
        command_publisher_ = this->create_publisher<interface::msg::RTK>("rtk_message", 10);
        // 创建定时器，100ms为周期，定时发布
        timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&rtk_node::timer_callback, this));
    }

private:
    void GPYBM_to_gps(char serial_buf[])
    {
        position = 0;
        line_string.clear();

        char *p_gpybm = strstr(serial_buf, "GPYBM");

        if (p_gpybm == NULL)
        {
            cout << "未检测到GPYBM字符串!" << endl;
            return;
        }

        line_string = p_gpybm;

        for (int i = 0; i < 3; i++)
        {
            position = line_string.find(",");
            line_string.erase(0, position + 1);
        }

        position = line_string.find(",");
        lat_s = line_string.substr(0, position);
        line_string.erase(0, position + 1);

        position = line_string.find(",");
        lon_s = line_string.substr(0, position);
        line_string.erase(0, position + 1);

        position = line_string.find(",");
        line_string.erase(0, position + 1);

        position = line_string.find(",");
        head_s = line_string.substr(0, position);
        line_string.erase(0, position + 1);

        for (int i = 0; i < 4; i++)
        {
            position = line_string.find(",");
            line_string.erase(0, position + 1);
        }

        position = line_string.find(",");
        sp_s = line_string.substr(0, position);
        line_string.erase(0, position + 1);

        for (int i = 0; i < 4; i++)
        {
            position = line_string.find(",");
            line_string.erase(0, position + 1);
        }

        position = line_string.find(",");
        p_q_s = line_string.substr(0, position);
        line_string.erase(0, position + 1);

        position = line_string.find(",");
        h_q_s = line_string.substr(0, position);
        line_string.erase(0, position + 1);

        lat = atof(lat_s.c_str());
        lon = atof(lon_s.c_str());
        head = atof(head_s.c_str());
        speed = atof(sp_s.c_str());
        p_quality = atof(p_q_s.c_str());
        h_quality = atof(h_q_s.c_str());
    }

    void timer_callback()
    {
        // 读取串口传来的定位信息
        memset(serial_buf, 0, sizeof(serial_buf));
        int num = boost_serial->serial_read(serial_buf, 200);
        // printf("num:%d \n",num);
        // printf("serial_buf:  ");
        // for (int i = 0; i < 300; i++)
        // {
        //     printf("%c", serial_buf[i]);
        // }
        // printf("\n");

        if (c_queue.size() >= 400)
        {
            std::queue<char> empty;
            swap(empty, c_queue);
        }

        for (int i = 0; i < num; i++)
        {
            if (serial_buf[i] == '*')
            {
                // c_queue.push(serial_buf[i]);
                memset(gps_buf, 0, sizeof(gps_buf));
                int j = 0;
                while (!c_queue.empty())
                {
                    gps_buf[j] = c_queue.front();
                    c_queue.pop();
                    j++;
                }
                // 解析定位信息

                // printf("gps_buf:  ");
                // for (int i = 0; i < 300; i++)
                // {
                //     printf("%c", gps_buf[i]);
                // }
                // printf("\n");

                GPYBM_to_gps(gps_buf);

                // 创建消息
                auto message = interface::msg::RTK();

                message.lat = lat;
                message.lon = lon;
                message.head = head;
                message.speed = speed;
                message.p_quality = p_quality;
                message.h_quality = h_quality;

                // 日志打印
                RCLCPP_INFO(this->get_logger(), "lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'",
                            message.lat, message.lon, message.head, message.speed, message.p_quality, message.h_quality);
                // 发布消息
                command_publisher_->publish(message);
            }
            else
            {
                c_queue.push(serial_buf[i]);
            }
        }
    }

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

    // 串口读取类指针
    Boost_serial *boost_serial;

    // 串口读取buffer
    char serial_buf[300];
    char gps_buf[300];

    double lat;
    double lon;
    double head;
    double speed;
    int p_quality; // 定位解状态：0=未定位或无效解 1=单点定位 4=定位RTK固定解 5=定位RTK浮点解
    int h_quality; // 定向解状态：0=未定位或无效解 1=单点定位 4=定位RTK固定解 5=定位RTK浮点解

    std::queue<char> c_queue;
    // 解析定位信息用到的中间变量
    string lat_s, lon_s, head_s, sp_s, p_q_s, h_q_s;
    int position;
    string line_string;
};

int main(int argc, char **argv)
{
    unsigned int a = -1;
    printf("%u\n", a);
    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<rtk_node>("rtk_node");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}