import time
import ctypes
from datetime import datetime
import livox_sdk

# ---------------------------------------------------------
# Ajuste isso pros seus valores conhecidos
# ---------------------------------------------------------
LIDAR_BROADCAST_CODE = None  # se você quiser forçar um SN específico
HOST_IP = "192.168.1.100"    # IP do PC (Points IP no viewer)
LIDAR_IP = "192.168.1.169"   # IP do LiDAR
POINT_PORT = 56301           # Point Cloud Port
CMD_PORT = 56100             # Porta de comando (já vimos no log)
IMU_PORT = 56401             # IMU Port
INFO_PORT = 56201            # Lidar Info Port
# ---------------------------------------------------------

# A estrutura de ponto que vem do Livox SDK2 normalmente é algo tipo:
# typedef struct {
#   uint32_t timestamp;
#   float x;
#   float y;
#   float z;
#   uint8_t reflectivity;
#   uint8_t tag;
# } LivoxPoint;
#
# O wrapper Python já converte isso pra você ou expõe um ponteiro pra array.
# Vamos supor que ele chama nosso callback com (data_ptr, data_num, timestamp).

# Callback chamado quando chegam pontos do LiDAR
@ctypes.CFUNCTYPE(None, ctypes.c_void_p, ctypes.c_uint32, ctypes.c_uint64)
def point_cloud_callback(data_ptr, data_num, packet_ts):
    # data_ptr = ponteiro pra array de pontos (LivoxPoint)
    # data_num = quantos pontos vieram nesse pacote
    # packet_ts = timestamp do pacote em nanossegundos ou microssegundos (depende da build)

    now_local = datetime.now().strftime("%H:%M:%S.%f")[:-3]

    # Só log leve pra testar leitura:
    print(f"[{now_local}] pacote recebido: {data_num} pontos | ts_sensor={packet_ts}")

    # Se você quiser inspecionar o primeiro ponto:
    # cria uma view python em cima da struct
    # Exemplo genérico, pode precisar adaptar:
    # first_point = livox_sdk.get_point(data_ptr, 0)
    # print("   primeiro ponto:", first_point.x, first_point.y, first_point.z, first_point.reflectivity)

def main():
    print("=== Mid-360 Reader - Inicializando SDK Livox ===")

    # 1) Inicializa o core do SDK
    ok = livox_sdk.init()
    if not ok:
        print("Falha ao inicializar Livox SDK")
        return

    # 2) Configura os canais de rede (host_ip, lidar_ip, portas)
    # Nem todo wrapper expõe isso direto; alguns assumem discovery automático.
    # Então a gente tenta primeiro discovery normal e depois bind manual.
    #
    # Se seu livox_sdk.py tiver um método tipo 'set_network_params' / 'setup_lidar',
    # você chama aqui. Exemplo (ajuste pro que existir no seu arquivo):
    try:
        livox_sdk.set_network_params(
            host_ip=HOST_IP,
            lidar_ip=LIDAR_IP,
            cmd_port=CMD_PORT,
            data_port=POINT_PORT,
            imu_port=IMU_PORT,
            info_port=INFO_PORT
        )
    except AttributeError:
        # Wrapper sem essa call explícita -> beleza, seguimos com discovery
        pass

    # 3) Registrar callback de ponto
    # A API costuma ter algo como 'register_point_cloud_callback'
    if hasattr(livox_sdk, "register_point_cloud_callback"):
        livox_sdk.register_point_cloud_callback(point_cloud_callback)
    else:
        print("⚠ Seu livox_sdk.py não tem register_point_cloud_callback; ajustar nomes depois.")
        print("   Procura por algo tipo 'SetPointCloudCallback' dentro do arquivo livox_sdk.py")
        # a partir daqui ele pode não receber pontos, mas vamos continuar pra ver se inicializa ok

    # 4) Conectar ao LiDAR
    # Há dois jeitos comuns nos samples:
    #  - via broadcast code (número de série tipo 47MDN7K0030169)
    #  - via IP direto
    #
    # Você tem SN: 47MDN7K0030169 no seu log.
    # Se o wrapper expõe add_lidar(), usa:
    connected = False
    if hasattr(livox_sdk, "add_lidar"):
        connected = livox_sdk.add_lidar("47MDN7K0030169", LIDAR_IP)
    elif hasattr(livox_sdk, "connect_lidar"):
        connected = livox_sdk.connect_lidar(LIDAR_IP, CMD_PORT)
    else:
        # fallback "vamos ver se já está conectado só por init()"
        connected = True

    if not connected:
        print("Não conseguiu conectar no LiDAR pelo wrapper. Checar função correta de conexão.")
        return

    print("✅ Conectado. Lendo pacotes... CTRL+C pra sair.\n")

    try:
        while True:
            # alguns wrappers exigem uma call de 'poll()' pra processar eventos C -> Python.
            if hasattr(livox_sdk, "poll"):
                livox_sdk.poll()

            time.sleep(0.05)
    except KeyboardInterrupt:
        print("Encerrando leitura...")
    finally:
        livox_sdk.uninit()
        print("SDK encerrado. Bye.")

if __name__ == "__main__":
    main()