import launch
import launch_ros.actions
import launch.substitutions
import launch.substitution
from launch.some_substitutions_type import SomeSubstitutionsType
import launch.conditions

import socket

def generate_launch_description():
    nodes = []
    nodes.append(make_router())
    return launch.LaunchDescription(nodes)

def setup_router_env():
    # In theory you can use this to set up the environment on the router
    # in a unique way.
    # I tried something like wat you see below at one point, though
    # it didn't pan out.
    # I'm just goint to leave it as an example here.
    #zenoh_runtime_settings = "( rx: (worker_threads: 2), acc: (worker_threads: 2), tx: (worker_threads: 2), app: (worker_threads: 2) )"
    #env = {"ZENOH_RUNTIME": zenoh_runtime_settings}
    env = None
    return env

def make_router():
    zenoh_is_rmw = launch.substitutions.EqualsSubstitution(
                    'rmw_zenoh_cpp',
                    launch.substitutions.EnvironmentVariable('RMW_IMPLEMENTATION', default_value='')
    )
    router_port_is_free = PortIsFree('7447')

    condition = launch.conditions.IfCondition(
        launch.substitutions.AndSubstitution(zenoh_is_rmw, router_port_is_free)
    )
    node = launch_ros.actions.Node(
        package = "rmw_zenoh_cpp",
        executable="rmw_zenohd",
        name="zenoh_router",
        condition=condition,
        additional_env=setup_router_env()
    )
    return node

######
# Custom actions/substitutions
######

class PortIsFree(launch.substitution.Substitution):
    def __init__(self, port: SomeSubstitutionsType):
        super().__init__()
        self._port = port

    def perform(self, context):
        port_as_string = self._port if isinstance(self._port, str) else self._port.perform(context)
        port_as_int = int(port_as_string)

        if is_port_in_use_for_tcp(port_as_int):
            return "False"
        return "True"

def is_port_in_use_for_tcp(port):
    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
        return sock.connect_ex(('localhost', port)) == 0