"""Config differential drive (2 bánh vi sai). Chỉnh số liệu theo robot thực tế.

Cost mult lấy theo bộ tuned unicycle_highcost_5cm.mprim (backward=40, turninplace=20,
forwardandturn=2): lùi và quay tại chỗ đắt tới mức chỉ được chọn khi không còn đường
khác — an toàn cho AMR có cảm biến chính hướng về phía trước. Muốn robot thoải mái
lùi/xoay hơn thì hạ dần (vd 5/5/3) nhưng đó là quyết định vận hành có chủ đích.
"""

from mprim_config import RobotConfig, KinematicType

config = RobotConfig(
    name="differential",                    # tên file output: differential.mprim
    kinematic_type=KinematicType.DIFFERENTIAL,  # 2 bánh vi sai -> nội suy unicycle

    resolution_m=0.05,          # [m] PHẢI khớp costmap resolution (costmap_global_params.yaml)
    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, để tinh chỉnh gần goal
    forward_long_m=0.40,        # [m] bước tiến dài ~8 cell — quãng đường 1 chu kỳ điều khiển
                                #     ở tốc độ hành trình
    backward_m=0.10,            # [m] bước lùi thẳng

    # Rẽ = cung tròn: (1,) nghĩa là chỉ có primitive rẽ 22.5 độ (giống bộ tham chiếu).
    # Thêm 2 vào tuple nếu cần rẽ 45 độ — đổi lại branching factor tăng, planner chậm hơn.
    turn_steps=(1,),
    turn_radius_m=0.70,         # [m] >= max_vel/max_yawrate = 0.4/0.6 ~ 0.67 để local planner
                                #     bám cung không phải giảm tốc; >= 0.66 để quantize 5cm
                                #     không làm méo cung

    has_backward_prims=True,        # cho phép lùi thẳng (cost 40 nên chỉ dùng khi kẹt)
    has_backward_turn_prims=False,  # lùi + rẽ tắt: hành động mù cảm biến, bật lại nếu thật cần
    has_turn_in_place_prims=True,   # cho phép quay tại chỗ (cost 20 nên hiếm khi được chọn)

    # SỐ NGUYÊN (SBPL đọc %d)
    forwardcostmult=1,          # tiến thẳng — mốc chuẩn
    backwardcostmult=40,        # lùi 0.1m "đắt" bằng tiến 4m -> chỉ lùi khi không còn cách
    forwardandturncostmult=2,   # rẽ đắt gấp đôi đi thẳng -> ưu tiên đường ít cua
    backwardandturncostmult=40, # (không dùng khi has_backward_turn_prims=False)
    turninplacecostmult=20,     # xoay 22.5 độ "đắt" bằng tiến ~4m -> tránh xoay giữa đường

    pose_spacing_m=0.05,        # [m] = resolution -> path SBPL trả ra có pose cách đều ~5cm

    # giới hạn động học thật (hybrid_local_planner_params.yaml: robot_max_v_ac / robot_max_w_ac)
    max_vel_mps=0.4,            # [m/s] vận tốc dài tối đa
    max_yawrate_radps=0.6,      # [rad/s] vận tốc xoay tối đa
)