import time
from datetime import datetime
from picamera2 import Picamera2
from picamera2.encoders import H264Encoder
import threading

class DualCameraRecorder:
    def __init__(self, width=1280, height=720, fps=60):
        """
        Initialize two cameras
        
        Args:
            width: video width
            height: video height
            fps: target FPS
        """
        self.width = width
        self.height = height
        self.fps = fps
        self.cameras = []
        self.encoders = []
        self.recording = False
        self.fps_counter = [0, 0]
        self.last_fps_time = time.time()
        
    def setup_cameras(self):
        """Setup two cameras"""
        for i in range(2):
            try:
                camera = Picamera2(i)
                
                # Use monochrome format Y10 (10-bit grayscale) for true black and white
                video_config = camera.create_video_configuration(
                    main={"size": (self.width, self.height), "format": "Y10"},
                    controls={"FrameRate": self.fps}
                )
                
                camera.configure(video_config)
                encoder = H264Encoder()
                
                self.cameras.append(camera)
                self.encoders.append(encoder)
                print(f"Camera {i} initialized successfully ({self.width}x{self.height}, {self.fps} FPS, monochrome)")
                
            except Exception as e:
                print(f"Error initializing camera {i}: {e}")
                if i == 0:
                    raise
                else:
                    print("Continuing with one camera")
                    break
    
    def start_recording(self):
        """Start video recording"""
        if not self.cameras:
            print("No cameras available for recording")
            return
        
        timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
        
        for i, (camera, encoder) in enumerate(zip(self.cameras, self.encoders)):
            try:
                filename = f"cam{i+1}_{timestamp}.h264"
                camera.start_recording(encoder, filename)
                print(f"Camera {i} recording to: {filename}")
                
                # Callback for FPS counting
                camera.post_callback = lambda request, idx=i: self.count_fps(idx)
                
            except Exception as e:
                print(f"Error starting recording for camera {i}: {e}")
        
        self.recording = True
        self.recording_start_time = time.time()
        
        # Start thread for FPS output
        self.fps_thread = threading.Thread(target=self.print_fps, daemon=True)
        self.fps_thread.start()
    
    def count_fps(self, cam_index):
        """Frame counter for FPS"""
        self.fps_counter[cam_index] += 1
    
    def stop_recording(self):
        """Stop video recording"""
        self.recording = False
        time.sleep(0.1)  # Give time for callback to complete
        
        for i, camera in enumerate(self.cameras):
            try:
                camera.stop_recording()
                print(f"Camera {i} stopped recording")
            except Exception as e:
                print(f"Error stopping recording for camera {i}: {e}")
    
    def print_fps(self):
        """Print FPS to console every second"""
        while self.recording:
            time.sleep(1)
            current_time = time.time()
            elapsed = current_time - self.last_fps_time
            
            if elapsed > 0:
                fps1 = self.fps_counter[0] / elapsed
                if len(self.cameras) > 1:
                    fps2 = self.fps_counter[1] / elapsed
                    print(f"FPS - Camera 1: {fps1:.2f}, Camera 2: {fps2:.2f}")
                else:
                    print(f"FPS - Camera 1: {fps1:.2f}")
                
                # Reset counters
                self.fps_counter = [0, 0]
                self.last_fps_time = current_time
    
    def run(self, duration_seconds=None):
        """
        Main work cycle
        
        Args:
            duration_seconds: recording duration in seconds (None for infinite)
        """
        try:
            self.setup_cameras()
            self.start_recording()
            
            if duration_seconds:
                print(f"\nRecording for {duration_seconds} seconds...")
                print("=" * 50)
                time.sleep(duration_seconds)
                self.stop_recording()
                print(f"\nRecording complete. Duration: {duration_seconds} sec")
            else:
                print("\nRecording started. Press Ctrl+C to stop")
                print("FPS will be printed every second")
                print("=" * 50)
                
                while True:
                    time.sleep(0.1)
                    
        except KeyboardInterrupt:
            print("\nStopping by user command...")
            self.stop_recording()
        except Exception as e:
            print(f"\nError occurred: {e}")
            self.stop_recording()
        finally:
            for i, camera in enumerate(self.cameras):
                try:
                    camera.close()
                except:
                    pass

def main():
    """
    Simple launch with configurable parameters
    """
    # CHANGE THESE PARAMETERS FOR TESTING
    WIDTH = 1456     # <-- Change width here
    HEIGHT = 1088    # <-- Change height here
    FPS = 60        # <-- Change FPS here (30, 60, 90, 120)
    DURATION = 10   # <-- Recording duration in seconds (None for manual stop)
    
    print(f"\n=== Recording Settings ===")
    print(f"Resolution: {WIDTH}x{HEIGHT}")
    print(f"Target FPS: {FPS}")
    print(f"Format: Y10 (true monochrome)")
    print(f"Codec: H.264")
    print(f"Duration: {DURATION if DURATION else 'manual stop'} sec")
    print("=" * 30)
    
    recorder = DualCameraRecorder(width=WIDTH, height=HEIGHT, fps=FPS)
    
    try:
        recorder.run(duration_seconds=DURATION)
    except KeyboardInterrupt:
        pass

if __name__ == "__main__":
    main()