#include <rclcpp/rclcpp.hpp>
#include <linux/joystick.h>
#include <fcntl.h>
#include <unistd.h>
#include <iostream>
#include <string>
#include "rc_msgs/msg/ps2_data.hpp"  // 引入 Ps2Data 消息类型

class PS2ControllerNode : public rclcpp::Node
{
public:
    PS2ControllerNode() : Node("ps2_controller_node")
    {
        this->declare_parameter<std::string>("device", "/dev/input/js0");
        device_path_ = this->get_parameter("device").as_string();

        joystick_fd_ = open(device_path_.c_str(), O_RDONLY | O_NONBLOCK);
        if (joystick_fd_ < 0)
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to open device: %s", device_path_.c_str());
            return;
        }

        RCLCPP_INFO(this->get_logger(), "Connected to device: %s", device_path_.c_str());

        ps2_data_ = std::make_shared<rc_msgs::msg::Ps2Data>();
        publisher_ = this->create_publisher<rc_msgs::msg::Ps2Data>("ps2_data", 10);

        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(10),  // 100Hz
            std::bind(&PS2ControllerNode::publishData, this));
    }

    ~PS2ControllerNode()
    {
        if (joystick_fd_ >= 0)
        {
            close(joystick_fd_);
        }
    }

private:
    void publishData()
    {
        readJoystick();
        publisher_->publish(*ps2_data_);
    }

    void readJoystick()
    {
        struct js_event event;
        while (read(joystick_fd_, &event, sizeof(event)) > 0)
        {
            switch (event.type & ~JS_EVENT_INIT)
            {
            case JS_EVENT_BUTTON:
                handleButtonEvent(event);
                break;
            case JS_EVENT_AXIS:
                handleAxisEvent(event);
                break;
            default:
                break;
            }
        }
    }

    void handleButtonEvent(const js_event &event)
    {
        switch (event.number)
        {
        case 10: ps2_data_->select = event.value; break;
        case 11: ps2_data_->start = event.value; break;
        case 6: ps2_data_->l1 = event.value; break;
        case 8: ps2_data_->l2 = event.value; break;
        case 7: ps2_data_->r1 = event.value; break;
        case 9: ps2_data_->r2 = event.value; break;
        case 3: ps2_data_->x = event.value; break;
        case 4: ps2_data_->y = event.value; break;
        case 0: ps2_data_->a = event.value; break;
        case 1: ps2_data_->b = event.value; break;
        default:
            break;
        }
    }

    void handleAxisEvent(const js_event &event)
    {
        constexpr float MAX_AXIS_VALUE = 32767.0f;  // 最大轴值
        constexpr float MIN_AXIS_VALUE = -32768.0f; // 最小轴值

        auto normalize = [](int value) -> float {
            return static_cast<float>(value) / MAX_AXIS_VALUE;
        };

        switch (event.number)
        {
        case 0: ps2_data_->lx = normalize(event.value); break;
        case 1: ps2_data_->ly = normalize(event.value); break;
        case 2: ps2_data_->rx = normalize(event.value); break;
        case 3: ps2_data_->ry = normalize(event.value); break;
        case 6: ps2_data_->left_right = normalize(event.value); break;
        case 7: ps2_data_->up_down = normalize(event.value); break;
        default:
            break;
        }
    }

    std::string device_path_;
    int joystick_fd_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<rc_msgs::msg::Ps2Data>::SharedPtr publisher_;
    std::shared_ptr<rc_msgs::msg::Ps2Data> ps2_data_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<PS2ControllerNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}