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

Uses the same board definition as panda_exo.py's even_larger_board:
  - 4×4 grid, DICT_6X6_250, IDs 226–241
  - marker_size  ≈ 32.63 mm
  - separation   ≈ 3.26 mm
  - Physical board ≈ 140 × 140 mm (square)

The board is mounted on the "hand" body (Panda flange), facing outward along
the gripper Z axis.  The link/exo meshes are hidden (alpha=0) — only the ArUco
plane is visible.
"""
import os
import numpy as np
from cv2 import aruco

import sys
_this_file = os.path.dirname(os.path.abspath(__file__))
sys.path.insert(0, os.path.join(_this_file, ".."))
from ExoConfigs.exoskeleton import ExoskeletonConfig, LinkConfig

# Re-use the board definition from panda_exo.py
from ExoConfigs.panda_exo import (
    link_boards, BOARD_LENGTH_EVEN_LARGER,
    aruco_dict, PANDA_MODEL_DIR, BOARD_IMG_DIR,
)

PANDA_MODEL_DIR = os.path.join(_this_file, "..", "robot_models", "franka_emika_panda")
SO100_MODEL_DIR = "robot_models/so100_model"

# ── ArUco board (from panda_exo.py) ─────────────────────────────────────
handeye_board = link_boards["even_larger_board"]
BOARD_WIDTH = BOARD_LENGTH_EVEN_LARGER   # square board
BOARD_HEIGHT = BOARD_LENGTH_EVEN_LARGER
BOARD_LENGTH = BOARD_LENGTH_EVEN_LARGER

# Board image already exists in the main board_imgs dir
BOARD_IMG_PATH = f"{BOARD_IMG_DIR}/even_larger_board.png"

# ── MuJoCo plane scale ──────────────────────────────────────────────────
# even_larger preset in exoskeleton.py uses scale 0.475 for this exact board size
_SF = 0.475 / BOARD_LENGTH_EVEN_LARGER
PLANE_SCALE_HANDEYE = (BOARD_WIDTH * _SF, BOARD_HEIGHT * _SF)  # should be (0.475, 0.475)

VIRTUAL_GRIPPER_BODY_NAME = "virtual_gripper_keypoint"


# ── ExoskeletonConfig ────────────────────────────────────────────────────
class PandaHandEyeConfig(ExoskeletonConfig):
    """Panda arm: ArUco calibration board mounted on the hand (end-effector)."""

    name = "Panda_HandEye"
    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 — we only want the ArUco plane visible
    exo_alpha = 0.0
    exo_link_alpha = 0.0

    aruco_boards = {
        "handeye_board": BOARD_IMG_PATH,
    }
    aruco_board_objects = {
        "handeye_board": handeye_board,
    }

    links = {
        "hand_board": LinkConfig(
            mujoco_name="hand_0",
            pybullet_name="hand",           # FK reads from the "hand" body
            robot_mesh_path="hand.stl",      # placeholder (hidden, alpha=0)
            exo_mesh_path="hand.stl",        # placeholder (hidden, alpha=0)
            aruco_offset_pos=np.array([40, 30.0, 90.0]),  # mm — at the flange, above grippers
            aruco_offset_rot=np.array([0, np.pi/2, 0]),
            aruco_board_name="handeye_board",
            board_length=BOARD_LENGTH,
            plane_scale=PLANE_SCALE_HANDEYE,
        ),
    }


PANDA_HANDEYE_CONFIG = PandaHandEyeConfig()