#!/usr/bin/env python3
"""
generate_mprim.py
------------------
CLI để sinh file .mprim từ 1 file config trong configs/.

Cách dùng:
    python3 generate_mprim.py configs/differential.py
    python3 generate_mprim.py configs/omnidirectional.py -o my_output.mprim
    python3 generate_mprim.py configs/ackermann.py

Muốn thêm robot mới: copy 1 file trong configs/, đổi số liệu, chạy lại.
KHÔNG cần sửa code trong mprim_core.py / kinematics.py.
"""

import argparse
import importlib.util
import os
import sys

sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))

from mprim_config import KinematicType
from mprim_core import generate_mprim_file
from kinematics import unicycle_poses, holonomic_poses

# differential và ackermann dùng chung interpolator: đường đi của xe bánh lái theo
# cung tròn trùng với nghiệm unicycle (đoạn thẳng + cung); khác nhau chỉ ở tập
# primitive và ràng buộc min_turning_radius (mprim_core xử lý).
KINEMATICS_MAP = {
    KinematicType.DIFFERENTIAL: unicycle_poses,
    KinematicType.OMNIDIRECTIONAL: holonomic_poses,
    KinematicType.ACKERMANN: unicycle_poses,
}


def load_config(config_path: str):
    spec = importlib.util.spec_from_file_location("robot_config_module", config_path)
    module = importlib.util.module_from_spec(spec)
    spec.loader.exec_module(module)
    if not hasattr(module, "config"):
        raise ValueError(f"{config_path} phải định nghĩa biến `config = RobotConfig(...)`")
    return module.config


def main():
    parser = argparse.ArgumentParser(description="Sinh file .mprim từ config robot")
    parser.add_argument("config", help="Đường dẫn tới file config Python (vd: configs/differential.py)")
    parser.add_argument("-o", "--output", default=None, help="Đường dẫn file .mprim output")
    args = parser.parse_args()

    cfg = load_config(args.config)
    kinematics_fn = KINEMATICS_MAP[cfg.kinematic_type]

    output_path = args.output or f"{cfg.name}.mprim"
    n, warnings = generate_mprim_file(cfg, kinematics_fn, output_path)

    print(f"[OK] {cfg.kinematic_type.value}: đã sinh {n} primitives -> {output_path}")
    if warnings:
        print(f"[WARN] {len(warnings)} cảnh báo:")
        for w in warnings:
            print(f"  - {w}")


if __name__ == "__main__":
    main()