import os
from pathlib import Path

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue


def generate_launch_description():
    bringup_dir = get_package_share_directory('origincar_base')
    launch_dir = os.path.join(bringup_dir, 'launch')
    imu_config = Path(get_package_share_directory('origincar_base'), 'config', 'imu.yaml')

    carto_slam = LaunchConfiguration('carto_slam', default='false')
    akmcar = LaunchConfiguration('akmcar', default='true')
    wall_preview = LaunchConfiguration('wall_preview', default='false')
    wall_preview_port = LaunchConfiguration('wall_preview_port', default='8772')

    origincar_base = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'base_serial.launch.py')),
        launch_arguments={'akmcar': akmcar}.items(),
    )

    robot_mode_description = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description.launch.py')),
    )

    base_to_gyro = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='base_to_gyro',
        arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'gyro_link'],
    )

    link_to_laser = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='link_to_laser',
        arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'laser'],
    )

    imu_filter_node = Node(
        package='imu_filter_madgwick',
        executable='imu_filter_madgwick_node',
        parameters=[imu_config],
    )

    wall_preview_server = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'wall_localization.launch.py')),
        condition=IfCondition(wall_preview),
        launch_arguments={
            'port': wall_preview_port,
        }.items(),
    )

    robot_description = ParameterValue(
        Command([
            'xacro ',
            os.path.join(get_package_share_directory('origincar_description'), 'urdf', 'origincar.urdf'),
        ]),
        value_type=str,
    )

    joint_state_publisher_node = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        parameters=[{'robot_description': robot_description}],
    )

    return LaunchDescription([
        DeclareLaunchArgument('carto_slam', default_value='false'),
        DeclareLaunchArgument('akmcar', default_value='true'),
        DeclareLaunchArgument('wall_preview', default_value='false'),
        DeclareLaunchArgument('wall_preview_port', default_value='8772'),
        origincar_base,
        base_to_gyro,
        joint_state_publisher_node,
        robot_mode_description,
        imu_filter_node,
        wall_preview_server,
    ])
