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

// 全局定义起始时间变量和定时器、全局发布器变量
ros::Time g_start_time;
ros::Timer g_timer;
ros::Publisher angle_pub_global;
ros::Publisher speed_pub_global;
cpp_tools::gameController m_contoller("/dev/input/js0",0.2);

// 全局变量用于模式切换
bool g_is_manual_mode = false;          // false: 自动模式, true: 手动模式
bool g_last_start_button_state = false; // 用于检测START按键的上升沿（按下瞬间）

// 自定义clamp函数，确保值在指定的最小和最大范围内
template<typename T>
T mclamp(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;

    // --- 新的舵机标定参数 (2025-06-26 更新) ---
    const double MAX_LEFT_ANGLE_DEG  = -30.0;
    const double MAX_RIGHT_ANGLE_DEG = 30.0;
    const double STRAIGHT_ANGLE_THRESHOLD_DEG = 5.0; 
    const double PWM_AT_MAX_LEFT      = 6.0;
    const double PWM_AT_CENTER_LEFT   = 7.2;
    const double PWM_AT_CENTER_RIGHT  = 10.0;
    const double PWM_AT_MAX_RIGHT     = 11.5;
    const double PWM_AT_ZERO_ANGLE = 8.25; 
    // --- 标定参数结束 ---

    angle_deg = mclamp(angle_deg, MAX_LEFT_ANGLE_DEG, MAX_RIGHT_ANGLE_DEG);
    double pwm_val;

    if (std::abs(angle_deg) <= STRAIGHT_ANGLE_THRESHOLD_DEG) {
        pwm_val = PWM_AT_ZERO_ANGLE;
    } else if (angle_deg < -STRAIGHT_ANGLE_THRESHOLD_DEG) {
        double turn_ratio = (angle_deg - MAX_LEFT_ANGLE_DEG) / (-STRAIGHT_ANGLE_THRESHOLD_DEG - MAX_LEFT_ANGLE_DEG);
        pwm_val = PWM_AT_MAX_LEFT + turn_ratio * (PWM_AT_CENTER_LEFT - PWM_AT_MAX_LEFT);
    } else { // angle_deg > STRAIGHT_ANGLE_THRESHOLD_DEG
        double turn_ratio = (angle_deg - STRAIGHT_ANGLE_THRESHOLD_DEG) / (MAX_RIGHT_ANGLE_DEG - STRAIGHT_ANGLE_THRESHOLD_DEG);
        pwm_val = PWM_AT_CENTER_RIGHT + turn_ratio * (PWM_AT_MAX_RIGHT - PWM_AT_CENTER_RIGHT);
    }
    return pwm_val;
}

// 将车辆的速度转换为油门PWM信号的函数 (此函数未修改)
double speedToThrottlePwm(double speed) {
    speed = mclamp(speed, -1000.0, 1000.0);
    double pwm = 9.0;

    if (speed > 0.01) {
        pwm = 9.4 + (speed / 1000.0);
    } else if (speed < -0.05) {
        pwm = 8.7 + (speed / 1000.0);
    } else {
        pwm = 9.0;
    }
    
    auto info = m_contoller.get2();
    if (info.X) {
        pwm = 9.3;
    }

    return pwm;
}

// 定时器回调函数 (此函数未修改)
void timerCallback(const ros::TimerEvent&)
{
    double elapsed = (ros::Time::now() - g_start_time).toSec();
    if (elapsed < 3.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("3 seconds passed, controller is now active.");
        g_timer.stop();
    }
}

// 接收转向控制消息的回调函数 (此函数未修改)
// 增加了时间判断，确保前3秒不处理消息
void steeringControlCallback(const std_msgs::Float64::ConstPtr& msg)
{
    if (g_is_manual_mode || (ros::Time::now() - g_start_time).toSec() < 3.0) {
        return;
    }

    std_msgs::Float64 output;
    double anglePwmOutput = angleToPwm(msg->data);
    output.data = anglePwmOutput;
    angle_pub_global.publish(output);
    ROS_INFO("STEERING CALLBACK: ANGLE_PWM: %.2f", output.data);
}

// 接收电机速度控制消息的回调函数 (此函数未修改)
// 增加了时间判断，确保前3秒不处理消息
void motorSpeedControlCallback(const std_msgs::Float64::ConstPtr& msg)
{
    if (g_is_manual_mode || (ros::Time::now() - g_start_time).toSec() < 3.0) {
        return;
    }

    std_msgs::Float64 output;
    double speedPwmOutput = speedToThrottlePwm(msg->data);
    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::NodeHandle nh;

    g_start_time = ros::Time::now();

    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);

    g_timer = nh.createTimer(ros::Duration(1.0), timerCallback);

    ros::Rate loop_rate(50);

    while (ros::ok())
    {
        // 关键修改：只有在程序运行3秒后，才开始处理手柄输入和模式切换
        if ((ros::Time::now() - g_start_time).toSec() >= 3.0)
        {
            auto controller_state = m_contoller.get2();

            // 检测START键是否被按下，用于切换模式
            if (controller_state.START && !g_last_start_button_state) {
                g_is_manual_mode = !g_is_manual_mode; // 切换模式
                if (g_is_manual_mode) {
                    ROS_INFO("----> Switched to MANUAL mode <----");
                    std_msgs::Float64 stop_msg;
                    stop_msg.data = 9.0;
                    angle_pub_global.publish(stop_msg);
                    speed_pub_global.publish(stop_msg);
                } else {
                    ROS_INFO("----> Switched to AUTOMATIC mode <----");
                }
            }
            g_last_start_button_state = controller_state.START;

            // 如果是手动模式，执行手动控制逻辑
            if (g_is_manual_mode) {
                std_msgs::Float64 angle_msg, speed_msg;
                
                angle_msg.data = 9.0;
                speed_msg.data = 9.0;

                if (controller_state.LS_lr > 0.5) {
                    angle_msg.data = 7.0;
                } else if (controller_state.LS_lr < -0.5) {
                    angle_msg.data = 10.0;
                }

                if (controller_state.LS_fr > 0.5) {
                    speed_msg.data = 10.0;
                } else if (controller_state.LS_fr < -0.5) {
                    speed_msg.data = 8.0;
                }
                
                angle_pub_global.publish(angle_msg);
                speed_pub_global.publish(speed_msg);
            }
        }

        // ros::spinOnce() 必须在循环中持续调用，以确保timerCallback能正常运行
        ros::spinOnce();
        
        loop_rate.sleep();
    }

    return 0;
}