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

#define SafetyLightPIN 13

ros::NodeHandle nh;

std_msgs::Bool autonomous_mode_msg;

void autonomousModeCallback(const std_msgs::Bool& msg)
{
  autonomous_mode_msg = msg;
}

void setup()
{
  pinMode(8, OUTPUT); // Set pin 8 as output
  digitalWrite(8, LOW); // Initialize output to LOW

  nh.initNode();
  nh.subscribe("/autonomous_mode", &autonomousModeCallback);
}

void loop()
{

  if (nh.connected() && autonomous_mode_msg.data)
  {
    digitalWrite(SafetyLightPIN, HIGH); // Set output to HIGH
  }
  else
  {
    digitalWrite(SafetyLightPIN, LOW); // Set output to LOW
  }

  nh.spinOnce();
  delay(100);

}