#!/usr/bin/env python3
"""
Issue #506: Profile Loader for SaltyBot Launch Profiles

Loads and merges launch parameter profiles (indoor, outdoor, demo) into
the full_stack.launch.py configuration.

Usage:
  from saltybot_bringup.profile_loader import ProfileLoader
  loader = ProfileLoader()
  profile_params = loader.load_profile('indoor')
  # Override parameters based on profile
"""

import os
import yaml
from typing import Dict, Any, Optional
from ament_index_python.packages import get_package_share_directory


class ProfileLoader:
    """Load and parse launch parameter profiles."""

    VALID_PROFILES = ["indoor", "outdoor", "demo"]

    def __init__(self):
        """Initialize profile loader with package paths."""
        self.pkg_dir = get_package_share_directory("saltybot_bringup")
        self.profiles_dir = os.path.join(self.pkg_dir, "config", "profiles")

    def load_profile(self, profile_name: str) -> Dict[str, Any]:
        """
        Load a profile by name.

        Args:
            profile_name: Profile name (indoor, outdoor, demo)

        Returns:
            Dictionary of profile parameters

        Raises:
            ValueError: If profile doesn't exist
            yaml.YAMLError: If profile YAML is invalid
        """
        if profile_name not in self.VALID_PROFILES:
            raise ValueError(
                f"Invalid profile '{profile_name}'. "
                f"Valid profiles: {', '.join(self.VALID_PROFILES)}"
            )

        profile_path = os.path.join(self.profiles_dir, f"{profile_name}.yaml")

        if not os.path.exists(profile_path):
            raise FileNotFoundError(f"Profile file not found: {profile_path}")

        with open(profile_path, "r") as f:
            profile = yaml.safe_load(f)

        if not profile:
            raise ValueError(f"Profile file is empty: {profile_path}")

        return profile

    def get_nav2_overrides(self, profile: Dict[str, Any]) -> Dict[str, Any]:
        """
        Extract Nav2-specific parameters from profile.

        Args:
            profile: Loaded profile dictionary

        Returns:
            Dictionary with nav2_params_file overrides
        """
        overrides = {}

        # Velocity limits
        if "max_linear_vel" in profile:
            overrides["max_vel_x"] = profile["max_linear_vel"]
            overrides["max_speed_xy"] = profile["max_linear_vel"]

        if "max_angular_vel" in profile:
            overrides["max_vel_theta"] = profile["max_angular_vel"]

        # Costmap parameters
        if "costmap" in profile and "inflation_radius" in profile["costmap"]:
            overrides["inflation_radius"] = profile["costmap"]["inflation_radius"]

        return overrides

    def get_launch_args(self, profile: Dict[str, Any]) -> Dict[str, str]:
        """
        Extract launch arguments from profile.

        Args:
            profile: Loaded profile dictionary

        Returns:
            Dictionary of launch argument name → value
        """
        args = {}

        # Core parameters
        args["max_linear_vel"] = str(profile.get("max_linear_vel", 0.2))
        args["follow_distance"] = str(
            profile.get("follower", {}).get("follow_distance", 1.5)
        )

        # Mode (indoor/outdoor affects SLAM vs GPS)
        if profile.get("slam", {}).get("enabled"):
            args["mode"] = "indoor"
        else:
            args["mode"] = "outdoor"

        # Feature toggles
        args["enable_perception"] = "true"

        if profile.get("tricks", {}).get("enabled"):
            args["enable_follower"] = "true"
        else:
            args["enable_follower"] = "false"

        return args

    def validate_profile(self, profile: Dict[str, Any]) -> bool:
        """
        Validate profile structure.

        Args:
            profile: Profile dictionary to validate

        Returns:
            True if valid, raises ValueError otherwise
        """
        required_keys = ["profile", "description"]

        for key in required_keys:
            if key not in profile:
                raise ValueError(f"Profile missing required key: {key}")

        return True

    def merge_profiles(
        self,
        base_profile: Dict[str, Any],
        override_profile: Dict[str, Any],
    ) -> Dict[str, Any]:
        """
        Deep merge profile dictionaries (override_profile takes precedence).

        Args:
            base_profile: Base profile (e.g., indoor)
            override_profile: Profile to merge in (higher priority)

        Returns:
            Merged profile dictionary
        """
        merged = base_profile.copy()

        for key, value in override_profile.items():
            if key in merged and isinstance(merged[key], dict) and isinstance(
                value, dict
            ):
                merged[key] = self.merge_profiles(merged[key], value)
            else:
                merged[key] = value

        return merged
