"""
calibration.py — Robot Tracker v007
Berechnet planare Homographie aus 4 Referenz-AprilTags.
Wandelt Kamera-Pixel in Weltkoordinaten (mm) um.
"""

import logging
import numpy as np
import cv2
from typing import Optional

logger = logging.getLogger(__name__)


class Calibration:
    def __init__(self, reference_tags: dict):
        """
        reference_tags: {tag_id: [x_mm, y_mm], ...}
        Beispiel: {0: [0, 0], 1: [100.5, 0], 2: [0, 181], 3: [100.5, 181]}
        """
        self.reference_tags = reference_tags
        self.H: Optional[np.ndarray] = None      # Homographie-Matrix Pixel → mm
        self.H_inv: Optional[np.ndarray] = None  # mm → Pixel
        self.calibrated = False

    def update(self, detected_tags: list) -> bool:
        """
        Versucht Homographie aus aktuell sichtbaren Referenz-Tags zu berechnen.
        Gibt True zurück wenn Kalibrierung erfolgreich.
        """
        # Nur Referenz-Tags filtern
        ref_detected = {
            t["id"]: t
            for t in detected_tags
            if t["id"] in self.reference_tags
        }

        if len(ref_detected) < 4:
            return False

        # Punkte aufbauen
        pts_pixel = []
        pts_world = []

        for tag_id, world_pos in self.reference_tags.items():
            if tag_id in ref_detected:
                tag = ref_detected[tag_id]
                pts_pixel.append([tag["x"], tag["y"]])
                pts_world.append(world_pos)

        if len(pts_pixel) < 4:
            return False

        src = np.array(pts_pixel, dtype=np.float32)
        dst = np.array(pts_world, dtype=np.float32)

        H, mask = cv2.findHomography(src, dst, cv2.RANSAC, 5.0)

        if H is None:
            logger.warning("Homographie-Berechnung fehlgeschlagen")
            return False

        self.H     = H
        self.H_inv = np.linalg.inv(H)
        self.calibrated = True
        logger.info("Kalibrierung erfolgreich!")
        return True

    def pixel_to_world(self, x_px: float, y_px: float) -> tuple[float, float]:
        """Pixel → Weltkoordinaten (mm)"""
        if not self.calibrated:
            return x_px, y_px
        pt = np.array([[[x_px, y_px]]], dtype=np.float32)
        result = cv2.perspectiveTransform(pt, self.H)
        return float(result[0][0][0]), float(result[0][0][1])

    def world_to_pixel(self, x_mm: float, y_mm: float) -> tuple[float, float]:
        """Weltkoordinaten (mm) → Pixel"""
        if not self.calibrated:
            return x_mm, y_mm
        pt = np.array([[[x_mm, y_mm]]], dtype=np.float32)
        result = cv2.perspectiveTransform(pt, self.H_inv)
        return float(result[0][0][0]), float(result[0][0][1])

    def get_field_corners_px(self) -> Optional[list]:
        """Gibt die Spielfeldecken in Pixel zurück (für Visualisierung)"""
        if not self.calibrated:
            return None
        corners_world = [
            [0, 0],
            [100.5, 0],
            [100.5, 181],
            [0, 181],
        ]
        return [self.world_to_pixel(x, y) for x, y in corners_world]