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

#include <pthread.h>
#include <stdlib.h>
#include <math.h>
#include <unistd.h>

using namespace std;

#define deg_rad (0.01745329252) // Transfer from angle degree to rad
#define R_LATI (6378137)
#define R_LONT (5407872) // 这个需要根据所在区域的纬度进行换算：R_LONT = R_LATI*cos(所在地的纬度转化为弧度)

string file_name = "common/txt/gps_load_now.txt";

int need_gather = -1;

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

double last_g_Lattitude = 0;
double last_g_Longitude = 0;
double last_g_Heading = 0;

int file_point_num = 0;
FILE *p = NULL;

int station_num = 0;

double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2)
{
    double x, y, length;
    x = (R_LONT) * (lonti2 - lonti1) * deg_rad; // 弧长公式
    y = (R_LATI) * (lati2 - lati1) * deg_rad;
    length = sqrt(x * x + y * y);
    return length;
}

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

        msg_subscribe_ = this->create_subscription<interface::msg::RTK>("rtk_message", 10, std::bind(&gather_node::msg_callback, this, std::placeholders::_1));
        gather_subscribe_ = this->create_subscription<interface::msg::GATHER>("GATHER", 10, std::bind(&gather_node::gather_callback, this, std::placeholders::_1));
        const char *file;
        file = file_name.c_str();
        std::cout << "file: " << file << std::endl;
        p = fopen(file, "w");
    }
    ~gather_node()
    {
        fclose(p);
    }

private:
    void msg_callback(const interface::msg::RTK::SharedPtr msg)
    {
        lat = msg->lat;
        lon = msg->lon;
        head = msg->head;
        p_quality = msg->p_quality;
        h_quality = msg->h_quality;
        std::cout << "gather lat" << lat << " msg lat:" << msg->lat << std::endl;
    }

    void gather_callback(const interface::msg::GATHER::SharedPtr msg)
    {
        need_gather = msg->gather;
        file_name = msg->file;
        printf("%d\n", need_gather);
        // std::cout << "need_gather:" << need_gather << std::endl;
        std::cout << "file_name:" << file_name.c_str() << std::endl;
        if (need_gather == 1)
        {
            const char *file;
            file = file_name.c_str();
            std::cout << "file: " << file << std::endl;
            p = fopen(file, "w");
        }
    }

    rclcpp::Subscription<interface::msg::RTK>::SharedPtr msg_subscribe_;

    rclcpp::Subscription<interface::msg::GATHER>::SharedPtr gather_subscribe_;
};

void gather_thread(void)
{
    // int file_exit = 0;
    while (true)
    {
        // if (need_gather == 1)
        {
            printf("采点:\n");
            if (file_point_num == 0 || last_g_Lattitude == 0.0)
            {
                last_g_Lattitude = lat;
                last_g_Longitude = lon;
                last_g_Heading = head;
                file_point_num++;
            }
            else
            {
                double distance = ntzx_GPS_length(last_g_Longitude, last_g_Lattitude, lon, lat);
                printf("distance: %lf ... speed: %lf\n", distance, speed);
                // printf("last_g_Longitude: %lf ... last_g_Lattitude: %lf\n", last_g_Longitude, last_g_Lattitude);
                // printf("lon: %lf ... lat: %lf\n", lon, lat);
                if (p_quality < 4 || h_quality < 4)
                {
                    cout << "定位信号质量差,检测下一个点！" << endl;
                    continue;
                }
                if (distance >= 1.0) // 每隔1米采样一次
                {
                    fprintf(p, "%.10lf\n%.11lf\n%lf\n%lf\n", lat, lon, head, speed);
                    fflush(p);
                    last_g_Lattitude = lat;
                    last_g_Longitude = lon;
                    last_g_Heading = head;
                    // file_point_num++;
                    station_num++;

                    printf("######################\n");
                    printf("hit!\n");
                    printf("######################\n");
                }
            }
        }
        // else if (need_gather == 0)
        // {
        //     if (p != NULL)
        //     {
        //         fclose(p);
        //         // file_exit = 0;
        //     }
        //     need_gather = -1;
        // }
        usleep(100000);
    }
}

void hit_thread(void)
{
    FILE *fp_stations = fopen("common/txt/stations.txt", "w");
    if (!fp_stations)
    {
        std::cerr << "Failed to open stations.txt." << std::endl;
        return;
    }

    FILE *fp_stop = fopen("common/txt/stop.txt", "w");
    if (!fp_stop)
    {
        std::cerr << "Failed to open stop.txt." << std::endl;
        return;
    }

    FILE *fp_lanes = fopen("common/txt/lanes.txt", "w");
    if (!fp_lanes)
    {
        std::cerr << "Failed to open lanes.txt." << std::endl;
        return;
    }

    usleep(5000);
    char ch;
    system("stty -icanon"); // 关闭缓冲区，输入字符无需按回车键直接接受
    int i = 0, j = 0, k = 0;
    while ((ch = getchar()) != EOF)
    {
        if (ch == 'a')
        {
            i++;
            cout << "\n========station!" << endl;
            fprintf(fp_stations, "%d ", station_num);
            if (i % 3 == 0)
                fprintf(fp_stations, "\n");
            fflush(fp_stations);
        }

        if (ch == 's')
        {
            j++;
            cout << "\n========stop line!" << endl;
            fprintf(fp_stop, "%d ", station_num);
            if (j % 3 == 0)
                fprintf(fp_stop, "\n");
            fflush(fp_stop);
        }

        if (ch == 'd')
        {
            k++;
            cout << "\n========stop line!" << endl;
            fprintf(fp_lanes, "%d ", station_num);
            if (k % 3 == 0)
                fprintf(fp_lanes, "\n");
            fflush(fp_lanes);
        }
    }
}

int main(int argc, char **argv)
{
    std::thread hitThread(hit_thread);
    std::thread gatherThread(gather_thread);

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

    hitThread.join();
    gatherThread.join();

    return 0;
}