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