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

ros::NodeHandle nh;

#define LASER_PIN 3

uint8_t currentBrightness = 0;

void laserCallback(const std_msgs::UInt8& msg) {
    currentBrightness = msg.data;
    analogWrite(LASER_PIN, currentBrightness);
}

ros::Subscriber<std_msgs::UInt8> sub("laser_brightness", &laserCallback);

std_msgs::UInt8 feedbackMsg;
ros::Publisher pub("laser_feedback", &feedbackMsg);

void setup() {
    pinMode(LASER_PIN, OUTPUT);
    analogWrite(LASER_PIN, 0);
    
    nh.initNode();
    nh.subscribe(sub);
    nh.advertise(pub);
}

void loop() {
    nh.spinOnce();
    
    feedbackMsg.data = currentBrightness;
    pub.publish(&feedbackMsg);
    
    delay(20);
}