from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import TimerAction, ExecuteProcess
from ament_index_python.packages import get_package_share_directory
import os


def generate_launch_description():
    rslidar_sdk_dir = get_package_share_directory("rslidar_sdk")
    pointcloud_merger_dir = get_package_share_directory("rslidar_pointcloud_merger")
    fu_dir = get_package_share_directory("fu")

    vehicle_params = Node(
        package="vehicle_params",
        executable="vehicle_params_node",
        output="screen",
        prefix="taskset -c 0",
    )

    can_radio_ctrl = TimerAction(
        period=0.2,
        actions=[
            Node(
                package="can_radio_ctrl",
                executable="can_radio_ctrl_node",
                output="screen",
                prefix="taskset -c 0",
            )
        ],
    )

    remote_ctrl = TimerAction(
        period=0.4,
        actions=[
            Node(
                package="remote_ctrl",
                executable="remote_ctrl_node",
                output="screen",
                prefix="taskset -c 2",
            )
        ],
    )

    mc = TimerAction(
        period=0.6,
        actions=[
            Node(
                package="mc",
                executable="mc_node",
                output="screen",
                prefix="taskset -c 2",
            )
        ],
    )

    ctrl_arbiter = TimerAction(
        period=0.8,
        actions=[
            Node(
                package="ctrl_arbiter",
                executable="ctrl_arbiter_node",
                output="screen",
                prefix="taskset -c 1",
            )
        ],
    )

    pl = TimerAction(
        period=1.0,
        actions=[
            Node(
                package="pl",
                executable="pl_node",
                output="screen",
                prefix="taskset -c 3",
            )
        ],
    )

    mqtt_report = TimerAction(
        period=1.3,
        actions=[
            Node(
                package="mqtt_report",
                executable="mqtt_report_node",
                output="screen",
                prefix="taskset -c 7",
            )
        ],
    )

    lidar_driver = TimerAction(
        period=1.6,
        actions=[
            ExecuteProcess(
                cmd=["ros2", "launch", "rslidar_sdk", "start_double.launch.py"],
                output="screen",
                prefix="taskset -c 4,5",
            )
        ],
    )

    pointcloud_merger = TimerAction(
        period=3.1,
        actions=[
            ExecuteProcess(
                cmd=[
                    "ros2",
                    "launch",
                    "rslidar_pointcloud_merger",
                    "merge_two_lidars.launch.py",
                ],
                output="screen",
                # prefix="taskset -c 6",
            )
        ],
    )

    rtk = TimerAction(
        period=3.4,
        actions=[
            Node(
                package="rtk",
                executable="rtk_node",
                output="screen",
                prefix="taskset -c 7",
            )
        ],
    )

    route = TimerAction(
        period=3.7,
        actions=[
            Node(
                package="route",
                executable="route_node",
                output="screen",
                prefix="taskset -c 7",
            )
        ],
    )

    sub = TimerAction(
        period=4.0,
        actions=[
            Node(
                package="sub",
                executable="sub_node",
                output="screen",
                prefix="taskset -c 7",
            )
        ],
    )

    task_manager = TimerAction(
        period=4.3,
        actions=[
            Node(
                package="task_manager",
                executable="task_manager_node",
                output="screen",
                prefix="taskset -c 7",
            )
        ],
    )

    fu = TimerAction(
        period=4.6,
        actions=[
            ExecuteProcess(
                cmd=["ros2", "launch", "fu", "fu.launch.py"],
                output="screen",
                prefix="taskset -c 7",
            )
        ],
    )

    return LaunchDescription(
        [
            vehicle_params,
            can_radio_ctrl,
            remote_ctrl,
            mc,
            ctrl_arbiter,
            pl,
            mqtt_report,
            lidar_driver,
            pointcloud_merger,
            rtk,
            route,
            sub,
            task_manager,
            fu,
        ]
    )