import cv2
import time

# Toggle these to test whether exposure is limiting delivered FPS.
USE_MANUAL_EXPOSURE = False
MANUAL_EXPOSURE = -6  # Typical V4L2/OpenCV manual range is negative values.

cap = cv2.VideoCapture(4, cv2.CAP_V4L2)
cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
cap.set(cv2.CAP_PROP_FPS, 30)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)
cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)

# For V4L2 in OpenCV: auto-exposure is usually 3, manual is usually 1.
if USE_MANUAL_EXPOSURE:
    ok_auto = cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, 1)
    ok_exp = cap.set(cv2.CAP_PROP_EXPOSURE, MANUAL_EXPOSURE)
else:
    ok_auto = cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, 3)
    ok_exp = True

actual_fourcc = int(cap.get(cv2.CAP_PROP_FOURCC))
print(f"Backend: {cap.getBackendName()}")
print(
    f"FOURCC: {chr(actual_fourcc & 0xFF)}"
    f"{chr((actual_fourcc >> 8) & 0xFF)}"
    f"{chr((actual_fourcc >> 16) & 0xFF)}"
    f"{chr((actual_fourcc >> 24) & 0xFF)}"
)
print(f"Resolution: {int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))}x{int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))}")
print(f"FPS: {cap.get(cv2.CAP_PROP_FPS)}")
print(
    f"Exposure mode: {'manual' if USE_MANUAL_EXPOSURE else 'auto'} "
    f"(set_auto_ok={ok_auto}, set_exp_ok={ok_exp})"
)
print(f"AutoExposure(readback): {cap.get(cv2.CAP_PROP_AUTO_EXPOSURE)}")
print(f"Exposure(readback): {cap.get(cv2.CAP_PROP_EXPOSURE)}")

frame_count = 0
start = time.perf_counter()
while cap.isOpened():
    t0 = time.perf_counter()
    ok = cap.grab()
    t1 = time.perf_counter()
    if not ok:
        break

    ret, frame = cap.retrieve()
    t2 = time.perf_counter()
    if not ret:
        continue

    cv2.imshow("webcam", frame)
    t3 = time.perf_counter()
    key = cv2.waitKey(1) & 0xFF
    t4 = time.perf_counter()

    frame_count += 1
    if frame_count % 30 == 0:
        elapsed = t4 - start
        print(
            f"Frame {frame_count}: "
            f"grab={1000*(t1-t0):.1f}ms, "
            f"retrieve={1000*(t2-t1):.1f}ms, "
            f"imshow={1000*(t3-t2):.1f}ms, "
            f"waitKey={1000*(t4-t3):.1f}ms, "
            f"avg_fps={frame_count/elapsed:.1f}"
        )

    if key == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()