code: package savi_ros_java.savi_ros_bdi.navigation;
  
  import jason.asSemantics.DefaultInternalAction;
  import jason.asSemantics.TransitionSystem;
  import jason.asSemantics.Unifier;
  import jason.asSyntax.NumberTerm;
  import jason.asSyntax.NumberTermImpl;
  import jason.asSyntax.Term;
  
  
  public class bearing extends DefaultInternalAction {
  
  
      private static final long serialVersionUID = 1L;
  
      public static final int lat1Index = 0;
      public static final int lon1Index = 1;
      public static final int lat2Index = 2;
      public static final int lon2Index = 3;
      public static final int bearingResultIndex = 4;
  
      @Override
  
      public Object execute(TransitionSystem ts, Unifier un, Term[] args) throws Exception {
  
          // execute the internal action
  
          ts.getAg().getLogger().info("executing internal action 'navigation.bearing'");
  
          try {
              // Get the parameters
              NumberTerm lat1Term = (NumberTerm) args[lat1Index];
              NumberTerm lon1Term = (NumberTerm) args[lon1Index];
              NumberTerm lat2Term = (NumberTerm) args[lat2Index];
              NumberTerm lon2Term = (NumberTerm) args[lon2Index];
  
              // Lat and Lon as radians
              double lat1 = lat1Term.solve();
              double lon1 = lon1Term.solve();
              double lat2 = lat2Term.solve();
              double lon2 = lon2Term.solve();
  
              // Calculate results
              //double range = calculateRange(lat1, lon1, lat2, lon2, "meters");
              double bearingResult = bearing.calculateBearing(lat1, lon1, lat2, lon2);
  
              // Create result terms
              //NumberTerm rangeTerm = new NumberTermImpl(range);
              NumberTerm bearingTerm = new NumberTermImpl(bearingResult);
  
              // Unify and return
              boolean successBearing = un.unifies(bearingTerm, args[bearingResultIndex]);
              return successBearing;
  
          } catch (Exception e) {
              ts.getAg().getLogger().info(e.toString());
              throw new Exception("Error in 'navigation.bearing'.");
          }
      }
  
      public static double calculateBearing(double lat1, double lon1, double lat2, double lon2) {
          // https://stackoverflow.com/questions/9457988/bearing-from-one-coordinate-to-another
  
          if (range.calculateRange(lat1, lon1, lat2, lon2, "meters") < 1) {
              return 0.0;
          }
  
          double longitude1 = lon1;
          double longitude2 = lon2;
          double latitude1 = Math.toRadians(lat1);
          double latitude2 = Math.toRadians(lat2);
          double longDiff = Math.toRadians(longitude2 - longitude1);
          double y = Math.sin(longDiff) * Math.cos(latitude2);
          double x = Math.cos(latitude1) * Math.sin(latitude2) - Math.sin(latitude1) * Math.cos(latitude2) * Math.cos(longDiff);
  
          double bearing = (Math.toDegrees(Math.atan2(y, x)) + 360) % 360;
  
          while (bearing > 180) {
              bearing = bearing - 360.0;
          }
          while (bearing < -180) {
              bearing = bearing + 360;
          }
          return bearing;
      }
  
  }
  