import os
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"  # Windows 下禁用硬件加速，避免兼容性问题

import cv2
import orisys
import numpy as np
from scipy.ndimage import gaussian_filter


def get_depth(div, fps, H=400, W=400, Z0=30, win_size=10):
        # Precompute
    dt = 1.0 / fps  # e.g., 1/30
    Z_map = np.full((H, W), Z0, dtype=np.float32)  # initial depth map


    for win_y in range(0, H, win_size):
        for win_x in range(0, W, win_size):
            y1, y2 = win_y, min(win_y + win_size, H)
            x1, x2 = win_x, min(win_x + win_size, W)
            
            # Robust divergence estimate
            D_win = np.median(div[y1:y2, x1:x2])
            
            # Avoid numerical issues
            if np.abs(D_win) < 1e-3:
                continue
                
            # Update depth in window
            Z_old = Z_map[y1, x1]  # or use window mean
            Z_new = Z_old * np.exp(-0.5 * D_win * dt)
            
            # Optionally fuse with dot-based depth if available
            # Z_new = alpha * Z_new + (1 - alpha) * Z_from_dots
            
            Z_map[y1:y2, x1:x2] = Z_new
    return Z_map

def main():
   
    
    # 方式1: 从视频文件读取
    # sensor = orisys.Sensor("./s1.mp4", isstitch=True, cuda=True, verbose=True)
    
    # 方式2: 从摄像头读取（取消注释使用）
    sensor = orisys.Sensor(0, isstitch=True, cuda=True, verbose=True)

    print("\n按 'q' 退出程序，按 'r' 重置光流追踪器")

    while True:
        # 步骤 2.1: 获取并拼接图像
        sensor.get_img()
        
        # 步骤 2.2: 计算形变场
        sensor.compute_deformation()
        
        fps, flow,vnormal, depth = sensor.read_info(
            sensor.info.FPS,
            sensor.info.VRAW,
            sensor.info.VNORMAL,
            sensor.info.DEPTH
        )

        # depth = get_depth_improved(vnormal, fps)
        
        print(f"FPS: {fps:5.1f}, 深度: {np.min(depth):8.2f}, {np.mean(depth):8.2f}, {np.max(depth):8.2f}")

        
        
     
        arrows = orisys.util.draw_arrows(
            sensor.img.copy(),  # 使用 copy() 避免修改原图
            vnormal,
            threshold=5,
            grid_spacing=10,
            arrow_scale=1.0
        )
        cv2.imshow("形变场 Deformation", arrows)

        # # 绘制深度图（热力图）
        # # 深度值缩放：中心值 30，范围 ±10（即 20-40 映射到 0-255）
        # depth_center = 30  # 中心深度值
        # depth_range = 5  # 变化范围 ±10
        # depth_normalized = (depth - (depth_center - depth_range)) / (2 * depth_range)  # 归一化到 0-1
        # depth_normalized = np.clip(depth_normalized, 0, 1)  # 限制在 0-1 范围
        depth_img = cv2.applyColorMap(
            (depth * 255.0).astype('uint8'),
            cv2.COLORMAP_HOT
        )
        cv2.imshow("深度图 Depth", depth_img)

        mag_img = orisys.util.draw_magnitude_map(
            sensor.img.copy(),
            vnormal,
            threshold=5,
            colormap=cv2.COLORMAP_HOT
        )
        cv2.imshow("magnitude map", mag_img)

        # =====================================================================
        # 4. 键盘控制
        # =====================================================================
        key = cv2.waitKey(1) & 0xFF
        if key == ord("q"):
            print("\n退出程序...")
            break
        if key == ord("r"):
            # 重置光流追踪器（当追踪漂移时使用）
            sensor.reset()
            print("光流追踪器已重置")
    
    # =========================================================================
    # 5. 清理资源
    # =========================================================================
    sensor.disconnect()  # 释放视频源和相关资源
    cv2.destroyAllWindows()


if __name__ == '__main__':
    main()