"""Go1 PRO 常量集中定义.

来源: tools/onboard/* 实测 + Unitree URDF + 官方电机规格.
"""

# ===== 网络 =====
MCU_IP = '192.168.123.10'
MCU_PORT = 8007
"""MCU UDP 端口 (从这里发/收 LowCmd/LowState)"""


# ===== 关节命名/索引 =====

JOINT_NAMES = [
    'FR_0', 'FR_1', 'FR_2',   # 0..2  右前 (髋/大腿/小腿)
    'FL_0', 'FL_1', 'FL_2',   # 3..5  左前
    'RR_0', 'RR_1', 'RR_2',   # 6..8  右后
    'RL_0', 'RL_1', 'RL_2',   # 9..11 左后
]

JOINT_TYPE = {i: ('hip' if i % 3 == 0 else 'thigh' if i % 3 == 1 else 'knee')
              for i in range(12)}

# motorCmdArray 用命名属性 (FR_0, FR_1, ...) 而不是 list, 这个表用于 getattr 索引
JOINT_ATTR = JOINT_NAMES  # 命名一致, 可直接 getattr


# ===== 关节限位 (rad) — 2026-06-20 实测校准 =====

JOINT_LIMITS = {
    # 推荐安全范围 (实测极限 + 5° 余量)
    'hip':   (-0.78, 0.78),    # _0 (实测 ~±0.85~0.89)
    'thigh': (-0.60, 3.50),    # _1 (实测 -0.69 ~ +3.95, 上限保守取 3.5)
    'knee':  (-2.70, -0.95),   # _2 (实测 -2.79 ~ -0.88, 注意是负值)
}

JOINT_LIMITS_URDF = {
    # Unitree 官方 URDF 标称值, 实际可能偏保守
    'hip':   (-0.863, 0.863),
    'thigh': (-0.686, 4.501),
    'knee':  (-2.818, -0.888),
}

JOINT_LIMITS_MEASURED = {
    # 前两条腿 (FR/FL) 完整转动 90s 实测 (2026-06-20)
    'FR_0': (-0.8627, +0.8515),
    'FR_1': (-0.6930, +3.9173),
    'FR_2': (-2.7648, -0.8847),
    'FL_0': (-0.8922, +0.8229),
    'FL_1': (-0.6676, +3.9508),
    'FL_2': (-2.7850, -0.9001),
    # 后腿 (RR/RL) 实测时未充分转动, 参考 FR/FL 对称
}


# ===== 电机最大力矩 (N·m) — Unitree 官方规格 =====

TAU_MAX = {
    'hip':   23.7,
    'thigh': 23.7,
    'knee':  35.55,
}

KT = 0.066  # 力矩常数 N·m/A, 用于 tau ↔ 电流 换算 (Go1 GO-M8010-6)

CRITICAL_FACTOR = 5.0  # 命令 tau > CRITICAL_FACTOR × TAU_MAX = 离谱, 触发紧急停机


# ===== 趴地 damping 姿态参考 (2026-06-20 实测, 4 腿对称) =====

DAMPING_POSE = {
    'FR_0': -0.37, 'FR_1': +1.18, 'FR_2': -2.77,
    'FL_0': +0.43, 'FL_1': +1.14, 'FL_2': -2.75,
    'RR_0': -0.37, 'RR_1': +1.18, 'RR_2': -2.76,
    'RL_0': +0.41, 'RL_1': +1.15, 'RL_2': -2.76,
}

# 半起立姿态 (example_position 用的 sin 中点, 不是真趴地)
HALF_STAND_POSE = {
    '_0': 0.0,    # hip 中位
    '_1': 1.2,    # thigh 前蹬
    '_2': -2.0,   # knee 半弯曲 (比 damping 的 -2.77 伸开 44°)
}


# ===== 控制相关 =====

DT_DEFAULT = 0.002      # 控制周期 (s), Mac 上可达 480Hz
SEND_DECIMATION = 1     # 发包降频比例 (1 = 全速发, 5 = 100Hz)
RCVBUF_SIZE = 4096      # socket RCVBUF, 小=自动丢弃积压