import rclpy
from rclpy.node import Node
from std_msgs.msg import Int32
import serial

class ArduinoBridge(Node):
    def __init__(self):
        super().__init__('arduino_bridge')

        self.publisher_ = self.create_publisher(Int32, 'water_level', 10)
        self.subscription = self.create_subscription(
            Int32,
            'servo_angle',
            self.servo_callback,
            10)

        self.ser = serial.Serial('/dev/ttyUSB0', 9600, timeout=1)

        self.create_timer(0.5, self.read_sensor)

    def read_sensor(self):
        if self.ser.in_waiting > 0:
            line = self.ser.readline().decode().strip()
            if line.startswith('WATER:'):
                level = int(line.split(':')[1])
                msg = Int32()
                msg.data = level
                self.publisher_.publish(msg)
                self.get_logger().info(f'Water level: {level}')

    def servo_callback(self, msg):
        angle = str(msg.data) + '\n'
        self.ser.write(angle.encode())
        self.get_logger().info(f'Sent servo angle: {msg.data}')

def main(args=None):
    rclpy.init(args=args)
    node = ArduinoBridge()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

