import os
import sys
import ctypes
from pathlib import Path
import numpy as np

# 获取项目根目录（当前文件所在目录的父目录）
project_root = Path(__file__).parent.parent.parent
bin_dir = project_root / "Bin"

# 添加Bin目录到Python模块搜索路径
if str(bin_dir) not in sys.path:
    sys.path.insert(0, str(bin_dir))

# 预加载依赖库，确保动态链接器能找到它们
lib_guide_stitch = bin_dir / "libGuideStitch.so"
if lib_guide_stitch.exists():
    try:
        ctypes.CDLL(str(lib_guide_stitch), mode=ctypes.RTLD_GLOBAL)
    except Exception as e:
        print(f"警告: 预加载libGuideStitch.so失败: {e}")

# 导入模块
from UStitcher import API_UnderStitch, FrameInfo

import cv2


frame_info = FrameInfo()



frame_info.nFrmID = 1
frame_info.craft.stPos.B = 39
frame_info.craft.stPos.L = 120
frame_info.craft.stPos.H = 1000
frame_info.camInfo.nFocus = 40
frame_info.camInfo.fPixelSize = 12

frame_info.servoInfo.fServoAz = 90
frame_info.servoInfo.fServoPt = -45

frame_info.nEvHeight = 1200

frame_info.nWidth = 1280
frame_info.nHeight = 1024



stitcher = API_UnderStitch.Create()


# 先初始化（设置原点）
pan_info = stitcher.Init(frame_info)
print(f"初始化成功，全景图尺寸: {pan_info.m_pan_width} x {pan_info.m_pan_height}")

def warpPointWithH(H, pt):
    wp = H @ np.array([pt[0],pt[1],1]).T
    return wp / wp[2]

for i in range(100):
    frame_info.nFrmID = i
    frame_info.craft.stPos.B = 39
    frame_info.craft.stPos.L = 120
    frame_info.craft.stPos.H = 1000
    frame_info.camInfo.nFocus = 40
    frame_info.camInfo.fPixelSize = 12

    frame_info.servoInfo.fServoAz = 90
    frame_info.servoInfo.fServoPt = -45

    H = stitcher.getHomography(frame_info)
    print(f"单应性矩阵 H:\n{H}")

    wp = warpPointWithH(H, np.array([100,111]))
    print(f"物方坐标:\n{wp}")

print("done")