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

#define LASER_PIN     3   // 激光笔 3号口
#define SERVO_PIN     5   // 舵机 5号口
#define SERVO_DEFAULT 0   // 默认角度：0度
#define SERVO_ACTIVE  70  // 触发角度：70度

Servo myServo;
ros::NodeHandle nh;

// 1. 激光自动复位定时器
bool isLaserAuto = false;
unsigned long laserStartTime = 0;

// 2. 舵机自动复位定时器
bool isServoAuto = false;
unsigned long servoStartTime = 0;

// ============================================
// 1. 激光控制回调 (target_recognition_rusult)
// ============================================
void laserCb(const std_msgs::String& msg) {
  String cmd = String(msg.data);
  cmd.toLowerCase();

  if (cmd == "yes") {
    // 自动模式：亮激光，启动 2 秒定时
    digitalWrite(LASER_PIN, HIGH);
    isLaserAuto = true;
    laserStartTime = millis();
  } 
  else if (cmd == "laser_on") {
    // 手动常亮
    digitalWrite(LASER_PIN, HIGH);
    isLaserAuto = false; 
  }
  else if (cmd == "off") {
    // 手动关闭
    digitalWrite(LASER_PIN, LOW);
    isLaserAuto = false; 
  }
}

// ============================================
// 2. 舵机控制回调 (/payload_drop_cmd)
// ============================================
void servoCb(const std_msgs::String& msg) {
  String cmd = String(msg.data);
  cmd.toLowerCase();

  if (cmd == "servo_1") {
    // 自动模式：转动 90 度，启动 2 秒定时
    myServo.write(SERVO_ACTIVE);
    isServoAuto = true;
    servoStartTime = millis();
  } 
  else if (cmd == "servo_on") {
    // 手动保持 90 度
    myServo.write(SERVO_ACTIVE);
    isServoAuto = false;
  }
  else if (cmd == "servo_off") {
    // 手动回到 0 度
    myServo.write(SERVO_DEFAULT);
    isServoAuto = false;
  }
}

ros::Subscriber<std_msgs::String> subTarget("target_recognition_rusult", &laserCb);
ros::Subscriber<std_msgs::String> subVision("/payload_drop_cmd", &servoCb);

void setup() {
  pinMode(LASER_PIN, OUTPUT);
  digitalWrite(LASER_PIN, LOW); // 默认关闭激光

  myServo.attach(SERVO_PIN);
  myServo.write(SERVO_DEFAULT); // 默认舵机归零

  nh.initNode();
  nh.subscribe(subTarget);
  nh.subscribe(subVision);
}

void loop() {
  unsigned long now = millis();

  // 激光 2 秒自动复位
  if (isLaserAuto && (now - laserStartTime >= 2000)) {
    digitalWrite(LASER_PIN, LOW);
    isLaserAuto = false;
  }

  // 舵机 2 秒自动复位
  if (isServoAuto && (now - servoStartTime >= 2000)) {
    myServo.write(SERVO_DEFAULT);
    isServoAuto = false;
  }

  nh.spinOnce();
  delay(1);
}