import pyrealsense2 as rs
import numpy as np

class CamaraD415:
    def __init__(self):
        self.pipeline = rs.pipeline()
        config = rs.config()
        config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 15)
        self.colorizer = rs.colorizer()
        try:
            self.pipeline.start(config)
            print("✅ Cámara D415 Iniciada (Modo Video + Lidar)")
        except Exception as e: print(f"❌ Error Cámara: {e}")

    def obtener_datos(self):
        try:
            frames = self.pipeline.wait_for_frames(timeout_ms=1000)
            depth_frame = frames.get_depth_frame()
            if not depth_frame: return None, None
            
            depth_color_frame = self.colorizer.colorize(depth_frame)
            imagen_video = np.asanyarray(depth_color_frame.get_data())
            
            profundidad_matriz = np.asanyarray(depth_frame.get_data())
            fila_central = profundidad_matriz[240, :] * 0.001 
            return fila_central, imagen_video
        except: return None, None

    def cerrar(self): self.pipeline.stop()