"""Config omnidirectional (mecanum/omni/swerve). Chỉnh số liệu theo robot thực tế."""

from mprim_config import RobotConfig, KinematicType

config = RobotConfig(
    name="omnidirectional",                 # tên file output: omnidirectional.mprim
    kinematic_type=KinematicType.OMNIDIRECTIONAL,  # holonomic -> nội suy tuyến tính x/y/theta

    resolution_m=0.05,          # [m] PHẢI khớp costmap resolution
    numberofangles=16,          # 16 góc heading -> 1 bước góc = 22.5 độ

    forward_short_m=0.10,       # [m] bước tiến ngắn ~2 cell
    forward_long_m=0.40,        # [m] bước tiến dài ~8 cell
    backward_m=0.10,            # [m] bước lùi thẳng

    turn_steps=(1,),            # chỉ có primitive rẽ 22.5 độ; thêm 2 nếu cần rẽ 45 độ
    turn_radius_m=0.70,         # [m] bán kính cung rẽ (omni vẫn dùng cung cho mượt)

    sidestep_m=0.15,            # [m] đặc thù omni: đi ngang giữ nguyên heading
    diagonal_m=0.15,            # [m] đặc thù omni: đi chéo 45 độ giữ nguyên heading

    has_backward_prims=True,        # cho phép lùi thẳng
    has_backward_turn_prims=True,   # omni lùi không mù như diff-drive, giữ bật
    has_turn_in_place_prims=True,   # cho phép quay tại chỗ

    # SỐ NGUYÊN (SBPL đọc %d). Omni lùi/xoay "rẻ" hơn diff-drive vì không cần quay đầu.
    forwardcostmult=1,          # tiến thẳng — mốc chuẩn
    backwardcostmult=3,         # lùi rẻ hơn diff-drive (robot đối xứng, không mù hẳn)
    forwardandturncostmult=3,   # vừa đi vừa rẽ
    backwardandturncostmult=4,  # lùi + rẽ
    turninplacecostmult=4,      # quay tại chỗ
    sidestepcostmult=2,         # đi ngang — hơi đắt hơn tiến để không "cua" bằng sidestep
    diagonalcostmult=2,         # đi chéo 45 độ

    pose_spacing_m=0.05,        # [m] khoảng cách pose trên path

    # chưa khai báo max_vel/max_yawrate: bỏ qua sanity-check động học
)