import sys
import pyzed.sl as sl
import cv2
import time
import threading
import signal
import os

exit_app = False
def signal_handler(signal, frame):
    global exit_app
    exit_app = True
    print("\nCtrl+C pressed. Exiting...")

signal.signal(signal.SIGINT, signal_handler)

class CameraReceiver:
    def __init__(self, port, camera_id):
        self.port = port
        self.camera_id = camera_id
        self.zed = sl.CameraOne()
        self.is_connected = False
        self.frame_count = 0
        self.save_path = f"camera_{camera_id}_images"
        
        # Create directory for saving images if it doesn't exist
        if not os.path.exists(self.save_path):
            os.makedirs(self.save_path)
            
    def connect(self):
        # Setup camera parameters
        init_parameters = sl.InitParametersOne()
        # init_parameters.depth_mode = sl.DEPTH_MODE.NONE
        init_parameters.sdk_verbose = 1
        init_parameters.set_from_stream("127.0.0.1", self.port)
        
        
        # Try to open the camera
        status = self.zed.open(init_parameters)
        if status != sl.ERROR_CODE.SUCCESS and status != sl.ERROR_CODE.INVALID_CALIBRATION_FILE:
            print(f"Camera {self.camera_id} Open : {status}. Failed to connect.")
            return False
            
        # If we reach here, we're connected
        self.runtime_parameters = sl.RuntimeParameters()
        self.left_image = sl.Mat()
        self.right_image = sl.Mat()
        self.is_connected = True
        
        print(f"Camera {self.camera_id} connected successfully on port {self.port}")
        self.print_camera_information()
        return True
    
    def print_camera_information(self):
        cam_info = self.zed.get_camera_information()
        print(f"\n--- Camera {self.camera_id} Information ---")
        print(f"ZED Model            : {cam_info.camera_model}")
        print(f"ZED Serial Number    : {cam_info.serial_number}")
        print(f"ZED Firmware         : {cam_info.camera_configuration.firmware_version}/{cam_info.sensors_configuration.firmware_version}")
        print(f"ZED Resolution       : {cam_info.camera_configuration.resolution.width}x{cam_info.camera_configuration.resolution.height}")
        print(f"ZED FPS              : {int(cam_info.camera_configuration.fps)}")
        
    def process_frames(self):
        if not self.is_connected:
            print(f"Camera {self.camera_id} not connected")
            return
        errors = 0
        while not exit_app:
            err = self.zed.grab()
            if err == sl.ERROR_CODE.SUCCESS:
                timestamp = self.zed.get_timestamp(sl.TIME_REFERENCE.IMAGE).get_milliseconds()
                
                self.zed.retrieve_image(self.left_image, sl.VIEW.LEFT)
                self.zed.retrieve_image(self.right_image, sl.VIEW.RIGHT)
                
                left_cv_image = self.left_image.get_data()
                right_cv_image = self.right_image.get_data()
                
                if left_cv_image is not None and right_cv_image is not None:
                    try:    
                        cv2.imwrite(f"{self.save_path}/left_{timestamp}.png", left_cv_image)
                        cv2.imwrite(f"{self.save_path}/right_{timestamp}.png", right_cv_image)
                    except Exception as e:
                        print(f"this is the {errors}'th image dropped")
                        errors += 1
                    
                self.frame_count += 1
                if self.frame_count % 10 == 0: 
                    print(f"Camera {self.camera_id} - Captured frame {self.frame_count}")
                    
            else:
                print(f"Camera {self.camera_id} - Error during capture: {err}")
                time.sleep(0.1)  # Small delay to prevent CPU overuse
                
        print(f"Camera {self.camera_id} - Stopping frame processing")
        
    def close(self):
        if self.is_connected:
            self.zed.close()
            print(f"Camera {self.camera_id} - Closed")
            
def main():
    receivers = [
        CameraReceiver(port=30000, camera_id="316310960"),
        CameraReceiver(port=30002, camera_id="310559220")
    ]
    
    connected_receivers = []
    for receiver in receivers:
        if receiver.connect():
            connected_receivers.append(receiver)
    
    if not connected_receivers:
        print("No cameras connected. Exiting.")
        return 1
    
    threads = []
    for receiver in connected_receivers:
        thread = threading.Thread(target=receiver.process_frames)
        thread.daemon = True  # Thread will exit when main program exits
        thread.start()
        threads.append(thread)
    
    try:
        while not exit_app:
            time.sleep(0.1)
    except KeyboardInterrupt:
        pass
    
    # Close all cameras
    for receiver in connected_receivers:
        receiver.close()
    
    # Close all OpenCV windows
    cv2.destroyAllWindows()
    print("Program exited successfully")
    return 0

if __name__ == "__main__":
    sys.exit(main())