code: package savi_ros_java.savi_ros_bdi.navigation.a_star;
  
  import java.io.File;
  import java.io.FileNotFoundException;
  import java.util.ArrayList;
  import java.util.Hashtable;
  import java.util.List;
  import java.util.Scanner;
  
  public class EnvironmentMap {
  
      private Hashtable<String, List<String>> locationGraph;
      private Hashtable<String, Location> locations;
      private static EnvironmentMap instance;
  
      public static EnvironmentMap getInstance(String path, boolean latLon) {
          if (instance == null) {
              instance = new EnvironmentMap(path, latLon);
          }
          return instance;
      }
  
      private EnvironmentMap(String path, boolean latLon) {
          this.locationGraph = new Hashtable<>();
          this.locations = new Hashtable<>();
          this.loadMapFile(path, latLon);
      }
  
      private void loadMapFile(String path, boolean latLon){
          try {
              File myObj = new File(path);
              Scanner myReader = new Scanner(myObj);
              while (myReader.hasNextLine()) {
                  String data = myReader.nextLine();
                  if (!this.isSkipLine(data)) {
                      if (data.contains("locationName")) {
                          this.parseLocation(data, latLon);
                      } else if (data.contains("possible")) {
                          this.parsePossible(data);
                      }
                  }
              }
              myReader.close();
          } catch (FileNotFoundException e) {
              System.out.println("An error occurred.");
              e.printStackTrace();
          }
      }
  
      /**
       * Check if the String line is a comment line or an empty line
       * @param line  The line to be checked
       * @return True if the line should be skipped (comment, blank line, no bracket), False otherwise.
       */
      private Boolean isSkipLine(String line) {
          if (line.contains("*")) {
              return true;
          } else if (line.contains("/")) {
              return true;
          } else if (!line.contains("(")) {
              return true;
          } else if (!line.contains(")")) {
              return true;
          } else {
              return false;
          }
      }
  
      private void parseLocation(String line, boolean latLon) {
          // Example location name string: locationName(b,[1,3]).
          int start;
          int end;
          String name;
  
          // For lat lon locations, x is latitude, y is longitude
          double x;
          double y;
  
          // Characters between the first '(' and the first ',' is the location name
          start = line.indexOf('(') + 1;
          end = line.indexOf(',');
          if (start == end) {
              name = Character.toString(line.charAt(start));
          } else {
              name = line.substring(start, end);
          }
  
          // Characters after '[' and before last ',' is the x location
          start = line.indexOf('[') + 1;
          end = line.lastIndexOf(',');
          if (start == end) {
              x = Double.parseDouble(Character.toString(line.charAt(start)));
          } else {
              x = Double.parseDouble(line.substring(start, end));
          }
  
          // Characters after last ',' and before ']' is the y location
          start = line.lastIndexOf(",") + 1;
          end = line.indexOf(']');
          if (start == end) {
              y = Double.parseDouble(Character.toString(line.charAt(start)));
          } else {
              y = Double.parseDouble(line.substring(start, end));
          }
  
          // Add the location to the location hashmap
          Location currentLocation;
          if (latLon) {
              currentLocation = new EarthLocation(x, y);
          } else {
              currentLocation = new GridLocation(x, y);
          }
          this.locations.put(name,currentLocation);
      }
  
      private void parsePossible(String line) {
          // Example string: Possible: possible(o,n).
          int start;
          int end;
          String a;
          String b;
  
          // Characters between the first '(' and the first ',' is the first location name
          start = line.indexOf('(') + 1;
          end = line.indexOf(',');
          if (start == end) {
              a = Character.toString(line.charAt(start));
          } else {
              a = line.substring(start, end);
          }
  
          // Characters between the first ',' and the first ')' is the second location name
          start = line.indexOf(',') + 1;
          end = line.indexOf(')');
          if (start == end) {
              b = Character.toString(line.charAt(start));
          } else {
              b = line.substring(start, end);
          }
  
          // Add this to the graph hashmap
          if (this.locationGraph.containsKey(a)) {
              // Append to the list
              List<String> nodes = this.locationGraph.get(a);
              nodes.add(b);
          } else {
              // make a new entry
              List<String> nodes = new ArrayList<String>();
              nodes.add(b);
              this.locationGraph.put(a,nodes);
          }
      }
  
      public List<String> getPossibleDestinations(String location) {
          return this.locationGraph.get(location);
      }
  
      public double getCost(String current, String next) {
          Location nextLocation = this.locations.get(next);
          Location currentLocation = this.locations.get(current);
          return currentLocation.range(nextLocation);
      }
  
      public double getHeuristic(String current, String goal) {
          return this.getCost(current,goal);
      }
  
      /**
       * Set an obstacle. Removes 'blocked' from list of places that are
       * accessible from 'current'
       */
      public void setObstacle(String current, String blocked) {
          List<String> possible = getPossibleDestinations(current);
  
          // Need to remove it from the list. It's a shallow copy, so just go with it.
          possible.remove(blocked);
      }
  }
  