#!/usr/bin/env python
"""
Load SVG annotations marked on a PGM map. The knowledgebase will name the map to match the YAML file's name, and
any existing map by the name is deleted in the process.
"""
from __future__ import print_function

import knowledge_representation
from knowledge_representation.map_loader import load_map_from_yaml, populate_with_map_annotations
import argparse
import warnings

import sys


def eprint(*args, **kwargs):
    print(*args, file=sys.stderr, **kwargs)


def load_map(ltmc, file_path, allow_errors=False):
    with warnings.catch_warnings(record=True) as w:
        map_metadata, annotations = load_map_from_yaml(file_path)
        if not allow_errors and len(w) != 0:
            for warning in w:
                eprint(warning.message)
            eprint("Map loading aborted due to warnings")
            exit(1)
        stats = populate_with_map_annotations(ltmc, map_metadata["name"], *annotations)
        if not allow_errors and len(w) != 0:
            for warning in w:
                eprint(warning.message)
            eprint("Map loading aborted due to warnings")
            exit(1)
        s_points, s_poses, s_regions, s_doors = stats
        print("Loaded map '{}' with {} points, {} poses, {} regions, {} doors"
              .format(map_metadata["name"], s_points, s_poses, s_regions, s_doors))


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("map_yaml_paths", type=str, nargs="+", help="Paths to ROS-style map YAML files")
    parser.add_argument("--allow-errors", type=bool, default=False, help="Allow warnings and non-fatal errors in "
                                                                         "loading to be ignored")
    # roslaunch passes additional arguments to <node> executables, so we'll gracefully ignore those
    args, unknown = parser.parse_known_args()
    ltmc = knowledge_representation.get_default_ltmc()
    for path in args.map_yaml_paths:
        load_map(ltmc, path, args.allow_errors)


if __name__ == "__main__":
    main()
