"""Panda arm with 6×6 ArUco board on base (same marker size as even_larger 4×4).

Larger board for detection quality experiments — 36 markers vs 16 in the 4×4.
Same exo mount STL (panda_base_redo_grip_align_larger.stl).
"""
import os
import numpy as np
from cv2 import aruco

from .exoskeleton import ExoskeletonConfig, LinkConfig

_this_dir = os.path.dirname(os.path.abspath(__file__))
PANDA_MODEL_DIR = os.path.join(os.path.dirname(_this_dir), "robot_models", "franka_emika_panda")
SO100_MODEL_DIR = "robot_models/so100_model"
BOARD_IMG_DIR = f"{SO100_MODEL_DIR}/../board_imgs"

aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250)

# Marker size: identical to the 4×4 even_larger_board
x = (4.75 * 2) / ((6 + 5 / 10) * 100)
x_coarse = 2 * x
BOARD_LENGTH_COARSE = 3 * x_coarse + 2 * (x_coarse / 10)
EVEN_LARGER_PLANE_RATIO = 75 / 50
BOARD_LENGTH_EVEN_LARGER = BOARD_LENGTH_COARSE * EVEN_LARGER_PLANE_RATIO
_x_even = BOARD_LENGTH_EVEN_LARGER / (4 + 3 / 10)  # ~32.63 mm marker size

# 6×6 board with the same marker size
n_6x6_row, n_6x6_col = 6, 6
BOARD_LENGTH_6X6 = n_6x6_row * _x_even + (n_6x6_row - 1) * (_x_even / 10)  # ~0.2121 m
board_ids_6x6 = np.arange(136, 136 + n_6x6_row * n_6x6_col, dtype=np.int32).reshape(-1, 1)
link_boards = {
    "6x6_board": aruco.GridBoard(
        size=(n_6x6_row, n_6x6_col),
        markerLength=_x_even,
        markerSeparation=_x_even / 10,
        dictionary=aruco_dict,
        ids=board_ids_6x6,
    ),
}

VIRTUAL_GRIPPER_BODY_NAME = "virtual_gripper_keypoint"

if 0:  # Generate PNG for 6x6_board (set to 1 and run this module to export)
    import cv2
    from pathlib import Path
    out_dir = Path(__file__).resolve().parents[1] / "robot_models" / "board_imgs"
    out_dir.mkdir(parents=True, exist_ok=True)
    out_path = out_dir / "6x6_board.png"
    img = link_boards["6x6_board"].generateImage((800, 800))
    cv2.imwrite(str(out_path), img)
    try:
        from PIL import Image
        pil_img = Image.open(out_path)
        board_length_in = BOARD_LENGTH_6X6 / 0.0254
        dpi = int(round(pil_img.width / board_length_in))
        pil_img.save(str(out_path), dpi=(dpi, dpi))
    except ImportError:
        pass
    print(f"Wrote {out_path}")


class PandaBaseOnly6x6Config(ExoskeletonConfig):
    """Panda arm: base has a 6×6 ArUco board (same marker size as 4×4 even_larger)."""

    name = "Panda_BaseOnly_6x6"
    base_xml_path = f"{PANDA_MODEL_DIR}/panda.xml"
    background_xml_path = f"{SO100_MODEL_DIR}/background.xml"
    compiler_meshdir = f"{PANDA_MODEL_DIR}/assets/"

    aruco_boards = {
        "6x6_board": f"{BOARD_IMG_DIR}/6x6_board.png",
    }
    aruco_board_objects = {
        "6x6_board": link_boards["6x6_board"],
    }

    links = {
        "larger_base": LinkConfig(
            mujoco_name="link0_8",
            pybullet_name="link0",
            robot_mesh_path="link0_8.obj",
            exo_mesh_path="../../so100_blender_testings/panda_base_redo_grip_align_larger.stl",
            aruco_offset_pos=np.array([46.6, -152.2, 44.0]),
            aruco_offset_rot=np.array([0, 0, 0]),
            aruco_board_name="6x6_board",
            board_length=BOARD_LENGTH_6X6,
        ),
    }


PANDA_BASE_ONLY_6X6_CONFIG = PandaBaseOnly6x6Config()