import cv2

from oak_fcc3_client import OakFcc3Client


with OakFcc3Client(
    fps=15,
    width=640,
    height=400,
    frame_type="RAW_BRUTO",
    output_dtype="uint8",
    capture_mode="AUTO",
    raw_policy="allow_single",
    sync_mode="best",
    sync_tolerance_ms=25.0,
    module_calibration_json="calibration/module_params.json",
) as cam:

    print("STATUS:", cam.get_status())

    while True:
        frame, meta, decoded = cam.get_next_decoded(timeout=2.0)

        print(
            "frame_id:", meta["frame_id"],
            "sources:", meta["payload_sources"],
            "decoded:", list(decoded.keys()),
            "sync_ms:", f"{meta.get('sync_dt_ms', 0):.2f}",
            "sync_ok:", meta.get("sync_ok"),
        )

        if "cam2" in decoded:
            rgb01 = decoded["cam2"]["image"]
            rgb_bgr = cv2.cvtColor((rgb01 * 255).astype("uint8"), cv2.COLOR_RGB2BGR)
            cv2.imshow("cam2 RGB decoded", rgb_bgr)

        if "cam0" in decoded:
            re01 = decoded["cam0"]["image"]
            cv2.imshow("cam0 RE decoded", (re01 * 255).astype("uint8"))

        if "cam1" in decoded:
            nir01 = decoded["cam1"]["image"]
            cv2.imshow("cam1 NIR decoded", (nir01 * 255).astype("uint8"))

        if cv2.waitKey(1) in (27, ord("q")):
            break

cv2.destroyAllWindows()