import time
import cv2
import numpy as np
from cam_3.multispectral_service import MultiSpectralService


svc = MultiSpectralService(host="192.168.105.6", port=5000, timeout=10)

try:
    svc.connect()

    print("PING:", svc.ping())
    print("STATUS:", svc.get_status())

    print("SET FPS:", svc.set_fps(15))
    print("SET RES:", svc.set_resolution(640, 480))
    print("SET CAPTURE MODE:", svc.set_capture_mode("AUTO"))
    print("SET FRAME TYPE:", svc.set_frame_type("RAW_BRUTO"))
    print("SET OUTPUT DTYPE:", svc.set_output_dtype("uint8"))

    print("BEGIN:", svc.begin(
        frame_type="RAW_BRUTO",
        output_dtype="uint8",
        capture_mode="AUTO",
    ))

    last_frame = None
    last_meta = None

    for i in range(1, 6):
        t0 = time.time()
        frame, meta = svc.capture_frame()
        tempo = time.time() - t0

        last_frame = frame
        last_meta = meta

        print(
            f"CAPTURE {i}: OK, Tempo={tempo:.4f}s, "
            f"type={type(frame)}, frame_type={meta.get('frame_type')}, "
            f"sources={meta.get('payload_sources')}"
        )

    print("CONFIG:", svc.get_config())
    print("STATUS FINAL:", svc.get_status())

finally:
    try:
        print("STOP:", svc.stop())
    except Exception:
        pass

    svc.disconnect()


# Salva uma imagem simples de preview
if last_frame is not None:
    if isinstance(last_frame, dict) and "cam2" in last_frame:
        # cam2 vem BGR do OpenCV
        cv2.imwrite("capture_cam2.jpg", last_frame["cam2"])
        print("[OK] Salvo: capture_cam2.jpg")

    elif isinstance(last_frame, np.ndarray):
        if last_frame.ndim == 3:
            # Pode ser CHW ou HWC
            img = last_frame
            if img.shape[0] in (3, 4, 5):
                img = np.transpose(img[:3], (1, 2, 0))

            if img.dtype != np.uint8:
                img = np.clip(img * 255.0, 0, 255).astype(np.uint8)

            cv2.imwrite("calibration/capture.jpg", img)
            print("[OK] Salvo: calibration/capture.jpg")
        else:
            cv2.imwrite("calibration/capture_gray.jpg", last_frame)
            print("[OK] Salvo: calibration/capture_gray.jpg")