"""
mprim_config.py
----------------
Cấu hình robot dùng chung cho mọi loại kinematic (differential / omnidirectional / ackermann).

Chỉnh các thông số trong file configs/*.py theo robot thực tế của bạn.
File này KHÔNG chứa logic sinh primitive — chỉ định nghĩa dữ liệu + validate.

Ràng buộc quan trọng của format SBPL (lý do cho các validate bên dưới):
- `additionalactioncostmult` được SBPL đọc bằng fscanf("%d") -> BẮT BUỘC là số nguyên,
  giá trị lẻ (1.5, 2.5...) làm hỏng cả file ngay từ primitive đầu tiên chứa nó.
- Pose cuối của quỹ đạo phải khớp endpose sau khi rời rạc hóa, nếu lệch quá nửa bước góc
  (= 180/numberofangles độ) SBPL từ chối nạp file.
- Khoảng cách giữa 2 intermediate pose liên tiếp không được vượt quá 1 cell, nếu không
  bước quét footprint (get_2d_motion_cells) sẽ bỏ sót cell trên đường đi.
"""

import math
from dataclasses import dataclass, field
from enum import Enum
from typing import Optional, Tuple


class KinematicType(str, Enum):
    DIFFERENTIAL = "differential"        # 2 bánh vi sai (unicycle model)
    OMNIDIRECTIONAL = "omnidirectional"  # mecanum / omni / swerve (holonomic)
    ACKERMANN = "ackermann"              # xe có bánh lái, bán kính quay tối thiểu


@dataclass
class RobotConfig:
    # --- Thông tin chung ---
    name: str                          # tên robot; dùng đặt tên file output <name>.mprim
    kinematic_type: KinematicType      # differential / omnidirectional / ackermann — quyết
                                       # định tập primitive và cách nội suy quỹ đạo

    # --- Lưới rời rạc hóa (phải khớp với costmap / planner config) ---
    resolution_m: float = 0.05         # [m] kích thước 1 cell — PHẢI khớp costmap resolution,
                                       # SBPL so sánh và từ chối file nếu lệch
    numberofangles: int = 16           # số góc heading rời rạc; bội số của 8, khuyến nghị 16
                                       # (16 góc -> 1 bước góc = 22.5 độ)

    # --- Primitive đi thẳng ---
    forward_short_m: float = 0.10      # [m] bước tiến ngắn, dùng để tinh chỉnh vị trí gần
                                       # goal; >= 0.75*resolution để không quantize về (0,0)
    forward_long_m: float = 0.40       # [m] bước tiến dài — quãng đường ~1 chu kỳ điều khiển
                                       # ở tốc độ hành trình; quyết định tốc độ expand của A*
    backward_m: float = 0.10           # [m] bước lùi thẳng (chỉ dùng khi has_backward_prims)

    # --- Primitive vừa đi vừa rẽ ---
    # Rẽ được mô tả bằng CUNG TRÒN: turn_steps liệt kê số bước góc rời rạc mỗi primitive rẽ
    # (1 = 22.5 độ với 16 góc), turn_radius_m là bán kính cung [m]. Điểm cuối được tự tính:
    #   dx = R*sin(dtheta), dy = R*(1 - cos(dtheta))
    # -> heading cuối LUÔN khớp góc rời rạc đích, không phải đoán độ lệch ngang bằng tay.
    turn_steps: Tuple[int, ...] = (1,) # các mức rẽ; (1,) = chỉ rẽ 22.5 độ, (1, 2) thêm rẽ 45
                                       # độ — mỗi mức thêm 2 primitive/góc (CCW + CW),
                                       # branching factor tăng -> planner chậm hơn
    turn_radius_m: float = 0.70        # [m] bán kính cung rẽ. Nên >= max_vel/max_yawrate để
                                       # local planner bám được không phải giảm tốc; và >=
                                       # resolution/(1-cos(bước góc)) (~0.66m với lưới 5cm/16
                                       # góc) để quantize không làm méo cung. Ackermann: bắt
                                       # buộc >= min_turning_radius_m.

    # --- Chỉ dùng cho OMNIDIRECTIONAL ---
    sidestep_m: float = 0.10           # [m] bước đi ngang (trái/phải) giữ nguyên heading
    diagonal_m: float = 0.10           # [m] chiều dài bước đi chéo 45 độ giữ nguyên heading

    # --- Chỉ dùng cho ACKERMANN ---
    min_turning_radius_m: float = 0.35  # [m] bán kính quay tối thiểu VẬT LÝ (đo từ robot
                                        # thật, không trượt bánh) — generator cảnh báo nếu
                                        # cung sau quantize vi phạm giá trị này

    # --- Bật/tắt từng họ primitive ---
    has_backward_prims: bool = True         # cho phép lùi thẳng (tắt cho robot kéo hàng
                                            # không được lùi kiểu noreverse_trolley)
    has_backward_turn_prims: bool = False   # cho phép lùi + rẽ; mặc định tắt vì AMR cảm biến
                                            # chính hướng trước thì lùi-rẽ là hành động mù
    has_turn_in_place_prims: bool = True    # quay tại chỗ (ackermann thật thường phải tắt)

    # --- Hệ số chi phí, theo convention SBPL: cost thật = cost_thời_gian * costmult ---
    # BẮT BUỘC số nguyên >= 1 (SBPL đọc bằng %d). Tham khảo bộ tuned unicycle_highcost:
    # backward=40, turninplace=20, forwardandturn=2 — lùi/xoay tại chỗ đắt tới mức chỉ được
    # chọn khi không còn đường nào khác.
    forwardcostmult: int = 1           # tiến thẳng — mốc chuẩn, luôn để 1
    backwardcostmult: int = 40         # lùi thẳng; 40 nghĩa là lùi 0.1m "đắt" bằng tiến 4m
    forwardandturncostmult: int = 2    # vừa tiến vừa rẽ; >1 để planner ưu tiên đường thẳng
    backwardandturncostmult: int = 40  # lùi + rẽ (chỉ dùng khi has_backward_turn_prims)
    turninplacecostmult: int = 20      # quay tại chỗ; giảm xuống nếu muốn robot xoay thay vì
                                       # đi vòng, nhưng path sẽ xuất hiện cụm pose trùng điểm
    sidestepcostmult: int = 10         # đi ngang (chỉ omni)
    diagonalcostmult: int = 2          # đi chéo 45 độ (chỉ omni)

    # --- Mật độ intermediate poses (quyết định độ đều của path SBPL trả ra) ---
    # Số sample được tính THEO CHIỀU DÀI: n = ceil(L / pose_spacing_m) + 1, nên mọi primitive
    # dài ngắn khác nhau đều có khoảng cách pose ~ pose_spacing_m -> path ra đều nhau.
    pose_spacing_m: float = 0.05        # [m] khoảng cách giữa 2 pose liên tiếp; phải <=
                                        # resolution_m để footprint sweep không bỏ sót cell
    pose_angular_spacing_deg: float = 5.625  # [deg] bước góc tối đa giữa 2 pose (quay tại chỗ
                                             # và phần cung tròn; footprint sweep cần đủ mịn)

    # --- Giới hạn động học để sanity-check (tùy chọn, None = bỏ qua) ---
    max_vel_mps: Optional[float] = None        # [m/s] vận tốc dài tối đa của robot thật
    max_yawrate_radps: Optional[float] = None  # [rad/s] vận tốc xoay tối đa; cùng max_vel
                                               # dùng để cảnh báo turn_radius quá gắt

    def validate(self):
        """Raise ValueError nếu config sai; trả về list cảnh báo (không chặn generate)."""
        errors = []
        warnings = []

        if self.numberofangles % 8 != 0:
            errors.append("numberofangles phải là bội số của 8 (khuyến nghị 16 hoặc 32).")
        if self.resolution_m <= 0:
            errors.append("resolution_m phải > 0.")

        # costmult: SBPL đọc bằng fscanf("%d") — số lẻ làm hỏng cả file
        for f in ("forwardcostmult", "backwardcostmult", "forwardandturncostmult",
                  "backwardandturncostmult", "turninplacecostmult",
                  "sidestepcostmult", "diagonalcostmult"):
            v = getattr(self, f)
            if not isinstance(v, int) or isinstance(v, bool) or v < 1:
                errors.append(f"{f} = {v!r}: costmult phải là SỐ NGUYÊN >= 1 "
                              "(SBPL đọc bằng fscanf %d, giá trị lẻ làm hỏng file).")

        # độ dài primitive thẳng: quá ngắn sẽ quantize về (0,0) ở góc 45 độ -> self-loop
        min_len = self.resolution_m * 0.75
        for f in ("forward_short_m", "forward_long_m"):
            if getattr(self, f) < min_len:
                errors.append(f"{f} = {getattr(self, f)}: phải >= 0.75*resolution_m "
                              f"({min_len:.3f}m), nếu không endpose quantize về (0,0) ở góc chéo.")
            elif getattr(self, f) < self.resolution_m:
                warnings.append(f"{f} < resolution_m: primitive chỉ đi được ~1 cell, "
                                "cân nhắc tăng lên.")
        if self.has_backward_prims and self.backward_m < min_len:
            errors.append(f"backward_m = {self.backward_m}: phải >= {min_len:.3f}m "
                          "(hoặc tắt has_backward_prims).")

        # turn prims
        if not self.turn_steps:
            errors.append("turn_steps rỗng: cần ít nhất 1 bước góc (vd (1,)).")
        for s in self.turn_steps:
            if not isinstance(s, int) or s < 1 or s > self.numberofangles // 4:
                errors.append(f"turn_steps chứa {s!r}: phải là int trong "
                              f"[1, {self.numberofangles // 4}] (tối đa 90 độ mỗi primitive).")
        if len(set(self.turn_steps)) != len(self.turn_steps):
            errors.append("turn_steps có phần tử trùng nhau.")
        if self.turn_radius_m <= 0:
            errors.append("turn_radius_m phải > 0.")
        else:
            step_rad = 2 * math.pi / self.numberofangles
            min_step = min(self.turn_steps or [1])
            dy_min = self.turn_radius_m * (1 - math.cos(min_step * step_rad))
            if dy_min < self.resolution_m:
                # dy < 1 cell nghĩa là sai số quantize cùng cỡ với chính hình dạng cung ->
                # cung bị méo nặng, bán kính thực tế sau quantize có thể nhỏ hơn nhiều
                r_need = self.resolution_m / (1 - math.cos(min_step * step_rad))
                warnings.append(
                    f"turn_radius_m = {self.turn_radius_m}: độ lệch ngang của cung rẽ nhỏ nhất "
                    f"({dy_min:.3f}m) < 1 cell -> quantize làm méo cung đáng kể. "
                    f"Khuyến nghị turn_radius_m >= {r_need:.2f}m với lưới hiện tại.")

        # sampling
        if self.pose_spacing_m <= 0:
            errors.append("pose_spacing_m phải > 0.")
        elif self.pose_spacing_m > self.resolution_m:
            errors.append(f"pose_spacing_m = {self.pose_spacing_m} > resolution_m: "
                          "footprint sweep của SBPL sẽ bỏ sót cell giữa 2 pose liên tiếp.")
        if self.pose_angular_spacing_deg <= 0:
            errors.append("pose_angular_spacing_deg phải > 0.")

        # giới hạn động học (tùy chọn)
        if self.max_vel_mps is not None and self.max_yawrate_radps is not None:
            r_min_kin = self.max_vel_mps / self.max_yawrate_radps
            if self.turn_radius_m < r_min_kin:
                warnings.append(
                    f"turn_radius_m = {self.turn_radius_m} < max_vel/max_yawrate = "
                    f"{r_min_kin:.3f}m: local planner phải giảm tốc mới bám được cung rẽ này.")

        # ackermann
        if self.kinematic_type == KinematicType.ACKERMANN:
            if self.min_turning_radius_m <= 0:
                errors.append("Ackermann cần min_turning_radius_m > 0.")
            elif self.turn_radius_m < self.min_turning_radius_m:
                errors.append(f"turn_radius_m = {self.turn_radius_m} < min_turning_radius_m = "
                              f"{self.min_turning_radius_m}: primitive vi phạm giới hạn lái vật lý.")
            if self.has_turn_in_place_prims:
                warnings.append("Ackermann bật has_turn_in_place_prims: xe bánh lái thường "
                                "KHÔNG quay tại chỗ được — chỉ giữ nếu robot có bánh xoay đặc biệt.")

        # omni
        if self.kinematic_type == KinematicType.OMNIDIRECTIONAL:
            for f in ("sidestep_m", "diagonal_m"):
                if getattr(self, f) < min_len:
                    errors.append(f"{f} = {getattr(self, f)}: phải >= {min_len:.3f}m.")

        if errors:
            raise ValueError("Config không hợp lệ:\n  - " + "\n  - ".join(errors))
        return warnings