from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction
from launch.event_handlers import OnProcessStart
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression 
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

from pathlib import Path



def generate_launch_description():

    is_local_test = DeclareLaunchArgument(
        'local_test',
        default_value="False",
        description='Launch with simulated components'
    )

    basedir = FindPackageShare("mg_control")
    urdfDescription = PathJoinSubstitution([basedir, 
                                            "assets",
                                            PythonExpression([
                                                "'mg_base_local.urdf' if ",
                                                LaunchConfiguration('local_test'),
                                                " else 'mg_base.urdf'"])
                                            ])
    yamlControllers = PathJoinSubstitution([basedir,
                                            "assets",
                                            PythonExpression([
                                                "'mg_base_controllers_local.yaml' if( ",
                                                LaunchConfiguration('local_test'),
                                                ") else 'mg_base_controllers.yaml'"])
                                            ])

    urdf = PythonExpression(["open('",urdfDescription, "').read()"])

    return LaunchDescription([
        is_local_test,
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            output="log",
            parameters=[{"robot_description": urdf}]
        ),
        Node(
            package="controller_manager",
            executable="ros2_control_node",
            parameters=[yamlControllers],
            output="log"
        ),
        Node(
            package='controller_manager',
            executable='spawner',
            output="screen",
            arguments=["joint_state_publisher"]
        ),
        Node(
            package="controller_manager",
            executable="spawner",
            arguments=["diffdrive_controller", 
                       "--param-file", 
                       yamlControllers, 
                    ]
        ) 
    ])