"""Panda arm with 4×2 rectangular ArUco board on the end-effector (hand).

Wider than tall to match the Panda EEF form factor.
Same marker size as the even_larger 4×4 board (~32.63 mm).
  - 4 columns × 2 rows, DICT_6X6_250
  - 8 markers total, IDs 242–249
  - Physical board ≈ 140 × 68 mm (W × H)
"""
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

# 4×2 rectangular board (4 cols, 2 rows)
n_4x2_cols, n_4x2_rows = 4, 2
BOARD_WIDTH_4X2 = n_4x2_cols * _x_even + (n_4x2_cols - 1) * (_x_even / 10)   # ~0.1403 m
BOARD_HEIGHT_4X2 = n_4x2_rows * _x_even + (n_4x2_rows - 1) * (_x_even / 10)  # ~0.0685 m
BOARD_LENGTH_4X2 = BOARD_WIDTH_4X2  # longest edge
board_ids_4x2 = np.arange(242, 242 + n_4x2_cols * n_4x2_rows, dtype=np.int32).reshape(-1, 1)
link_boards = {
    "4x2_board": aruco.GridBoard(
        size=(n_4x2_cols, n_4x2_rows),
        markerLength=_x_even,
        markerSeparation=_x_even / 10,
        dictionary=aruco_dict,
        ids=board_ids_4x2,
    ),
}
handeye_board = link_boards["4x2_board"]

# MuJoCo plane scale
_SF = 0.475 / BOARD_LENGTH_EVEN_LARGER
PLANE_SCALE_4X2 = (BOARD_WIDTH_4X2 * _SF, BOARD_HEIGHT_4X2 * _SF)

VIRTUAL_GRIPPER_BODY_NAME = "virtual_gripper_keypoint"

if 0:  # Generate PNG for 4x2_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 / "4x2_handeye_board.png"
    img_h = int(round(800 * BOARD_HEIGHT_4X2 / BOARD_WIDTH_4X2))
    img = link_boards["4x2_board"].generateImage((800, img_h))
    cv2.imwrite(str(out_path), img)
    try:
        from PIL import Image
        pil_img = Image.open(out_path)
        dpi = int(round(pil_img.width / (BOARD_WIDTH_4X2 / 0.0254)))
        pil_img.save(str(out_path), dpi=(dpi, dpi))
    except ImportError:
        pass
    print(f"Wrote {out_path}  ({BOARD_WIDTH_4X2*1000:.1f} x {BOARD_HEIGHT_4X2*1000:.1f} mm)")


class PandaHandEye4x2Config(ExoskeletonConfig):
    """Panda arm: 4×2 rectangular ArUco board on the hand (end-effector)."""

    name = "Panda_HandEye_4x2"
    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/"

    # Hide the link/exo overlay meshes — only the ArUco plane visible
    exo_alpha = 0.0
    exo_link_alpha = 0.0

    aruco_boards = {
        "handeye_board": f"{BOARD_IMG_DIR}/4x2_handeye_board.png",
    }
    aruco_board_objects = {
        "handeye_board": handeye_board,
    }

    links = {
        "hand_board": LinkConfig(
            mujoco_name="hand_0",
            pybullet_name="hand",
            robot_mesh_path="hand.stl",
            exo_mesh_path="hand.stl",
            aruco_offset_pos=np.array([40, 30.0, 90.0]),  # mm — same as 4x4 handeye
            aruco_offset_rot=np.array([0, np.pi/2, 0]),
            aruco_board_name="handeye_board",
            board_length=BOARD_LENGTH_4X2,
            plane_scale=PLANE_SCALE_4X2,
        ),
    }


PANDA_HANDEYE_4X2_CONFIG = PandaHandEye4x2Config()