import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, Command
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue


def generate_launch_description():
    urdf_file = os.path.join(
        get_package_share_directory('raspbot_v2'), 'urdf', 'raspbot_v2.urdf.xacro'
    )
    controllers_config = os.path.join(
        get_package_share_directory('raspbot_v2'), 'config', 'controllers.yaml'
    )

    robot_description = ParameterValue(
        Command([
            'xacro ', urdf_file,
            ' wheel_separation:=', LaunchConfiguration('wheel_base'),
            ' controllers_config:=', controllers_config,
        ]),
        value_type=str,
    )

    return LaunchDescription([

        # ── Motor controller arguments ────────────────────────────────────
        DeclareLaunchArgument('wheel_base', default_value='0.3',
                              description='Distance between left and right wheels (m)'),
        DeclareLaunchArgument('max_speed',  default_value='1.0',
                              description='Maximum motor speed in library units'),

        # ── Ultrasonic sensor arguments ───────────────────────────────────
        DeclareLaunchArgument('ultrasonic_rate_hz', default_value='10.0',
                              description='Ultrasonic sensor publish rate (Hz)'),

        # ── Camera orientation arguments ──────────────────────────────────
        DeclareLaunchArgument('pan_center_deg',  default_value='0.0',
                              description='Pan angle at startup and shutdown (degrees, ROS convention)'),
        DeclareLaunchArgument('tilt_center_deg', default_value='-15.0',
                              description='Tilt angle at startup and shutdown (degrees, ROS convention)'),

        # ── TF / robot description ────────────────────────────────────────
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            parameters=[{
                'robot_description': robot_description,
                'use_sim_time': False,
            }],
            output='screen',
        ),

        # Static: map → odom (identity — replace with SLAM/AMCL when ready)
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='map_to_odom',
            arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
            output='screen',
        ),

        # Static: odom → base_footprint (placeholder until odometry is added)
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='odom_to_base',
            arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_footprint'],
            output='screen',
        ),

        # ── Nodes ─────────────────────────────────────────────────────────
        Node(
            package='raspbot_v2',
            executable='motor_controller',
            name='motor_controller',
            parameters=[{
                'wheel_base': LaunchConfiguration('wheel_base'),
                'max_speed':  LaunchConfiguration('max_speed'),
            }],
            output='screen',
        ),

        Node(
            package='raspbot_v2',
            executable='camera_orientation',
            name='camera_orientation',
            parameters=[{
                'pan_center_deg':  LaunchConfiguration('pan_center_deg'),
                'tilt_center_deg': LaunchConfiguration('tilt_center_deg'),
            }],
            output='screen',
        ),

        Node(
            package='raspbot_v2',
            executable='ultrasonic',
            name='ultrasonic',
            parameters=[{
                'publish_rate_hz': LaunchConfiguration('ultrasonic_rate_hz'),
            }],
            output='screen',
        ),

        Node(
            package='raspbot_v2',
            executable='led_controller',
            name='led_controller',
            output='screen',
        ),

    ])