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