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

import cv2
import orisys
import numpy as np



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 = sensor.read_info(
            sensor.info.FPS,
            sensor.info.VRAW,
            sensor.info.VNORMAL
        )

        # 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)


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

        # 计算扩张焦点
        # foe_result = orisys.util.compute_foe(vnormal,min_magnitude=1,expansion_threshold=0.01,downsample_factor=1)
        foe_result = sensor.get_foe(multiple_foe=False, min_magnitude=1, expansion_threshold=0.01, downsample_factor=1,denoise_sigma=10)
        # 在图像上绘制焦点和区域
        image_with_foe = orisys.util.draw_foe(sensor.img.copy(), foe_result)
        cv2.imshow("Focus of Expansion", image_with_foe)
        

        # =====================================================================
        # 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()