#include <ros.h>
#include <std_msgs/String.h>

ros::NodeHandle nh;

#define LASER_PIN 3
#define SERVO_PIN 5

// 使用char数组或String存储状态
String currentstate_laser = "close";
String currentstate_servo = "close";

// 激光回调：接收 "yes" 打开，其他关闭
void laserCallback(const std_msgs::String& msg) {
    currentstate_laser = msg.data;
    if (currentstate_laser == "yes") {
        analogWrite(LASER_PIN, 255);  // 激光全开
    } else {
        analogWrite(LASER_PIN, 0);    // 激光关闭
    }
}

// 舵机回调：接收 "servo_1" 转到90度
void servoCallback(const std_msgs::String& msg) {
    currentstate_servo = msg.data;
    if (currentstate_servo == "servo_1") {
        analogWrite(SERVO_PIN, 23);   // 约90度（舵机PWM: 0.5ms~2.5ms对应0~180度）
    } else {
        analogWrite(SERVO_PIN, 0);     // 0度位置
    }
}

// 修正：订阅者变量名不能重复，且类型名大小写要正确
ros::Subscriber<std_msgs::String> sub_laser("/caim/target_recognition_result", &laserCallback);
ros::Subscriber<std_msgs::String> sub_servo("payload_drop_cmd", &servoCallback);

// 修正：发布者变量名不能重复
std_msgs::String feedbackMsg_laser;
ros::Publisher pub_laser("laser_feedback", &feedbackMsg_laser);

std_msgs::String feedbackMsg_servo;
ros::Publisher pub_servo("servo_feedback", &feedbackMsg_servo);

void setup() {
    pinMode(LASER_PIN, OUTPUT);
    analogWrite(LASER_PIN, 0);
    
    pinMode(SERVO_PIN, OUTPUT);
    analogWrite(SERVO_PIN, 0);
    
    nh.initNode();
    
    // 注册两个订阅者
    nh.subscribe(sub_laser);
    nh.subscribe(sub_servo);
    
    // 注册两个发布者
    nh.advertise(pub_laser);
    nh.advertise(pub_servo);
}

void loop() {
    nh.spinOnce();
    
    // 发布反馈状态
    feedbackMsg_laser.data = currentstate_laser.c_str();
    pub_laser.publish(&feedbackMsg_laser);
    
    feedbackMsg_servo.data = currentstate_servo.c_str();
    pub_servo.publish(&feedbackMsg_servo);
    
    delay(20);
}