#!/usr/bin/env python3
"""
Generate YAML params mapping for BehaviorTree action nodes.

This script parses a BehaviorTree XML (e.g., src/brain/config/bt_carry_boxes.xml),
extracts each leaf action node's instance name (the 'name' attribute) and node type
tag (e.g., MoveWaist_H), resolves the corresponding .action definition from a
neighboring hivecore_robot_interfaces package, and writes a YAML configuration file.

Output YAML format (list):
  - name: <bt_node_name>
    params: |  # YAML snippet as a string
      field_a: 0
      field_b: []

By default, interface types are resolved from ../hivecore_robot_interfaces relative to
this repository root, matching other generator scripts.

Usage:
  python3 src/scripts/gen_bt_params.py \
    --xml src/brain/config/bt_carry_boxes.xml \
    --interfaces ../hivecore_robot_interfaces \
    --out src/brain/config/bt_carry_boxes.params.yaml

Notes:
  - Only the Goal part of .action files is considered (fields before the first ---).
  - For unknown/complex ROS types, placeholder structures are emitted (e.g., for
    geometry_msgs/PoseStamped). Otherwise the field is set to null as a placeholder.
"""

from __future__ import annotations

import argparse
import os
import re
import sys
import textwrap
import xml.etree.ElementTree as ET
from typing import Dict, List, Optional, Sequence, Tuple

import yaml


PRIMITIVES_DEFAULTS: Dict[str, object] = {
    # integers
    'int8': 0, 'int16': 0, 'int32': 0, 'int64': 0,
    'uint8': 0, 'uint16': 0, 'uint32': 0, 'uint64': 0,
    'int': 0,
    # floats
    'float32': 0.0, 'float64': 0.0, 'float': 0.0, 'double': 0.0,
    # misc
    'bool': False,
    'string': '',
    'time': 0,
}


def to_snake(s: str) -> str:
    out = ''
    for i, ch in enumerate(s):
        if ch.isupper():
            if i > 0:
                out += '_'
            out += ch.lower()
        else:
            out += ch
    return out


def snake_to_camel(s: str) -> str:
    parts = re.split(r'[_\-]', s)
    return ''.join(p.capitalize() for p in parts if p)


def find_file_case_insensitive(dirname: str, filename: str) -> Optional[str]:
    if not os.path.isdir(dirname):
        return None
    target = filename.lower()
    for e in os.listdir(dirname):
        if e.lower() == target:
            return os.path.join(dirname, e)
    return None


def resolve_interface_subdir(root: str, sub: str) -> str:
    candidates = [
        os.path.join(root, sub),
        os.path.join(root, 'src', sub),
        os.path.join(root, 'interfaces', sub),
    ]
    for c in candidates:
        if os.path.isdir(c):
            return c
    return candidates[0]


def parse_action_file(path: str) -> List[Tuple[str, str]]:
    """Parse a .action file, return list of (ros_type, field_name) only for Goal part."""
    try:
        with open(path, 'r', encoding='utf-8') as f:
            content = f.read()
    except FileNotFoundError:
        return []
    parts = content.split('\n---\n')
    goal_part = parts[0] if parts else ''
    fields: List[Tuple[str, str]] = []
    for line in goal_part.splitlines():
        line = line.strip()
        if not line or line.startswith('#'):
            continue
        m = re.match(r"^([A-Za-z0-9_\[\]/]+)\s+([A-Za-z0-9_]+).*$", line)
        if m:
            ty = m.group(1)
            name = m.group(2)
            fields.append((ty, name))
    return fields


def locate_action_file(interfaces_root: str, base: str) -> Optional[str]:
    action_dir = resolve_interface_subdir(interfaces_root, 'action')
    # 1) Try as-is (CamelCase)
    candidate = os.path.join(action_dir, f'{base}.action')
    if os.path.exists(candidate):
        return candidate
    # 2) Try CamelCase of snake
    camel = snake_to_camel(base)
    if camel != base:
        candidate2 = os.path.join(action_dir, f'{camel}.action')
        if os.path.exists(candidate2):
            return candidate2
    # 3) Try snake_case
    snake = to_snake(base)
    candidate3 = os.path.join(action_dir, f'{snake}.action')
    if os.path.exists(candidate3):
        return candidate3
    # 4) Case-insensitive lookup
    found = find_file_case_insensitive(action_dir, f'{base}.action')
    if found:
        return found
    # Not found
    return None


def make_placeholder_for_type(ros_type: str) -> object:
    """Return a default placeholder Python value for a ROS field type.

    - Primitive -> default numeric/bool/string
    - Primitive[] -> empty list
    - geometry_msgs/PoseStamped -> nested dict with common keys
    - geometry_msgs/TwistStamped -> nested dict with common keys
    - Otherwise -> None
    """
    is_array = ros_type.endswith('[]')
    base = ros_type[:-2] if is_array else ros_type
    # normalize namespace
    base_norm = base.replace('::', '/')
    base_norm = base_norm.replace('/msg/', '/').replace('/Msg/', '/')

    if is_array:
        return []
    if '/' not in base_norm:
        # primitive
        return PRIMITIVES_DEFAULTS.get(base_norm, None)
    if base_norm in ("geometry_msgs/PoseStamped",):
        return {
            'frame_id': '',
            'stamp_sec': 0,
            'stamp_nanosec': 0,
            'position': {'x': 0.0, 'y': 0.0, 'z': 0.0},
            'orientation': {'x': 0.0, 'y': 0.0, 'z': 0.0, 'w': 1.0},
        }
    if base_norm in ("geometry_msgs/TwistStamped",):
        return {
            'frame_id': '',
            'stamp_sec': 0,
            'stamp_nanosec': 0,
            'linear': {'x': 0.0, 'y': 0.0, 'z': 0.0},
            'angular': {'x': 0.0, 'y': 0.0, 'z': 0.0},
        }
    # unknown complex -> None
    return None


def build_yaml_params_from_fields(fields: Sequence[Tuple[str, str]]) -> str:
    """Create a YAML snippet string from .action Goal fields."""
    mapping: Dict[str, object] = {}
    for ros_type, name in fields:
        mapping[name] = make_placeholder_for_type(ros_type)
    # Dump as YAML block string
    # Ensure multi-line by setting default_flow_style=False
    snippet = yaml.safe_dump(mapping, allow_unicode=True, sort_keys=False)
    # Guarantee trailing newline for nicer block scalars
    if not snippet.endswith('\n'):
        snippet += '\n'
    return snippet


def collect_bt_nodes(xml_path: str) -> List[Tuple[str, str]]:
    """Return list of (instance_name, node_tag) in source order for elements with a 'name' attr.

    Heuristic: We primarily care about leaf actions registered as <SkillName>_H. To be inclusive,
    we collect any XML element with a 'name' attribute that is not a container like Sequence,
    Fallback, RetryUntilSuccessful, etc. Containers typically don't have a 'name' in our trees,
    and their tags are known; but we simply include all with 'name' and let type resolution
    decide (missing interface files will yield empty params).
    """
    tree = ET.parse(xml_path)
    root = tree.getroot()
    out: List[Tuple[str, str]] = []
    for elem in root.iter():
        name = elem.attrib.get('name')
        if not name:
            continue
        tag = elem.tag
        out.append((name, tag))
    return out


def main():
    ap = argparse.ArgumentParser(description='Generate YAML params for BT nodes.')
    ap.add_argument('--xml', default='src/brain/config/bt_carry_boxes.xml', help='Path to BehaviorTree XML')
    ap.add_argument('--interfaces', default='../hivecore_robot_interfaces', help='Path to hivecore_robot_interfaces root')
    ap.add_argument('--out', default='src/brain/config/bt_carry_boxes.params.default.yaml', help='Output YAML path')
    args = ap.parse_args()

    # Resolve repo root as two levels up from this script (src/scripts/.. -> repo)
    repo_root = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', '..'))
    xml_path = args.xml if os.path.isabs(args.xml) else os.path.join(repo_root, args.xml)
    interfaces_root = args.interfaces if os.path.isabs(args.interfaces) else os.path.join(repo_root, args.interfaces)
    out_path = args.out if os.path.isabs(args.out) else os.path.join(repo_root, args.out)

    if not os.path.exists(xml_path):
        print(f'ERROR: XML not found: {xml_path}', file=sys.stderr)
        sys.exit(2)

    nodes = collect_bt_nodes(xml_path)
    results = []

    for instance_name, node_tag in nodes:
        # Map tag to interface base by stripping trailing '_H' if present
        base = node_tag[:-2] if node_tag.endswith('_H') else node_tag
        action_file = locate_action_file(interfaces_root, base)
        fields: List[Tuple[str, str]] = parse_action_file(action_file) if action_file else []
        yaml_params = build_yaml_params_from_fields(fields)
        results.append({'name': instance_name, 'params': yaml_params})

    # Write output as YAML list; ensure deterministic order as in XML
    os.makedirs(os.path.dirname(out_path), exist_ok=True)
    with open(out_path, 'w', encoding='utf-8') as f:
        yaml.safe_dump(results, f, allow_unicode=True, sort_keys=False)

    print(f'Wrote {len(results)} entries to {out_path}')


if __name__ == '__main__':
    main()
