import time
from datetime import datetime
import livox_sdk

# callback Python que será convertido pra callback C
def on_point_cloud(handle, data_ptr, data_num, timestamp_ns, client_data):
    # handle: ID interno do lidar (uint32)
    # data_ptr: ponteiro pra array de LivoxPoint
    # data_num: quantos pontos vieram neste pacote
    # timestamp_ns: timestamp do pacote (normalmente em nanossegundos)
    # client_data: não usamos (None)

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

    # só logar pra validar que tá rolando stream:
    print(f"[{now_str}] handle={handle} pts={data_num} ts={timestamp_ns}")

    # se quiser ler o primeiro ponto pra testar:
    if data_num > 0:
        first_point = data_ptr[0]
        # cada ponto tem x,y,z em metros (float), reflectivity (0-255)
        # cuidado pra não printar tudo sempre pq vai floodar 😅
        print(f"   first: x={first_point.x:.2f} y={first_point.y:.2f} z={first_point.z:.2f} refl={first_point.reflectivity}")

def main():
    print("=== Mid-360 Python Reader ===")

    # 1) inicializa SDK apontando para o config.json que você já fez funcionar
    ok = livox_sdk.init(r"config.json")
    if not ok:
        print("❌ Falha ao inicializar SDK (confere caminho do config.json)")
        return
    print("SDK Init OK")

    # 2) registra callback
    if not livox_sdk.set_point_cloud_callback(on_point_cloud):
        print("❌ Falha ao registrar callback de nuvem de pontos")
        livox_sdk.uninit()
        return
    print("Callback registrado")

    # 3) start streaming (se necessário)
    if not livox_sdk.start():
        print("⚠ Aviso: não consegui chamar start(), mas vou continuar assim mesmo")

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

    try:
        while True:
            # aqui a DLL está chamando nosso callback em background thread.
            # a gente só dorme pra não fechar o processo.
            time.sleep(0.1)
    except KeyboardInterrupt:
        print("Encerrando...")
    finally:
        livox_sdk.stop()
        livox_sdk.uninit()
        print("Fechou limpo")

if __name__ == "__main__":
    main()