import socket
import threading
import time
import subprocess
import netifaces

# 初始化全局变量
client_processes = {}
process_id_counter = 0
lock = threading.Lock()

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):
    socket_server = socket.socket()  # 创建socket对象
    socket_server.bind((host, port))  # 绑定主机和端口
    socket_server.listen(5)  # 监听传入的连接
    print(f"服务端已启动，地址{host}，端口{port}")
    print(f"正在等待客户端连接...")
    num = 0
    try:
        while True:
            num += 1
            conn, address = socket_server.accept()  # 接受连接
            print(f"服务端已接受到客户端 {num}号 的连接请求，客户端信息：{address}")
            client_handler = threading.Thread(target=handle_client, args=(conn, address, num))
            client_handler.start()
    except KeyboardInterrupt:
        print("服务器正在关闭...")
        socket_server.close()
        for thread in threading.enumerate():
            if thread != threading.current_thread():
                thread.join()
    except Exception as e:
        print(f"服务器发生错误：{e}")
        socket_server.close()

def handle_client(conn, address, num):
    global process_id_counter
    with conn:
        while True:
            time.sleep(0.01)
            try:
                data_from_client_bytes = conn.recv(1024)
                if len(data_from_client_bytes) == 0:
                    print(f"客户端 {num}号 已断开连接")
                    break

                data_from_client_hex = ' '.join([hex(x) for x in data_from_client_bytes])
                print(f"客户端 {num}号:{address} 发来的消息是：{data_from_client_hex}")

                if len(data_from_client_bytes) > 4:
                    data = data_from_client_bytes[4]

                    with lock:
                        process_id_counter += 1
                        process_id = process_id_counter

                    if data == 0x0:
                        # process = subprocess.Popen(['roslaunch', 'pibot_navigation', 'nav.launch'])
                        process = subprocess.Popen(['roslaunch', 'upbot_vision', 'upbot_vision.launch'])

                        print("启动定位导航")
                        conn.send(f"定位导航已启动，进程标识符为 {process_id}".encode("UTF-8"))
                    elif data == 0x5:
                        process = subprocess.Popen(['roslaunch', 'upbot_mapping', 'upbot_mapping.launch'])
                        conn.send(f"手持标签划定范围已启动，进程标识符为 {process_id}".encode("UTF-8"))
                    elif data == 0x2:
                        process = subprocess.Popen(['roslaunch', 'ipa_room_exploration', 'room_exploration_action_server.launch'])
                        conn.send(f"全覆盖服务端已启动，进程标识符为 {process_id}".encode("UTF-8"))
                        process = subprocess.Popen(['roslaunch', 'ipa_room_exploration', 'room_exploration_client.launch', 'robot_env:=4_'])
                        conn.send(f"全覆盖客户端已启动，进程标识符为 {process_id}".encode("UTF-8"))
                    elif data == 0x7:
                        conn.send(f"返回充电已启动,进程标识符为 {process_id}".encode("UTF-8"))
                    elif data == 0x10:
                        conn.send(f"基站初始化已启动,进程标识符为 {process_id}".encode("UTF-8"))

                    client_processes.setdefault(num, []).append((process_id, process))
            except ConnectionResetError:
                print(f"客户端 {num}号:{address} 连接重置")
                break
            except Exception as e:
                print(f"客户端 {num}号:{address} 发生异常：{e}")
                break

if __name__ == '__main__':
    local_ip = get_local_ip()
    print("本机在局域网内的IP地址:", local_ip)
    server_host = local_ip
    server_port = 19890

    create_server_socket(server_host, server_port)