import socket
import threading
import subprocess
import netifaces


# LAUNCH_FILES = {
#     '0x00': ['roslaunch', 'pibot_navigation', 'nav.launch'],
#     '0x01': ['roslaunch', 'upbot_location', 'upbot_location.launch'],
#     'launch_file_1': ['roslaunch', 'package_name', 'launch_file_1.launch'],
#     'launch_file_2': ['roslaunch', 'package_name', 'launch_file_2.launch']
# }
client_processes = {}
process_id_counter = 0
def get_local_ip():
    interfaces = netifaces.interfaces()
    for interface in interfaces:
        addresses = netifaces.ifaddresses(interface)
        if netifaces.AF_INET in addresses:
            for address_info in addresses[netifaces.AF_INET]:
                ip_address = address_info['addr']
                if ip_address.startswith('192.168.') or ip_address.startswith('10.'):
                    return ip_address

def create_server_socket(host, port):
    try:
        socket_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        socket_server.bind((host, port))
        socket_server.listen(5)
        print(f"服务端已启动，地址{host}，端口{port}")
        print("正在等待客户端连接...")

        client_num = 0
        while True:
            client_num += 1
            conn, address = socket_server.accept()
            print(f"服务端已接受到客户端 {client_num} 号的连接请求，客户端信息：{address}")
            client_handler = threading.Thread(target=handle_client, args=(conn, address, client_num))
            client_handler.start()
    except Exception as e:
        print(f"错误：{e}")
    finally:
        socket_server.close()

def handle_client(conn, address, client_num):
    global process_id_counter
    try:
        while True:
            data_from_client = conn.recv(1024).decode("UTF-8")
            if not data_from_client:
                break
            print(f"客户端 {client_num} 号({address}) 发来的消息是：{data_from_client}")
            data = data_from_client.strip()
            if data == '1':
                process_id_counter += 1
                process_id = process_id_counter
                process = subprocess.Popen(['roslaunch', 'pibot_navigation', 'nav.launch'])
                client_processes.setdefault(client_num, []).append((process_id, process))
                conn.send(f"定位导航已启动，进程标识符为 {process_id}".encode("UTF-8"))
            elif data == '2':
                process_id_counter += 1
                process_id = process_id_counter
                process = subprocess.Popen(['roslaunch', 'upbot_mapping', 'upbot_mapping.launch'])
                client_processes.setdefault(client_num, []).append((process_id, process))
                conn.send(f"手持标签划定范围已启动，进程标识符为 {process_id}".encode("UTF-8"))
            elif data == '3':
                process_id_counter += 1
                process_id = process_id_counter
                process = subprocess.Popen(['roslaunch', 'ipa_room_exploration', 'room_exploration_action_server.launch'])
                client_processes.setdefault(client_num, []).append((process_id, process))
                conn.send(f"全覆盖服务端已启动，进程标识符为 {process_id}".encode("UTF-8"))
            elif data == '4':
                process_id_counter += 1
                process_id = process_id_counter
                process = subprocess.Popen(['roslaunch', 'ipa_room_exploration', 'room_exploration_client.launch', 'robot_env:=4_'])
                client_processes.setdefault(client_num, []).append((process_id, process))
                conn.send(f"全覆盖客户端已启动，进程标识符为 {process_id}".encode("UTF-8"))
            elif data.startswith('stop'):
                stop_data = data.split()
                if len(stop_data) == 2:
                    process_id_to_stop = int(stop_data[1])
                    if client_num in client_processes:
                        for process_tuple in client_processes[client_num]:
                            if process_tuple[0] == process_id_to_stop:
                                process_tuple[1].terminate()
                                client_processes[client_num].remove(process_tuple)
                                conn.send(f"已停止客户端 {client_num} 号的进程 {process_id_to_stop}".encode("UTF-8"))
                                break
                        else:
                            conn.send(f"客户端 {client_num} 号没有标识符为 {process_id_to_stop} 的进程".encode("UTF-8"))
                    else:
                        conn.send("没有启动的进程需要停止".encode("UTF-8"))
                else:
                    conn.send("无效的停止命令格式".encode("UTF-8"))
            else:
                response = f"已收到消息: {data}"
                conn.send(response.encode("UTF-8"))
            
    except Exception as e:
        print(f"客户端 {client_num} 号({address}) 处理时发生错误：{e}")
    finally:
        conn.close()
        print(f"客户端 {client_num} 号({address}) 已断开连接。")

if __name__ == '__main__':
    local_ip = get_local_ip()
    print("本机在局域网内的IP地址:", local_ip)
    
    server_port = int(input("请输入服务端端口号: "))
    create_server_socket(local_ip, server_port)