import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import IncludeLaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
from launch.substitutions import Command


def generate_launch_description():

    # Check if we're told to use sim time
    lidar_vertical_fov_upper = LaunchConfiguration('lidar_vertical_fov_upper')
    lidar_vertical_fov_lower = LaunchConfiguration('lidar_vertical_fov_lower')
    lidar_horizontal_fov_upper = LaunchConfiguration('lidar_horizontal_fov_upper')
    lidar_horizontal_fov_lower = LaunchConfiguration('lidar_horizontal_fov_lower')


    # Process the URDF file
    package_name='sensor_sim' #<--- CHANGE ME
    pkg_path = os.path.join(get_package_share_directory(package_name))
    xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
    robot_description_config = Command(['xacro ', xacro_file, ' lidar_vertical_fov_upper:=', lidar_vertical_fov_upper, ' lidar_vertical_fov_lower:=', lidar_vertical_fov_lower, ' lidar_horizontal_fov_upper:=', lidar_horizontal_fov_upper, ' lidar_horizontal_fov_lower:=', lidar_horizontal_fov_lower])
    world_path = os.path.join(pkg_path,'worlds')

    # Create a robot_state_publisher node
    params = {'robot_description': robot_description_config, 'use_sim_time': True, 'publish_frequency': 0.0001, 'use_tf_static': False}
    node_robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[params]
    )

    gazebo = IncludeLaunchDescription(
                PythonLaunchDescriptionSource([os.path.join(
                    get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')]),
                    launch_arguments=[('gz_args', [f" -r -v 0 {world_path}/test_world.sdf"])],
             )

    # Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
    spawn_entity = Node(package='ros_gz_sim', executable='create',
        arguments=[
            '-topic', 'robot_description',
            '-name', 'spawn_robot',
            '-z', '1',
            '-x', '0',
            ],
        output='screen'
    )

    # Bridge
    bridge = Node(
        package='ros_gz_bridge',
        executable='parameter_bridge',
        arguments=[
            '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
            '/lidar/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked',
        ],
        output='screen'
    )

    lidar_visualizer = Node(
        package='sensor_sim',
        executable='lidar_visualizer.py',
        name='lidar_visualizer',
        output='screen'
    )

    # Launch!
    return LaunchDescription([
        DeclareLaunchArgument(
            'lidar_vertical_fov_upper',
            default_value='45',
            description='Upper limit of FOV of lidar'),
        DeclareLaunchArgument(
            'lidar_vertical_fov_lower',
            default_value='-45',
            description='Lower limit of FOV of lidar'),
        DeclareLaunchArgument(
            'lidar_horizontal_fov_upper',
            default_value='45',
            description='Upper limit of FOV of lidar'),
        DeclareLaunchArgument(
            'lidar_horizontal_fov_lower',
            default_value='-45',
            description='Lower limit of FOV of lidar'),
        node_robot_state_publisher,
        gazebo,
        spawn_entity,
        bridge,
        lidar_visualizer
    ])