"""Exo-style ArUco config for the medium_single_redo arm."""

from dataclasses import dataclass
from pathlib import Path
from typing import Dict, Tuple

import cv2
import numpy as np
from cv2 import aruco

from robot_configs import medium_single_redo as arm_cfg


@dataclass
class BoardSpec:
    name: str
    ids: np.ndarray
    grid_size: Tuple[int, int]
    board_size_m: float
    detectable_board_size_m: float
    outer_margin_frac: float
    texture_file: str
    board: "cv2.aruco_Board"


def board_size_from_entry(entry: dict) -> float:
    size_raw = float(entry.get("size", 50.0))
    board_size_m = size_raw if size_raw <= 2.0 else (size_raw / 1000.0)
    if bool(entry.get("size_is_blender_plane_scale", False)):
        board_size_m *= 2.0
    return board_size_m


def board_marker_and_sep(board_size_m: float, grid_size: Tuple[int, int]) -> Tuple[float, float]:
    # Must match medium_single_redo generation: markerLength : markerSeparation = 0.8 : 0.2.
    # unit_span sums those relative units across the grid, so 1 unit = board_size_m / unit_span,
    # and the marker itself is 0.8 of those units (not the full unit).
    gx, _ = int(grid_size[0]), int(grid_size[1])
    unit_span = gx * 0.8 + (gx - 1) * 0.2
    marker_len = 0.8 * board_size_m / unit_span
    marker_sep = 0.2 * board_size_m / unit_span
    return marker_len, marker_sep


class OurSO100ExoConfig:
    """ArUco board definitions for printed sheet / overlay."""

    name = "ourso100_medium_single_redo"
    aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_250)
    image_path = Path(__file__).resolve().parent / "scratch" / "arucos_on_sheet.png"
    assets_dir = Path(__file__).resolve().parent / "assets"
    large_board_name = "base"

    board_specs: Dict[str, BoardSpec] = {}
    for link_name, entry in arm_cfg.aruco_positions.items():
        grid = tuple(entry.get("aruco_grid_size", (1, 1)))
        start_id = int(entry.get("aruco_id", 0))
        ids = np.arange(start_id, start_id + int(grid[0]) * int(grid[1]), dtype=np.int32)
        board_size = board_size_from_entry(entry)
        outer_margin_frac = float(entry.get("outer_margin_frac", 0.0))
        outer_margin_frac = max(0.0, min(0.45, outer_margin_frac))
        # OpenCV board object points should match the detectable inner grid region.
        detectable_board_size = board_size * (1.0 - 2.0 * outer_margin_frac)
        marker_len, marker_sep = board_marker_and_sep(detectable_board_size, grid)
        board = aruco.GridBoard(
            size=(int(grid[0]), int(grid[1])),
            markerLength=float(marker_len),
            markerSeparation=float(marker_sep),
            dictionary=aruco_dict,
            ids=ids.reshape(-1, 1),
        )
        board_specs[link_name] = BoardSpec(
            name=link_name,
            ids=ids,
            grid_size=grid,
            board_size_m=board_size,
            detectable_board_size_m=detectable_board_size,
            outer_margin_frac=outer_margin_frac,
            texture_file=str(entry.get("texture_file", f"aruco_{link_name}.png")),
            board=board,
        )