import rclpy
from rclpy.node import Node
from rcl_interfaces.msg import Log
import csv
import os

class RosoutLogger(Node):
    def __init__(self):
        super().__init__('rosout_logger_csv')

        # Log file path
        self.log_dir = '/BA/rosout_logs'
        os.makedirs(self.log_dir, exist_ok=True)
        self.csv_file_path = os.path.join(self.log_dir, 'rosout_log.csv')

        # Initialize CSV with header if it doesn't exist
        if not os.path.exists(self.csv_file_path):
            with open(self.csv_file_path, mode='w', newline='') as f:
                writer = csv.writer(f)
                writer.writerow(['timestamp', 'level', 'logger_name', 'message'])

        # Subscribe to /rosout
        self.subscription = self.create_subscription(
            Log,
            '/rosout',
            self.rosout_callback,
            10
        )

    def rosout_callback(self, msg: Log):
        # Format timestamp
        timestamp = f"{msg.stamp.sec}.{str(msg.stamp.nanosec).zfill(9)}"

        # Convert level integer to text
        level_dict = {
            Log.DEBUG: 'DEBUG',
            Log.INFO: 'INFO',
            Log.WARN: 'WARN',
            Log.ERROR: 'ERROR',
            Log.FATAL: 'FATAL'
        }
        level_text = level_dict.get(msg.level, f"LEVEL_{msg.level}")

        # Write row to CSV
        with open(self.csv_file_path, mode='a', newline='') as f:
            writer = csv.writer(f)
            writer.writerow([timestamp, level_text, msg.name, msg.msg])

def main():
    rclpy.init()
    node = RosoutLogger()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("Interrupted by user.")
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()