import pyrealsense2 as rs
import numpy as np
import matplotlib.pyplot as plt
import cv2
import time

def capture_and_plot_realsense():
    # Try to connect to RealSense camera
    try:
        # Configure the RealSense pipeline
        pipeline = rs.pipeline()
        config = rs.config()
        
        # First check if any devices are connected
        ctx = rs.context()
        if len(ctx.devices) == 0:
            print("No RealSense devices connected. Generating simulated image instead.")
            return False

        # Get device info and supported formats
        device = ctx.devices[0]
        print(f"Found device: {device.get_info(rs.camera_info.name)}")
        
        # Configure color stream - try different formats if needed
        try:
            config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
            pipeline.start(config)
        except RuntimeError:
            # If bgr8 fails, try with yuyv format
            config = rs.config()
            config.enable_stream(rs.stream.color, 640, 480, rs.format.yuyv, 30)
            pipeline.start(config)
            
        # Wait for frames
        for i in range(5):  # Wait for a few frames to let auto-exposure adjust
            frames = pipeline.wait_for_frames()
            time.sleep(0.1)
            
        frames = pipeline.wait_for_frames()
        color_frame = frames.get_color_frame()
        
        if not color_frame:
            print("Could not retrieve color frame. Generating simulated image instead.")
            pipeline.stop()
            return False
            
        # Convert to numpy array
        color_image = np.asanyarray(color_frame.get_data())
        
        # Convert to RGB if needed
        if color_image.ndim == 2:  # For yuyv format
            color_image = cv2.cvtColor(color_image, cv2.COLOR_YUV2RGB_YUYV)
        else:  # For bgr8 format
            color_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)
            
        # Display the image
        plt.figure(figsize=(10, 8))
        plt.imshow(color_image)
        plt.title("Image from RealSense Camera")
        plt.axis('off')
        plt.tight_layout()
        plt.show()
        
        # Clean up
        pipeline.stop()
        return True
        
    except Exception as e:
        print(f"Error with RealSense camera: {str(e)}")
        return False

def create_simulated_image():
    """Create a simulated camera image when no RealSense camera is available"""
    # Create a gradient background
    h, w = 480, 640
    y, x = np.ogrid[0:h, 0:w]
    background = np.zeros((h, w, 3), dtype=np.uint8)
    
    # Create a colorful gradient
    background[:, :, 0] = 255 * (x / w)  # Red channel
    background[:, :, 1] = 255 * (y / h)  # Green channel
    background[:, :, 2] = 255 * (1 - y / h)  # Blue channel
    
    # Add some simulated shapes
    cv2.putText(background, "RealSense Camera Simulation", (80, 70), 
                cv2.FONT_HERSHEY_DUPLEX, 1, (255, 255, 255), 2)
    cv2.putText(background, "No camera connected", (150, 120), 
                cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
    
    # Add some geometric shapes
    cv2.circle(background, (320, 240), 100, (255, 0, 0), 3)
    cv2.rectangle(background, (150, 150), (490, 330), (0, 255, 0), 2)
    cv2.line(background, (0, 0), (639, 479), (0, 0, 255), 4)
    
    # Add depth effect
    for i in range(5):
        radius = 30 + i*15
        cv2.circle(background, (500, 120), radius, (255, 255, 255), 2-i//2)
    
    # Display the image
    plt.figure(figsize=(10, 8))
    plt.imshow(background)
    plt.title("Simulated Camera Image")
    plt.axis('off')
    plt.tight_layout()
    plt.show()

# Main execution
if not capture_and_plot_realsense():
    create_simulated_image()