import os
import yaml

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def create_nodes(context):
    package_dir = get_package_share_directory('odin_ros_driver')
    config_file = LaunchConfiguration('config_file').perform(context)

    with open(config_file, 'r', encoding='utf-8') as stream:
        reprojection_params = yaml.safe_load(stream) or {}

    reprojection_params.setdefault('synced_cloud_topic', '/odin1/sync/cloud_in_cam')
    reprojection_params.setdefault('synced_odometry_topic', '/odin1/sync/odometry')
    reprojection_params.setdefault('synced_wiwc_topic', '/odin1/sync/wiwc')
    reprojection_params.setdefault('synced_image_topic', '/odin1/sync/image')
    sync_camera_compressed = int(reprojection_params.get('register_keys.sync_camera_compressed', 0))
    reprojection_params['synced_image_compressed'] = sync_camera_compressed
    reprojection_params.setdefault('register_keys.sync_topic_prefix', '/odin1/sync')
    reprojection_params['calib_file_path'] = os.path.join(package_dir, 'config', 'calib.yaml')

    synced_target_observation_node = Node(
        package='odin_ros_driver',
        executable='cloud_reprojection_synced_ros2_node',
        name='cloud_reprojection_synced_ros2_node',
        output='screen',
        parameters=[reprojection_params]
    )

    return [synced_target_observation_node]


def generate_launch_description():
    package_dir = get_package_share_directory('odin_ros_driver')

    config_file_arg = DeclareLaunchArgument(
        'config_file',
        default_value=os.path.join(package_dir, 'config', 'control_command.yaml'),
        description='Path to the control config YAML file'
    )

    ld = LaunchDescription()
    ld.add_action(SetEnvironmentVariable('RCUTILS_COLORIZED_OUTPUT', '1'))
    ld.add_action(config_file_arg)
    ld.add_action(OpaqueFunction(function=lambda context: create_nodes(context)))
    return ld