From 81797885ce38edf58d786b3e54555aca3070a08e Mon Sep 17 00:00:00 2001
From: daveluff <daveluff>
Date: Mon, 28 Nov 2005 22:42:23 +0000
Subject: [PATCH] Add a lower-bound type navaid lookup, and the ability to
 specify navaid type in the find nearest lookup, for the GPS code

---
 src/Navaids/navlist.cxx   | 120 +++++++++++++++++++++++++++++++-------
 src/Navaids/navlist.hxx   |  12 +++-
 src/Navaids/navrecord.hxx |  30 ++++++++--
 3 files changed, 135 insertions(+), 27 deletions(-)

diff --git a/src/Navaids/navlist.cxx b/src/Navaids/navlist.cxx
index 4b058e725..4e1bea4c8 100644
--- a/src/Navaids/navlist.cxx
+++ b/src/Navaids/navlist.cxx
@@ -32,6 +32,18 @@
 #include "navlist.hxx"
 
 
+// Return true if the nav record matches the type
+static bool isTypeMatch(const FGNavRecord* n, fg_nav_types type) {
+    switch(type) {
+    case FG_NAV_ANY: return(true);
+    case FG_NAV_VOR: return(n->get_type() == 3);
+    case FG_NAV_NDB: return(n->get_type() == 2);
+    case FG_NAV_ILS: return(n->get_type() == 4);	// Note - very simplified, only matches loc as part of full ILS.
+    default: return false;
+    }
+}
+
+
 // Constructor
 FGNavList::FGNavList( void ) {
 }
@@ -163,6 +175,72 @@ FGNavRecord *FGNavList::findByIdent( const char* ident,
 }
 
 
+nav_list_type FGNavList::findFirstByIdent( string ident, fg_nav_types type, bool exact)
+{
+    nav_list_type n2;
+    n2.clear();
+    
+    int iType;
+    if(type == FG_NAV_VOR) iType = 3;
+    else if(type == FG_NAV_NDB) iType = 2;
+    else return(n2);
+    
+    nav_ident_map_iterator it;
+    if(exact) {
+	it = ident_navaids.find(ident);
+    } else {
+	bool typeMatch = false;
+	int safety_count = 0;
+	it = ident_navaids.lower_bound(ident);
+	while(!typeMatch) {
+	    nav_list_type n0 = it->second;	
+	    // local copy, so we should be able to do anything with n0.
+	    // Remove the types that don't match request.
+	    for(nav_list_iterator it0 = n0.begin(); it0 != n0.end();) {
+		FGNavRecord* nv = *it0;
+		if(nv->get_type() == iType) {
+		    typeMatch = true;
+		    ++it0;
+		} else {
+		    it0 = n0.erase(it0);
+		}
+	    }
+	    if(typeMatch) {
+		return(n0);
+	    }
+	    if(it == ident_navaids.begin()) {
+		// We didn't find a match before reaching the beginning of the map
+		n0.clear();
+		return(n0);		
+	    }
+	    safety_count++;
+	    if(safety_count == 1000000) {
+		SG_LOG(SG_INSTR, SG_ALERT,
+		       "safety_count triggered exit from while loop in findFirstByIdent!");
+		break;
+	    }
+	    ++it;
+	    if(it == ident_navaids.end()) {
+		n0.clear();
+		return(n0);
+	    }
+	}
+    }
+    if(it == ident_navaids.end()) {
+	n2.clear();
+	return(n2);
+    } else {
+	nav_list_type n1 = it->second;
+	n2.clear();
+	for(nav_list_iterator it2 = n1.begin(); it2 != n1.end(); ++it2) {
+	    FGNavRecord* nv = *it2;
+	    if(nv->get_type() == iType) n2.push_back(nv);
+	}
+	return(n2);
+    }
+}
+
+
 // Given an Ident and optional freqency, return the first matching
 // station.
 FGNavRecord *FGNavList::findByIdentAndFreq( const char* ident, const double freq )
@@ -269,7 +347,7 @@ FGNavRecord *FGNavList::findNavFromList( const Point3D &aircraft,
 
 // returns the closest entry to the give lon/lat/elev
 FGNavRecord *FGNavList::findClosest( double lon_rad, double lat_rad,
-                                     double elev_m )
+                                     double elev_m, fg_nav_types type)
 {
     FGNavRecord *result = NULL;
     double diff;
@@ -291,7 +369,7 @@ FGNavRecord *FGNavList::findClosest( double lon_rad, double lat_rad,
     latidx += 90;
 
     int master_index = lonidx * 1000 + latidx;
-    
+
     nav_list_type navs = navaids_by_tile[ master_index ];
     // cout << "Master index = " << master_index << endl;
     // cout << "beacon search length = " << beacons.size() << endl;
@@ -306,24 +384,26 @@ FGNavRecord *FGNavList::findClosest( double lon_rad, double lat_rad,
     double min_dist = 999999999.0;
 
     for ( ; current != last ; ++current ) {
-	// cout << "  testing " << (*current)->get_ident() << endl;
-	Point3D station = Point3D( (*current)->get_x(),
-				   (*current)->get_y(),
-				   (*current)->get_z() );
-	// cout << "    aircraft = " << aircraft << " station = " << station 
-	//      << endl;
-
-	double d = aircraft.distance3Dsquared( station ); // meters^2
-	// cout << "  distance = " << d << " (" 
-	//      << FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER 
-	//         * FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER
-	//      << ")" << endl;
-
-	// cout << "  range = " << sqrt(d) << endl;
-
-	if ( d < min_dist ) {
-	    min_dist = d;
-            result = (*current);
+	if(isTypeMatch(*current, type)) {
+	    // cout << "  testing " << (*current)->get_ident() << endl;
+	    Point3D station = Point3D( (*current)->get_x(),
+				       (*current)->get_y(),
+				       (*current)->get_z() );
+	    // cout << "    aircraft = " << aircraft << " station = " << station 
+	    //      << endl;
+    
+	    double d = aircraft.distance3Dsquared( station ); // meters^2
+	    // cout << "  distance = " << d << " (" 
+	    //      << FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER 
+	    //         * FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER
+	    //      << ")" << endl;
+    
+	    // cout << "  range = " << sqrt(d) << endl;
+    
+	    if ( d < min_dist ) {
+		min_dist = d;
+		result = (*current);
+	    }
 	}
     }
 
diff --git a/src/Navaids/navlist.hxx b/src/Navaids/navlist.hxx
index 705572aed..3d5872952 100644
--- a/src/Navaids/navlist.hxx
+++ b/src/Navaids/navlist.hxx
@@ -51,11 +51,13 @@ typedef nav_map_type::iterator nav_map_iterator;
 typedef nav_map_type::const_iterator nav_map_const_iterator;
 
 typedef map < string, nav_list_type > nav_ident_map_type;
+typedef nav_ident_map_type::iterator nav_ident_map_iterator;
 typedef map < string, tacan_list_type > tacan_ident_map_type;
 
 class FGNavList {
 
-    nav_list_type navlist;
+    //nav_list_type navlist;	// DCL - this doesn't appear to be used any more
+    				// and can probably be removed.
     nav_list_type carrierlist;
     nav_map_type navaids;
     nav_map_type navaids_by_tile;
@@ -87,6 +89,12 @@ public:
 
     // locate closest item in the DB matching the requested ident
     FGNavRecord *findByIdent( const char* ident, const double lon, const double lat );
+	
+    // Find items of requested type with closest exact or subsequent ident 
+    // (by ASCII code value) to that supplied.
+    // Supplying true for exact forces only exact matches to be returned (similar to above function)
+    // Returns an empty list if no match found - calling function should check for this!
+    nav_list_type findFirstByIdent( string ident, fg_nav_types type, bool exact = false );
 
     // Given an Ident and optional freqency, return the first matching
     // station.
@@ -94,7 +102,7 @@ public:
                                      const double freq = 0.0 );
 
     // returns the closest entry to the give lon/lat/elev
-    FGNavRecord *findClosest( double lon_rad, double lat_rad, double elev_m );
+    FGNavRecord *findClosest( double lon_rad, double lat_rad, double elev_m, fg_nav_types type = FG_NAV_ANY );
 
     // given a frequency returns the first matching entry
     FGNavRecord *findStationByFreq( double frequency );
diff --git a/src/Navaids/navrecord.hxx b/src/Navaids/navrecord.hxx
index 8b950abc5..89d227428 100644
--- a/src/Navaids/navrecord.hxx
+++ b/src/Navaids/navrecord.hxx
@@ -48,11 +48,20 @@ SG_USING_STD(istream);
 #define FG_DME_DEFAULT_RANGE 50 // nm
 #define FG_NAV_MAX_RANGE 300    // nm
 
+// Shield the rest of FG from possibly changing details of Robins navaid type numbering system.
+// Currently only the GPS code uses this - extra types (LOC, GS etc) may need to be added
+// should other FG code choose to use this. 
+enum fg_nav_types {
+    FG_NAV_VOR,
+    FG_NAV_NDB,
+    FG_NAV_ILS,
+    FG_NAV_ANY
+};
 
 class FGNavRecord {
 
     int type;
-    double lon, lat;            // location in geodetic coords
+    double lon, lat;            // location in geodetic coords (degrees)
     double elev_ft;
     double x, y, z;             // location in cartesian coords (earth centered)
     int freq;
@@ -75,10 +84,11 @@ public:
     inline ~FGNavRecord(void) {}
 
     inline int get_type() const { return type; }
-    inline double get_lon() const { return lon; }
-    inline void set_lon( double l ) { lon = l; }
-    inline double get_lat() const { return lat; }
-    inline void set_lat( double l ) { lat = l; }
+    inline fg_nav_types get_fg_type() const;
+    inline double get_lon() const { return lon; }	// degrees
+    inline void set_lon( double l ) { lon = l; }	// degrees
+    inline double get_lat() const { return lat; }	// degrees
+    inline void set_lat( double l ) { lat = l; }	// degrees
     inline double get_elev_ft() const { return elev_ft; }
     inline void set_elev_ft( double e ) { elev_ft = e; }
     inline double get_x() const { return x; }
@@ -116,6 +126,16 @@ FGNavRecord::FGNavRecord(void) :
 }
 
 
+inline fg_nav_types FGNavRecord::get_fg_type() const {
+    switch(type) {
+    case 2: return(FG_NAV_NDB);
+    case 3: return(FG_NAV_VOR);
+    case 4: return(FG_NAV_ILS);
+    default: return(FG_NAV_ANY);
+    }
+}
+
+
 inline istream&
 operator >> ( istream& in, FGNavRecord& n )
 {