"""Panda arm with 4×4 even_larger ArUco board on base.

Larger exo mount STL (panda_base_redo_grip_align_larger.stl) with straight
rotation and updated ArUco offset. Saved from panda_exo.py before reverting
to the original 3×3 config.
"""
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)
n_aruco_row, n_aruco_col = 6, 6
x = (4.75 * 2) / ((n_aruco_row + (n_aruco_row - 1) / 10) * 100)
board_ids_base = np.arange(136, 172, dtype=np.int32).reshape(-1, 1)
link_boards = {
    "larger_base": aruco.GridBoard(
        size=(n_aruco_row, n_aruco_col),
        markerLength=x,
        markerSeparation=x / 10,
        dictionary=aruco_dict,
        ids=board_ids_base,
    ),
}
BOARD_LENGTH_LARGE = n_aruco_row * x + (n_aruco_row - 1) * (x / 10)

n_coarse_row, n_coarse_col = 3, 3
x_coarse = 2 * x
board_ids_coarse = np.arange(217, 217 + 9, dtype=np.int32).reshape(-1, 1)
link_boards["larger_coarse_board"] = aruco.GridBoard(
    size=(n_coarse_row, n_coarse_col),
    markerLength=x_coarse,
    markerSeparation=x_coarse / 10,
    dictionary=aruco_dict,
    ids=board_ids_coarse,
)
BOARD_LENGTH_COARSE = n_coarse_row * x_coarse + (n_coarse_row - 1) * (x_coarse / 10)

EVEN_LARGER_PLANE_RATIO = 75 / 50
BOARD_LENGTH_EVEN_LARGER = BOARD_LENGTH_COARSE * EVEN_LARGER_PLANE_RATIO
n_even_row, n_even_col = 4, 4
_x_even = BOARD_LENGTH_EVEN_LARGER / (n_even_row + (n_even_row - 1) / 10)
board_ids_even_larger = np.arange(226, 226 + n_even_row * n_even_col, dtype=np.int32).reshape(-1, 1)
link_boards["even_larger_board"] = aruco.GridBoard(
    size=(n_even_row, n_even_col),
    markerLength=_x_even,
    markerSeparation=_x_even / 10,
    dictionary=aruco_dict,
    ids=board_ids_even_larger,
)

VIRTUAL_GRIPPER_BODY_NAME = "virtual_gripper_keypoint"


class PandaBaseOnly4x4Config(ExoskeletonConfig):
    """Panda arm: base has a 4×4 even_larger ArUco board, larger exo mount."""

    name = "Panda_BaseOnly_4x4"
    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 = {
        "larger_base": f"{BOARD_IMG_DIR}/larger_base.png",
        "larger_coarse_board": f"{BOARD_IMG_DIR}/larger_coarse_board.png",
        "even_larger_board": f"{BOARD_IMG_DIR}/even_larger_board.png",
    }
    aruco_board_objects = {
        "larger_base": link_boards["even_larger_board"],
        "larger_coarse_board": link_boards["larger_coarse_board"],
        "even_larger_board": link_boards["even_larger_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="even_larger_board",
            board_length=BOARD_LENGTH_EVEN_LARGER,
        ),
    }


PANDA_BASE_ONLY_4X4_CONFIG = PandaBaseOnly4x4Config()