"""
测试AprilTag检测和透视校正效果

检查：
1. AprilTag是否能被准确检测
2. 透视畸变程度（边长差异）
3. 重投影误差（校正质量）
4. px/mm标定参数
"""

import cv2
import sys
from pathlib import Path
import logging
import numpy as np

# 添加src路径
sys.path.insert(0, str(Path(__file__).parent))

try:
    from .apriltag_calibration import AprilTagCalibrator
except ImportError:  # pragma: no cover - fallback for direct script usage
    from apriltag_calibration import AprilTagCalibrator

logging.basicConfig(level=logging.INFO, format='%(levelname)s: %(message)s')


def test_apriltag_detection(image_dir: str, apriltag_size_mm: float = 3.76, num_samples: int = 10):
    """
    测试AprilTag检测和透视校正

    参数:
        image_dir: 图像目录
        apriltag_size_mm: AprilTag尺寸（毫米）
        num_samples: 测试样本数量
    """
    calibrator = AprilTagCalibrator(tag_size_mm=apriltag_size_mm)

    image_files = list(Path(image_dir).glob('*.jpg'))[:num_samples]

    print(f"\n{'='*70}")
    print(f"AprilTag检测测试 - 共测试 {len(image_files)} 张图片")
    print(f"AprilTag尺寸: {apriltag_size_mm} mm")
    print(f"{'='*70}\n")

    detection_success = 0
    distortion_stats = []
    reprojection_errors = []
    px_per_mm_values = []

    failed_images = []

    for idx, image_file in enumerate(image_files, 1):
        print(f"[{idx}/{len(image_files)}] {image_file.name}")

        # 读取图像
        image = cv2.imread(str(image_file))
        if image is None:
            print(f"  ❌ 无法读取图像\n")
            continue

        # AprilTag检测（启用透视校正）
        result = calibrator.detect(image, use_homography=True)

        if not result or not result['detected']:
            print(f"  ❌ 未检测到AprilTag\n")
            failed_images.append(image_file.name)
            continue

        detection_success += 1

        # 提取关键指标
        tag_id = result['tag_id']
        tag_size_px = result['tag_size_px']
        px_per_mm = result['px_per_mm']
        px_per_mm_corrected = result.get('px_per_mm_corrected', px_per_mm)
        distortion_ratio = result['distortion_ratio']
        reprojection_error = result.get('reprojection_error', 0)

        distortion_stats.append(distortion_ratio)
        reprojection_errors.append(reprojection_error)
        px_per_mm_values.append(px_per_mm_corrected)

        # 输出详细信息
        print(f"  ✓ Tag ID: {tag_id}")
        print(f"  • Tag尺寸: {tag_size_px:.2f} px")
        print(f"  • 边长差异: {distortion_ratio*100:.2f}% ", end='')

        if distortion_ratio < 0.02:
            print("✓ 极小畸变")
        elif distortion_ratio < 0.05:
            print("✓ 轻微畸变")
        elif distortion_ratio < 0.10:
            print("⚠️  中等畸变")
        else:
            print("⚠️⚠️  严重畸变")

        print(f"  • 重投影误差: {reprojection_error:.3f} px ", end='')
        if reprojection_error < 0.5:
            print("✓ 优秀")
        elif reprojection_error < 1.0:
            print("✓ 良好")
        elif reprojection_error < 2.0:
            print("⚠️  一般")
        else:
            print("❌ 较差")

        print(f"  • px/mm比例: {px_per_mm_corrected:.3f}")

        # 边长详情
        side_lengths = result['side_lengths_px']
        print(f"  • 边长详情: [{side_lengths[0]:.1f}, {side_lengths[1]:.1f}, "
              f"{side_lengths[2]:.1f}, {side_lengths[3]:.1f}] px")

        # 保存第一张图片的可视化
        if idx == 1:
            vis_image = calibrator.visualize(image, result)
            output_path = Path('../outputs') / 'apriltag_test_sample.jpg'
            output_path.parent.mkdir(exist_ok=True)
            cv2.imwrite(str(output_path), vis_image)
            print(f"\n  📊 可视化结果已保存: {output_path}")

        print()

    # 统计总结
    print(f"{'='*70}")
    print("检测总结")
    print(f"{'='*70}")
    print(f"检测成功率: {detection_success}/{len(image_files)} ({detection_success/len(image_files)*100:.1f}%)")

    if detection_success > 0:
        print(f"\n【透视畸变统计】")
        print(f"  平均边长差异: {np.mean(distortion_stats)*100:.2f}%")
        print(f"  最小/最大: {np.min(distortion_stats)*100:.2f}% / {np.max(distortion_stats)*100:.2f}%")

        severe_distortion = sum(1 for d in distortion_stats if d > 0.05)
        if severe_distortion > 0:
            print(f"  ⚠️  {severe_distortion} 张图片存在明显畸变 (>5%)")
        else:
            print(f"  ✓ 所有图片畸变较小 (<5%)")

        print(f"\n【重投影误差统计】")
        print(f"  平均误差: {np.mean(reprojection_errors):.3f} px")
        print(f"  最小/最大: {np.min(reprojection_errors):.3f} / {np.max(reprojection_errors):.3f} px")

        if np.mean(reprojection_errors) < 1.0:
            print(f"  ✓ 校正质量优秀")
        else:
            print(f"  ⚠️  校正质量一般，建议检查拍摄角度")

        print(f"\n【标定参数统计】")
        print(f"  平均 px/mm: {np.mean(px_per_mm_values):.3f}")
        print(f"  标准差: {np.std(px_per_mm_values):.3f}")
        print(f"  变异系数: {np.std(px_per_mm_values)/np.mean(px_per_mm_values)*100:.2f}%")

        if np.std(px_per_mm_values) / np.mean(px_per_mm_values) < 0.05:
            print(f"  ✓ 标定参数一致性好 (CV<5%)")
        else:
            print(f"  ⚠️  标定参数波动较大，可能影响精度")

    if len(failed_images) > 0:
        print(f"\n【检测失败的图片】")
        for fname in failed_images:
            print(f"  • {fname}")

    print(f"\n{'='*70}")

    # 给出建议
    if detection_success == len(image_files):
        print("✅ AprilTag检测完美！可以进行完整分析")
        return True
    elif detection_success > len(image_files) * 0.8:
        print("⚠️  部分图片AprilTag检测失败，建议检查这些图片")
        print("   可能原因：Tag遮挡、光照过暗/过曝、Tag尺寸过小")
        return True
    else:
        print("❌ AprilTag检测成功率过低！")
        print("   建议：检查AprilTag是否清晰可见、尺寸是否正确")
        return False


if __name__ == "__main__":
    test_apriltag_detection(
        image_dir='../data/raw_images',
        apriltag_size_mm=37.58,
        num_samples=10
    )
