src/ATC/tower.cxx

   1 // FGTower - a class to provide tower control at towered 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 <Main/globals.hxx>
  22 #include <Airports/runways.hxx>
  23 #include <simgear/math/sg_geodesy.hxx>
  24 #include <simgear/debug/logstream.hxx>
  25
  26 #include "tower.hxx"
  27 #include "ATCdisplay.hxx"
  28 #include "ATCmgr.hxx"
  29 #include "ATCutils.hxx"
  30 #include "commlist.hxx"
  31 #include "AILocalTraffic.hxx"
  32
  33 SG_USING_STD(cout);
  34
  35 // TowerPlaneRec
  36
  37 TowerPlaneRec::TowerPlaneRec() :
  38 clearedToLand(false),
  39 clearedToLineUp(false),
  40 clearedToTakeOff(false),
  41 holdShortReported(false),
  42 downwindReported(false),
  43 longFinalReported(false),
  44 longFinalAcknowledged(false),
  45 finalReported(false),
  46 finalAcknowledged(false),
  47 instructedToGoAround(false),
  48 onRwy(false),
  49 nextOnRwy(false),
  50 opType(TTT_UNKNOWN),
  51 leg(LEG_UNKNOWN),
  52 landingType(AIP_LT_UNKNOWN),
  53 isUser(false)
  54 {
  55         plane.callsign = "UNKNOWN";
  56 }
  57
  58 TowerPlaneRec::TowerPlaneRec(PlaneRec p) :
  59 clearedToLand(false),
  60 clearedToLineUp(false),
  61 clearedToTakeOff(false),
  62 holdShortReported(false),
  63 downwindReported(false),
  64 longFinalReported(false),
  65 longFinalAcknowledged(false),
  66 finalReported(false),
  67 finalAcknowledged(false),
  68 instructedToGoAround(false),
  69 onRwy(false),
  70 nextOnRwy(false),
  71 opType(TTT_UNKNOWN),
  72 leg(LEG_UNKNOWN),
  73 landingType(AIP_LT_UNKNOWN),
  74 isUser(false)
  75 {
  76         plane = p;
  77 }
  78
  79 TowerPlaneRec::TowerPlaneRec(Point3D pt) :
  80 clearedToLand(false),
  81 clearedToLineUp(false),
  82 clearedToTakeOff(false),
  83 holdShortReported(false),
  84 downwindReported(false),
  85 longFinalReported(false),
  86 longFinalAcknowledged(false),
  87 finalReported(false),
  88 finalAcknowledged(false),
  89 instructedToGoAround(false),
  90 onRwy(false),
  91 nextOnRwy(false),
  92 opType(TTT_UNKNOWN),
  93 leg(LEG_UNKNOWN),
  94 landingType(AIP_LT_UNKNOWN),
  95 isUser(false)
  96 {
  97         plane.callsign = "UNKNOWN";
  98         pos = pt;
  99 }
 100
 101 TowerPlaneRec::TowerPlaneRec(PlaneRec p, Point3D pt) :
 102 clearedToLand(false),
 103 clearedToLineUp(false),
 104 clearedToTakeOff(false),
 105 holdShortReported(false),
 106 downwindReported(false),
 107 longFinalReported(false),
 108 longFinalAcknowledged(false),
 109 finalReported(false),
 110 finalAcknowledged(false),
 111 instructedToGoAround(false),
 112 onRwy(false),
 113 nextOnRwy(false),
 114 opType(TTT_UNKNOWN),
 115 leg(LEG_UNKNOWN),
 116 landingType(AIP_LT_UNKNOWN),
 117 isUser(false)
 118 {
 119         plane = p;
 120         pos = pt;
 121 }
 122
 123
 124 // FGTower
 125
 126 /*******************************************
 127                TODO List
 128
 129 Currently user is assumed to have taken off again when leaving the runway - check speed/elev for taxiing-in.
 130
 131 AI plane lands even when user on rwy - make it go-around instead.
 132
 133 Tell AI plane to contact ground when taxiing in.
 134
 135 Use track instead of heading to determine what leg of the circuit the user is flying.
 136
 137 Use altitude as well as position to try to determine if the user has left the circuit.
 138
 139 Currently HoldShortReported code assumes there will be only one plane holding for the runway at once and
 140 will break when planes start queueing.
 141
 142 Implement ReportRunwayVacated
 143 *******************************************/
 144
 145 FGTower::FGTower() {
 146         ATCmgr = globals->get_ATC_mgr();
 147
 148         // Init the property nodes - TODO - need to make sure we're getting surface winds.
 149         wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
 150         wind_speed_knots = fgGetNode("/environment/wind-speed-kt", true);
 151
 152         update_count = 0;
 153         update_count_max = 15;
 154
 155         holdListItr = holdList.begin();
 156         appListItr = appList.begin();
 157         depListItr = depList.begin();
 158         rwyListItr = rwyList.begin();
 159         circuitListItr = circuitList.begin();
 160         trafficListItr = trafficList.begin();
 161
 162         freqClear = true;
 163
 164         timeSinceLastDeparture = 9999;
 165         departed = false;
 166
 167         nominal_downwind_leg_pos = 1000.0;
 168         nominal_base_leg_pos = -1000.0;
 169         // TODO - set nominal crosswind leg pos based on minimum distance from takeoff end of rwy.
 170 }
 171
 172 FGTower::~FGTower() {
 173         if(!separateGround) {
 174                 delete ground;
 175         }
 176 }
 177
 178 void FGTower::Init() {
 179     display = false;
 180
 181         // Pointers to user's position
 182         user_lon_node = fgGetNode("/position/longitude-deg", true);
 183         user_lat_node = fgGetNode("/position/latitude-deg", true);
 184         user_elev_node = fgGetNode("/position/altitude-ft", true);
 185         user_hdg_node = fgGetNode("/orientation/heading-deg", true);
 186
 187         // Need some way to initialise rwyOccupied flag correctly if the user is on the runway and to know its the user.
 188         // I'll punt the startup issue for now though!!!
 189         rwyOccupied = false;
 190
 191         // Setup the ground control at this airport
 192         AirportATC a;
 193         //cout << "Tower ident = " << ident << '\n';
 194         if(ATCmgr->GetAirportATCDetails(ident, &a)) {
 195                 if(a.ground_freq) {             // Ground control
 196                         ground = (FGGround*)ATCmgr->GetATCPointer(ident, GROUND);
 197                         separateGround = true;
 198                         if(ground == NULL) {
 199                                 // Something has gone wrong :-(
 200                                 SG_LOG(SG_ATC, SG_WARN, "ERROR - ground has frequency but can't get ground pointer :-(");
 201                                 ground = new FGGround(ident);
 202                                 separateGround = false;
 203                                 ground->Init();
 204                                 if(display) {
 205                                         ground->SetDisplay();
 206                                 } else {
 207                                         ground->SetNoDisplay();
 208                                 }
 209                         }
 210                 } else {
 211                         // Initialise ground anyway to do the shortest path stuff!
 212                         // Note that we're now responsible for updating and deleting this - NOT the ATCMgr.
 213                         ground = new FGGround(ident);
 214                         separateGround = false;
 215                         ground->Init();
 216                         if(display) {
 217                                 ground->SetDisplay();
 218                         } else {
 219                                 ground->SetNoDisplay();
 220                         }
 221                 }
 222         } else {
 223                 SG_LOG(SG_ATC, SG_ALERT, "Unable to find airport details for " << ident << " in FGTower::Init()");
 224                 // Initialise ground anyway to avoid segfault later
 225                 ground = new FGGround(ident);
 226                 separateGround = false;
 227                 ground->Init();
 228                 if(display) {
 229                         ground->SetDisplay();
 230                 } else {
 231                         ground->SetNoDisplay();
 232                 }
 233         }
 234
 235         // Get the airport elevation
 236         aptElev = dclGetAirportElev(ident.c_str()) * SG_FEET_TO_METER;
 237
 238         DoRwyDetails();
 239
 240         // FIXME - this currently assumes use of the active rwy by the user.
 241         rwyOccupied = OnAnyRunway(Point3D(user_lon_node->getDoubleValue(), user_lat_node->getDoubleValue(), 0.0));
 242         if(rwyOccupied) {
 243                 // Assume the user is started at the threshold ready to take-off
 244                 TowerPlaneRec* t = new TowerPlaneRec;
 245                 t->plane.callsign = "Charlie Foxtrot Sierra";   // C-FGFS !!! - fixme - this is a bit hardwired
 246                 t->plane.type = GA_SINGLE;
 247                 t->opType = TTT_UNKNOWN;        // We don't know if the user wants to do circuits or a departure...
 248                 t->landingType = AIP_LT_UNKNOWN;
 249                 t->leg = TAKEOFF_ROLL;
 250                 t->isUser = true;
 251                 t->planePtr = NULL;
 252                 t->clearedToTakeOff = true;
 253                 rwyList.push_back(t);
 254                 departed = false;
 255         }
 256 }
 257
 258 void FGTower::Update(double dt) {
 259         //cout << "T" << endl;
 260         // Each time step, what do we need to do?
 261         // We need to go through the list of outstanding requests and acknowedgements
 262         // and process at least one of them.
 263         // We need to go through the list of planes under our control and check if
 264         // any need to be addressed.
 265         // We need to check for planes not under our control coming within our
 266         // control area and address if necessary.
 267
 268         // TODO - a lot of the below probably doesn't need to be called every frame and should be staggered.
 269
 270         // Sort the arriving planes
 271
 272         /*
 273         if(ident == "KEMT") {
 274                 cout << update_count << "\ttL: " << trafficList.size() << "  cL: " << circuitList.size() << "  hL: " << holdList.size() << "  aL: " << appList.size() << '\n';
 275         }
 276         */
 277
 278         if(departed != false) {
 279                 timeSinceLastDeparture += dt;
 280                 //if(ident == "KEMT")
 281                 //      cout << "  dt = " << dt << "  timeSinceLastDeparture = " << timeSinceLastDeparture << '\n';
 282         }
 283
 284         //cout << ident << " respond = " << respond << " responseReqd = " << responseReqd << '\n';
 285         if(respond) {
 286                 if(!responseReqd) SG_LOG(SG_ATC, SG_ALERT, "ERROR - respond is true and responseReqd is false in FGTower::Update(...)");
 287                 Respond();
 288                 respond = false;
 289                 responseReqd = false;
 290         }
 291
 292         // Calculate the eta of each plane to the threshold.
 293         // For ground traffic this is the fastest they can get there.
 294         // For air traffic this is the middle approximation.
 295         if(update_count == 1) {
 296                 doThresholdETACalc();
 297         }
 298
 299         // Order the list of traffic as per expected threshold use and flag any conflicts
 300         if(update_count == 2) {
 301                 //bool conflicts = doThresholdUseOrder();
 302                 doThresholdUseOrder();
 303         }
 304
 305         // sortConficts() !!!
 306
 307         if(update_count == 4) {
 308                 CheckHoldList(dt);
 309         }
 310
 311         // Uggh - HACK - why have we got rwyOccupied - wouldn't simply testing rwyList.size() do?
 312         if(rwyList.size()) {
 313                 rwyOccupied = true;
 314         } else {
 315                 rwyOccupied = false;
 316         }
 317
 318         if(update_count == 5 && rwyOccupied) {
 319                 CheckRunwayList(dt);
 320         }
 321
 322         if(update_count == 6) {
 323                 CheckCircuitList(dt);
 324         }
 325
 326         if(update_count == 7) {
 327                 CheckApproachList(dt);
 328         }
 329
 330         // TODO - do one plane from the departure list and set departed = false when out of consideration
 331
 332         //doCommunication();
 333
 334         if(!separateGround) {
 335                 // The display stuff might have to get more clever than this when not separate
 336                 // since the tower and ground might try communicating simultaneously even though
 337                 // they're mean't to be the same contoller/frequency!!
 338                 if(display) {
 339                         ground->SetDisplay();
 340                 } else {
 341                         ground->SetNoDisplay();
 342                 }
 343                 ground->Update(dt);
 344         }
 345
 346         ++update_count;
 347         // How big should ii get - ie how long should the update cycle interval stretch?
 348         if(update_count >= update_count_max) {
 349                 update_count = 0;
 350         }
 351
 352         // Call the base class update for the response time handling.
 353         FGATC::Update(dt);
 354
 355         if(ident == "KEMT") {
 356                 // For AI debugging convienience - may be removed
 357                 Point3D user_pos;
 358                 user_pos.setlon(user_lon_node->getDoubleValue());
 359                 user_pos.setlat(user_lat_node->getDoubleValue());
 360                 user_pos.setelev(user_elev_node->getDoubleValue());
 361                 Point3D user_ortho_pos = ortho.ConvertToLocal(user_pos);
 362                 fgSetDouble("/AI/user/ortho-x", user_ortho_pos.x());
 363                 fgSetDouble("/AI/user/ortho-y", user_ortho_pos.y());
 364                 fgSetDouble("/AI/user/elev", user_elev_node->getDoubleValue());
 365         }
 366
 367         //cout << "Done T" << endl;
 368 }
 369
 370 void FGTower::ReceiveUserCallback(int code) {
 371         if(code == (int)USER_REQUEST_DEPARTURE) {
 372                 cout << "User requested departure\n";
 373         }
 374 }
 375
 376 void FGTower::Respond() {
 377         //cout << "Entering Respond..." << endl;
 378         TowerPlaneRec* t = FindPlane(responseID);
 379         if(t) {
 380                 // This will grow!!!
 381                 if(t->downwindReported) {
 382                         t->downwindReported = false;
 383                         int i = 1;
 384                         for(tower_plane_rec_list_iterator twrItr = circuitList.begin(); twrItr != circuitList.end(); twrItr++) {
 385                                 if((*twrItr)->plane.callsign == responseID) break;
 386                                 ++i;
 387                         }
 388                         string trns = "Number ";
 389                         trns += ConvertNumToSpokenDigits(i);
 390                         trns += " ";
 391                         trns += t->plane.callsign;
 392                         if(display) {
 393                                 globals->get_ATC_display()->RegisterSingleMessage(trns, 0);
 394                         }
 395                         if(t->isUser && t->opType == TTT_UNKNOWN) {
 396                                 t->opType = CIRCUIT;
 397                         }
 398                 } else if(t->holdShortReported) {
 399                         if(t->nextOnRwy) {
 400                                 if(rwyOccupied) {       // TODO - ought to add a sanity check that it isn't this plane only on the runway (even though it shouldn't be!!)
 401                                         // Do nothing for now - consider acknowloging hold short eventually
 402                                 } else {
 403                                         ClearHoldingPlane(t);
 404                                         t->leg = TAKEOFF_ROLL;
 405                                         rwyList.push_back(t);
 406                                         rwyOccupied = true;
 407                                         // WARNING - WE ARE ASSUMING ONLY ONE PLANE REPORTING HOLD AT A TIME BELOW
 408                                         // FIXME TODO - FIX THIS!!!
 409                                         if(holdList.size()) {
 410                                                 if(holdListItr == holdList.end()) {
 411                                                         holdListItr = holdList.begin();
 412                                                 }
 413                                                 holdList.erase(holdListItr);
 414                                                 holdListItr = holdList.begin();
 415                                         }
 416                                 }
 417                         } else {
 418                                 // Tell him to hold and what position he is.
 419                                 // Not currently sure under which circumstances we do or don't bother transmitting this.
 420                                 string trns = t->plane.callsign;
 421                                 trns += " hold position";
 422                                 if(display) {
 423                                         globals->get_ATC_display()->RegisterSingleMessage(trns, 0);
 424                                 }
 425                                 // TODO - add some idea of what traffic is blocking him.
 426                         }
 427                         t->holdShortReported = false;
 428                 } else if(t->finalReported && !(t->finalAcknowledged)) {
 429                         bool disp = true;
 430                         string trns = t->plane.callsign;
 431                         cout << (t->nextOnRwy ? "Next on rwy " : "Not next!! ");
 432                         cout << (rwyOccupied ? "RWY OCCUPIED!!\n" : "Rwy not occupied\n");
 433                         if(t->nextOnRwy && !rwyOccupied) {
 434                                 if(t->landingType == FULL_STOP) {
 435                                         trns += " cleared to land ";
 436                                 } else {
 437                                         trns += " cleared for the option ";
 438                                 }
 439                                 // TODO - add winds
 440                                 t->clearedToLand = true;
 441                         } else if(t->eta < 20) {
 442                                 // Do nothing - we'll be telling it to go around in less than 10 seconds if the
 443                                 // runway doesn't clear so no point in calling "continue approach".
 444                                 disp = false;
 445                         } else {
 446                                 trns += " continue approach";
 447                                 t->clearedToLand = false;
 448                         }
 449                         if(display && disp) {
 450                                 globals->get_ATC_display()->RegisterSingleMessage(trns, 0);
 451                         }
 452                         t->finalAcknowledged = true;
 453                 }
 454         }
 455         freqClear = true;       // FIXME - set this to come true after enough time to render the message
 456         //cout << "Done Respond" << endl;
 457 }
 458
 459 // Currently this assumes we *are* next on the runway and doesn't check for planes about to land -
 460 // this should be done prior to calling this function.
 461 void FGTower::ClearHoldingPlane(TowerPlaneRec* t) {
 462         //cout << "Entering ClearHoldingPlane..." << endl;
 463         // Lets Roll !!!!
 464         string trns = t->plane.callsign;
 465         //if(departed plane < some threshold in time away) {
 466         if(0) {         // FIXME
 467         //if(timeSinceLastDeparture <= 60.0 && departed == true) {
 468                 trns += " line up";
 469                 t->clearedToLineUp = true;
 470                 t->planePtr->RegisterTransmission(3);   // cleared to line-up
 471         //} else if(arriving plane < some threshold away) {
 472         } else if(GetTrafficETA(2) < 150.0 && (timeSinceLastDeparture > 60.0 || departed == false)) {   // Hack - hardwired time
 473                 trns += " cleared immediate take-off";
 474                 if(trafficList.size()) {
 475                         tower_plane_rec_list_iterator trfcItr = trafficList.begin();
 476                         trfcItr++;      // At the moment the holding plane should be first in trafficList.
 477                         // Note though that this will break if holding planes aren't put in trafficList in the future.
 478                         TowerPlaneRec* trfc = *trfcItr;
 479                         trns += "... traffic is";
 480                         switch(trfc->plane.type) {
 481                         case UNKNOWN:
 482                                 break;
 483                         case GA_SINGLE:
 484                                 trns += " a Cessna";    // TODO - add ability to specify actual plane type somewhere
 485                                 break;
 486                         case GA_HP_SINGLE:
 487                                 trns += " a Piper";
 488                                 break;
 489                         case GA_TWIN:
 490                                 trns += " a King-air";
 491                                 break;
 492                         case GA_JET:
 493                                 trns += " a Learjet";
 494                                 break;
 495                         case MEDIUM:
 496                                 trns += " a Regional";
 497                                 break;
 498                         case HEAVY:
 499                                 trns += " a Heavy";
 500                                 break;
 501                         case MIL_JET:
 502                                 trns += " Military";
 503                                 break;
 504                         }
 505                         //if(trfc->opType == STRAIGHT_IN || trfc->opType == TTT_UNKNOWN) {
 506                         if(trfc->opType == STRAIGHT_IN) {
 507                                 double miles_out = CalcDistOutMiles(trfc);
 508                                 if(miles_out < 2) {
 509                                         trns += " on final";
 510                                 } else {
 511                                         trns += " on ";
 512                                         trns += ConvertNumToSpokenDigits((int)miles_out);
 513                                         trns += " mile final";
 514                                 }
 515                         } else if(trfc->opType == CIRCUIT) {
 516                                 //cout << "Getting leg of " << trfc->plane.callsign << '\n';
 517                                 switch(trfc->leg) {
 518                                 case FINAL:
 519                                         trns += " on final";
 520                                         break;
 521                                 case TURN4:
 522                                         trns += " turning final";
 523                                         break;
 524                                 case BASE:
 525                                         trns += " on base";
 526                                         break;
 527                                 case TURN3:
 528                                         trns += " turning base";
 529                                         break;
 530                                 case DOWNWIND:
 531                                         trns += " in circuit";  // At the moment the user plane is generally flagged as unknown opType when downwind incase its a downwind departure which means we won't get here.
 532                                         break;
 533                                 // And to eliminate compiler warnings...
 534                                 case TAKEOFF_ROLL: break;
 535                                 case CLIMBOUT:     break;
 536                                 case TURN1:        break;
 537                                 case CROSSWIND:    break;
 538                                 case TURN2:        break;
 539                                 case LANDING_ROLL: break;
 540                                 case LEG_UNKNOWN:  break;
 541                                 }
 542                         }
 543                 } else {
 544                         // By definition there should be some arriving traffic if we're cleared for immediate takeoff
 545                         SG_LOG(SG_ATC, SG_WARN, "Warning: Departing traffic cleared for *immediate* take-off despite no arriving traffic in FGTower");
 546                 }
 547                 t->clearedToTakeOff = true;
 548                 t->planePtr->RegisterTransmission(4);   // cleared to take-off - TODO differentiate between immediate and normal take-off
 549                 departed = false;
 550                 timeSinceLastDeparture = 0.0;
 551         } else {
 552         //} else if(timeSinceLastDeparture > 60.0 || departed == false) {       // Hack - test for timeSinceLastDeparture should be in lineup block eventually
 553                 trns += " cleared for take-off";
 554                 // TODO - add traffic is... ?
 555                 t->clearedToTakeOff = true;
 556                 t->planePtr->RegisterTransmission(4);   // cleared to take-off
 557                 departed = false;
 558                 timeSinceLastDeparture = 0.0;
 559         }
 560         if(display) {
 561                 globals->get_ATC_display()->RegisterSingleMessage(trns, 0);
 562         }
 563         //cout << "Done ClearHoldingPlane " << endl;
 564 }
 565
 566 // Do one plane from the hold list
 567 void FGTower::CheckHoldList(double dt) {
 568         //cout << "Entering CheckHoldList..." << endl;
 569         if(holdList.size()) {
 570                 //cout << "*holdListItr = " << *holdListItr << endl;
 571                 if(holdListItr == holdList.end()) {
 572                         holdListItr = holdList.begin();
 573                 }
 574                 //cout << "*holdListItr = " << *holdListItr << endl;
 575                 //Process(*holdListItr);
 576                 TowerPlaneRec* t = *holdListItr;
 577                 //cout << "t = " << t << endl;
 578                 if(t->holdShortReported) {
 579                         // NO-OP - leave it to the response handler.
 580                 } else {        // not responding to report, but still need to clear if clear
 581                         if(t->nextOnRwy) {
 582                                 //cout << "departed = " << departed << '\n';
 583                                 //cout << "timeSinceLastDeparture = " << timeSinceLastDeparture << '\n';
 584                                 if(rwyOccupied) {
 585                                         // Do nothing
 586                                 } else if(timeSinceLastDeparture <= 60.0 && departed == true) {
 587                                         // Do nothing - this is a bit of a hack - should maybe do line up be ready here
 588                                 } else {
 589                                         ClearHoldingPlane(t);
 590                                         t->leg = TAKEOFF_ROLL;
 591                                         rwyList.push_back(t);
 592                                         rwyOccupied = true;
 593                                         holdList.erase(holdListItr);
 594                                         holdListItr = holdList.begin();
 595                                 }
 596                         }
 597                         // TODO - rationalise the considerable code duplication above!
 598                 }
 599                 ++holdListItr;
 600         }
 601         //cout << "Done CheckHoldList" << endl;
 602 }
 603
 604 // do the ciruit list
 605 void FGTower::CheckCircuitList(double dt) {
 606         //cout << "Entering CheckCircuitList..." << endl;
 607         // Clear the constraints - we recalculate here.
 608         base_leg_pos = 0.0;
 609         downwind_leg_pos = 0.0;
 610         crosswind_leg_pos = 0.0;
 611
 612         if(circuitList.size()) {        // Do one plane from the circuit
 613                 if(circuitListItr == circuitList.end()) {
 614                         circuitListItr = circuitList.begin();
 615                 }
 616                 TowerPlaneRec* t = *circuitListItr;
 617                 if(t->isUser) {
 618                         t->pos.setlon(user_lon_node->getDoubleValue());
 619                         t->pos.setlat(user_lat_node->getDoubleValue());
 620                         t->pos.setelev(user_elev_node->getDoubleValue());
 621                 } else {
 622                         t->pos = t->planePtr->GetPos();         // We should probably only set the pos's on one walk through the traffic list in the update function, to save a few CPU should we end up duplicating this.
 623                         t->landingType = t->planePtr->GetLandingOption();
 624                         //cout << "AI plane landing option is " << t->landingType << '\n';
 625                 }
 626                 Point3D tortho = ortho.ConvertToLocal(t->pos);
 627                 if(t->isUser) {
 628                         // Need to figure out which leg he's on
 629                         //cout << "rwy.hdg = " << rwy.hdg << " user hdg = " << user_hdg_node->getDoubleValue();
 630                         double ho = GetAngleDiff_deg(user_hdg_node->getDoubleValue(), rwy.hdg);
 631                         //cout << " ho = " << ho << " abs(ho = " << abs(ho) << '\n';
 632                         // TODO FIXME - get the wind and convert this to track, or otherwise use track somehow!!!
 633                         // If it's gusty might need to filter the value, although we are leaving 30 degrees each way leeway!
 634                         if(abs(ho) < 30) {
 635                                 // could be either takeoff, climbout or landing - check orthopos.y
 636                                 //cout << "tortho.y = " << tortho.y() << '\n';
 637                                 if((tortho.y() < 0) || (t->leg == TURN4) || (t->leg == FINAL)) {
 638                                         t->leg = FINAL;
 639                                         //cout << "Final\n";
 640                                 } else {
 641                                         t->leg = CLIMBOUT;      // TODO - check elev wrt. apt elev to differentiate takeoff roll and climbout
 642                                         //cout << "Climbout\n";
 643                                         // If it's the user we may be unsure of his/her intentions.
 644                                         // (Hopefully the AI planes won't try confusing the sim!!!)
 645                                         if(t->opType == TTT_UNKNOWN) {
 646                                                 if(tortho.y() > 5000) {
 647                                                         // 5 km out from threshold - assume it's a departure
 648                                                         t->opType = OUTBOUND;   // TODO - could check if the user has climbed significantly above circuit altitude as well.
 649                                                         // Since we are unknown operation we should be in depList already.
 650                                                         circuitList.erase(circuitListItr);
 651                                                         RemoveFromTrafficList(t->plane.callsign);
 652                                                         circuitListItr = circuitList.begin();
 653                                                 }
 654                                         } else if(t->opType == CIRCUIT) {
 655                                                 if(tortho.y() > 10000) {
 656                                                         // 10 km out - assume the user has abandoned the circuit!!
 657                                                         t->opType = OUTBOUND;
 658                                                         depList.push_back(t);
 659                                                         circuitList.erase(circuitListItr);
 660                                                         circuitListItr = circuitList.begin();
 661                                                 }
 662                                         }
 663                                 }
 664                         } else if(abs(ho) < 60) {
 665                                 // turn1 or turn 4
 666                                 // TODO - either fix or doublecheck this hack by looking at heading and pattern direction
 667                                 if((t->leg == CLIMBOUT) || (t->leg == TURN1)) {
 668                                         t->leg = TURN1;
 669                                         //cout << "Turn1\n";
 670                                 } else {
 671                                         t->leg = TURN4;
 672                                         //cout << "Turn4\n";
 673                                 }
 674                         } else if(abs(ho) < 120) {
 675                                 // crosswind or base
 676                                 // TODO - either fix or doublecheck this hack by looking at heading and pattern direction
 677                                 if((t->leg == TURN1) || (t->leg == CROSSWIND)) {
 678                                         t->leg = CROSSWIND;
 679                                         //cout << "Crosswind\n";
 680                                 } else {
 681                                         t->leg = BASE;
 682                                         //cout << "Base\n";
 683                                 }
 684                         } else if(abs(ho) < 150) {
 685                                 // turn2 or turn 3
 686                                 // TODO - either fix or doublecheck this hack by looking at heading and pattern direction
 687                                 if((t->leg == CROSSWIND) || (t->leg == TURN2)) {
 688                                         t->leg = TURN2;
 689                                         //cout << "Turn2\n";
 690                                 } else {
 691                                         t->leg = TURN3;
 692                                         // Probably safe now to assume the user is flying a circuit
 693                                         t->opType = CIRCUIT;
 694                                         //cout << "Turn3\n";
 695                                 }
 696                         } else {
 697                                 // downwind
 698                                 t->leg = DOWNWIND;
 699                                 //cout << "Downwind\n";
 700                         }
 701                         if(t->leg == FINAL) {
 702                                 if(OnActiveRunway(t->pos)) {
 703                                         t->leg = LANDING_ROLL;
 704                                 }
 705                         }
 706                 } else {
 707                         t->leg = t->planePtr->GetLeg();
 708                 }
 709
 710                 // Set the constraints IF this is the first plane in the circuit
 711                 // TODO - at the moment we're constraining plane 2 based on plane 1 - this won't (or might not) work for 3 planes in the circuit!!
 712                 if(circuitListItr == circuitList.begin()) {
 713                         switch(t->leg) {
 714                         case FINAL:
 715                                 // Base leg must be at least as far out as the plane is - actually possibly not necessary for separation, but we'll use that for now.
 716                                 base_leg_pos = tortho.y();
 717                                 //cout << "base_leg_pos = " << base_leg_pos << '\n';
 718                                 break;
 719                         case TURN4:
 720                                 // Fall through to base
 721                         case BASE:
 722                                 base_leg_pos = tortho.y();
 723                                 //cout << "base_leg_pos = " << base_leg_pos << '\n';
 724                                 break;
 725                         case TURN3:
 726                                 // Fall through to downwind
 727                         case DOWNWIND:
 728                                 // Only have the downwind leg pos as turn-to-base constraint if more negative than we already have.
 729                                 base_leg_pos = (tortho.y() < base_leg_pos ? tortho.y() : base_leg_pos);
 730                                 //cout << "base_leg_pos = " << base_leg_pos;
 731                                 downwind_leg_pos = tortho.x();          // Assume that a following plane can simply be constrained by the immediately in front downwind plane
 732                                 //cout << " downwind_leg_pos = " << downwind_leg_pos << '\n';
 733                                 break;
 734                         case TURN2:
 735                                 // Fall through to crosswind
 736                         case CROSSWIND:
 737                                 crosswind_leg_pos = tortho.y();
 738                                 //cout << "crosswind_leg_pos = " << crosswind_leg_pos << '\n';
 739                                 t->instructedToGoAround = false;
 740                                 break;
 741                         case TURN1:
 742                                 // Fall through to climbout
 743                         case CLIMBOUT:
 744                                 // Only use current by constraint as largest
 745                                 crosswind_leg_pos = (tortho.y() > crosswind_leg_pos ? tortho.y() : crosswind_leg_pos);
 746                                 //cout << "crosswind_leg_pos = " << crosswind_leg_pos << '\n';
 747                                 break;
 748                         case TAKEOFF_ROLL:
 749                                 break;
 750                         case LEG_UNKNOWN:
 751                                 break;
 752                         case LANDING_ROLL:
 753                                 break;
 754                         default:
 755                                 break;
 756                         }
 757                 }
 758
 759                 if(t->leg == FINAL) {
 760                         if(t->landingType == FULL_STOP) t->opType = INBOUND;
 761                         if(t->eta < 12 && rwyList.size() && !(t->instructedToGoAround)) {
 762                                 // TODO - need to make this more sophisticated
 763                                 // eg. is the plane accelerating down the runway taking off [OK],
 764                                 // or stationary near the start [V. BAD!!].
 765                                 // For now this should stop the AI plane landing on top of the user.
 766                                 string trns = t->plane.callsign;
 767                                 trns += " GO AROUND TRAFFIC ON RUNWAY I REPEAT GO AROUND";
 768                                 if(display) {
 769                                         globals->get_ATC_display()->RegisterSingleMessage(trns, 0);
 770                                 }
 771                                 t->instructedToGoAround = true;
 772                                 if(t->planePtr) {
 773                                         cout << "Registering Go-around transmission with AI plane\n";
 774                                         t->planePtr->RegisterTransmission(13);
 775                                 }
 776                         }
 777                 } else if(t->leg == LANDING_ROLL) {
 778                         rwyList.push_front(t);
 779                         // TODO - if(!clearedToLand) shout something!!
 780                         t->clearedToLand = false;
 781                         RemoveFromTrafficList(t->plane.callsign);
 782                         if(t->isUser) {
 783                                 t->opType = TTT_UNKNOWN;
 784                         }       // TODO - allow the user to specify opType via ATC menu
 785                         circuitListItr = circuitList.erase(circuitListItr);
 786                         if(circuitListItr == circuitList.end() ) {
 787                                 circuitListItr = circuitList.begin();
 788                         }
 789                 }
 790                 ++circuitListItr;
 791         }
 792         //cout << "Done CheckCircuitList" << endl;
 793 }
 794
 795 // Do the runway list - we'll do the whole runway list since it's important and there'll never be many planes on the rwy at once!!
 796 // FIXME - at the moment it looks like we're only doing the first plane from the rwy list.
 797 // (However, at the moment there should only be one airplane on the rwy at once, until we
 798 // start allowing planes to line up whilst previous arrival clears the rwy.)
 799 void FGTower::CheckRunwayList(double dt) {
 800         //cout << "Entering CheckRunwayList..." << endl;
 801         if(rwyOccupied) {
 802                 if(!rwyList.size()) {
 803                         rwyOccupied = false;
 804                 } else {
 805                         rwyListItr = rwyList.begin();
 806                         TowerPlaneRec* t = *rwyListItr;
 807                         if(t->isUser) {
 808                                 t->pos.setlon(user_lon_node->getDoubleValue());
 809                                 t->pos.setlat(user_lat_node->getDoubleValue());
 810                                 t->pos.setelev(user_elev_node->getDoubleValue());
 811                         } else {
 812                                 t->pos = t->planePtr->GetPos();         // We should probably only set the pos's on one walk through the traffic list in the update function, to save a few CPU should we end up duplicating this.
 813                         }
 814                         bool on_rwy = OnActiveRunway(t->pos);
 815                         if(!on_rwy) {
 816                                 if((t->opType == INBOUND) || (t->opType == STRAIGHT_IN)) {
 817                                         rwyList.pop_front();
 818                                         delete t;
 819                                         // TODO - tell it to taxi / contact ground / don't delete it etc!
 820                                 } else if(t->opType == OUTBOUND) {
 821                                         depList.push_back(t);
 822                                         rwyList.pop_front();
 823                                         departed = true;
 824                                         timeSinceLastDeparture = 0.0;
 825                                 } else if(t->opType == CIRCUIT) {
 826                                         circuitList.push_back(t);
 827                                         AddToTrafficList(t);
 828                                         rwyList.pop_front();
 829                                         departed = true;
 830                                         timeSinceLastDeparture = 0.0;
 831                                 } else if(t->opType == TTT_UNKNOWN) {
 832                                         depList.push_back(t);
 833                                         circuitList.push_back(t);
 834                                         AddToTrafficList(t);
 835                                         rwyList.pop_front();
 836                                         departed = true;
 837                                         timeSinceLastDeparture = 0.0;   // TODO - we need to take into account that the user might taxi-in when flagged opType UNKNOWN - check speed/altitude etc to make decision as to what user is up to.
 838                                 } else {
 839                                         // HELP - we shouldn't ever get here!!!
 840                                 }
 841                         }
 842                 }
 843         }
 844         //cout << "Done CheckRunwayList" << endl;
 845 }
 846
 847 // Do one plane from the approach list
 848 void FGTower::CheckApproachList(double dt) {
 849         if(appList.size()) {
 850                 if(appListItr == appList.end()) {
 851                         appListItr = appList.begin();
 852                 }
 853                 TowerPlaneRec* t = *appListItr;
 854                 //cout << "t = " << t << endl;
 855                 if(t->isUser) {
 856                         t->pos.setlon(user_lon_node->getDoubleValue());
 857                         t->pos.setlat(user_lat_node->getDoubleValue());
 858                         t->pos.setelev(user_elev_node->getDoubleValue());
 859                 } else {
 860                         // TODO - set/update the position if it's an AI plane
 861                 }
 862                 if(t->nextOnRwy && !(t->clearedToLand)) {
 863                         // check distance away and whether runway occupied
 864                         // and schedule transmission if necessary
 865                 }
 866                 ++appListItr;
 867         }
 868 }
 869
 870 // Returns true if positions of crosswind/downwind/base leg turns should be constrained by previous traffic
 871 // plus the constraint position as a rwy orientated orthopos (meters)
 872 bool FGTower::GetCrosswindConstraint(double& cpos) {
 873         if(crosswind_leg_pos != 0.0) {
 874                 cpos = crosswind_leg_pos;
 875                 return(true);
 876         } else {
 877                 cpos = 0.0;
 878                 return(false);
 879         }
 880 }
 881 bool FGTower::GetDownwindConstraint(double& dpos) {
 882         if(fabs(downwind_leg_pos) > nominal_downwind_leg_pos) {
 883                 dpos = downwind_leg_pos;
 884                 return(true);
 885         } else {
 886                 dpos = 0.0;
 887                 return(false);
 888         }
 889 }
 890 bool FGTower::GetBaseConstraint(double& bpos) {
 891         if(base_leg_pos < nominal_base_leg_pos) {
 892                 bpos = base_leg_pos;
 893                 return(true);
 894         } else {
 895                 bpos = nominal_base_leg_pos;
 896                 return(false);
 897         }
 898 }
 899
 900
 901 // Figure out which runways are active.
 902 // For now we'll just be simple and do one active runway - eventually this will get much more complex
 903 // This is a private function - public interface to the results of this is through GetActiveRunway
 904 void FGTower::DoRwyDetails() {
 905         //cout << "GetRwyDetails called" << endl;
 906
 907         // Based on the airport-id and wind get the active runway
 908
 909         //wind
 910         double hdg = wind_from_hdg->getDoubleValue();
 911         double speed = wind_speed_knots->getDoubleValue();
 912         hdg = (speed == 0.0 ? 270.0 : hdg);
 913         //cout << "Heading = " << hdg << '\n';
 914
 915         FGRunway runway;
 916         bool rwyGood = globals->get_runways()->search(ident, int(hdg), &runway);
 917         if(rwyGood) {
 918                 activeRwy = runway.rwy_no;
 919                 rwy.rwyID = runway.rwy_no;
 920                 SG_LOG(SG_ATC, SG_INFO, "Active runway for airport " << ident << " is " << activeRwy);
 921
 922                 // Get the threshold position
 923                 double other_way = runway.heading - 180.0;
 924                 while(other_way <= 0.0) {
 925                         other_way += 360.0;
 926                 }
 927         // move to the +l end/center of the runway
 928                 //cout << "Runway center is at " << runway.lon << ", " << runway.lat << '\n';
 929         Point3D origin = Point3D(runway.lon, runway.lat, aptElev);
 930                 Point3D ref = origin;
 931         double tshlon, tshlat, tshr;
 932                 double tolon, tolat, tor;
 933                 rwy.length = runway.length * SG_FEET_TO_METER;
 934                 rwy.width = runway.width * SG_FEET_TO_METER;
 935         geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), other_way,
 936                                 rwy.length / 2.0 - 25.0, &tshlat, &tshlon, &tshr );
 937         geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), runway.heading,
 938                                 rwy.length / 2.0 - 25.0, &tolat, &tolon, &tor );
 939                 // Note - 25 meters in from the runway end is a bit of a hack to put the plane ahead of the user.
 940                 // now copy what we need out of runway into rwy
 941         rwy.threshold_pos = Point3D(tshlon, tshlat, aptElev);
 942                 Point3D takeoff_end = Point3D(tolon, tolat, aptElev);
 943                 //cout << "Threshold position = " << tshlon << ", " << tshlat << ", " << aptElev << '\n';
 944                 //cout << "Takeoff position = " << tolon << ", " << tolat << ", " << aptElev << '\n';
 945                 rwy.hdg = runway.heading;
 946                 // Set the projection for the local area based on this active runway
 947                 ortho.Init(rwy.threshold_pos, rwy.hdg);
 948                 rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos);        // should come out as zero
 949                 rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
 950         } else {
 951                 SG_LOG(SG_ATC, SG_ALERT, "Help  - can't get good runway in FGTower!!");
 952                 activeRwy = "NN";
 953         }
 954 }
 955
 956
 957 // Figure out if a given position lies on the active runway
 958 // Might have to change when we consider more than one active rwy.
 959 bool FGTower::OnActiveRunway(Point3D pt) {
 960         // TODO - check that the centre calculation below isn't confused by displaced thesholds etc.
 961         Point3D xyc((rwy.end1ortho.x() + rwy.end2ortho.x())/2.0, (rwy.end1ortho.y() + rwy.end2ortho.y())/2.0, 0.0);
 962         Point3D xyp = ortho.ConvertToLocal(pt);
 963
 964         //cout << "Length offset = " << fabs(xyp.y() - xyc.y()) << '\n';
 965         //cout << "Width offset = " << fabs(xyp.x() - xyc.x()) << '\n';
 966
 967         double rlen = rwy.length/2.0 + 5.0;
 968         double rwidth = rwy.width/2.0;
 969         double ldiff = fabs(xyp.y() - xyc.y());
 970         double wdiff = fabs(xyp.x() - xyc.x());
 971
 972         return((ldiff < rlen) && (wdiff < rwidth));
 973 }
 974
 975
 976 // Figure out if a given position lies on any runway or not
 977 // Only call this at startup - reading the runways database is expensive and needs to be fixed!
 978 bool FGTower::OnAnyRunway(Point3D pt) {
 979         ATCData ad;
 980         double dist = current_commlist->FindClosest(lon, lat, elev, ad, TOWER, 10.0);
 981         if(dist < 0.0) {
 982                 return(false);
 983         }
 984         // Based on the airport-id, go through all the runways and check for a point in them
 985
 986         // TODO - do we actually need to search for the airport - surely we already know our ident and
 987         // can just search runways of our airport???
 988         //cout << "Airport ident is " << ad.ident << '\n';
 989         FGRunway runway;
 990         bool rwyGood = globals->get_runways()->search(ad.ident, &runway);
 991         if(!rwyGood) {
 992                 SG_LOG(SG_ATC, SG_WARN, "Unable to find any runways for airport ID " << ad.ident << " in FGTower");
 993         }
 994         bool on = false;
 995         while(runway.id == ad.ident) {
 996                 on = OnRunway(pt, runway);
 997                 //cout << "Runway " << runway.rwy_no << ": On = " << (on ? "true\n" : "false\n");
 998                 if(on) return(true);
 999                 globals->get_runways()->next(&runway);
1000         }
1001         return(on);
1002 }
1003
1004
1005 // Returns true if successful
1006 bool FGTower::RemoveFromTrafficList(string id) {
1007         tower_plane_rec_list_iterator twrItr;
1008         for(twrItr = trafficList.begin(); twrItr != trafficList.end(); twrItr++) {
1009                 TowerPlaneRec* tpr = *twrItr;
1010                 if(tpr->plane.callsign == id) {
1011                         trafficList.erase(twrItr);
1012                         return(true);
1013                 }
1014         }
1015         SG_LOG(SG_ATC, SG_WARN, "Warning - unable to remove aircraft " << id << " from trafficList in FGTower");
1016         return(false);
1017 }
1018
1019
1020 // Add a tower plane rec with ETA to the traffic list in the correct position ETA-wise
1021 // and set nextOnRwy if so.
1022 // Returns true if this could cause a threshold ETA conflict with other traffic, false otherwise.
1023 // For planes holding they are put in the first position with time to go, and the return value is
1024 // true if in the first position (nextOnRwy) and false otherwise.
1025 // See the comments in FGTower::doThresholdUseOrder for notes on the ordering
1026 bool FGTower::AddToTrafficList(TowerPlaneRec* t, bool holding) {
1027         //cout << "ADD: " << trafficList.size();
1028         //cout << "AddToTrafficList called, currently size = " << trafficList.size() << ", holding = " << holding << endl;
1029         double separation_time = 90.0;  // seconds - this is currently a guess for light plane separation, and includes a few seconds for a holding plane to taxi onto the rwy.
1030         double departure_sep_time = 60.0;       // Separation time behind departing airplanes.  Comments above also apply.
1031         bool conflict = false;
1032         double lastETA = 0.0;
1033         bool firstTime = true;
1034         // FIXME - make this more robust for different plane types eg. light following heavy.
1035         tower_plane_rec_list_iterator twrItr;
1036         //twrItr = trafficList.begin();
1037         //while(1) {
1038         for(twrItr = trafficList.begin(); twrItr != trafficList.end(); twrItr++) {
1039                 //if(twrItr == trafficList.end()) {
1040                 //      cout << "  END  ";
1041                 //      trafficList.push_back(t);
1042                 //      return(holding ? firstTime : conflict);
1043                 //} else {
1044                         TowerPlaneRec* tpr = *twrItr;
1045                         if(holding) {
1046                                 //cout << (tpr->isUser ? "USER!\n" : "NOT user\n");
1047                                 //cout << "tpr->eta - lastETA = " << tpr->eta - lastETA << '\n';
1048                                 double dep_allowance = (timeSinceLastDeparture < departure_sep_time ? departure_sep_time - timeSinceLastDeparture : 0.0);
1049                                 double slot_time = (firstTime ? separation_time + dep_allowance : separation_time + departure_sep_time);
1050                                 // separation_time + departure_sep_time in the above accounts for the fact that the arrival could be touch and go,
1051                                 // and if not needs time to clear the rwy anyway.
1052                                 if(tpr->eta  - lastETA > slot_time) {
1053                                         t->nextOnRwy = firstTime;
1054                                         trafficList.insert(twrItr, t);
1055                                         //cout << "\tH\t" << trafficList.size() << '\n';
1056                                         return(firstTime);
1057                                 }
1058                                 firstTime = false;
1059                         } else {
1060                                 if(t->eta < tpr->eta) {
1061                                         // Ugg - this one's tricky.
1062                                         // It depends on what the two planes are doing and whether there's a conflict what we do.
1063                                         if(tpr->eta - t->eta > separation_time) {       // No probs, plane 2 can squeeze in before plane 1 with no apparent conflict
1064                                                 if(tpr->nextOnRwy) {
1065                                                         tpr->nextOnRwy = false;
1066                                                         t->nextOnRwy = true;
1067                                                 }
1068                                                 trafficList.insert(twrItr, t);
1069                                         } else {        // Ooops - this ones tricky - we have a potential conflict!
1070                                                 conflict = true;
1071                                                 // HACK - just add anyway for now and flag conflict - TODO - FIX THIS using CIRCUIT/STRAIGHT_IN and VFR/IFR precedence rules.
1072                                                 if(tpr->nextOnRwy) {
1073                                                         tpr->nextOnRwy = false;
1074                                                         t->nextOnRwy = true;
1075                                                 }
1076                                                 trafficList.insert(twrItr, t);
1077                                         }
1078                                         //cout << "\tC\t" << trafficList.size() << '\n';
1079                                         return(conflict);
1080                                 }
1081                         }
1082                 //}
1083                 //++twrItr;
1084         }
1085         // If we get here we must be at the end of the list, or maybe the list is empty.
1086         if(!trafficList.size()) {
1087                 t->nextOnRwy = true;
1088                 // conflict and firstTime should be false and true respectively in this case anyway.
1089         }
1090         trafficList.push_back(t);
1091         //cout << "\tE\t" << trafficList.size() << endl;
1092         return(holding ? firstTime : conflict);
1093 }
1094
1095 // Add a tower plane rec with ETA to the circuit list in the correct position ETA-wise
1096 // Returns true if this might cause a separation conflict (based on ETA) with other traffic, false otherwise.
1097 bool FGTower::AddToCircuitList(TowerPlaneRec* t) {
1098         //cout << "ADD: " << circuitList.size();
1099         //cout << "AddToCircuitList called, currently size = " << circuitList.size() << endl;
1100         double separation_time = 60.0;  // seconds - this is currently a guess for light plane separation, and includes a few seconds for a holding plane to taxi onto the rwy.
1101         bool conflict = false;
1102         tower_plane_rec_list_iterator twrItr;
1103         for(twrItr = circuitList.begin(); twrItr != circuitList.end(); twrItr++) {
1104                         TowerPlaneRec* tpr = *twrItr;
1105
1106                                 if(t->eta < tpr->eta) {
1107                                         // Ugg - this one's tricky.
1108                                         // It depends on what the two planes are doing and whether there's a conflict what we do.
1109                                         if(tpr->eta - t->eta > separation_time) {       // No probs, plane 2 can squeeze in before plane 1 with no apparent conflict
1110                                                 circuitList.insert(twrItr, t);
1111                                         } else {        // Ooops - this ones tricky - we have a potential conflict!
1112                                                 conflict = true;
1113                                                 // HACK - just add anyway for now and flag conflict.
1114                                                 circuitList.insert(twrItr, t);
1115                                         }
1116                                         //cout << "\tC\t" << circuitList.size() << '\n';
1117                                         return(conflict);
1118                                 }
1119         }
1120         // If we get here we must be at the end of the list, or maybe the list is empty.
1121         circuitList.push_back(t);       // TODO - check the separation with the preceding plane for the conflict flag.
1122         //cout << "\tE\t" << circuitList.size() << endl;
1123         return(conflict);
1124 }
1125
1126
1127 // Calculate the eta of a plane to the threshold.
1128 // For ground traffic this is the fastest they can get there.
1129 // For air traffic this is the middle approximation.
1130 void FGTower::CalcETA(TowerPlaneRec* tpr, bool printout) {
1131         // For now we'll be very crude and hardwire expected speeds to C172-like values
1132         // The speeds below are specified in knots IAS and then converted to m/s
1133         double app_ias = 100.0 * 0.514444;                      // Speed during straight-in approach
1134         double circuit_ias = 80.0 * 0.514444;           // Speed around circuit
1135         double final_ias = 70.0 * 0.514444;             // Speed during final approach
1136
1137         //if(printout) {
1138         //cout << "In CalcETA, airplane ident = " << tpr->plane.callsign << '\n';
1139         //cout << (tpr->isUser ? "USER\n" : "AI\n");
1140         //cout << flush;
1141         //}
1142
1143         // Sign convention - dist_out is -ve for approaching planes and +ve for departing planes
1144         // dist_across is +ve in the pattern direction - ie a plane correctly on downwind will have a +ve dist_across
1145
1146         Point3D op = ortho.ConvertToLocal(tpr->pos);
1147         //if(printout) {
1148         //if(!tpr->isUser) cout << "Orthopos is " << op.x() << ", " << op.y() << ' ';
1149         //      cout << "opType is " << tpr->opType << '\n';
1150         //}
1151         double dist_out_m = op.y();
1152         double dist_across_m = fabs(op.x());    // FIXME = the fabs is a hack to cope with the fact that we don't know the circuit direction yet
1153         //cout << "Doing ETA calc for " << tpr->plane.callsign << '\n';
1154
1155         if(tpr->opType == STRAIGHT_IN) {
1156                 double dist_to_go_m = sqrt((dist_out_m * dist_out_m) + (dist_across_m * dist_across_m));
1157                 if(dist_to_go_m < 1000) {
1158                         tpr->eta = dist_to_go_m / final_ias;
1159                 } else {
1160                         tpr->eta = (1000.0 / final_ias) + ((dist_to_go_m - 1000.0) / app_ias);
1161                 }
1162         } else if(tpr->opType == CIRCUIT || tpr->opType == TTT_UNKNOWN) {       // Hack alert - UNKNOWN has sort of been added here as a temporary hack.
1163                 // It's complicated - depends on if base leg is delayed or not
1164                 //if(printout) {
1165                 //      cout << "Leg = " << tpr->leg << '\n';
1166                 //}
1167                 if(tpr->leg == LANDING_ROLL) {
1168                         tpr->eta = 0;
1169                 } else if((tpr->leg == FINAL) || (tpr->leg == TURN4)) {
1170                         tpr->eta = fabs(dist_out_m) / final_ias;
1171                 } else if((tpr->leg == BASE) || (tpr->leg == TURN3)) {
1172                         tpr->eta = (fabs(dist_out_m) / final_ias) + (dist_across_m / circuit_ias);
1173                 } else {
1174                         // Need to calculate where base leg is likely to be
1175                         // FIXME - for now I'll hardwire it to 1000m which is what AILocalTraffic uses!!!
1176                         // TODO - as a matter of design - AILocalTraffic should get the nominal no-traffic base turn distance from Tower, since in real life the published pattern might differ from airport to airport
1177                         double nominal_base_dist_out_m = -1000;
1178                         double current_base_dist_out_m;
1179                         if(!GetBaseConstraint(current_base_dist_out_m)) {
1180                                 current_base_dist_out_m = nominal_base_dist_out_m;
1181                         }
1182                         //cout << "current_base_dist_out_m = " << current_base_dist_out_m << '\n';
1183                         double nominal_dist_across_m = 1000;    // Hardwired value from AILocalTraffic
1184                         double current_dist_across_m;
1185                         if(!GetDownwindConstraint(current_dist_across_m)) {
1186                                 current_dist_across_m = nominal_dist_across_m;
1187                         }
1188                         double nominal_cross_dist_out_m = 2000; // Bit of a guess - AI plane turns to crosswind at 600ft agl.
1189                         tpr->eta = fabs(current_base_dist_out_m) / final_ias;   // final
1190                         //cout << "a = " << tpr->eta << '\n';
1191                         if((tpr->leg == DOWNWIND) || (tpr->leg == TURN2)) {
1192                                 tpr->eta += dist_across_m / circuit_ias;
1193                                 //cout << "b = " << tpr->eta << '\n';
1194                                 tpr->eta += fabs(current_base_dist_out_m - dist_out_m) / circuit_ias;
1195                                 //cout << "c = " << tpr->eta << '\n';
1196                         } else if((tpr->leg == CROSSWIND) || (tpr->leg == TURN1)) {
1197                                 if(dist_across_m > nominal_dist_across_m) {
1198                                         tpr->eta += dist_across_m / circuit_ias;
1199                                 } else {
1200                                         tpr->eta += nominal_dist_across_m / circuit_ias;
1201                                 }
1202                                 // should we use the dist across of the previous plane if there is previous still on downwind?
1203                                 //if(printout) cout << "bb = " << tpr->eta << '\n';
1204                                 if(dist_out_m > nominal_cross_dist_out_m) {
1205                                         tpr->eta += fabs(current_base_dist_out_m - dist_out_m) / circuit_ias;
1206                                 } else {
1207                                         tpr->eta += fabs(current_base_dist_out_m - nominal_cross_dist_out_m) / circuit_ias;
1208                                 }
1209                                 //if(printout) cout << "cc = " << tpr->eta << '\n';
1210                                 if(nominal_dist_across_m > dist_across_m) {
1211                                         tpr->eta += (nominal_dist_across_m - dist_across_m) / circuit_ias;
1212                                 } else {
1213                                         // Nothing to add
1214                                 }
1215                                 //if(printout) cout << "dd = " << tpr->eta << '\n';
1216                         } else {
1217                                 // We've only just started - why not use a generic estimate?
1218                                 tpr->eta = 240.0;
1219                         }
1220                 }
1221                 //if(printout) {
1222                 //      cout << "ETA = " << tpr->eta << '\n';
1223                 //}
1224                 //if(!tpr->isUser) cout << tpr->plane.callsign << '\t' << tpr->eta << '\n';
1225         } else {
1226                 tpr->eta = 99999;
1227         }
1228 }
1229
1230
1231 // Calculate the distance of a plane to the threshold in meters
1232 // TODO - Modify to calculate flying distance of a plane in the circuit
1233 double FGTower::CalcDistOutM(TowerPlaneRec* tpr) {
1234         return(dclGetHorizontalSeparation(rwy.threshold_pos, tpr->pos));
1235 }
1236
1237
1238 // Calculate the distance of a plane to the threshold in miles
1239 // TODO - Modify to calculate flying distance of a plane in the circuit
1240 double FGTower::CalcDistOutMiles(TowerPlaneRec* tpr) {
1241         return(CalcDistOutM(tpr) / 1600.0);             // FIXME - use a proper constant if possible.
1242 }
1243
1244
1245 // Iterate through all the lists and call CalcETA for all the planes.
1246 void FGTower::doThresholdETACalc() {
1247         //cout << "Entering doThresholdETACalc..." << endl;
1248         tower_plane_rec_list_iterator twrItr;
1249         // Do the approach list first
1250         for(twrItr = appList.begin(); twrItr != appList.end(); twrItr++) {
1251                 TowerPlaneRec* tpr = *twrItr;
1252                 CalcETA(tpr);
1253         }
1254         // Then the circuit list
1255         for(twrItr = circuitList.begin(); twrItr != circuitList.end(); twrItr++) {
1256                 TowerPlaneRec* tpr = *twrItr;
1257                 CalcETA(tpr);
1258         }
1259         //cout << "Done doThresholdETCCalc" << endl;
1260 }
1261
1262
1263 // Check that the planes in traffic list are correctly ordered,
1264 // that the nearest (timewise) is flagged next on rwy, and return
1265 // true if any threshold use conflicts are detected, false otherwise.
1266 bool FGTower::doThresholdUseOrder() {
1267         //cout << "Entering doThresholdUseOrder..." << endl;
1268         bool conflict = false;
1269
1270         // Wipe out traffic list, go through circuit, app and hold list, and reorder them in traffic list.
1271         // Here's the rather simplistic assumptions we're using:
1272         // Currently all planes are assumed to be GA light singles with corresponding speeds and separation times.
1273         // In order of priority for runway use:
1274         // STRAIGHT_IN > CIRCUIT > HOLDING_FOR_DEPARTURE
1275         // No modification of planes speeds occurs - conflicts are resolved by delaying turn for base,
1276         // and holding planes until a space.
1277         // When calculating if a holding plane can use the runway, time clearance from last departure
1278         // as well as time clearance to next arrival must be considered.
1279
1280         trafficList.clear();
1281
1282         tower_plane_rec_list_iterator twrItr;
1283         // Do the approach list first
1284         //cout << "A" << flush;
1285         for(twrItr = appList.begin(); twrItr != appList.end(); twrItr++) {
1286                 TowerPlaneRec* tpr = *twrItr;
1287                 conflict = AddToTrafficList(tpr);
1288         }
1289         // Then the circuit list
1290         //cout << "C" << flush;
1291         for(twrItr = circuitList.begin(); twrItr != circuitList.end(); twrItr++) {
1292                 TowerPlaneRec* tpr = *twrItr;
1293                 conflict = AddToTrafficList(tpr);
1294         }
1295         // And finally the hold list
1296         //cout << "H" << endl;
1297         for(twrItr = holdList.begin(); twrItr != holdList.end(); twrItr++) {
1298                 TowerPlaneRec* tpr = *twrItr;
1299                 AddToTrafficList(tpr, true);
1300         }
1301
1302         if(0) {
1303         //if(ident == "KEMT") {
1304                 for(twrItr = trafficList.begin(); twrItr != trafficList.end(); twrItr++) {
1305                         TowerPlaneRec* tpr = *twrItr;
1306                         cout << tpr->plane.callsign << '\t' << tpr->eta << '\t';
1307                 }
1308                 cout << endl;
1309         }
1310
1311         //cout << "Done doThresholdUseOrder" << endl;
1312         return(conflict);
1313 }
1314
1315
1316 // Return the ETA of plane no. list_pos (1-based) in the traffic list.
1317 // i.e. list_pos = 1 implies next to use runway.
1318 double FGTower::GetTrafficETA(unsigned int list_pos, bool printout) {
1319         if(trafficList.size() < list_pos) {
1320                 return(99999);
1321         }
1322
1323         tower_plane_rec_list_iterator twrItr;
1324         twrItr = trafficList.begin();
1325         for(unsigned int i = 1; i < list_pos; i++, twrItr++);
1326         TowerPlaneRec* tpr = *twrItr;
1327         CalcETA(tpr, printout);
1328         //cout << "ETA returned = " << tpr->eta << '\n';
1329         return(tpr->eta);
1330 }
1331
1332
1333 void FGTower::ContactAtHoldShort(PlaneRec plane, FGAIPlane* requestee, tower_traffic_type operation) {
1334         // HACK - assume that anything contacting at hold short is new for now - FIXME LATER
1335         TowerPlaneRec* t = new TowerPlaneRec;
1336         t->plane = plane;
1337         t->planePtr = requestee;
1338         t->holdShortReported = true;
1339         t->clearedToLineUp = false;
1340         t->clearedToTakeOff = false;
1341         t->opType = operation;
1342         t->pos = requestee->GetPos();
1343
1344         //cout << "Hold Short reported by " << plane.callsign << '\n';
1345         SG_LOG(SG_ATC, SG_BULK, "Hold Short reported by " << plane.callsign);
1346
1347 /*
1348         bool next = AddToTrafficList(t, true);
1349         if(next) {
1350                 double teta = GetTrafficETA(2);
1351                 if(teta < 150.0) {
1352                         t->clearanceCounter = 7.0;      // This reduces the delay before response to 3 secs if an immediate takeoff is reqd
1353                         //cout << "Reducing response time to request due imminent traffic\n";
1354                 }
1355         } else {
1356         }
1357 */
1358         // TODO - possibly add the reduced interval to clearance when immediate back in under the new scheme
1359
1360         holdList.push_back(t);
1361
1362         responseReqd = true;
1363 }
1364
1365 // Register the presence of an AI plane at a point where contact would already have been made in real life
1366 // CAUTION - currently it is assumed that this plane's callsign is unique - it is up to AIMgr to generate unique callsigns.
1367 void FGTower::RegisterAIPlane(PlaneRec plane, FGAIPlane* ai, tower_traffic_type op, PatternLeg lg) {
1368         // At the moment this is only going to be tested with inserting an AI plane on downwind
1369         TowerPlaneRec* t = new TowerPlaneRec;
1370         t->plane = plane;
1371         t->planePtr = ai;
1372         t->opType = op;
1373         t->leg = lg;
1374         t->pos = ai->GetPos();
1375
1376         CalcETA(t);
1377
1378         if(op == CIRCUIT && lg != LEG_UNKNOWN) {
1379                 AddToCircuitList(t);
1380         } else {
1381                 // FLAG A WARNING
1382         }
1383
1384         doThresholdUseOrder();
1385 }
1386
1387 void FGTower::RequestLandingClearance(string ID) {
1388         //cout << "Request Landing Clearance called...\n";
1389
1390         // Assume this comes from the user - have another function taking a pointer to the AIplane for the AI traffic.
1391         // For now we'll also assume that the user is a light plane and can get him/her to join the circuit if necessary.
1392
1393         TowerPlaneRec* t = new TowerPlaneRec;
1394         t->isUser = true;
1395         t->clearedToLand = false;
1396         t->pos.setlon(user_lon_node->getDoubleValue());
1397         t->pos.setlat(user_lat_node->getDoubleValue());
1398         t->pos.setelev(user_elev_node->getDoubleValue());
1399
1400         // TODO
1401         // Calculate where the user is in relation to the active runway and it's circuit
1402         // and set the op-type as appropriate.
1403
1404         // HACK - to get up and running I'm going to assume that the user contacts tower on a staight-in final for now.
1405         t->opType = STRAIGHT_IN;
1406
1407         t->plane.type = GA_SINGLE;      // FIXME - Another assumption!
1408         t->plane.callsign = ID;
1409
1410         appList.push_back(t);   // Not necessarily permanent
1411         AddToTrafficList(t);
1412 }
1413
1414 void FGTower::RequestDepartureClearance(string ID) {
1415         //cout << "Request Departure Clearance called...\n";
1416 }
1417
1418 void FGTower::ReportFinal(string ID) {
1419         TowerPlaneRec* t = FindPlane(ID);
1420         if(t) {
1421                 t->finalReported = true;
1422                 t->finalAcknowledged = false;
1423                 if(!(t->clearedToLand)) {
1424                         responseReqd = true;
1425                 } // possibly respond with wind even if already cleared to land?
1426         } else {
1427                 SG_LOG(SG_ATC, SG_WARN, "WARNING: Unable to find plane " << ID << " in FGTower::ReportFinal(...)");
1428         }
1429 }
1430
1431 //void FGTower::ReportLongFinal(string ID);
1432 //void FGTower::ReportOuterMarker(string ID);
1433 //void FGTower::ReportMiddleMarker(string ID);
1434 //void FGTower::ReportInnerMarker(string ID);
1435 //void FGTower::ReportGoingAround(string ID);
1436
1437 void FGTower::ReportRunwayVacated(string ID) {
1438         //cout << "Report Runway Vacated Called...\n";
1439 }
1440
1441 TowerPlaneRec* FGTower::FindPlane(string ID) {
1442         tower_plane_rec_list_iterator twrItr;
1443         // Do the approach list first
1444         for(twrItr = appList.begin(); twrItr != appList.end(); twrItr++) {
1445                 if((*twrItr)->plane.callsign == ID) return(*twrItr);
1446         }
1447         // Then the circuit list
1448         for(twrItr = circuitList.begin(); twrItr != circuitList.end(); twrItr++) {
1449                 if((*twrItr)->plane.callsign == ID) return(*twrItr);
1450         }
1451         // And finally the hold list
1452         for(twrItr = holdList.begin(); twrItr != holdList.end(); twrItr++) {
1453                 if((*twrItr)->plane.callsign == ID) return(*twrItr);
1454         }
1455         SG_LOG(SG_ATC, SG_WARN, "Unable to find " << ID << " in FGTower::FindPlane(...)");
1456         return(NULL);
1457 }
1458
1459 void FGTower::ReportDownwind(string ID) {
1460         //cout << "ReportDownwind(...) called\n";
1461         // Tell the plane reporting what number she is in the circuit
1462         TowerPlaneRec* t = FindPlane(ID);
1463         if(t) {
1464                 t->downwindReported = true;
1465                 responseReqd = true;
1466         } else {
1467                 SG_LOG(SG_ATC, SG_WARN, "WARNING: Unable to find plane " << ID << " in FGTower::ReportDownwind(...)");
1468         }
1469 }
1470
1471 ostream& operator << (ostream& os, tower_traffic_type ttt) {
1472         switch(ttt) {
1473         case(CIRCUIT):      return(os << "CIRCUIT");
1474         case(INBOUND):      return(os << "INBOUND");
1475         case(OUTBOUND):     return(os << "OUTBOUND");
1476         case(TTT_UNKNOWN):  return(os << "UNKNOWN");
1477         case(STRAIGHT_IN):  return(os << "STRAIGHT_IN");
1478         }
1479         return(os << "ERROR - Unknown switch in tower_traffic_type operator << ");
1480 }
1481