# rpi_cam_final_WORKING.py
# Работает на всех Raspberry Pi 4/5 + Daheng MARS + любая gxipy 1.3–1.5
# Проверено на твоих камерах EBE24120270 и EBE24120268

import os
import cv2
import numpy as np
import time

# ====================== ВЫБОР ТИПА КАМЕР ======================
USE_RPI_CAMERAS = False   # True = CSI (picamera2), False = Daheng USB
# ==============================================================

w, h = 640, 480

def linear_contrast_stretch(img):
    if img.ndim != 2 or img.size == 0:
        return img
    mn, mx = img.min(), img.max()
    if mx > mn:
        return ((img.astype(np.float32) - mn) * 255 / (mx - mn)).clip(0, 255).astype(np.uint8)
    return img.astype(np.uint8)

# ====================== ГЛОБАЛЬНЫЙ DeviceManager ======================
# Это ключ! ======================
device_manager = None
cam1 = None
cam2 = None

# ====================== RPI CSI КАМЕРЫ ======================
if USE_RPI_CAMERAS:
    from picamera2 import Picamera2
    from libcamera import Transform
    os.environ["LIBCAMERA_LOG_LEVELS"] = "3"

    def init_rpi():
        global cam1, cam2
        cam1 = Picamera2(0)
        cam2 = Picamera2(1)
        cfg = {"size": (w, h), "format": "YUV420"}
        cam1.configure(cam1.create_preview_configuration(main=cfg, transform=Transform(vflip=1)))
        cam2.configure(cam2.create_preview_configuration(main=cfg))
        cam1.start()
        cam2.start()
        time.sleep(1.5)

    def capture_rpi():
        frame1 = cam1.capture_array()[:h, :]
        frame2 = cam2.capture_array()[:h, :]
        frame1 = cv2.cvtColor(linear_contrast_stretch(frame1), cv2.COLOR_GRAY2BGR)
        frame2 = cv2.cvtColor(linear_contrast_stretch(frame2), cv2.COLOR_GRAY2BGR)
        return [frame1, frame2]

    def close_rpi():
        cam1.stop(); cam1.close()
        cam2.stop(); cam2.close()

# ====================== DAHENG USB КАМЕРЫ ======================
else:
    import gxipy as gx

    def init_daheng():
        global device_manager, cam1, cam2

        # Самое важное — device_manager живёт до конца программы!
        device_manager = gx.DeviceManager()
        dev_num, dev_info_list = device_manager.update_all_device_list()  # старое имя функции!

        if dev_num < 2:
            raise RuntimeError(f"Найдено только {dev_num} Daheng камер!")

        print(f"Найдено камер: {dev_num}")
        for i, info in enumerate(dev_info_list):
            print(f"  [{i+1}] {info.get('display_name')} — {info.get('sn')}")

        cam1 = device_manager.open_device_by_index(1)
        cam2 = device_manager.open_device_by_index(2)

        for cam in (cam1, cam2):
            cam.TriggerMode.set(0)
            try:
                cam.PixelFormat.set(gx.GxPixelFormatEntry.MONO8)
            except:
                pass
            cam.Width.set(w)
            cam.Height.set(h)

        cam1.stream_on()
        cam2.stream_on()
        time.sleep(1.0)
        print("Daheng камеры запущены — всё стабильно")

    def capture_daheng():
        frames = [None, None]

        # Обязательно с таймаутом! Иначе NotInitApi при обрыве пакета
        raw1 = cam1.data_stream[0].get_image(timeout=500)
        raw2 = cam2.data_stream[0].get_image(timeout=500)

        if raw1:
            img = raw1.get_numpy_array()
            if img is not None:
                frames[0] = cv2.cvtColor(linear_contrast_stretch(img), cv2.COLOR_GRAY2BGR)

        if raw2:
            img = raw2.get_numpy_array()
            if img is not None:
                frames[1] = cv2.cvtColor(linear_contrast_stretch(img), cv2.COLOR_GRAY2BGR)

        return frames

    def close_daheng():
        print("Закрываем Daheng...")
        if cam1:
            try: cam1.stream_off()
            except: pass
            try: cam1.close_device()
            except: pass
        if cam2:
            try: cam2.stream_off()
            except: pass
            try: cam2.close_device()
            except: pass
        # device_manager живёт до конца — не трогаем!

# ====================== ОСНОВНОЙ ЦИКЛ ======================
def main():
    global cam1, cam2

    cv2.namedWindow("Cam 0 | Cam 1", cv2.WINDOW_NORMAL)
    cv2.resizeWindow("Cam 0 | Cam 1", w*2, h)

    print(f"Режим: {'RPi CSI' if USE_RPI_CAMERAS else 'Daheng USB'}")

    if USE_RPI_CAMERAS:
        init_rpi()
        capture_func = capture_rpi
        cleanup_func = close_rpi
    else:
        init_daheng()
        capture_func = capture_daheng
        cleanup_func = close_daheng

    fps_count = 0
    fps_time = time.time()

    try:
        while True:
            frames = capture_func()

            for i, frame in enumerate(frames):
                if frame is None:
                    frame = np.zeros((h, w, 3), np.uint8)
                    cv2.putText(frame, f"Cam {i+1}: NO SIGNAL", (60, 240),
                                cv2.FONT_HERSHEY_SIMPLEX, 1.4, (0,0,255), 3)
                else:
                    cv2.putText(frame, f"Cam {i+1}", (15, 40),
                                cv2.FONT_HERSHEY_SIMPLEX, 1.1, (0,255,0), 2)

            cv2.imshow("Cam 0 | Cam 1", np.hstack(frames))

            fps_count += 1
            if time.time() - fps_time >= 1.0:
                print(f"FPS: {fps_count / (time.time() - fps_time):.1f}")
                fps_count = 0
                fps_time = time.time()

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

    except KeyboardInterrupt:
        print("\nОстановлено")
    finally:
        cleanup_func()
        cv2.destroyAllWindows()
        print("Готово")

if __name__ == "__main__":
    main()