from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import Command, LaunchConfiguration, IfElseSubstitution
from launch_ros.actions import Node, LifecycleNode
from launch_ros.substitutions import FindPackageShare
import os


def generate_launch_description():

    pkg_share = FindPackageShare("").find("toid_control")
    params = os.path.join(pkg_share, "params", "toid_general_params.yaml")
    default_rviz_config_path = os.path.join(pkg_share, "rviz", "rviz.rviz")

    description_pkg_share = FindPackageShare("").find("toid_bot_description")
    default_model_path = os.path.join(
        description_pkg_share, "src", "toid_bot_description.urdf"
    )

    visualize = LaunchConfiguration("visualize")

    visualize_arg = DeclareLaunchArgument(
        "visualize", default_value="False", description="Whether to launch rviz2"
    )

    use_mock = LaunchConfiguration("use_mock")

    use_mock_arg = DeclareLaunchArgument(
        "use_mock", default_value="True", description="Whether to use mock controller"
    )

    odom_broadcast = Node(
        package="toid_odometry",
        executable="toid_odometry",
        name="map_to_odom_broadcaster",
        emulate_tty=True,
        parameters=[
            {
                "mock_odom": use_mock,
                "serial_port": "/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E66368254F4F4036-if02",
            }
        ],
    )

    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="screen",
        emulate_tty=True,
        parameters=[
            {
                "robot_description": Command(
                    [
                        "xacro ",
                        default_model_path,
                        " use_mock:=",
                        use_mock,
                        " serial_port:=/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E66368254F4F4036-if00",
                    ]
                )
            }
        ],
    )

    controller_manager = Node(
        package="controller_manager",
        executable="ros2_control_node",
        output="screen",
        emulate_tty=True,
        parameters=[params],
        arguments=["--ros-args", "--log-level", "warn"],
    )

    joint_state_broadcaster = Node(
        package="controller_manager",
        executable="spawner",
        output="screen",
        emulate_tty=True,
        arguments=["joint_state_broadcaster", "--ros-args", "--log-level", "warn"],
    )

    velocity_controller = Node(
        package="controller_manager",
        executable="spawner",
        output="screen",
        emulate_tty=True,
        arguments=[
            "velocity_controller",
            "--inactive",
            "-p",
            params,
            "--ros-args",
            "--log-level",
            "warn",
        ],
    )

    diffbot_base_controller = Node(
        package="controller_manager",
        executable="spawner",
        output="both",
        emulate_tty=True,
        arguments=[
            "diffdrive_controller",
            "-p",
            params,
            "--controller-ros-args",
            "-r diffdrive_controller/cmd_vel:=/cmd_vel",
            "--controller-ros-args",
            "-r diffdrive_controller/odom:=/odom",
            "--controller-ros-args",
            IfElseSubstitution(
                use_mock,
                "--param odom_frame_id:=odom",
                "--param odom_frame_id:=odom_expected",
            ),
            "--controller-ros-args",
            IfElseSubstitution(
                use_mock,
                "--param enable_odom_tf:=true",
                "--param enable_odom_tf:=false",
            ),
            "--ros-args",
            "--log-level",
            "warn",
        ],
    )

    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        emulate_tty=True,
        arguments=["-d", default_rviz_config_path, "--ros-args", "--log-level", "warn"],
        condition=IfCondition(visualize),
    )

    return LaunchDescription(
        [
            visualize_arg,
            use_mock_arg,
            odom_broadcast,
            robot_state_publisher,
            controller_manager,
            joint_state_broadcaster,
            diffbot_base_controller,
            velocity_controller,
            rviz_node,
        ]
    )