"""平面控制点标定和像素/世界坐标转换。"""
import math


class CalibrationError(ValueError):
    pass


def _cv():
    try:
        import cv2
        import numpy as np
        return cv2, np
    except Exception as exc:
        raise CalibrationError("OpenCV/Numpy 不可用，无法执行标定") from exc


def map_point(homography, u, v):
    """使用 3x3 Homography 将像素点映射到世界平面。"""
    _cv2, np = _cv()
    matrix = np.asarray(homography, dtype=np.float64)
    if matrix.shape != (3, 3) or not np.isfinite(matrix).all():
        raise CalibrationError("Homography 格式无效")
    p = matrix.dot(np.asarray([float(u), float(v), 1.0], dtype=np.float64))
    if abs(float(p[2])) < 1e-10:
        raise CalibrationError("像素点无法投影到地面")
    return float(p[0] / p[2]), float(p[1] / p[2])


def _area_ratio(points, frame_width, frame_height):
    cv2, np = _cv()
    hull = cv2.convexHull(np.asarray(points, dtype=np.float32))
    denom = max(1.0, float(frame_width) * float(frame_height))
    return float(cv2.contourArea(hull)) / denom


def solve_planar_calibration(observations, frame_width, frame_height,
                             max_validation_mean_m=1.0):
    """根据拟合点求 H，并使用完全独立的验证点评价世界坐标误差。"""
    cv2, np = _cv()
    try:
        fw, fh = int(frame_width), int(frame_height)
    except Exception as exc:
        raise CalibrationError("图像尺寸无效") from exc
    if fw <= 0 or fh <= 0:
        raise CalibrationError("图像尺寸必须大于 0")

    fit = [p for p in observations if p.get("role", "fit") == "fit"]
    verify = [p for p in observations if p.get("role") == "verify"]
    if len(fit) < 4:
        raise CalibrationError("至少需要 4 个拟合点")
    if len(verify) < 3:
        raise CalibrationError("至少需要 3 个独立验证点")

    def pixels(rows):
        return np.asarray([[float(p["u"]), float(p["v"])] for p in rows], dtype=np.float64)

    def worlds(rows):
        return np.asarray([[float(p["x"]), float(p["y"])] for p in rows], dtype=np.float64)

    src, dst = pixels(fit), worlds(fit)
    if not np.isfinite(src).all() or not np.isfinite(dst).all():
        raise CalibrationError("控制点包含非有限数值")
    if _area_ratio(src, fw, fh) < 0.01:
        raise CalibrationError("拟合点共线或过度集中，请扩大点位分布")
    if float(cv2.contourArea(cv2.convexHull(dst.astype(np.float32)))) < 0.01:
        raise CalibrationError("世界坐标点共线或过度集中")

    matrix, mask = cv2.findHomography(src, dst, cv2.RANSAC, 0.75)
    if matrix is None or not np.isfinite(matrix).all() or abs(float(np.linalg.det(matrix))) < 1e-12:
        raise CalibrationError("无法计算稳定的 Homography")

    fit_projection = cv2.perspectiveTransform(src.reshape(-1, 1, 2), matrix).reshape(-1, 2)
    fit_errors = np.linalg.norm(fit_projection - dst, axis=1)
    verify_src, verify_dst = pixels(verify), worlds(verify)
    verify_projection = cv2.perspectiveTransform(verify_src.reshape(-1, 1, 2), matrix).reshape(-1, 2)
    verify_errors = np.linalg.norm(verify_projection - verify_dst, axis=1)

    fit_rmse = math.sqrt(float(np.mean(np.square(fit_errors))))
    validation_mean = float(np.mean(verify_errors))
    validation_max = float(np.max(verify_errors))
    warnings = []
    if len(fit) < 8:
        warnings.append("拟合点少于建议的 8 个")
    coverage = _area_ratio(src, fw, fh)
    if coverage < 0.15:
        warnings.append("拟合点仅覆盖画面 %.1f%%，建议增加边缘和远端点" % (coverage * 100.0))
    inliers = int(mask.sum()) if mask is not None else len(fit)
    if inliers < len(fit):
        warnings.append("RANSAC 排除了 %d 个异常拟合点" % (len(fit) - inliers))

    detail = []
    fit_index = verify_index = 0
    for p in observations:
        row = dict(p)
        if p.get("role", "fit") == "fit":
            row["error_m"] = float(fit_errors[fit_index])
            fit_index += 1
        else:
            row["error_m"] = float(verify_errors[verify_index])
            verify_index += 1
        detail.append(row)

    return {
        "homography": matrix.tolist(),
        "fit_rmse_m": fit_rmse,
        "validation_mean_m": validation_mean,
        "validation_max_m": validation_max,
        "coverage_ratio": coverage,
        "inlier_count": inliers,
        "is_valid": validation_mean <= float(max_validation_mean_m),
        "warnings": warnings,
        "observations": detail,
    }


def foot_point_world(box, homography, width_m, height_m, boundary_margin_m=1.0):
    if not box or len(box) != 4:
        return None
    u = (float(box[0]) + float(box[2])) * 0.5
    v = float(box[3])
    x, y = map_point(homography, u, v)
    margin = max(0.0, float(boundary_margin_m))
    if x < -margin or y < -margin or x > float(width_m) + margin or y > float(height_m) + margin:
        return None
    return max(0.0, min(float(width_m), x)), max(0.0, min(float(height_m), y))