import time
import cv2
import numpy as np

from oak_fcc3_service import OakFcc3Service


svc = OakFcc3Service(timeout=10)

last_frame = None
last_meta = None

try:
    print("CONNECT:", 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, 400))
    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",
    ))

    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')}, "
            f"sync_ms={meta.get('sync_dt_ms', 0):.2f}, "
            f"sync_ok={meta.get('sync_ok')}"
        )

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

finally:
    try:
        print("STOP:", svc.stop())
    except Exception as e:
        print("STOP ERRO:", e)

    try:
        print("DISCONNECT:", svc.disconnect())
    except Exception as e:
        print("DISCONNECT ERRO:", e)


if last_frame is not None:
    if isinstance(last_frame, dict) and "cam2" in last_frame:
        cv2.imwrite("calibration/capture_cam2.jpg", last_frame["cam2"])
        print("[OK] Salvo: calibration/capture_cam2.jpg")

    elif isinstance(last_frame, dict):
        first_id = list(last_frame.keys())[0]
        cv2.imwrite(f"calibration/capture_{first_id}.jpg", last_frame[first_id])
        print(f"[OK] Salvo: calibration/capture_{first_id}.jpg")

    elif isinstance(last_frame, np.ndarray):
        cv2.imwrite("calibration/capture.jpg", last_frame)
        print("[OK] Salvo: calibration/capture.jpg")