import os
import sys
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0" 
import cv2

# Ensure benchmark uses local workspace code, not site-packages installation.
_local_src = os.path.join(os.path.dirname(__file__), "src")
if _local_src not in sys.path:
    sys.path.insert(0, _local_src)

import orisys
import numpy as np
import time

def main():
    print(f"Using orisys module: {orisys.__file__}")
    # 步骤1：创建传感器对象 (必要步骤)，
    # config_name为配置文件名（内置配置文件名）或路径（自定义配置文件路径）
    # cal_path为校准文件路径, 每一个设备需要创建/使用一个校准文件

    # 方式1: 从视频文件读取
    sensor = orisys.Sensor("s1.mp4", config_name="./config/prototype_of3.json", cal_path= "./config/dd01a_0.npy", isstitch=True, cuda=False, verbose=True, poisson_backend='opencl')

    # 方式2: 从摄像头读取（取消注释使用） 
    # sensor = orisys.Sensor(0, config_name="./config/prototype_of3.json", cal_path= "./config/cal.npy", isstitch=True, cuda=True, verbose=True)

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


    while True:
        # 步骤2：信息提取
        # 步骤 2.1: 获取并拼接图像 (必要步骤)
        start_time = time.perf_counter()
        sensor.get_img() 
        end_time = time.perf_counter()
        get_img_time = end_time - start_time
        # if sensor.detect_motion(threshold=2):
        #     print("motion detected")
        # 步骤 2.2: 计算形变场 (必要步骤)
        start_time = time.perf_counter()
        sensor.compute_deformation()
        end_time = time.perf_counter()
        compute_deformation_time = end_time - start_time
        # 步骤 2.3: 计算触碰点 
        start_time = time.perf_counter()
        sensor.compute_contact()
        end_time = time.perf_counter()
        compute_contact_time = end_time - start_time

    
        #步骤 2.4: 读取信息
        start_time = time.perf_counter()
        fps, flow, vnormal, img, contour, centroid, depth_map = sensor.read_info(
            sensor.info.FPS,
            sensor.info.VRAW,
            sensor.info.VNORMAL,
            sensor.info.IMG,
            sensor.info.CONTOUR,
            sensor.info.CENTROID,
            sensor.info.DEPTH
        )
        end_time = time.perf_counter()
        read_info_time = end_time - start_time
        #步骤 3: 可视化显示
        # 步骤 3.1: 绘制形变场
        start_time = time.perf_counter()
        # arrows = orisys.util.draw_arrows(
        #     img,
        #     flow,
        #     threshold=4,
        #     grid_spacing=40,
        #     arrow_scale=1.0
        # )
        # cv2.imshow("Deformation", arrows)
        
        # # 步骤 3.2: 在图像上触碰点和区域
        # image_with_foe = orisys.util.draw_contact(img, contour, centroid)
        # cv2.imshow("Contact Centroid", image_with_foe)

        # # 步骤 3.3: 绘制深度图
        # # 归一化散度值到0-255范围（使用绝对值，然后归一化）
        # div_abs = depth_map
        # if div_abs.max() > div_abs.min():
        #     div_normalized = ((div_abs - div_abs.min()) / (div_abs.max() - div_abs.min()) * 255).astype(np.uint8)
        # else:
        #     div_normalized = np.zeros_like(div_abs, dtype=np.uint8)
        
        # # 应用颜色映射（使用JET：蓝色=低散度，红色=高散度）
        # cv2.imshow("Depth Map", cv2.applyColorMap(div_normalized, cv2.COLORMAP_JET))

        #步骤 3.4: 打印资料
        # print(f"fps: {fps}")
        end_time = time.perf_counter()
        display_time = end_time - start_time
        
        # Get detailed timing from sensor (capture vs processing)
        capture_time = sensor._last_capture_time
        processing_time = sensor._last_processing_time
        
        total_time = get_img_time + compute_deformation_time + compute_contact_time + read_info_time + display_time
        print(f"get_img: {get_img_time*1000:.2f}ms (capture: {capture_time*1000:.2f}ms, processing: {processing_time*1000:.2f}ms), deformation: {compute_deformation_time*1000:.2f}ms, contact: {compute_contact_time*1000:.2f}ms, read: {read_info_time*1000:.2f}ms, display: {display_time*1000:.2f}ms, total: {total_time*1000:.2f}ms")
            
        #步骤 3.5: 键盘控制
        key = cv2.waitKey(1) & 0xFF
        if key == ord("q"):
            print("\n退出程序...")
            break
        if key == ord("r"):
            # 重置光流追踪器（当追踪漂移时使用）
            sensor.reset()
            print("传感器已重置")
        
    # =========================================================================
    # 4. 清理资源
    # =========================================================================
    sensor.disconnect()  # 释放视频源和相关资源


if __name__ == '__main__':
    main()