#!/usr/bin/env python3
"""
Generate a C++ header `robot_config.hpp` from YAML `robot_config.yaml`.

Requirements:
- The generated header must parse the YAML file at runtime (do not embed YAML values).
- Provide a clear C++ interface to query configuration by name and key.
- Follow Google C++ style (CamelCase types/functions, namespace lower_case, member_ with trailing underscore, include guards).

YAML example:
    - name: brain
        version: 1.0.0
        skill_file: robot_skills.yaml
    - name: cerebrum_node
        version: 1.0.0
        bt_config_file: bt_carry_boxes.xml
        bt_params_file: bt_carry_boxes.params.yaml

The header exposes:
    - brain::robot_config::Entry: holds a name and a map of string keys->string values.
    - brain::robot_config::RobotConfig: loader and query API.
        * bool LoadFromFile(const std::string& path)
        * const Entry* Find(const std::string& name) const
        * std::vector<std::string> Names() const
        * std::optional<std::string> GetValue(const std::string& name, const std::string& key) const
        * Typed helpers for all keys present in YAML (CamelCase)

Usage:
    python3 src/scripts/gen_robot_config.py \
        --in src/brain/config/robot_config.yaml \
        --out src/brain/include/robot_config.hpp
"""
from __future__ import annotations

import argparse
import os
from pathlib import Path
from typing import List

import yaml


def to_camel(s: str) -> str:
    import re
    parts = re.split(r"[^A-Za-z0-9]+", s)
    parts = [p for p in parts if p]
    camel = "".join(p[:1].upper() + p[1:] for p in parts)
    if not camel or not camel[0].isalpha():
        camel = "X" + camel
    return camel


def main() -> None:
    parser = argparse.ArgumentParser(description="Generate robot_config.hpp from YAML")
    parser.add_argument("--in", dest="inp", default="src/brain/config/robot_config.yaml")
    parser.add_argument("--out", dest="out", default="src/brain/include/brain/robot_config.hpp")
    args = parser.parse_args()

    repo_root = Path(__file__).resolve().parents[2]
    in_path = Path(args.inp) if os.path.isabs(args.inp) else repo_root / args.inp
    out_path = Path(args.out) if os.path.isabs(args.out) else repo_root / args.out

    if not in_path.exists():
        raise SystemExit(f"Config YAML not found: {in_path}")

    data = yaml.safe_load(in_path.read_text(encoding="utf-8")) or []
    if not isinstance(data, list):
        raise SystemExit("robot_config.yaml must be a YAML sequence (list)")

    # Collect all keys across items (ensure 'name' exists first)
    key_set = {"name"}
    preferred_order = ["version", "skill_file"]
    for item in data:
        if isinstance(item, dict):
            for k in item.keys():
                key_set.add(k)

    # Build ordered keys: name, preferred if present, then remaining alphabetically
    keys: List[str] = ["name"]
    for k in preferred_order:
        if k in key_set and k not in keys:
            keys.append(k)
    keys.extend(sorted(k for k in key_set if k not in keys))

    out_path.parent.mkdir(parents=True, exist_ok=True)
    with out_path.open("w", encoding="utf-8") as out:
        out.write("// GENERATED FILE - do not edit by hand\n")
        out.write("// Generated by src/scripts/gen_robot_config.py\n")
        out.write("#ifndef BRAIN_ROBOT_CONFIG_HPP_\n")
        out.write("#define BRAIN_ROBOT_CONFIG_HPP_\n\n")
        out.write("#include <yaml-cpp/yaml.h>\n")
        out.write("#include <cstddef>\n")
        out.write("#include <optional>\n")
        out.write("#include <string>\n")
        out.write("#include <string_view>\n")
        out.write("#include <unordered_map>\n")
        out.write("#include <utility>\n")
        out.write("#include <vector>\n")
        out.write("#include <ament_index_cpp/get_package_share_directory.hpp>\n\n")
        out.write("namespace brain { namespace robot_config {\n\n")
        out.write("// Represents a single entry parsed from robot_config.yaml.\n")
        out.write("struct Entry {\n")
        out.write("  std::string name;\n")
        out.write("  std::unordered_map<std::string, std::string> fields;\n")
        out.write("};\n\n")
        # If YAML contains config_params_path, emit a small struct type used by the accessor
        if 'config_params_path' in keys:
            out.write("// Pair type for config + params entries\n")
            out.write("struct BtConfigParam {\n")
            out.write("  std::string config;\n")
            out.write("  std::string param;\n")
            out.write("};\n\n")
        out.write("// Loader and query interface for robot configuration.\n")
        out.write("class RobotConfig {\n")
        out.write(" public:\n")
        out.write("  RobotConfig(std::string path = \"\") {\n")
        out.write("    if (path.empty()) {\n")
        out.write(
            "      LoadFromFile(ament_index_cpp::get_package_share_directory(\"brain\") + std::string(\"/config/robot_config.yaml\"));\n"
        )
        out.write("    } else {\n")
        out.write("      LoadFromFile(path);\n")
        out.write("    }\n")
        out.write("  }\n")
        out.write("  // Parse YAML from a file path. Returns true on success.\n")
        out.write("  bool LoadFromFile(const std::string& path) {\n")
        out.write("    entries_.clear();\n")
        out.write("    YAML::Node root = YAML::LoadFile(path);\n")
        out.write("    if (!root || !root.IsSequence()) { return false; }\n")
        out.write("    for (const auto& node : root) {\n")
        out.write("      if (!node.IsMap()) { continue; }\n")
        out.write("      Entry e;\n")
        out.write("      auto name_it = node[\"name\"];\n")
        out.write("      if (name_it && name_it.IsScalar()) { e.name = name_it.as<std::string>(); }\n")
        out.write("      for (auto it = node.begin(); it != node.end(); ++it) {\n")
        out.write("        const auto key = it->first.as<std::string>();\n")
        out.write("        if (key == \"name\") { continue; }\n")
        out.write("        if (it->second) {\n")
        out.write("          if (it->second.IsScalar()) {\n")
        out.write("            e.fields.emplace(key, it->second.as<std::string>());\n")
        out.write("          } else {\n")
        out.write("            e.fields.emplace(key, YAML::Dump(it->second));\n")
        out.write("          }\n")
        out.write("        }\n")
        out.write("      }\n")
        out.write("      if (!e.name.empty()) { entries_.emplace_back(std::move(e)); }\n")
        out.write("    }\n")
        out.write("    return true;\n")
        out.write("  }\n\n")
        out.write("  // Return pointer to an Entry with matching name or nullptr if not found.\n")
        out.write("  const Entry* Find(const std::string& name) const {\n")
        out.write("    for (const auto& e : entries_) { if (e.name == name) return &e; }\n")
        out.write("    return nullptr;\n")
        out.write("  }\n\n")
        out.write("  // List all entry names.\n")
        out.write("  std::vector<std::string> Names() const {\n")
        out.write("    std::vector<std::string> out; out.reserve(entries_.size());\n")
        out.write("    for (const auto& e : entries_) out.push_back(e.name);\n")
        out.write("    return out;\n")
        out.write("  }\n\n")
        out.write("  // Generic value getter by name and key.\n")
        out.write(
            "  std::optional<std::string> GetValue(const std::string& name, const std::string& key) const {\n"
        )
        out.write("    const Entry* e = Find(name); if (!e) return std::nullopt;\n")
        out.write("    auto it = e->fields.find(key);\n")
        out.write("    if (it == e->fields.end()) return std::nullopt;\n")
        out.write("    return it->second;\n")
        out.write("  }\n\n")

        # Typed getters for all keys except 'name' -- simple string accessors
        for k in keys:
            if k == "name":
                continue
            func = to_camel(k)
            # Special-case config_params_path to return parsed sequence of {config,param}
            if k == 'config_params_path':
                out.write(
                    f"  std::optional<std::vector<BtConfigParam>> {func}(const std::string& name) const " + "{\n"
                )
                out.write(f"    auto v = GetValue(name, \"{k}\");\n")
                out.write("    if (!v) return std::nullopt;\n")
                out.write("    try {\n")
                out.write("      YAML::Node n = YAML::Load(*v);\n")
                out.write("      if (n.IsSequence()) {\n")
                out.write("        std::vector<BtConfigParam> res;\n")
                out.write("        for (const auto & it : n) {\n")
                out.write("          BtConfigParam p;\n")
                out.write("          if (it[\"config\"] && it[\"config\"].IsScalar()) p.config = it[\"config\"].as<std::string>();\n")
                out.write("          if (it[\"param\"] && it[\"param\"].IsScalar()) p.param = it[\"param\"].as<std::string>();\n")
                out.write("          res.push_back(std::move(p));\n")
                out.write("        }\n")
                out.write("        return res;\n")
                out.write("      }\n")
                out.write("    } catch (...) {}\n")
                out.write("    return std::nullopt;\n")
                out.write("  }\n\n")
                continue
            out.write(
                f"  std::optional<std::string> {func}(const std::string& name) const " + "{\n"
            )
            out.write(f"    return GetValue(name, \"{k}\");\n")
            out.write("  }\n\n")

        out.write(" private:\n")
        out.write("  std::vector<Entry> entries_;\n")
        out.write("};\n\n")
        out.write("}} // namespace robot_config; } // namespace brain\n\n")
        out.write("#endif  // BRAIN_ROBOT_CONFIG_HPP_\n")

    print(f"Wrote {out_path}")


if __name__ == "__main__":
    main()

