import os
import lifecycle_msgs.msg
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import EmitEvent, LogInfo, RegisterEventHandler
from launch.events import matches_action, Shutdown
from launch_ros.actions import LifecycleNode, Node
from launch_ros.events.lifecycle import ChangeState
from launch_ros.event_handlers import OnStateTransition


package_name = "kouster"


def generate_launch_description():
    # log format
    os.environ["RCUTILS_CONSOLE_OUTPUT_FORMAT"] = (
        "[{time}] [{name}] [{severity}] {message}"
    )

    pkg_params_dir = os.path.join(get_package_share_directory("kouster"), "params")
    params_file = os.path.join(pkg_params_dir, "os_sensor_cloud_image.yaml")

    os_sensor = LifecycleNode(
        package="ouster_ros",
        executable="os_sensor",
        name="os1_sensor_node",
        parameters=[params_file],
        output="screen",
    )

    sensor_configure_event = EmitEvent(
        event=ChangeState(
            lifecycle_node_matcher=matches_action(os_sensor),
            transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,
        )
    )

    sensor_activate_event = RegisterEventHandler(
        OnStateTransition(
            target_lifecycle_node=os_sensor,
            goal_state="inactive",
            entities=[
                LogInfo(msg="os_sensor activating..."),
                EmitEvent(
                    event=ChangeState(
                        lifecycle_node_matcher=matches_action(os_sensor),
                        transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE,
                    )
                ),
            ],
            handle_once=True,
        )
    )

    sensor_finalized_event = RegisterEventHandler(
        OnStateTransition(
            target_lifecycle_node=os_sensor,
            goal_state="finalized",
            entities=[
                LogInfo(
                    msg="Failed to communicate with the sensor in a timely manner."
                ),
                EmitEvent(event=Shutdown(reason="Couldn't communicate with sensor")),
            ],
        )
    )

    os_cloud = Node(
        package="ouster_ros",
        executable="os_cloud",
        name="os1_cloud_node",
        parameters=[params_file],
        output="screen",
    )

    os_image = Node(
        package="ouster_ros",
        executable="os_image",
        name="os1_image_node",
        parameters=[params_file],
        output="screen",
    )

    return LaunchDescription(
        [
            os_sensor,
            os_cloud,
            os_image,
            sensor_configure_event,
            sensor_activate_event,
            sensor_finalized_event,
        ]
    )