import pyzed.sl as sl
#import cv2
#import numpy as np

def main():
    zed = sl.Camera()

    # Initialize parameters
    init_params = sl.InitParameters()
    init_params.camera_resolution = sl.RESOLUTION.HD1080
    init_params.sdk_verbose = True

    # 1. Define your serial numbers
    sn_left = 305072903  
    sn_right = 307352076 
    sn_virtual = 115258193 

    # 2. Map the physical cameras to the virtual rig
    init_params.input.set_virtual_stereo_from_serial_numbers(sn_left, sn_right, sn_virtual)

    # 3. Supply the optional OpenCV calibration file
    # Note the leading forward slash for an absolute path
    #init_params.optional_opencv_calibration_file = "/home/beetracker/zed-opencv-calibration/build/stereo_calibration/zed_calibration_115258193.yml"

    print(f"Opening Virtual Stereo Rig with custom OpenCV calibration...")
    err = zed.open(init_params)
    print(str(err))
    if err != sl.ERROR_CODE.SUCCESS:
        print(f"Error: {err}")
        exit(1)

    print("Success! Live feed starting...")
    image_left = sl.Mat()
    image_right = sl.Mat()
    
    while True:
        if zed.grab() == sl.ERROR_CODE.SUCCESS:
            zed.retrieve_image(image_left, sl.VIEW.LEFT)
            zed.retrieve_image(image_right, sl.VIEW.RIGHT)
            
            frame_l = image_left.get_data()
            frame_r = image_right.get_data()
            h_stack = np.hstack((frame_l, frame_r))
            
            cv2.imshow("ZED X One - Virtual Stereo (OpenCV Calib)", cv2.resize(h_stack, (1280, 480)))

        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    zed.close()
    cv2.destroyAllWindows()

if __name__ == "__main__":
    main()