import numpy as np
import time
import math
import os

from Ai_tottle.touying.ImageReproject_python import ir_target_positioning, ir_red_line_reproject, ir_img_reproject, \
    CamParas,Point

# from img_types import Point, CamParas
# from image_reproject import ir_target_positioning, ir_red_line_reproject, ir_img_reproject

os.environ['GDAL_DISABLE_EXCEPTIONS'] = 'NO'  # 显式启用异常处理
# 定义常量
pi = math.pi

def test_target_positioning():
    """测试目标定位功能"""
    # 输入参数
    u = 1520  # 示例图像u坐标
    v = 2512  # 示例图像v坐标
    interval = 0.05  # 高程迭代间隔
    gimbal_yaw = 66.40 * pi / 180  # 示例云台偏航角
    gimbal_pitch = -90 * pi / 180  # 示例云台俯仰角
    gimbal_roll = 0 * pi / 180  # 示例云台横滚角
    h = 590.834  # 示例相机高度
    l = (103 + 59 / 60.0 + 26.07 / 3600.0) * pi / 180  # 示例相机经度
    b = (30 + 45 / 60.0 + 46.48 / 3600.0) * pi / 180  # 示例相机纬度
    img_width = 4032  # 示例图像宽度
    img_height = 3024  # 示例图像高度
    
    # 相机参数
    cam_paras = CamParas(
        fx=2795.68899,
        fy=2795.68899
    )
    
    # DEM文件路径
    # dem_file = "/home/GW/ImageReproject_python/dem.tif"  # 确保此文件存在
    dem_file = r"D:\project\test-touying\ImageReproject_python\dem.tif"  # 确保此文件存在

    # 测量执行时间
    start_time = time.time()
    
    # 执行目标定位
    x, y, z = ir_target_positioning(
        u, v, interval, gimbal_pitch, gimbal_yaw, gimbal_roll,
        h, l, b, cam_paras, img_width, img_height, dem_file
    )
    
    # 计算执行时间
    elapsed_time = (time.time() - start_time) * 1000  # 转换为毫秒
    
    # 输出结果
    print(f"目标定位耗时: {elapsed_time:.2f} ms")
    print(f"目标位置: 经度={x }°, 纬度={y }°, 高度={z:.2f}m")

def read_txt(filename):
    """从文本文件读取点坐标"""
    points = []
    try:
        with open(filename, 'r') as f:
            for line in f:
                parts = line.strip().split()
                if len(parts) >= 4:  # 标签 x y z
                    label = parts[0]
                    x = float(parts[1])
                    y = float(parts[2])
                    z = float(parts[3])
                    point = Point(x, y, z)
                    points.append(point)
    except Exception as e:
        print(f"读取文件失败: {filename}, 错误: {e}")
    
    return points

def test_red_line_reproject():
    """测试红线重投影功能"""
    # 输入参数
    gimbal_yaw = 66.40 * pi / 180  # 示例云台偏航角
    gimbal_pitch = -90 * pi / 180  # 示例云台俯仰角
    gimbal_roll = 0 * pi / 180  # 示例云台横滚角
    h = 590.834  # 示例相机高度
    l = (103 + 59 / 60.0 + 26.07 / 3600.0) * pi / 180  # 示例相机经度
    b = (30 + 45 / 60.0 + 46.48 / 3600.0) * pi / 180  # 示例相机纬度
    img_width = 4032  # 示例图像宽度
    img_height = 3024  # 示例图像高度
    
    # 相机参数
    cam_paras = CamParas(
        fx=2795.68899,
        fy=2795.68899
    )
    
    # 图像文件路径
    img_file = r"D:\project\test-touying\ImageReproject_python\test_image.jpeg"  # 确保此文件存在
    img_save_file = r"D:\project\test-touying\ImageReproject_python\test_image_reprojected.jpg"
    
    # 读取点坐标
    points = []  # 如果有点坐标文件，可以使用read_txt函数读取
    points =read_txt(r"D:\project\test-touying\ImageReproject_python\points.txt")
    # 测量执行时间
    start_time = time.time()
    
    # 执行红线重投影
    ir_red_line_reproject(
        gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b,
        cam_paras, img_width, img_height, points, img_file, img_save_file
    )
    
    # 计算执行时间
    elapsed_time = (time.time() - start_time) * 1000  # 转换为毫秒
    
    # 输出结果
    print(f"红线重投影耗时: {elapsed_time:.2f} ms")

def test_img_reproject():
    """测试图像重投影功能"""
    # 输入参数
    obj_longitude = 103.990109  # 目标点经度(度)
    obj_latitude = 30.762915  # 目标点纬度(度)
    obj_height = 481.64  # 目标点高度
    
    gimbal_yaw = 66.40 * pi / 180  # 示例云台偏航角
    gimbal_pitch = -90 * pi / 180  # 示例云台俯仰角
    gimbal_roll = 0 * pi / 180  # 示例云台横滚角
    h = 590.834  # 示例相机高度
    l = (103 + 59 / 60.0 + 26.07 / 3600.0)* pi / 180   # 示例相机经度
    b = (30 + 45 / 60.0 + 46.48 / 3600.0)* pi / 180   # 示例相机纬度
    img_width = 4032  # 示例图像宽度
    img_height = 3024  # 示例图像高度
    
    # 相机参数
    cam_paras = CamParas(
        fx=2795.68899,
        fy=2795.68899
    )
    
    # 测量执行时间
    start_time = time.time()
    
    # 执行图像重投影
    pixel_x, pixel_y = ir_img_reproject(
        gimbal_pitch, gimbal_yaw, gimbal_roll, h, l, b,
        cam_paras, img_width, img_height, obj_longitude, obj_latitude, obj_height
    )
    
    # 计算执行时间
    elapsed_time = (time.time() - start_time) * 1000  # 转换为毫秒
    
    # 输出结果
    print(f"图像重投影耗时: {elapsed_time:.2f} ms")
    print(f"目标点在图像上的坐标: x={pixel_x:.2f}, y={pixel_y:.2f}")

def main():
    """主函数"""
    # # 测试目标定位功能
    # try:
    #     test_target_positioning()
    # except Exception as e:
    #     print(f"目标定位测试失败: {e}")
    
    # # # 测试红线重投影功能
    # try:
    #     test_red_line_reproject()
    # except Exception as e:
    #     print(f"红线重投影测试失败: {e}")
    
    # # 测试图像重投影功能
    try:
        test_img_reproject()
    except Exception as e:
        print(f"图像重投影测试失败: {e}")

if __name__ == "__main__":
    main()