#include "rclcpp/rclcpp.hpp"
#include "interface/msg/rtk.hpp"
#include "interface/msg/pl.hpp"
#include "interface/msg/route.hpp"
#include "interface/msg/ctrl.hpp"
#include "interface/msg/route.hpp"

#include <pthread.h>
#include <string>

#include "pl.hpp"
#include "traffic_light.hpp"

pthread_t plThread_t;

class pl_node : public rclcpp::Node
{
public:
    pl_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(&pl_node::msg_callback,
                                                                                  this,
                                                                                  std::placeholders::_1));
        route_subscribe_jingmen = this->create_subscription<interface::msg::ROUTE>("ROUTE_JINGMEN",
                                                                                   10,
                                                                                   std::bind(&pl_node::route_callback_jingmen,
                                                                                             this,
                                                                                             std::placeholders::_1));
        route_subscribe_ = this->create_subscription<interface::msg::ROUTE>("ROUTE",
                                                                            10,
                                                                            std::bind(&pl_node::route_callback,
                                                                                      this,
                                                                                      std::placeholders::_1));
        command_subscribe_ = this->create_subscription<interface::msg::CTRL>("ctrl_command",
                                                                             10,
                                                                             std::bind(&pl_node::command_callback,
                                                                                       this,
                                                                                       std::placeholders::_1));

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

private:
    // 收到话题数据的回调函数
    void msg_callback(const interface::msg::RTK::SharedPtr msg)
    {
        // RCLCPP_INFO(this->get_logger(), "lat:'%.9lf',lon:'%.9lf',head:'%lf',speed:'%lf',p_quality:'%d',h_quality:'%d'",
        //             msg->lat, msg->lon, msg->head, msg->speed, msg->p_quality, msg->h_quality);

        if (msg->p_quality < 4 || msg->h_quality < 4)
        {
            g_rtk.reliability = 0;
            RCLCPP_INFO(this->get_logger(), "rtk信号差,停车!");
        }
        else
        {
            g_rtk.reliability = 1;
        }

        g_rtk.lat = msg->lat;
        g_rtk.lon = msg->lon;
        g_rtk.direction = msg->head;
    }

    void command_callback(const interface::msg::CTRL::SharedPtr msg)
    {
        last_mode = mode;
        mode = msg->mode;
#ifdef ZXWL

        if (mode != 1 && last_mode == 1)
        {
            pthread_cancel(plThread_t);
            std::cout << "Controlled mode stopped." << std::endl;
        }
        else if (mode == 1 && last_mode != 1)
        {
            pthread_create(&plThread_t, NULL, pl_thread, NULL);
            std::cout << "Auto driving started." << std::endl;
        }

#endif // #ifdef ZXWL
    }

    void route_callback(const interface::msg::ROUTE::SharedPtr msg)
    {
        filename = msg->file;
        std::cout << "route filename: " << filename << std::endl;
        // d_file = 1;
    }

    void route_callback_jingmen(const interface::msg::ROUTE::SharedPtr msg)
    {
        routeId = msg->route_id;
        d_file = 1;
    }

    void timer_callback()
    {
        // 创建消息
        interface::msg::PL message;
        message.x = x;
        message.y = y;

        message.door = command_door;
        message.speed = (float)pl_speed;

        // 1开始减速 0恢复速度
        if (slowdown_flag == 1) // 进站 开始减速
            message.drive_mode = 2;
        else if (slowdown_flag == 0) // 出站 恢复速度
        {
            if (drive_mode == Direction::STRAIGHT_D) // 直行
                message.drive_mode = 0;
            else if (drive_mode == Direction::LEFT_TURN || drive_mode == Direction::RIGHT_TURN) // 转弯
                message.drive_mode = 1;
        }

        message.id_route = routeId;
        message.id_station = id_station;

        // std::cout << "[ pl_node ] : " << routeId << std::endl;

        // std::cout << "[ pl_node ] : "
        //           << "x : " << message.x
        //           << " == y : " << message.y
        //           << " == drive_mode : " << message.drive_mode
        //           << std::endl;

        // 发布消息
        msg_publisher_->publish(message);
    }

    // 声名定时器指针
    rclcpp::TimerBase::SharedPtr timer_;
    // 声明话题发布者指针
    rclcpp::Publisher<interface::msg::PL>::SharedPtr msg_publisher_;
    // 声明订阅者
    rclcpp::Subscription<interface::msg::RTK>::SharedPtr msg_subscribe_;
    rclcpp::Subscription<interface::msg::ROUTE>::SharedPtr route_subscribe_;
    rclcpp::Subscription<interface::msg::ROUTE>::SharedPtr route_subscribe_jingmen;
    rclcpp::Subscription<interface::msg::CTRL>::SharedPtr command_subscribe_;
};

int main(int argc, char **argv)
{
    // std::thread TrafficLightMsgThread(mqtt_sub_TrafficLightMsg);
#ifdef ZXWL

#else
    pthread_create(&plThread_t, NULL, pl_thread, NULL);
#endif // #ifdef ZXWL

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

    // TrafficLightMsgThread.join();
    pthread_join(plThread_t, NULL);

    return 0;
}
// message.aim_x.clear();
// message.aim_y.clear();
// message.aim_x.resize(19);
// message.aim_y.resize(19);
// for (int i = 0; i < 19; i++)
// {
//     message.aim_x[i] = aim_x[i];
//     message.aim_y[i] = aim_y[i];
// }