From f5cc3b9b9ce5d8358ce645badd3d6bfde6a717d2 Mon Sep 17 00:00:00 2001
From: curt <curt>
Date: Thu, 12 Oct 2000 01:06:07 +0000
Subject: [PATCH] Lot's of massaging to get the WAYPOINT hold mechanism to work
 with the new SGRoute manager.  Changed logic and gui a bit to match.

---
 src/Autopilot/auto_gui.cxx | 115 +++++++++++++++++--------------------
 src/Autopilot/auto_gui.hxx |  40 +------------
 src/Autopilot/newauto.cxx  |  87 +++++++++++++++++++---------
 src/Autopilot/newauto.hxx  |  10 ++--
 4 files changed, 120 insertions(+), 132 deletions(-)

diff --git a/src/Autopilot/auto_gui.cxx b/src/Autopilot/auto_gui.cxx
index 862167b88..47b17d68c 100644
--- a/src/Autopilot/auto_gui.cxx
+++ b/src/Autopilot/auto_gui.cxx
@@ -40,7 +40,9 @@
 #include <GUI/gui.h>
 #include <Main/bfi.hxx>
 #include <Main/fg_init.hxx>
+#include <Main/globals.hxx>
 #include <Main/options.hxx>
+#include <Navaids/fixlist.hxx>
 
 #include "auto_gui.hxx"
 #include "newauto.hxx"
@@ -585,72 +587,51 @@ void TgtAptDialog_OK (puObject *)
     
     char *s;
     TgtAptDialogInput->getValue(&s);
-    TgtAptId = s;
-    
+
+    string tmp = s;
+    double alt = 0.0;
+    int pos = tmp.find( "," );
+    if ( pos != string::npos ) {
+	TgtAptId = tmp.substr( 0, pos );
+	string alt_str = tmp.substr( pos + 1 );
+	alt = atof( alt_str.c_str() );
+	if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
+	    alt *= FEET_TO_METER;
+	}
+    } else {
+	TgtAptId = tmp;
+    }
+
     TgtAptDialog_Cancel( NULL );
     
-    if ( TgtAptId.length() ) {
-        // set initial position from TgtAirport id
+    FGAirport a;
+    FGFix f;
+    double t1, t2;
+    if ( fgFindAirportID( TgtAptId, &a ) ) {
+
+	 FG_LOG( FG_GENERAL, FG_INFO,
+		 "Adding waypoint (airport) = " << TgtAptId );
         
-	FGPath path( current_options.get_fg_root() );
-	path.append( "Airports" );
-	path.append( "simple.mk4" );
-        FGAirports airports( path.c_str() );
-        FGAirport a;
+	 sprintf( NewTgtAirportId, "%s", TgtAptId.c_str() );
+
+	 SGWayPoint wp( a.longitude, a.latitude, alt,
+			SGWayPoint::WGS84, TgtAptId );
+	 globals->get_route()->add_waypoint( wp );
+    } else if ( current_fixlist->query( TgtAptId, 0.0, 0.0, 0.0,
+					&f, &t1, &t2 ) )
+    {
+	 FG_LOG( FG_GENERAL, FG_INFO,
+		 "Adding waypoint (fix) = " << TgtAptId );
         
-        FG_LOG( FG_GENERAL, FG_INFO,
-                "Attempting to set starting position from airport code "
-                << s );
-        
-        if ( airports.search( TgtAptId, &a ) )
-	    {
-		double course, reverse, distance;
-		//            fgAPset_tgt_airport_id( TgtAptId.c_str() );
-		current_options.set_airport_id( TgtAptId.c_str() );
-		sprintf( NewTgtAirportId, "%s", TgtAptId.c_str() );
-			
-		current_autopilot->set_WayPoint( a.longitude, a.latitude,
-						 TgtAptId );
-		// current_autopilot->set_TargetLatitude( a.latitude );
-		// current_autopilot->set_TargetLongitude( a.longitude );
-		current_autopilot->MakeTargetLatLonStr(
-				     current_autopilot->get_TargetLatitude(),
-				     current_autopilot->get_TargetLongitude() );
-			
-		current_autopilot->set_old_lat( FGBFI::getLatitude() );
-		current_autopilot->set_old_lon( FGBFI::getLongitude() );
-			
-		// need to test for iter
-		if( ! geo_inverse_wgs_84( FGBFI::getAltitude() * FEET_TO_METER,
-					  FGBFI::getLatitude(),
-					  FGBFI::getLongitude(),
-					  current_autopilot->get_TargetLatitude(),
-					  current_autopilot->get_TargetLongitude(),
-					  &course,
-					  &reverse,
-					  &distance ) ) {
-		    current_autopilot->set_TargetHeading( course );
-		    current_autopilot->MakeTargetHeadingStr(
-				      current_autopilot->get_TargetHeading() );
-		    current_autopilot->set_TargetDistance( distance );
-		    current_autopilot->MakeTargetDistanceStr( distance );
-		    // This changes the AutoPilot Heading
-		    // following cast needed
-		    ApHeadingDialogInput->
-			setValue((float)current_autopilot->get_TargetHeading() );
-		    // Force this !
-		    current_autopilot->set_HeadingEnabled( true );
-		    current_autopilot->set_HeadingMode(
-                                             FGAutopilot::FG_HEADING_WAYPOINT );
-		}
-	    } else {
-		TgtAptId  += " not in database.";
-		mkDialog(TgtAptId.c_str());
-	    }
+	 sprintf( NewTgtAirportId, "%s", TgtAptId.c_str() );
+
+	 SGWayPoint wp( f.get_lon(), f.get_lat(), alt,
+			SGWayPoint::WGS84, TgtAptId );
+	 globals->get_route()->add_waypoint( wp );
+    } else {
+	TgtAptId  += " not in database.";
+	mkDialog(TgtAptId.c_str());
     }
-    // get_control_values();
-    //    if( PauseMode != t->getPause() )
-    //        t->togglePauseMode();
 }
 
 void TgtAptDialog_Reset(puObject *)
@@ -661,7 +642,7 @@ void TgtAptDialog_Reset(puObject *)
     TgtAptDialogInput->setCursor( 0 ) ;
 }
 
-void NewTgtAirport(puObject *cb)
+void AddWayPoint(puObject *cb)
 {
     //  strncpy( NewAirportId, current_options.get_airport_id().c_str(), 16 );
     sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() );
@@ -670,6 +651,16 @@ void NewTgtAirport(puObject *cb)
     FG_PUSH_PUI_DIALOG( TgtAptDialog );
 }
 
+void PopWayPoint(puObject *cb)
+{
+    globals->get_route()->delete_first();
+}
+
+void ClearRoute(puObject *cb)
+{
+    globals->get_route()->clear();
+}
+
 void NewTgtAirportInit(void)
 {
     FG_LOG( FG_AUTOPILOT, FG_INFO, " enter NewTgtAirportInit()" );
diff --git a/src/Autopilot/auto_gui.hxx b/src/Autopilot/auto_gui.hxx
index 5de09364e..9892a5ef4 100644
--- a/src/Autopilot/auto_gui.hxx
+++ b/src/Autopilot/auto_gui.hxx
@@ -41,49 +41,15 @@ FG_USING_STD(string);
 // Defines
 #define AP_CURRENT_HEADING -1
 
-
 // prototypes
-// void fgAPToggleWayPoint( void );
-// void fgAPToggleHeading( void );
-// void fgAPToggleAltitude( void );
-// void fgAPToggleTerrainFollow( void );
-// void fgAPToggleAutoThrottle( void );
-
-// bool fgAPTerrainFollowEnabled( void );
-// bool fgAPAltitudeEnabled( void );
-// bool fgAPHeadingEnabled( void );
-// bool fgAPWayPointEnabled( void );
-// bool fgAPAutoThrottleEnabled( void );
-
-// void fgAPAltitudeAdjust( double inc );
-// void fgAPHeadingAdjust( double inc );
-// void fgAPAutoThrottleAdjust( double inc );
-
-// void fgAPHeadingSet( double value );
-
-// double fgAPget_TargetLatitude( void );
-// double fgAPget_TargetLongitude( void );
-// // double fgAPget_TargetHeading( void );
-// double fgAPget_TargetDistance( void );
-// double fgAPget_TargetAltitude( void );
-
-// char *fgAPget_TargetLatitudeStr( void );
-// char *fgAPget_TargetLongitudeStr( void );
-// char *fgAPget_TargetDistanceStr( void );
-// char *fgAPget_TargetHeadingStr( void );
-// char *fgAPget_TargetAltitudeStr( void );
-// char *fgAPget_TargetLatLonStr( void );
-
-//void fgAPset_tgt_airport_id( const string );
-//string fgAPget_tgt_airport_id( void );
-
-// void fgAPReset(void);
 
 class puObject;
 void fgAPAdjust( puObject * );
 void NewHeading(puObject *cb);
 void NewAltitude(puObject *cb);
-void NewTgtAirport(puObject *cb);
+void AddWayPoint(puObject *cb);
+void PopWayPoint(puObject *cb);
+void ClearRoute(puObject *cb);
 
 void NewTgtAirportInit();
 void fgAPAdjustInit() ;
diff --git a/src/Autopilot/newauto.cxx b/src/Autopilot/newauto.cxx
index 9955b6b21..1630837fe 100644
--- a/src/Autopilot/newauto.cxx
+++ b/src/Autopilot/newauto.cxx
@@ -37,6 +37,7 @@
 #include <Controls/controls.hxx>
 #include <FDM/flight.hxx>
 #include <Main/bfi.hxx>
+#include <Main/globals.hxx>
 #include <Main/options.hxx>
 #include <Scenery/scenery.hxx>
 
@@ -116,7 +117,8 @@ void FGAutopilot::MakeTargetDistanceStr( double distance ) {
     }
     major = (int)eta;
     minor = (int)((eta - (int)eta) * 60.0);
-    sprintf( TargetDistanceStr, "APDistance %.2f NM  ETA %d:%02d",
+    sprintf( TargetDistanceStr, "%s %.2f NM  ETA %d:%02d",
+	     waypoint.get_id().c_str(),
 	     distance*METER_TO_NM, major, minor );
     // cout << "distance = " << distance*METER_TO_NM
     //      << "  gndsp = " << get_ground_speed() 
@@ -146,7 +148,7 @@ void FGAutopilot::init() {
     // Initialize target location to startup location
     old_lat = FGBFI::getLatitude();
     old_lon = FGBFI::getLongitude();
-    set_WayPoint( old_lon, old_lat, "default" );
+    // set_WayPoint( old_lon, old_lat, 0.0, "default" );
 
     MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() );
 	
@@ -215,7 +217,7 @@ void FGAutopilot::reset() {
 	
     // TargetLatitude = FGBFI::getLatitude();
     // TargetLongitude = FGBFI::getLongitude();
-    set_WayPoint( FGBFI::getLongitude(), FGBFI::getLatitude(), "reset" );
+    // s<et_WayPoint( FGBFI::getLongitude(), FGBFI::getLatitude(), 0.0, "reset" );
 
     MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() );
 }
@@ -272,6 +274,7 @@ int FGAutopilot::run() {
 	
     double lat = FGBFI::getLatitude();
     double lon = FGBFI::getLongitude();
+    double alt = FGBFI::getAltitude() * FEET_TO_METER;
 
 #ifdef FG_FORCE_AUTO_DISENGAGE
     // see if somebody else has changed them
@@ -333,7 +336,7 @@ int FGAutopilot::run() {
 	} else if ( heading_mode == FG_HEADING_WAYPOINT ) {
 	    // update target heading to waypoint
 
-	    double wp_course, /*wp_reverse,*/ wp_distance;
+	    double wp_course, wp_distance;
 
 #ifdef DO_fgAP_CORRECTED_COURSE
 	    // compute course made good
@@ -352,7 +355,8 @@ int FGAutopilot::run() {
 
 	    // compute course to way_point
 	    // need to test for iter
-	    waypoint.CourseAndDistance( lon, lat, &wp_course, &wp_distance );
+	    waypoint.CourseAndDistance( lon, lat, alt, 
+					&wp_course, &wp_distance );
 
 #ifdef DO_fgAP_CORRECTED_COURSE
 	    corrected_course = course - wp_course;
@@ -366,12 +370,24 @@ int FGAutopilot::run() {
 		// corrected_course = course - wp_course;
 		TargetHeading = NormalizeDegrees(wp_course);
 	    } else {
-		printf("distance(%f) to close\n", wp_distance);
-		// Real Close -- set heading hold to current heading
-		// and Ring the arival bell !!
-		heading_mode = FG_HEADING_LOCK;
-		// use current heading
-		TargetHeading = FGBFI::getHeading();
+		cout << "Reached waypoint within " << wp_distance << "meters"
+		     << endl;
+
+		// pop off this waypoint from the list
+		if ( globals->get_route()->size() ) {
+		    globals->get_route()->delete_first();
+		}
+
+		// see if there are more waypoints on the list
+		if ( globals->get_route()->size() ) {
+		    // more waypoints
+		    set_HeadingMode( FG_HEADING_WAYPOINT );
+		} else {
+		    // end of the line
+		    heading_mode = FG_HEADING_LOCK;
+		    // use current heading
+		    TargetHeading = FGBFI::getHeading();
+		}
 	    }
 	    MakeTargetHeadingStr( TargetHeading );
 	    // Force this just in case
@@ -623,24 +639,41 @@ void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) {
 	// set heading hold to current heading
 	TargetHeading = FGBFI::getHeading();
     } else if ( heading_mode == FG_HEADING_WAYPOINT ) {
-	double course, /*reverse,*/ distance;
-	// turn on location hold
-	// turn on heading hold
-	old_lat = FGBFI::getLatitude();
-	old_lon = FGBFI::getLongitude();
-			
-	waypoint.CourseAndDistance( FGBFI::getLongitude(), FGBFI::getLatitude(),
-				    &course, &distance );
-	TargetHeading = course;
-	TargetDistance = distance;
-	MakeTargetDistanceStr( distance );
+	if ( globals->get_route()->size() ) {
+	    double course, distance;
 
-	FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetLocation: ( "
-		<< get_TargetLatitude()  << " "
-		<< get_TargetLongitude() << " ) "
-		);
+	    old_lat = FGBFI::getLatitude();
+	    old_lon = FGBFI::getLongitude();
+
+	    waypoint = globals->get_route()->get_first();
+	    waypoint.CourseAndDistance( FGBFI::getLongitude(),
+					FGBFI::getLatitude(),
+					FGBFI::getLatitude() * FEET_TO_METER,
+					&course, &distance );
+	    TargetHeading = course;
+	    TargetDistance = distance;
+	    MakeTargetLatLonStr( waypoint.get_target_lat(),
+				 waypoint.get_target_lon() );
+	    MakeTargetDistanceStr( distance );
+
+	    if ( waypoint.get_target_alt() > 0.0 ) {
+		TargetAltitude = waypoint.get_target_alt();
+		altitude_mode = FG_ALTITUDE_LOCK;
+		set_AltitudeEnabled( true );
+		MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET );
+	    }
+
+	    FG_LOG( FG_COCKPIT, FG_INFO, " set_HeadingMode: ( "
+		    << get_TargetLatitude()  << " "
+		    << get_TargetLongitude() << " ) "
+		    );
+	} else {
+	    // no more way points, default to heading lock.
+	    heading_mode = FG_HEADING_LOCK;
+	    TargetHeading = FGBFI::getHeading();
+	}
     }
-	
+
     MakeTargetHeadingStr( TargetHeading );			
     update_old_control_values();
 }
diff --git a/src/Autopilot/newauto.hxx b/src/Autopilot/newauto.hxx
index 5ec7c924a..feeca7ab4 100644
--- a/src/Autopilot/newauto.hxx
+++ b/src/Autopilot/newauto.hxx
@@ -135,18 +135,16 @@ public:
     inline bool get_AutoThrottleEnabled() const { return auto_throttle; }
     void set_AutoThrottleEnabled( bool value );
 
-    inline void set_WayPoint( const double lon, const double lat,
-			      const string s ) {
-	waypoint = SGWayPoint( lon, lat, SGWayPoint::WGS84, "Current WP" );
-    }
+    /* inline void set_WayPoint( const double lon, const double lat, 
+			      const double alt, const string s ) {
+	waypoint = SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, "Current WP" );
+    } */
     inline double get_TargetLatitude() const {
 	return waypoint.get_target_lat();
     }
     inline double get_TargetLongitude() const {
 	return waypoint.get_target_lon();
     }
-    // inline void set_TargetLatitude( double val ) { TargetLatitude = val; }
-    // inline void set_TargetLongitude( double val ) { TargetLongitude = val; }
     inline void set_old_lat( double val ) { old_lat = val; }
     inline void set_old_lon( double val ) { old_lon = val; }
     inline double get_TargetHeading() const { return TargetHeading; }