import os.path

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch.conditions import IfCondition
import math

def generate_launch_description():
    # ----- microros -----
    servo_uros_node = Node(
        package='micro_ros_agent',
        executable='micro_ros_agent',
        arguments=["serial", "--dev", "/dev/ttyACM0", "--baud", "921600"],
    )

    servo_angle_tf_node = Node(
        package='sherpa_lidar_utils',
        executable='servo_angle_tf.py',
    )

    # ----- lidar position -----
    lidar_position_base_tf_node = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        arguments=['0', '0', '0', '0', '0', '0', '1', 'velodyne', 'lidar_base'],
    )

    # ----- lidar topics -----
    vlp16_driver_node = Node(
        package='velodyne_driver',
        executable='velodyne_driver_node',
        arguments=["--ros-args", "-p", "model:=VLP16", "-p", "rpm:=600.0", "-p", "device_ip:=10.42.200.201"],
        remappings=[
            ('/velodyne_packets', '/lidar/vlp16/packets'),
        ],
    )

    vlp16_pointcloud_node = Node(
        package='velodyne_pointcloud',
        executable='velodyne_transform_node',
        arguments=["--ros-args", "-p", f"calibration:={os.path.join(get_package_share_directory('sherpa'), 'config', 'vlp-16-pointcloud.yaml')}", "-p", "model:=VLP16"],
        remappings=[
            ('/velodyne_packets', '/lidar/vlp16/packets'),
            ('/velodyne_points', '/lidar/vlp16/points'),
        ]
    )

    mid40_driver_node = Node(
        package='livox_ros2_driver',
        executable='livox_ros2_driver_node',
        arguments=[],
        remappings=[
            ('/livox/lidar', '/lidar/mid40/points'),
        ],
    )

    # ----- pointcloud downsampling -----
    downsample_vlp16_node = Node(
        package='target_tracking',
        executable='cloud_preprocessing_node',
        parameters=[
            {"topic_in": "/lidar/vlp16/points"},
            {"topic_out": "/lidar/vlp16/downsampled"},
            {"x_min": 0.0},
            {"x_max": 100.0},
            {"z_min": -5.0},
            {"z_max": 10.0},
            {"tan_h_fov": math.pi}, # 180°  
            {"tan_v_fov": math.pi / 5}, # ~36°  
        ],
    )

    downsample_mid40_node = Node(
        package='target_tracking',
        executable='cloud_preprocessing_node',
        parameters=[
            {"topic_in": "/lidar/mid40/points"},
            {"topic_out": "/lidar/mid40/downsampled"},
            {"x_min": 0.0},
            {"x_max": 100.0},
            {"z_min": -5.0},
            {"z_max": 10.0},
            {"tan_h_fov": math.pi / 4}, # 45°  
            {"tan_v_fov": math.pi / 4}, # 45°  
        ],
    )

    # ----- create ld -----
    ld = LaunchDescription()   

    ld.add_action(servo_uros_node)
    ld.add_action(servo_angle_tf_node)
    ld.add_action(lidar_position_base_tf_node)
    ld.add_action(vlp16_driver_node)
    ld.add_action(vlp16_pointcloud_node)
    ld.add_action(mid40_driver_node)
    ld.add_action(downsample_vlp16_node)
    ld.add_action(downsample_mid40_node)
    
    return ld
