"""
camera_tf.launch.py — Static TF transforms for 4× IMX219 surround camera array

Sensor head geometry:
  All cameras mount on a circular sensor head (radius 5cm) at the same height.
  Cameras point outward at 90° intervals.
  The sensor_head_link frame is the centre of the ring.

  camera_front_link:  x=+0.05, yaw=  0°  (faces robot +X / forward)
  camera_left_link:   y=+0.05, yaw=+90°  (faces robot +Y / left)
  camera_rear_link:   x=-0.05, yaw=180°  (faces robot -X / rear)
  camera_right_link:  y=-0.05, yaw=-90°  (faces robot -Y / right)

  Camera optical frame convention: z=forward, x=right, y=down
  We publish the body frame here; optical frame offset is per-camera calibration.

Parent frame: sensor_head_link (expected to be published by robot URDF/state publisher)
"""

import math
from launch import LaunchDescription
from launch_ros.actions import Node


def _static_tf(parent, child, x=0.0, y=0.0, z=0.0, yaw_deg=0.0):
    """Return a static_transform_publisher node (x y z yaw pitch roll parent child)."""
    yaw = math.radians(yaw_deg)
    # static_transform_publisher args: x y z yaw pitch roll frame_id child_frame_id
    return Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name=f"tf_{child.replace('_link', '')}",
        arguments=[
            str(x), str(y), str(z),
            str(yaw), "0.0", "0.0",
            parent, child,
        ],
        output="screen",
    )


def generate_launch_description():
    r = 0.05  # sensor head radius (metres) — adjust to actual hardware

    return LaunchDescription([
        # Front camera: faces +X (robot forward)
        _static_tf(
            "sensor_head_link", "camera_front_link",
            x=r, y=0.0, z=0.0, yaw_deg=0.0,
        ),
        # Left camera: faces +Y (robot left)
        _static_tf(
            "sensor_head_link", "camera_left_link",
            x=0.0, y=r, z=0.0, yaw_deg=90.0,
        ),
        # Rear camera: faces -X (robot rear)
        _static_tf(
            "sensor_head_link", "camera_rear_link",
            x=-r, y=0.0, z=0.0, yaw_deg=180.0,
        ),
        # Right camera: faces -Y (robot right)
        _static_tf(
            "sensor_head_link", "camera_right_link",
            x=0.0, y=-r, z=0.0, yaw_deg=-90.0,
        ),
    ])
