import time
import subprocess
from subprocess import Popen
import atexit

def register_process_killer(process):
    def kill_subprocess(pid):
        subprocess.run(["kill", "-SIGKILL", str(pid)])
        print("process {} was killed.".format(pid))
    atexit.register(kill_subprocess, process.pid)

class SubprocessWrapper(object):
    def __init__(self, command):
        self.process = Popen(command)
        register_process_killer(self.process)

    # It is not guaranteed that __del__() methods are called when the interpreter exits.
    #  def __del__(self):
        #  self.process.kill()


class ComponentWrapper(SubprocessWrapper):
    def __init__(self, component_name, robot_name, port):
        super().__init__(["rosrun", "route_guidance_ros", component_name, str(port), robot_name])


class NavigationComponent(ComponentWrapper):
    def __init__(self, robot_name, port):
        super().__init__("Navigation.py", robot_name, port)


class SysInfoComponent(ComponentWrapper):
    def __init__(self, robot_name, port):
        super().__init__("System_Information.py", robot_name, port)


class SpeechRecogComponent(ComponentWrapper):
    def __init__(self, robot_name, port):
        super().__init__("Dummy_Speech_Recognition.py", robot_name, port)


class SubEngineWrapper(SubprocessWrapper):
    def __init__(self, engine_port):
        super().__init__(["python3", "sub_engine.py", str(engine_port)])


class MainEngineWrapper(SubprocessWrapper):
    def __init__(self):
        super().__init__(["python3", "main_engine.py"])


def launch_components_and_subengines(**robot_port_table):
    process = []
    for robot_name, engine_port in robot_port_table.items():
        process.append(NavigationComponent(robot_name, engine_port+1))
        process.append(SysInfoComponent(robot_name, engine_port+2))
        process.append(SpeechRecogComponent(robot_name, engine_port+3))
    print("Components Constructed.")

    command = [
        "rosrun", "route_guidance_ros", "Person_Localization.py",
        "8043", "human1"
    ]
    process.append(SubprocessWrapper(command))
    print("Person Localization Component Constructed.")
    time.sleep(10)

    for engine_port in robot_port_table.values():
        process.append(SubEngineWrapper(engine_port))
    print("Sub Engine Constructed.")
    time.sleep(5)

    return process


def setup_single_robot():
    return launch_components_and_subengines(robot1=8000)


def setup_multi_robot():
    process = launch_components_and_subengines(robot1=8010, robot2=8020, robot3=8030)
    command = [
        "rosrun", "route_guidance_ros", "VirtualNavigation.py",
        "8041", "robot1", "robot2", "robot3"
    ]
    process.append(SubprocessWrapper(command))
    print("Virtual Navigation Component Constructed.")
    time.sleep(5)
    process.append(MainEngineWrapper())
    print("Main Engine Constructed.")
    time.sleep(10)

    return process
