#include "radio_ctrl/uart_handler.h"
#include "radio_ctrl/get_config.h"
#include <iostream>
#include <algorithm>
#include <rclcpp/rclcpp.hpp>
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include "sweeper_interfaces/msg/can_frame.hpp"

namespace sweeperMsg = sweeper_interfaces::msg;

constexpr float DEG_PER_SEC_TO_RPM = 1.0f / 6.0f;
constexpr float GEAR_RATIO = 7.0f;
constexpr float DELTA_T = 0.02f; // 20ms
const uint16_t speed[3] = {192, 992, 1792};

class SBUSNode : public rclcpp::Node
{
public:
    SBUSNode() : Node("sbus_node"), current_feedback_angle(0.0f)
    {
        if (load_config(config))
        {
            std::cout << "Serial Port: " << config.serial_port << "\n";
            std::cout << "Baudrate: " << config.baudrate << "\n";
            std::cout << "MCU RPM Max: " << config.mcu_rpm_max << "\n";
            std::cout << "EPS Angle Max: " << config.eps_angle_max << "\n";
        }

        // 发布控制指令消息的发布器
        pub_ = this->create_publisher<sweeperMsg::McCtrl>("radio_mc_ctrl", 10);

        // 订阅CAN反馈的回调函数
        can_sub_ = this->create_subscription<sweeperMsg::CanFrame>(
            "can_data", 10,
            std::bind(&SBUSNode::can_callback, this, std::placeholders::_1));

        // 初始化串口读取（读取遥控器数据）
        uart_handler_ = std::make_shared<UartHandler>(config.serial_port, config.baudrate);
        if (!uart_handler_->open_serial())
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to open serial port!");
            return;
        }
        uart_handler_->start_reading(); // 启动串口后台读线程

        // 创建周期定时器（每20ms回调一次控制逻辑，50Hz）
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(20),
            std::bind(&SBUSNode::timer_callback, this));
    }

    ~SBUSNode()
    {
        if (uart_handler_)
        {
            uart_handler_->stop_reading();
        }
    }

private:
    // 定时器回调函数，用于周期性发布控制指令
    void timer_callback()
    {
        static int MCU_RPM_MAX = config.mcu_rpm_max;
        static float EPS_ANGLE_MAX = config.eps_angle_max;

        bool data_safe = uart_handler_->get_data_safe(); // 获取数据安全性

        auto msg = sweeperMsg::McCtrl(); // 控制消息对象
        uint16_t ch_data[8];             // 各通道遥控数据

        if (data_safe) // 数据安全，进行数据解析并发布
        {
            // 赋值与打印
            for (int i = 0; i < 10; ++i)
            {
                ch_data[i] = uart_handler_->get_channel_value(i);
                // printf("ch[%d]:%d   ", i, ch_data[i]);
            }
            // printf("\n");

            if (ch_data[6] == 192) // 是否使能车辆控制
            {
                msg.brake = 0;
                // 挡位选择
                if (ch_data[7] == 192)
                {
                    msg.gear = 0; // D挡
                }
                else if (ch_data[7] == 1792)
                {
                    msg.gear = 1; // R挡
                }

                // 油门 / 刹车逻辑
                if (ch_data[1] <= speed[1])
                {
                    msg.brake = 1;
                    msg.rpm = 0;
                }
                else
                {
                    msg.brake = 0;
                    msg.rpm = static_cast<uint8_t>(MCU_RPM_MAX * (ch_data[1] - speed[1]) / (speed[2] - speed[1]));
                }

                // 一键清扫
                if (ch_data[5] == 1792)
                {
                    msg.edge_brush_lift = false;
                    msg.sweep_ctrl = false;
                    msg.spray = false;
                    msg.mud_flap = false;
                    msg.dust_shake = false;
                }
                else if (ch_data[5] == 192)
                {
                    msg.edge_brush_lift = true;
                    msg.sweep_ctrl = true;
                    msg.spray = true;
                    msg.mud_flap = true;
                    msg.dust_shake = false;
                }

                // 转向逻辑
                float target_angle = (992 - static_cast<float>(ch_data[3])) / 800 * EPS_ANGLE_MAX;

                // 角速度（度/秒）
                float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T;

                // 电机转速（单位：rpm）
                float motor_rpm = angle_speed * DEG_PER_SEC_TO_RPM * GEAR_RATIO;

                // 限制电机转速到 [120, 1500] 范围，防止过小/过大
                uint16_t can_speed = std::clamp(
                    static_cast<uint16_t>(motor_rpm),
                    static_cast<uint16_t>(120),
                    static_cast<uint16_t>(1500));

                msg.angle = target_angle;
                msg.angle_speed = can_speed;
            }
            else // 未使能状态，发送安全默认控制指令
            {
                msg.brake = 1;
                msg.gear = 0;
                msg.rpm = 0;
                msg.angle = 0;
                msg.angle_speed = 120;
                msg.edge_brush_lift = false;
                msg.sweep_ctrl = false;
                msg.spray = false;
                msg.mud_flap = false;
                msg.dust_shake = false;
            }

            // 发布控制消息
            pub_->publish(msg);

            RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:"
                                                       << "\n  brake: " << static_cast<int>(msg.brake)                           // uint8
                                                       << "\n  gear: " << static_cast<int>(msg.gear)                             // uint8
                                                       << "\n  rpm: " << static_cast<int>(msg.rpm)                               // uint8
                                                       << "\n  ehb_enable: " << static_cast<int>(msg.ehb_anable)                 // bool
                                                       << "\n  ehb_brake_pressure: " << static_cast<int>(msg.ehb_brake_pressure) // uint8
                                                       << "\n  angle: " << msg.angle                                             // float32
                                                       << "\n  angle_speed: " << msg.angle_speed                                 // uint16
                                                       << "\n  edge_brush_lift: " << static_cast<int>(msg.edge_brush_lift)       // bool
                                                       << "\n  sweep_ctrl: " << static_cast<int>(msg.sweep_ctrl)                 // bool
                                                       << "\n  spray: " << static_cast<int>(msg.spray)                           // bool
                                                       << "\n  mud_flap: " << static_cast<int>(msg.mud_flap)                     // bool
                                                       << "\n  dust_shake: " << static_cast<int>(msg.dust_shake)                 // bool
                                                       << "\n  left_light: " << static_cast<int>(msg.left_light)                 // bool
                                                       << "\n  right_light: " << static_cast<int>(msg.right_light)               // bool
                                                       << "\n  brake_light: " << static_cast<int>(msg.brake_light)               // bool
                                                       << "\n  headlight: " << static_cast<int>(msg.headlight)                   // bool
            );
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "Waiting for radio control data...");
            return; // 下次循环，等待数据安全
        }
    }

    // CAN反馈回调函数（用于获取当前转向角度）
    void can_callback(const sweeperMsg::CanFrame::SharedPtr msg)
    {
        // 判断是否为转向反馈帧（ID=0x401，且数据长度大于5）
        if (msg->id == 0x401 && msg->dlc >= 5)
        {
            // 第3、4字节拼接为原始角度数据
            uint16_t raw_value = static_cast<uint16_t>((msg->data[3] << 8) | msg->data[4]);

            // 数据偏移：减去1024并除以5，得到角度（单位：度）
            current_feedback_angle = (raw_value - 1024) / 7.0f;
        }
    }

    // ROS2 发布/订阅/定时器/串口读取器
    rclcpp::Publisher<sweeperMsg::McCtrl>::SharedPtr pub_;
    rclcpp::Subscription<sweeperMsg::CanFrame>::SharedPtr can_sub_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::shared_ptr<UartHandler> uart_handler_;

    float current_feedback_angle; // 当前反馈角度（从CAN中读取）
    RmoCtlConfig config;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<SBUSNode>()); // 启动节点
    rclcpp::shutdown();
    return 0;
}