]> git.mxchange.org Git - flightgear.git/blob - src/ATCDCL/ground.cxx
A generic, configurable autobrake system. Not fully features yet, but works quite...
[flightgear.git] / src / ATCDCL / ground.cxx
1 // FGGround - a class to provide ground control at larger airports.
2 //
3 // Written by David Luff, started March 2002.
4 //
5 // Copyright (C) 2002  David C. Luff - david.luff@nottingham.ac.uk
6 //
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
11 //
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
15 // General Public License for more details.
16 //
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
20
21 #ifdef HAVE_CONFIG_H
22 #  include <config.h>
23 #endif
24
25 #include <iostream>
26
27 #include <simgear/misc/sg_path.hxx>
28 #include <simgear/math/sg_random.h>
29 #include <simgear/debug/logstream.hxx>
30 #include <simgear/misc/sgstream.hxx>
31 #include <simgear/constants.h>
32 #include <Main/globals.hxx>
33
34 #include <stdlib.h>
35 #include <fstream>
36
37 #include "ground.hxx"
38 #include "ATCutils.hxx"
39 #include "AILocalTraffic.hxx"
40 #include "ATCmgr.hxx"
41
42 using std::ifstream;
43 using std::cout;
44
45 node::node() {
46 }
47
48 node::~node() {
49         for(unsigned int i=0; i < arcs.size(); ++i) {
50                 delete arcs[i];
51         }
52 }
53
54 // Make sure that a_path.cost += distance is safe from the moment it's created.
55 a_path::a_path() {
56         cost = 0;
57 }
58
59 FGGround::FGGround() {
60         ATCmgr = globals->get_ATC_mgr();
61         _type = GROUND;
62         networkLoadOK = false;
63         ground_traffic.erase(ground_traffic.begin(), ground_traffic.end());
64         ground_traffic_itr = ground_traffic.begin();
65         
66         // Init the property nodes - TODO - need to make sure we're getting surface winds.
67         wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
68         wind_speed_knots = fgGetNode("/environment/wind-speed-kt", true);
69         
70         // TODO - get the actual airport elevation
71         aptElev = 0.0;
72 }
73
74 FGGround::FGGround(const string& id) {
75         ATCmgr = globals->get_ATC_mgr();
76         networkLoadOK = false;
77         ground_traffic.erase(ground_traffic.begin(), ground_traffic.end());
78         ground_traffic_itr = ground_traffic.begin();
79         ident = id;
80         
81         // Init the property nodes - TODO - need to make sure we're getting surface winds.
82         wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
83         wind_speed_knots = fgGetNode("/environment/wind-speed-kt", true);
84         
85         // TODO - get the actual airport elevation
86         aptElev = 0.0;
87 }
88
89 FGGround::~FGGround() {
90 }
91
92 void FGGround::ParseRwyExits(node* np, char* es) {
93         char* token;
94         char estr[20];
95         strcpy(estr, es);
96         const char delimiters[] = "-";
97         token = strtok(estr, delimiters);
98         while(token != NULL) {
99                 int i = atoi(token);
100                 //cout << "token = " << token << endl;
101                 //cout << "rwy number = " << i << endl;
102                 //runways[(atoi(token))].exits.push_back(np);
103                 runways[i].exits.push_back(np);
104                 //cout << "token = " << token << '\n';
105                 token = strtok(NULL, delimiters);
106         }
107 }
108         
109
110 // Load the ground logical network of the current instances airport
111 // Return true if successfull.
112 // TODO - currently the file is assumed to reside in the base/ATC directory.
113 // This might change to something more thought out in the future.
114 // NOTE - currently it is assumed that all nodes are loaded before any arcs.
115 // It won't work ATM if this doesn't hold true.
116 bool FGGround::LoadNetwork() {
117         node* np;
118         arc* ap;
119         Gate* gp;
120         
121         int gateCount = 0;      // This is used to allocate gateID's from zero upwards
122         // This may well change in the future - probably to reading in the real-world
123         // gate numbers from file.
124         
125         ifstream fin;
126         SGPath path = globals->get_fg_root();
127         //string taxiPath = "ATC/" + ident + ".taxi";
128         string taxiPath = "ATC/KEMT.taxi";      // FIXME - HARDWIRED FOR TESTING
129         path.append(taxiPath);
130         
131         SG_LOG(SG_ATC, SG_INFO, "Trying to read taxiway data for " << ident << "...");
132         //cout << "Trying to read taxiway data for " << ident << "..." << endl;
133         fin.open(path.c_str(), ios::in);
134         if(!fin) {
135                 SG_LOG(SG_ATC, SG_ALERT, "Unable to open taxiway data input file " << path.c_str());
136                 //cout << "Unable to open taxiway data input file " << path.c_str() << endl;
137                 return(false);
138         }
139         
140         char ch;
141         char buf[30];
142         while(!fin.eof()) {
143                 fin >> buf;
144                 // Node, arc, or [End]?
145                 //cout << "Read in ground network element type = " << buf << endl;
146                 if(!strcmp(buf, "[End]")) {             // TODO - maybe make this more robust to spelling errors by just looking for '['
147                         SG_LOG(SG_ATC, SG_INFO, "Done reading " << path.c_str() << endl);
148                         break;
149                 } else if(!strcmp(buf, "N")) {
150                         // Node
151                         np = new node;
152                         np->struct_type = NODE;
153                         fin >> buf;
154                         np->nodeID = atoi(buf);
155                         fin >> buf;
156                         np->pos.setLongitudeDeg(atof(buf));
157                         fin >> buf;
158                         np->pos.setLatitudeDeg(atof(buf));
159                         fin >> buf;
160                         np->pos.setElevationM(atof(buf));
161                         fin >> buf;             // node type
162                         if(!strcmp(buf, "J")) {
163                                 np->type = JUNCTION;
164                         } else if(!strcmp(buf, "T")) {
165                                 np->type = TJUNCTION;
166                         } else if(!strcmp(buf, "H")) {
167                                 np->type = HOLD;
168                         } else {
169                                 SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown node type in taxi network...\n");
170                                 delete np;
171                                 return(false);
172                         }
173                         fin >> buf;             // rwy exit information - gets parsed later - FRAGILE - will break if buf is reused.
174                         // Now the name
175                         fin >> ch;      // strip the leading " off
176                         np->name = "";
177                         while(1) {
178                                 fin.unsetf(ios::skipws);
179                                 fin >> ch;
180                                 if((ch == '"') || (ch == 0x0A)) {
181                                         break;
182                                 }   // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
183                                 np->name += ch;
184                         }
185                         fin.setf(ios::skipws);
186                         network.push_back(np);
187                         // FIXME - fragile - replies on buf not getting modified from exits read to here
188                         // see if we also need to push it onto the runway exit list
189                         //cout << "strlen(buf) = " << strlen(buf) << endl;
190                         if(strlen(buf) > 2) {
191                                 //cout << "Calling ParseRwyExits for " << buf << endl;
192                                 ParseRwyExits(np, buf);
193                         }
194                 } else if(!strcmp(buf, "A")) {
195                         ap = new arc;
196                         ap->struct_type = ARC;
197                         fin >> buf;
198                         ap->n1 = atoi(buf);
199                         fin >> buf;
200                         ap->n2 = atoi(buf);
201                         fin >> buf;
202                         if(!strcmp(buf, "R")) {
203                                 ap->type = RUNWAY;
204                         } else if(!strcmp(buf, "T")) {
205                                 ap->type = TAXIWAY;
206                         } else {
207                                 SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown arc type in taxi network...\n");
208                                 delete ap;
209                                 return(false);
210                         }
211                         // directed?
212                         fin >> buf;
213                         if(!strcmp(buf, "Y")) {
214                                 ap->directed = true;
215                         } else if(!strcmp(buf, "N")) {
216                                 ap->directed = false;
217                         } else {
218                                 SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown arc directed value in taxi network - should be Y/N !!!\n");
219                                 delete ap;
220                                 return(false);
221                         }                       
222                         // Now the name
223                         ap->name = "";
224                         while(1) {
225                                 fin.unsetf(ios::skipws);
226                                 fin >> ch;
227                                 ap->name += ch;
228                                 if((ch == '"') || (ch == 0x0A)) {
229                                         break;
230                                 }   // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
231                         }
232                         fin.setf(ios::skipws);
233                         ap->distance = (int)dclGetHorizontalSeparation(network[ap->n1]->pos, network[ap->n2]->pos);
234                         //cout << "Distance  = " << ap->distance << '\n';
235                         network[ap->n1]->arcs.push_back(ap);
236                         network[ap->n2]->arcs.push_back(ap);
237                 } else if(!strcmp(buf, "G")) {
238                         gp = new Gate;
239                         gp->struct_type = NODE;
240                         gp->type = GATE;
241                         fin >> buf;
242                         gp->nodeID = atoi(buf);
243                         fin >> buf;
244                         gp->pos.setLongitudeDeg(atof(buf));
245                         fin >> buf;
246                         gp->pos.setLatitudeDeg(atof(buf));
247                         fin >> buf;
248                         gp->pos.setElevationM(atof(buf));
249                         fin >> buf;             // gate type - ignore this for now
250                         fin >> buf;             // gate heading
251                         gp->heading = atoi(buf);
252                         // Now the name
253                         gp->name = "";
254                         while(1) {
255                                 fin.unsetf(ios::skipws);
256                                 fin >> ch;
257                                 gp->name += ch;
258                                 if((ch == '"') || (ch == 0x0A)) {
259                                         break;
260                                 }   // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
261                         }
262                         fin.setf(ios::skipws);
263                         gp->id = gateCount;             // Warning - this will likely change in the future.
264                         gp->used = false;
265                         network.push_back(gp);
266                         gates[gateCount] = gp;
267                         gateCount++;
268                 } else {
269                         // Something has gone seriously pear-shaped
270                         SG_LOG(SG_ATC, SG_ALERT, "********* ERROR - unknown ground network element type... aborting read of " << path.c_str() << '\n');
271                         return(false);
272                 }
273                 
274                 fin >> skipeol;         
275         }
276         return(true);
277 }
278
279 void FGGround::Init() {
280         untowered = false;
281         
282         // Figure out which is the active runway - TODO - it would be better to have ground call tower
283         // for runway operation details, but at the moment we can't guarantee that tower control at a given airport
284         // will be initialised before ground so we can't do that.
285         DoRwyDetails();
286         //cout << "In FGGround::Init, active rwy is " << activeRwy << '\n';
287         ortho.Init(rwy.threshold_pos, rwy.hdg);
288
289         networkLoadOK = LoadNetwork();
290 }
291
292 void FGGround::Update(double dt) {
293         // Each time step, what do we need to do?
294         // We need to go through the list of outstanding requests and acknowedgements
295         // and process at least one of them.
296         // We need to go through the list of planes under our control and check if
297         // any need to be addressed.
298         // We need to check for planes not under our control coming within our 
299         // control area and address if necessary.
300         
301         // Lets take the example of a plane which has just contacted ground
302         // following landing - presumably requesting where to go?
303         // First we need to establish the position of the plane within the logical network.
304         // Next we need to decide where its going.
305         
306         if(ground_traffic.size()) {
307                 if(ground_traffic_itr == ground_traffic.end()) {
308                         ground_traffic_itr = ground_traffic.begin();
309                 }
310                 
311                 //Process(*ground_traffic_itr);
312                 GroundRec* g = *ground_traffic_itr;
313                 if(g->taxiRequestOutstanding) {
314                         double responseTime = 10.0;             // seconds - this should get more sophisticated at some point
315                         if(g->clearanceCounter > responseTime) {
316                                 // DO CLEARANCE
317                                 // TODO - move the mechanics of making up the transmission out of the main Update(...) routine.
318                                 string trns = "";
319                                 trns += g->plane.callsign;
320                                 trns += " taxi holding point runway ";  // TODO - add the holding point name
321                                 // eg " taxi holding point G2 runway "
322                                 trns += ConvertRwyNumToSpokenString(activeRwy);
323                                 if(_display) {
324                                         fgSetString("/sim/messages/ground", trns.c_str());
325                                 }
326                                 g->planePtr->RegisterTransmission(1);   // cleared to taxi
327                                 g->clearanceCounter = 0.0;
328                                 g->taxiRequestOutstanding = false;
329                         } else {
330                                 g->clearanceCounter += (dt * ground_traffic.size());
331                         }
332                 } else if(((FGAILocalTraffic*)(g->planePtr))->AtHoldShort()) {          // That's a hack - eventually we should monitor actual position
333                         // HACK ALERT - the automatic cast to AILocalTraffic has to go once we have other sorts working!!!!! FIXME TODO
334                         // NOTE - we don't need to do the contact tower bit unless we have separate tower and ground
335                         string trns = g->plane.callsign;
336                         trns += " contact Tower ";
337                         double f = globals->get_ATC_mgr()->GetFrequency(ident, TOWER) / 100.0;
338                         char buf[10];
339                         sprintf(buf, "%.2f", f);
340                         trns += buf;
341                         if(_display) {
342                                 fgSetString("/sim/messages/ground", trns.c_str());
343                         }
344                         g->planePtr->RegisterTransmission(2);   // contact tower
345                         delete *ground_traffic_itr;
346                         ground_traffic.erase(ground_traffic_itr);
347                         ground_traffic_itr = ground_traffic.begin();
348                 }                               
349                 ++ground_traffic_itr;
350         }
351         
352         // Call the base class update for the response time handling.
353         FGATC::Update(dt);
354 }
355
356 // Figure out which runways are active.
357 // For now we'll just be simple and do one active runway - eventually this will get much more complex
358 // Copied from FGTower - TODO - it would be better to implement this just once, and have ground call tower
359 // for runway operation details, but at the moment we can't guarantee that tower control at a given airport
360 // will be initialised before ground so we can't do that.
361 void FGGround::DoRwyDetails() {
362         //cout << "GetRwyDetails called" << endl;
363                 
364   const FGAirport* apt = fgFindAirportID(ident);
365   assert(apt);
366         FGRunway* runway = apt->getActiveRunwayForUsage();
367
368   activeRwy = runway->ident();
369   rwy.rwyID = runway->ident();
370   SG_LOG(SG_ATC, SG_INFO, "In FGGround, active runway for airport " << ident << " is " << activeRwy);
371   
372   // Get the threshold position
373   double other_way = runway->headingDeg() - 180.0;
374   while(other_way <= 0.0) {
375     other_way += 360.0;
376   }
377     // move to the +l end/center of the runway
378   //cout << "Runway center is at " << runway._lon << ", " << runway._lat << '\n';
379   double tshlon = 0.0, tshlat = 0.0, tshr = 0.0;
380   double tolon = 0.0, tolat = 0.0, tor = 0.0;
381   rwy.length = runway->lengthM();
382   geo_direct_wgs_84 ( aptElev, runway->latitude(), runway->longitude(), other_way, 
383                       rwy.length / 2.0 - 25.0, &tshlat, &tshlon, &tshr );
384   geo_direct_wgs_84 ( aptElev, runway->latitude(), runway->longitude(), runway->headingDeg(), 
385                       rwy.length / 2.0 - 25.0, &tolat, &tolon, &tor );
386   // Note - 25 meters in from the runway end is a bit of a hack to put the plane ahead of the user.
387   // now copy what we need out of runway into rwy
388   rwy.threshold_pos = SGGeod::fromDegM(tshlon, tshlat, aptElev);
389   SGGeod takeoff_end = SGGeod::fromDegM(tolon, tolat, aptElev);
390   //cout << "Threshold position = " << tshlon << ", " << tshlat << ", " << aptElev << '\n';
391   //cout << "Takeoff position = " << tolon << ", " << tolat << ", " << aptElev << '\n';
392   rwy.hdg = runway->headingDeg();
393   // Set the projection for the local area based on this active runway
394   ortho.Init(rwy.threshold_pos, rwy.hdg);       
395   rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos);      // should come out as zero
396   rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
397 }
398
399 // Return a random gate ID of an unused gate.
400 // Two error values may be returned and must be checked for by the calling function:
401 // -2 signifies that no gates exist at this airport.
402 // -1 signifies that all gates are currently full.
403 int FGGround::GetRandomGateID() {
404         // Check that this airport actually has some gates!!
405         if(!gates.size()) {
406                 return(-2);
407         }
408
409         gate_vec_type gateVec;
410         int num = 0;
411         int thenum;
412         int ID;
413         
414         gatesItr = gates.begin();
415         while(gatesItr != gates.end()) {
416                 if((gatesItr->second)->used == false) {
417                         gateVec.push_back(gatesItr->second);
418                         num++;
419                 }
420                 ++gatesItr;
421         }
422
423         // Check that there are some unused gates!
424         if(!gateVec.size()) {
425                 return(-1);
426         }
427         
428         // Randomly select one from the list
429         sg_srandom_time();
430         thenum = (int)(sg_random() * gateVec.size());
431         ID = gateVec[thenum]->id;
432         
433         return(ID);
434 }
435
436 // Return a pointer to an unused gate node
437 Gate* FGGround::GetGateNode() {
438         int id = GetRandomGateID();
439         if(id < 0) {
440                 return(NULL);
441         } else {
442                 return(gates[id]);
443         }
444 }
445
446
447 node* FGGround::GetHoldShortNode(const string& rwyID) {
448         return(NULL);   // TODO - either implement me or remove me!!!
449 }
450
451
452 // WARNING - This is hardwired to my prototype logical network format
453 // and will almost certainly change when Bernie's stuff comes on-line.
454 // Returns NULL if it can't find a valid node.
455 node* FGGround::GetThresholdNode(const string& rwyID) {
456         // For now go through all the nodes and parse their names
457         // Maybe in the future we'll map threshold nodes by ID
458         //cout << "Size of network is " << network.size() << '\n';
459         for(unsigned int i=0; i<network.size(); ++i) {
460                 //cout << "Name = " << network[i]->name << '\n';
461                 if(network[i]->name.size()) {
462                         string s = network[i]->name;
463                         // Warning - the next bit is fragile and dependent on my current naming scheme
464                         //cout << "substr = " << s.substr(0,3) << '\n';
465                         //cout << "size of s = " << s.size() << '\n'; 
466                         if(s.substr(0,3) == "rwy") {
467                                 //cout << "subsubstr = " << s.substr(4, s.size() - 4) << '\n';
468                                 if(s.substr(4, s.size() - 4) == rwyID) {
469                                         return network[i];
470                                 }
471                         }
472                 }
473         }
474         return NULL;
475 }
476
477
478 // Get a path from a point on a runway to a gate
479 // TODO !!
480
481 // Get a path from a node to another node
482 // Eventually we will need complex algorithms for this taking other traffic,
483 // shortest path and suitable paths into accout.
484 // For now we'll just call the shortest path algorithm.
485 ground_network_path_type FGGround::GetPath(node* A, node* B) {  
486         return(GetShortestPath(A, B));
487 };
488
489 // Get a path from a node to a runway threshold
490 ground_network_path_type FGGround::GetPath(node* A, const string& rwyID) {
491         node* b = GetThresholdNode(rwyID);
492         if(b == NULL) {
493                 SG_LOG(SG_ATC, SG_ALERT, "ERROR - unable to find path to runway theshold in ground.cxx for airport " << ident << '\n');
494                 ground_network_path_type emptyPath;
495                 emptyPath.erase(emptyPath.begin(), emptyPath.end());
496                 return(emptyPath);
497         }
498         return GetShortestPath(A, b);
499 }
500
501 // Get a path from a node to a runway hold short point
502 // Bit of a hack this at the moment!
503 ground_network_path_type FGGround::GetPathToHoldShort(node* A, const string& rwyID) {
504         ground_network_path_type path = GetPath(A, rwyID);
505         path.pop_back();        // That should be the threshold stripped of 
506         path.pop_back();        // and that should be the arc from hold short to threshold
507         // This isn't robust though - TODO - implement properly!
508         return(path);
509 }
510
511 // A shortest path algorithm from memory (ie. I can't find the bl&*dy book again!)
512 // I'm sure there must be enchancements that we can make to this, such as biasing the
513 // order in which the nodes are searched out from in favour of those geographically
514 // closer to the destination.
515 // Note that we are working with the master set of nodes and arcs so we mustn't change
516 // or delete them -  we only delete the paths that we create during the algorithm.
517 ground_network_path_type FGGround::GetShortestPath(node* A, node* B) {
518         a_path* pathPtr;
519         shortest_path_map_type pathMap;
520         node_array_type nodesLeft;
521         
522         // Debugging check
523         int pathsCreated = 0;
524         
525         // Initialise the algorithm
526         nodesLeft.push_back(A);
527         pathPtr = new a_path;
528         pathsCreated++;
529         pathPtr->path.push_back(A);
530         pathPtr->cost = 0;
531         pathMap[A->nodeID] = pathPtr;
532         bool solution_found = false;    // Flag to indicate that at least one candidate path has been found
533         int solution_cost = -1;                 // Cost of current best cost solution.  -1 indicates no solution found yet.
534         a_path solution_path;           
535                                                                                         
536         node* nPtr;     // nPtr is used to point to the node we are currently working with
537         
538         while(nodesLeft.size()) {
539                 //cout << "\n*****nodesLeft*****\n";
540                 //for(unsigned int i=0; i<nodesLeft.size(); ++i) {
541                         //cout << nodesLeft[i]->nodeID << '\n';
542                 //}
543                 //cout << "*******************\n\n";
544                 nPtr = *nodesLeft.begin();              // Thought - definate optimization possibilities here in the choice of which nodes we process first.
545                 nodesLeft.erase(nodesLeft.begin());
546                 //cout << "Walking out from node " << nPtr->nodeID << '\n';
547                 for(unsigned int i=0; i<nPtr->arcs.size(); ++i) {
548                         //cout << "ARC TO " << ((nPtr->arcs[i]->n1 == nPtr->nodeID) ? nPtr->arcs[i]->n2 : nPtr->arcs[i]->n1) << '\n';
549                 }
550                 if((solution_found) && (solution_cost <= pathMap[nPtr->nodeID]->cost)) {
551                         // Do nothing - we've already found a solution and this partial path is already more expensive
552                 } else {
553                         // This path could still be better than the current solution - check it out
554                         for(unsigned int i=0; i<(nPtr->arcs.size()); i++) {
555                                 // Map the new path against the end node, ie. *not* the one we just started with.
556                                 unsigned int end_nodeID = ((nPtr->arcs[i]->n1 == nPtr->nodeID) ? nPtr->arcs[i]->n2 : nPtr->arcs[i]->n1);
557                                 //cout << "end_nodeID = " << end_nodeID << '\n';
558                                 //cout << "pathMap size is " << pathMap.size() << '\n';
559                                 if(end_nodeID == nPtr->nodeID) {
560                                         //cout << "Circular arc!\n";
561                                         // Then its a circular arc - don't bother!!
562                                         //nPtr->arcs.erase(nPtr->arcs.begin() + i);
563                                 } else {
564                                         // see if the end node is already in the map or not
565                                         if(pathMap.find(end_nodeID) == pathMap.end()) {
566                                                 //cout << "Not in the map" << endl;;
567                                                 // Not in the map - easy!
568                                                 pathPtr = new a_path;
569                                                 pathsCreated++;
570                                                 *pathPtr = *pathMap[nPtr->nodeID];      // *copy* the path
571                                                 pathPtr->path.push_back(nPtr->arcs[i]);
572                                                 pathPtr->path.push_back(network[end_nodeID]);
573                                                 pathPtr->cost += nPtr->arcs[i]->distance;
574                                                 pathMap[end_nodeID] = pathPtr;
575                                                 nodesLeft.push_back(network[end_nodeID]);       // By definition this can't be in the list already, or
576                                                 // it would also have been in the map and hence OR'd with this one.
577                                                 if(end_nodeID == B->nodeID) {
578                                                         //cout << "Solution found!!!" << endl;
579                                                         // Since this node wasn't in the map this is by definition the first solution
580                                                         solution_cost = pathPtr->cost;
581                                                         solution_path = *pathPtr;
582                                                         solution_found = true;
583                                                 }
584                                         } else {
585                                                 //cout << "Already in the map" << endl;
586                                                 // In the map - not so easy - need to get rid of an arc from the higher cost one.
587                                                 //cout << "Current cost of node " << end_nodeID << " is " << pathMap[end_nodeID]->cost << endl;
588                                                 int newCost = pathMap[nPtr->nodeID]->cost + nPtr->arcs[i]->distance;
589                                                 //cout << "New cost is of node " << nPtr->nodeID << " is " << newCost << endl;
590                                                 if(newCost >= pathMap[end_nodeID]->cost) {
591                                                         // No need to do anything.
592                                                         //cout << "Not doing anything!" << endl;
593                                                 } else {
594                                                         delete pathMap[end_nodeID];
595                                                         pathsCreated--;
596                                                         
597                                                         pathPtr = new a_path;
598                                                         pathsCreated++;
599                                                         *pathPtr = *pathMap[nPtr->nodeID];      // *copy* the path
600                                                         pathPtr->path.push_back(nPtr->arcs[i]);
601                                                         pathPtr->path.push_back(network[end_nodeID]);
602                                                         pathPtr->cost += nPtr->arcs[i]->distance;
603                                                         pathMap[end_nodeID] = pathPtr;
604                                                         
605                                                         // We need to add this node to the list-to-do again to force a recalculation 
606                                                         // onwards from this node with the new lower cost to node cost.
607                                                         nodesLeft.push_back(network[end_nodeID]);
608                                                         
609                                                         if(end_nodeID == B->nodeID) {
610                                                                 //cout << "Solution found!!!" << endl;
611                                                                 // Need to check if there is a previous better solution
612                                                                 if((solution_cost < 0) || (pathPtr->cost < solution_cost)) {
613                                                                         solution_cost = pathPtr->cost;
614                                                                         solution_path = *pathPtr;
615                                                                         solution_found = true;
616                                                                 }
617                                                         }
618                                                 }
619                                         }
620                                 }
621                         }
622                 }
623         }
624         
625         // delete all the paths before returning
626         shortest_path_map_iterator spItr = pathMap.begin();
627         while(spItr != pathMap.end()) {
628                 if(spItr->second != NULL) {
629                         delete spItr->second;
630                         --pathsCreated;
631                 }
632                 ++spItr;
633         }
634         
635         //cout << "pathsCreated = " << pathsCreated << '\n';
636         if(pathsCreated > 0) {
637                 SG_LOG(SG_ATC, SG_ALERT, "WARNING - Possible memory leak in FGGround::GetShortestPath\n\
638                                                                           Please report to flightgear-devel@flightgear.org\n");
639         }
640         
641         //cout << (solution_found ? "Result: solution found\n" : "Result: no solution found\n");
642         return(solution_path.path);             // TODO - we really ought to have a fallback position incase a solution isn't found.
643 }
644                 
645
646
647 // Randomly or otherwise populate some of the gates with parked planes
648 // (might eventually be done by the AIMgr if and when lots of AI traffic is generated)
649
650 // Return a list of exits from a given runway
651 // It is up to the calling function to check for non-zero size of returned array before use
652 node_array_type FGGround::GetExits(const string& rwyID) {
653         // FIXME - get a 07L or similar in here and we're stuffed!!!
654         return(runways[atoi(rwyID.c_str())].exits);
655 }
656
657 void FGGround::RequestDeparture(const PlaneRec& plane, FGAIEntity* requestee) {
658         // For now we'll just automatically clear all planes to the runway hold.
659         // This communication needs to be delayed 20 sec or so from receiving the request.
660         // Even if display=false we still need to start the timer in case display=true when communication starts.
661         // We also need to bear in mind we also might have other outstanding communications, although for now we'll punt that issue!
662         // FIXME - sort the above!
663         
664         // HACK - assume that anything requesting departure is new for now - FIXME LATER
665         GroundRec* g = new GroundRec;
666         g->plane = plane;
667         g->planePtr = requestee;
668         g->taxiRequestOutstanding = true;
669         g->clearanceCounter = 0;
670         g->cleared = false;
671         g->incoming = false;
672         // TODO - need to handle the next 3 as well
673     //node* destination;
674     //node* last_clearance;
675         
676         ground_traffic.push_back(g);
677 }
678
679 #if 0
680 void FGGround::NewArrival(plane_rec plane) {
681         // What are we going to do here?
682         // We need to start a new ground_rec and add the plane_rec to it
683         // We need to decide what gate we are going to clear it to.
684         // Then we need to add clearing it to that gate to the pending transmissions queue? - or simply transmit?
685         // Probably simply transmit for now and think about a transmission queue later if we need one.
686         // We might need one though in order to add a little delay for response time.
687         ground_rec* g = new ground_rec;
688         g->plane_rec = plane;
689         g->current_pos = ConvertWGS84ToXY(plane.pos);
690         g->node = GetNode(g->current_pos);  // TODO - might need to sort out node/arc here
691         AssignGate(g);
692         g->cleared = false;
693         ground_traffic.push_back(g);
694         NextClearance(g);
695 }
696
697 void FGGround::NewContact(plane_rec plane) {
698         // This is a bit of a convienience function at the moment and is likely to change.
699         if(at a gate or apron)
700                 NewDeparture(plane);
701         else
702                 NewArrival(plane);
703 }
704
705 void FGGround::NextClearance(ground_rec &g) {
706         // Need to work out where we can clear g to.
707         // Assume the pilot doesn't need progressive instructions
708         // We *should* already have a gate or holding point assigned by the time we get here
709         // but it wouldn't do any harm to check.
710         
711         // For now though we will hardwire it to clear to the final destination.
712 }
713
714 void FGGround::AssignGate(ground_rec &g) {
715         // We'll cheat for now - since we only have the user's aircraft and a couple of airports implemented
716         // we'll hardwire the gate!
717         // In the long run the logic of which gate or area to send the plane to could be somewhat non-trivial.
718 }
719 #endif //0
720