#!/usr/bin/env python3

import math
import os
import sys
import yaml
from typing import Dict

from ament_index_python.packages import get_package_share_directory

def print_error(message: str) -> None:
    """Print an error message in red color."""
    print("\033[91m{}\033[0m".format(message), file=sys.stderr)

def construct_angle_radians(loader, node) -> float:
    """Utility function to construct radian values from yaml."""
    value = loader.construct_scalar(node)
    try:
        return float(value)
    except SyntaxError:
        raise Exception("Invalid expression: %s" % value)

def construct_angle_degrees(loader, node) -> float:
    """Utility function for converting degrees into radians from yaml."""
    return math.radians(construct_angle_radians(loader, node))

def load_yaml(package_name: str, file_path: str) -> Dict:
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
        yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
    except Exception:
        print_error("Error: YAML support not available; install python-yaml")
        sys.exit(1)

    try:
        with open(absolute_file_path) as file:
            return yaml.safe_load(file)
    except OSError:  # parent of IOError, OSError *and* WindowsError where available
        print_error("Error: Unable to open the file {}".format(absolute_file_path))
        sys.exit(1)

def load_yaml_abs(absolute_file_path: str) -> Dict:
    try:
        yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
        yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
    except Exception:
        print_error("Error: YAML support not available; install python-yaml")
        sys.exit(1)

    try:
        with open(absolute_file_path) as file:
            return yaml.safe_load(file)
    except OSError:  # parent of IOError, OSError *and* WindowsError where available
        print_error("Error: Unable to open the file {}".format(absolute_file_path))
        sys.exit(1)