//输入转向角左转为正，右转为负
#include "ros/ros.h"                    // 包含ROS的基本头文件
#include "std_msgs/Float64.h"           // 包含用于发布和订阅64位浮点数的消息类型
#include <iostream>                     // C++标准输入输出库
#include <iomanip>                      // 用于格式化输出
#include <cmath>
using namespace std;

// 全局定义起始时间变量和定时器、全局发布器变量
ros::Time g_start_time;
ros::Timer g_timer;
ros::Publisher angle_pub_global;
ros::Publisher speed_pub_global;

// 自定义clamp函数，确保值在指定的最小和最大范围内
template<typename T>
T clamp(T val, T minVal, T maxVal) {
    return max(minVal, min(maxVal, val));  // 返回三者中的中间值
}

// 将车辆的转向角度转换为PWM信号的函数
double angleToPwm(double input_angle_rad) {
    // 1. 将输入的弧度转换为度，并反转符号
    double angle_deg = -input_angle_rad * 180.0 / M_PI;
    angle_deg = clamp(angle_deg, -30.0, 30.0);
    double pwm_val;
    // 3. 应用分段线性映射
    if (angle_deg <= -4.0) {
        pwm_val = 6.0 + (angle_deg - (-30.0)) * (7.2 - 6.0) / (-4.0 - (-30.0));
    } else if (angle_deg < 4.0) { // angle_deg > -4.0 且 angle_deg < 4.0
        pwm_val = 7.2 + (angle_deg - (-4.0)) * (10.8 - 7.2) / (4.0 - (-4.0));
    } else { // angle_deg >= 4.0
        pwm_val = 10.8 + (angle_deg - 4.0) * (12.0 - 10.8) / (30.0 - 4.0);
    }
    return pwm_val;
}

// 将车辆的速度转换为油门PWM信号的函数
double speedToThrottlePwm(double speed) {
    speed = clamp(speed, -1000.0, 1000.0);  // 限制速度在-1000到1000 RPM之间
    double pwm = 9.0;  // 默认PWM值

    if (speed > 0.01) {
        pwm = 9.4+(speed / 1000.0);  // 正速映射到9.3以上的PWM值
    } else if (speed < -0.05) {
        pwm = 8.7 + (speed / 1000.0);  // 负速映射到8.7以下的PWM值
    } else {
        pwm = 9.0;
    }

    return pwm;  // 返回计算得到的PWM值
}

// 定时器回调函数：在程序启动后的前10秒每秒发布一次默认PWM值9.0
void timerCallback(const ros::TimerEvent&)
{
    double elapsed = (ros::Time::now() - g_start_time).toSec();
    if (elapsed < 5.0)
    {
        std_msgs::Float64 msg;
        msg.data = 9.0;
        angle_pub_global.publish(msg);
        speed_pub_global.publish(msg);
        ROS_INFO("TIMER OUTPUT: ANGLE_PWM: %.2f, SPEED_PWM: %.2f", msg.data, msg.data);
    }
    else
    {
        ROS_INFO("10秒已过，停止定时器输出.");
        g_timer.stop();  // 停止定时器
    }
}

// 接收转向控制消息的回调函数（10秒内忽略订阅的消息）
void steeringControlCallback(const std_msgs::Float64::ConstPtr& msg)
{
//    if ((ros::Time::now() - g_start_time).toSec() < 10.0)
//    {
//        // 前10秒内，不处理订阅消息（定时器已发布固定值）
//        return;
//    }
    std_msgs::Float64 output;  // 创建输出消息
    double anglePwmOutput = angleToPwm(msg->data);  // 计算转向PWM输出值
    output.data = anglePwmOutput;
    angle_pub_global.publish(output);  // 发布输出消息
    ROS_INFO("STEERING CALLBACK: ANGLE_PWM: %.2f", output.data);
}

// 接收电机速度控制消息的回调函数（10秒内忽略订阅的消息）
void motorSpeedControlCallback(const std_msgs::Float64::ConstPtr& msg)
{
    if ((ros::Time::now() - g_start_time).toSec() < 10.0)
    {
        // 前10秒内，不处理订阅消息（定时器已发布固定值）
        return;
    }
    std_msgs::Float64 output;  // 创建输出消息
    double speedPwmOutput = speedToThrottlePwm(msg->data);  // 计算电机PWM输出值
    output.data = speedPwmOutput;
    speed_pub_global.publish(output);  // 发布输出消息
    ROS_INFO("MOTOR CALLBACK: SPEED_PWM: %.2f", output.data);
}

// 主函数
int main(int argc, char **argv)
{
    ros::init(argc, argv, "vehicle_control");  // 初始化ROS节点，节点名为"vehicle_control"
    ros::NodeHandle nh;  // 创建节点句柄

    // 初始化全局起始时间
    g_start_time = ros::Time::now();

    // 创建两个发布者，用于发布角度和速度的PWM信号，并保存到全局变量中
    angle_pub_global = nh.advertise<std_msgs::Float64>("angle_pwm_output", 10);
    speed_pub_global = nh.advertise<std_msgs::Float64>("speed_pwm_output", 10);

    // 创建两个订阅者，用于订阅角度和速度的控制指令
    ros::Subscriber steering_sub = nh.subscribe("/vehicle/steering_control", 10, steeringControlCallback);
    ros::Subscriber motor_speed_sub = nh.subscribe("/vehicle/motor_speed_control", 10, motorSpeedControlCallback);

    // 创建定时器：前10秒每1秒调用一次timerCallback发布固定PWM值9.0
    g_timer = nh.createTimer(ros::Duration(1.0), timerCallback);

    ros::spin();  // 进入ROS事件循环，等待消息到来

    return 0;  // 正常退出程序
}