from picamera2 import Picamera2
from collections import deque, defaultdict
import os 


os.environ["LIBCAMERA_LOG_LEVELS"] = "3"
w, h = 640, 480


def make_camera(index):
    picam = Picamera2(index) 
    
    base_config = {"FrameDurationLimits": (16667, 16667),
                   "FrameRate": 60,
                   "Saturation": 0}
                  
    config = picam.create_preview_configuration(
        main={
            "size": (w, h), 
            "format": "YUV420",
        },
        controls=base_config)
    
    picam.configure(config)
    time.sleep(2.0) 
    actual_config = picam.camera_configuration()
    print(f"camera actual controls:")
    pprint.pprint(actual_config)
    
    return picam


def grab_sync_pair(picams: List[Any], frame_duration: float):
    req0 = picams[0].capture_request()
    req1 = picams[1].capture_request()
    while True:
        ts0 = req0.get_metadata()['SensorTimestamp'] / 1000  # use microseconds
        ts1 = req1.get_metadata()['SensorTimestamp'] / 1000
        if ts0 + frame_duration / 2 < ts1:  # req0 too early, next frame should match better
            req0.release()
            req0 = picams[0].capture_request()
        elif ts1 + frame_duration / 2 < ts0:  # req1 too early
            req1.release()
            req1 = picams[1].capture_request()
        else:
            return req0, req1    


def check_sync(cfg, duration=float("inf"), threshold=30.0, window=20):
        
    duration = float(duration)
    threshold = float(threshold)
    window = int(window)

    set_trigger_mode(enable=True)

    picams = [make_camera(i) for i in range(2)]

    frame_duration = 1000000 / 60.
    
    for cam in picams:
        cam.start()
 
    deltas = deque(maxlen=window)
    start_time = time.time()
    status = True    
    
    try:
        c = 0
        while True:
            # break condition for finite-duration runs
            if duration != float("inf") and time.time() - start_time > duration:
                break
            
            req0, req1 = grab_sync_pair(picams, frame_duration)
            
            ts0 = req0.get_metadata()['SensorTimestamp'] / 1000  # use microseconds
            ts1 = req1.get_metadata()['SensorTimestamp'] / 1000
            
            diff = abs(ts0 - ts1)
            deltas.append(diff)

            c += 1
            if c % 20 == 0:
                print("timestamp delta between two cameras: ", ts0, "and", ts1, "difference", diff)
            
            req0.release()
            req1.release()
            
            if duration != float("inf") and len(deltas) == window:
                avg_diff = sum(deltas) / len(deltas)
                if avg_diff > threshold:
                    status = False
       
    except KeyboardInterrupt:
        # Ctrl + C to properly stop cameras
        print("Stopping cameras (Ctrl+C pressed)...")
        status = False if duration == float("inf") else status
    finally:
        [cam.stop() for cam in picams if cam is not None]
        [cam.close() for cam in picams if cam is not None]
        print("Cameras stopped.")    
        
    if duration == float("inf"):
        return None
    return status