// FGGround - a class to provide ground control at larger airports.
//
// Written by David Luff, started March 2002.
//
// Copyright (C) 2002  David C. Luff - david.luff@nottingham.ac.uk
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

#include <simgear/misc/sg_path.hxx>
#include <simgear/math/sg_random.h>
#include <simgear/debug/logstream.hxx>
#include <simgear/misc/sgstream.hxx>
#include <simgear/constants.h>
#include <Main/globals.hxx>

#include <stdlib.h>
#include STL_FSTREAM

#include "ground.hxx"
#include "ATCmgr.hxx"
#include "ATCutils.hxx"
#include "ATCdisplay.hxx"
#include "AILocalTraffic.hxx"

SG_USING_STD(ifstream);
SG_USING_STD(cout);

node::node() {
}

node::~node() {
	for(unsigned int i=0; i < arcs.size(); ++i) {
		delete arcs[i];
	}
}

// Make sure that a_path.cost += distance is safe from the moment it's created.
a_path::a_path() {
	cost = 0;
}

FGGround::FGGround() {
	display = false;
	networkLoadOK = false;
	ground_traffic.erase(ground_traffic.begin(), ground_traffic.end());
	ground_traffic_itr = ground_traffic.begin();
}

FGGround::FGGround(string id) {
	display = false;
	networkLoadOK = false;
	ground_traffic.erase(ground_traffic.begin(), ground_traffic.end());
	ground_traffic_itr = ground_traffic.begin();
	ident = id;
}

FGGround::~FGGround() {
}

void FGGround::ParseRwyExits(node* np, char* es) {
	char* token;
	char estr[20];
	strcpy(estr, es);
	const char delimiters[] = "-";
	token = strtok(estr, delimiters);
	while(token != NULL) {
		int i = atoi(token);
		//cout << "token = " << token << endl;
		//cout << "rwy number = " << i << endl;
		//runways[(atoi(token))].exits.push_back(np);
		runways[i].exits.push_back(np);
		//cout << "token = " << token << '\n';
		token = strtok(NULL, delimiters);
	}
}
	

// Load the ground logical network of the current instances airport
// Return true if successfull.
// TODO - currently the file is assumed to reside in the base/ATC directory.
// This might change to something more thought out in the future.
// NOTE - currently it is assumed that all nodes are loaded before any arcs.
// It won't work ATM if this doesn't hold true.
bool FGGround::LoadNetwork() {
	node* np;
	arc* ap;
	Gate* gp;
	
	int gateCount = 0;	// This is used to allocate gateID's from zero upwards
	// This may well change in the future - probably to reading in the real-world
	// gate numbers from file.
	
	ifstream fin;
	SGPath path = globals->get_fg_root();
	//string taxiPath = "ATC/" + ident + ".taxi";
	string taxiPath = "ATC/KEMT.taxi";	// FIXME - HARDWIRED FOR TESTING
	path.append(taxiPath);
	
	SG_LOG(SG_ATC, SG_INFO, "Trying to read taxiway data for " << ident << "...");
	//cout << "Trying to read taxiway data for " << ident << "..." << endl;
	fin.open(path.c_str(), ios::in);
	if(!fin) {
		SG_LOG(SG_ATC, SG_ALERT, "Unable to open taxiway data input file " << path.c_str());
		//cout << "Unable to open taxiway data input file " << path.c_str() << endl;
		return(false);
	}
	
	char ch;
	char buf[30];
	while(!fin.eof()) {
		fin >> buf;
		// Node, arc, or [End]?
		//cout << "Read in ground network element type = " << buf << endl;
		if(!strcmp(buf, "[End]")) {		// TODO - maybe make this more robust to spelling errors by just looking for '['
			SG_LOG(SG_ATC, SG_INFO, "Done reading " << path.c_str() << endl);
			break;
		} else if(!strcmp(buf, "N")) {
			// Node
			np = new node;
			np->struct_type = NODE;
			fin >> buf;
			np->nodeID = atoi(buf);
			fin >> buf;
			np->pos.setlon(atof(buf));
			fin >> buf;
			np->pos.setlat(atof(buf));
			fin >> buf;
			np->pos.setelev(atof(buf));
			fin >> buf;		// node type
			if(!strcmp(buf, "J")) {
				np->type = JUNCTION;
			} else if(!strcmp(buf, "T")) {
				np->type = TJUNCTION;
			} else if(!strcmp(buf, "H")) {
				np->type = HOLD;
			} else {
				SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown node type in taxi network...\n");
				delete np;
				return(false);
			}
			fin >> buf;		// rwy exit information - gets parsed later - FRAGILE - will break if buf is reused.
			// Now the name
			fin >> ch;	// strip the leading " off
			np->name = "";
			while(1) {
				fin.unsetf(ios::skipws);
				fin >> ch;
				if((ch == '"') || (ch == 0x0A)) {
					break;
				}   // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
				np->name += ch;
			}
			fin.setf(ios::skipws);
			network.push_back(np);
			// FIXME - fragile - replies on buf not getting modified from exits read to here
			// see if we also need to push it onto the runway exit list
			//cout << "strlen(buf) = " << strlen(buf) << endl;
			if(strlen(buf) > 2) {
				//cout << "Calling ParseRwyExits for " << buf << endl;
				ParseRwyExits(np, buf);
			}
		} else if(!strcmp(buf, "A")) {
			ap = new arc;
			ap->struct_type = ARC;
			fin >> buf;
			ap->n1 = atoi(buf);
			fin >> buf;
			ap->n2 = atoi(buf);
			fin >> buf;
			if(!strcmp(buf, "R")) {
				ap->type = RUNWAY;
			} else if(!strcmp(buf, "T")) {
				ap->type = TAXIWAY;
			} else {
				SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown arc type in taxi network...\n");
				delete ap;
				return(false);
			}
			// directed?
			fin >> buf;
			if(!strcmp(buf, "Y")) {
				ap->directed = true;
			} else if(!strcmp(buf, "N")) {
				ap->directed = false;
			} else {
				SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown arc directed value in taxi network - should be Y/N !!!\n");
				delete ap;
				return(false);
			}			
			// Now the name
			ap->name = "";
			while(1) {
				fin.unsetf(ios::skipws);
				fin >> ch;
				ap->name += ch;
				if((ch == '"') || (ch == 0x0A)) {
					break;
				}   // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
			}
			fin.setf(ios::skipws);
			ap->distance = (int)dclGetHorizontalSeparation(network[ap->n1]->pos, network[ap->n2]->pos);
			//cout << "Distance  = " << ap->distance << '\n';
			network[ap->n1]->arcs.push_back(ap);
			network[ap->n2]->arcs.push_back(ap);
		} else if(!strcmp(buf, "G")) {
			gp = new Gate;
			gp->struct_type = NODE;
			gp->type = GATE;
			fin >> buf;
			gp->nodeID = atoi(buf);
			fin >> buf;
			gp->pos.setlon(atof(buf));
			fin >> buf;
			gp->pos.setlat(atof(buf));
			fin >> buf;
			gp->pos.setelev(atof(buf));
			fin >> buf;		// gate type - ignore this for now
			fin >> buf;		// gate heading
			gp->heading = atoi(buf);
			// Now the name
			gp->name = "";
			while(1) {
				fin.unsetf(ios::skipws);
				fin >> ch;
				gp->name += ch;
				if((ch == '"') || (ch == 0x0A)) {
					break;
				}   // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
			}
			fin.setf(ios::skipws);
			gp->id = gateCount;		// Warning - this will likely change in the future.
			gp->used = false;
			network.push_back(gp);
			gates[gateCount] = gp;
			gateCount++;
		} else {
			// Something has gone seriously pear-shaped
			SG_LOG(SG_ATC, SG_ALERT, "********* ERROR - unknown ground network element type... aborting read of " << path.c_str() << '\n');
			return(false);
		}
		
		fin >> skipeol;		
	}
	return(true);
}

void FGGround::Init() {
	display = false;
	
	//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	// For now we'll hardwire the threshold end FIXME FIXME FIXME - use actual active rwy
	Point3D P010(-118.037483, 34.081358, 296 * SG_FEET_TO_METER);
	double hdg = 25.32;
	ortho.Init(P010, hdg);
	// FIXME TODO FIXME TODO
	// TODO FIXME TODO FIXME
	//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	
	networkLoadOK = LoadNetwork();
}

void FGGround::Update(double dt) {
	// Each time step, what do we need to do?
	// We need to go through the list of outstanding requests and acknowedgements
	// and process at least one of them.
	// We need to go through the list of planes under our control and check if
	// any need to be addressed.
	// We need to check for planes not under our control coming within our 
	// control area and address if necessary.
	
	// Lets take the example of a plane which has just contacted ground
	// following landing - presumably requesting where to go?
	// First we need to establish the position of the plane within the logical network.
	// Next we need to decide where its going.
	
	if(ground_traffic.size()) {
		if(ground_traffic_itr == ground_traffic.end()) {
			ground_traffic_itr = ground_traffic.begin();
		}
		
		//Process(*ground_traffic_itr);
		GroundRec* g = *ground_traffic_itr;
		if(g->taxiRequestOutstanding) {
			double responseTime = 10.0;		// seconds - this should get more sophisticated at some point
			if(g->clearanceCounter > responseTime) {
				// DO CLEARANCE
				// TODO - move the mechanics of making up the transmission out of the main Update(...) routine.
				string trns = "";
				trns += g->plane.callsign;
				trns += " taxi holding point runway ";	// TODO - add the holding point name
				// eg " taxi holding point G2 runway "
				//trns += "
				if(display) {
					globals->get_ATC_display()->RegisterSingleMessage(trns, 0);
				}
				g->planePtr->RegisterTransmission(1);	// cleared to taxi
				g->clearanceCounter = 0.0;
				g->taxiRequestOutstanding = false;
			} else {
				g->clearanceCounter += (dt * ground_traffic.size());
			}
		} else if(((FGAILocalTraffic*)(g->planePtr))->AtHoldShort()) {		// That's a hack - eventually we should monitor actual position
			// HACK ALERT - the automatic cast to AILocalTraffic has to go once we have other sorts working!!!!! FIXME TODO
			// NOTE - we don't need to do the contact tower bit unless we have separate tower and ground
			string trns = g->plane.callsign;
			trns += " contact Tower ";
			double f = globals->get_ATC_mgr()->GetFrequency(ident, TOWER);
			char buf[10];
			sprintf(buf, "%f", f);
			trns += buf;
			if(display) {
				globals->get_ATC_display()->RegisterSingleMessage(trns, 0);
			}
			g->planePtr->RegisterTransmission(2);	// contact tower
			delete *ground_traffic_itr;
			ground_traffic.erase(ground_traffic_itr);
			ground_traffic_itr = ground_traffic.begin();
		}				
		++ground_traffic_itr;
	}
}

// Return a random gate ID of an unused gate.
// Two error values may be returned and must be checked for by the calling function:
// -2 signifies that no gates exist at this airport.
// -1 signifies that all gates are currently full.
int FGGround::GetRandomGateID() {
	// Check that this airport actually has some gates!!
	if(!gates.size()) {
		return(-2);
	}

	gate_vec_type gateVec;
	int num = 0;
	int thenum;
	int ID;
	
	gatesItr = gates.begin();
	while(gatesItr != gates.end()) {
		if((gatesItr->second)->used == false) {
			gateVec.push_back(gatesItr->second);
			num++;
		}
		++gatesItr;
	}

	// Check that there are some unused gates!
	if(!gateVec.size()) {
		return(-1);
	}
	
	// Randomly select one from the list
	sg_srandom_time();
	thenum = (int)(sg_random() * gateVec.size());
	ID = gateVec[thenum]->id;
	
	return(ID);
}

// Return a pointer to an unused gate node
Gate* FGGround::GetGateNode() {
	int id = GetRandomGateID();
	if(id < 0) {
		return(NULL);
	} else {
		return(gates[id]);
	}
}


// WARNING - This is hardwired to my prototype logical network format
// and will almost certainly change when Bernie's stuff comes on-line.
node* FGGround::GetThresholdNode(string rwyID) {
	// For now go through all the nodes and parse their names
	// Maybe in the future we'll map threshold nodes by ID
	//cout << "Size of network is " << network.size() << '\n';
	for(unsigned int i=0; i<network.size(); ++i) {
		//cout << "Name = " << network[i]->name << '\n';
		if(network[i]->name.size()) {
			string s = network[i]->name;
			// Warning - the next bit is fragile and dependent on my current naming scheme
			//cout << "substr = " << s.substr(0,3) << '\n';
			//cout << "size of s = " << s.size() << '\n'; 
			if(s.substr(0,3) == "rwy") {
				//cout << "subsubstr = " << s.substr(4, s.size() - 4) << '\n';
				if(s.substr(4, s.size() - 4) == rwyID) {
					return network[i];
				}
			}
		}
	}
	return NULL;
}


// Get a path from a point on a runway to a gate
// TODO !!

// Get a path from a node to another node
// Eventually we will need complex algorithms for this taking other traffic,
// shortest path and suitable paths into accout.
// For now we'll just call the shortest path algorithm.
ground_network_path_type FGGround::GetPath(node* A, node* B) {	
	return(GetShortestPath(A, B));
};

// Get a path from a node to a runway threshold
ground_network_path_type FGGround::GetPath(node* A, string rwyID) {
	node* b = GetThresholdNode(rwyID);
	if(b == NULL) {
		SG_LOG(SG_ATC, SG_ALERT, "ERROR - unable to find path to runway theshold in ground.cxx\n");
		ground_network_path_type emptyPath;
		emptyPath.erase(emptyPath.begin(), emptyPath.end());
		return(emptyPath);
	}
	return GetShortestPath(A, b);
}

// Get a path from a node to a runway hold short point
// Bit of a hack this at the moment!
ground_network_path_type FGGround::GetPathToHoldShort(node* A, string rwyID) {
	ground_network_path_type path = GetPath(A, rwyID);
	path.pop_back();	// That should be the threshold stripped of 
	path.pop_back();	// and that should be the arc from hold short to threshold
	// This isn't robust though - TODO - implement properly!
	return(path);
}

// A shortest path algorithm from memory (ie. I can't find the bl&*dy book again!)
// I'm sure there must be enchancements that we can make to this, such as biasing the
// order in which the nodes are searched out from in favour of those geographically
// closer to the destination.
// Note that we are working with the master set of nodes and arcs so we mustn't change
// or delete them -  we only delete the paths that we create during the algorithm.
ground_network_path_type FGGround::GetShortestPath(node* A, node* B) {
	a_path* pathPtr;
	shortest_path_map_type pathMap;
	node_array_type nodesLeft;
	
	// Debugging check
	int pathsCreated = 0;
	
	// Initialise the algorithm
	nodesLeft.push_back(A);
	pathPtr = new a_path;
	pathsCreated++;
	pathPtr->path.push_back(A);
	pathPtr->cost = 0;
	pathMap[A->nodeID] = pathPtr;
	bool solution_found = false;	// Flag to indicate that at least one candidate path has been found
	int solution_cost = -1;			// Cost of current best cost solution.  -1 indicates no solution found yet.
	a_path solution_path;		
											
	node* nPtr;	// nPtr is used to point to the node we are currently working with
	
	while(nodesLeft.size()) {
		//cout << "\n*****nodesLeft*****\n";
		//for(unsigned int i=0; i<nodesLeft.size(); ++i) {
			//cout << nodesLeft[i]->nodeID << '\n';
		//}
		//cout << "*******************\n\n";
		nPtr = *nodesLeft.begin();		// Thought - definate optimization possibilities here in the choice of which nodes we process first.
		nodesLeft.erase(nodesLeft.begin());
		//cout << "Walking out from node " << nPtr->nodeID << '\n';
		for(unsigned int i=0; i<nPtr->arcs.size(); ++i) {
			//cout << "ARC TO " << ((nPtr->arcs[i]->n1 == nPtr->nodeID) ? nPtr->arcs[i]->n2 : nPtr->arcs[i]->n1) << '\n';
		}
		if((solution_found) && (solution_cost <= pathMap[nPtr->nodeID]->cost)) {
			// Do nothing - we've already found a solution and this partial path is already more expensive
		} else {
			// This path could still be better than the current solution - check it out
			for(unsigned int i=0; i<(nPtr->arcs.size()); i++) {
				// Map the new path against the end node, ie. *not* the one we just started with.
				unsigned int end_nodeID = ((nPtr->arcs[i]->n1 == nPtr->nodeID) ? nPtr->arcs[i]->n2 : nPtr->arcs[i]->n1);
				//cout << "end_nodeID = " << end_nodeID << '\n';
				//cout << "pathMap size is " << pathMap.size() << '\n';
				if(end_nodeID == nPtr->nodeID) {
					//cout << "Circular arc!\n";
					// Then its a circular arc - don't bother!!
					//nPtr->arcs.erase(nPtr->arcs.begin() + i);
				} else {
					// see if the end node is already in the map or not
					if(pathMap.find(end_nodeID) == pathMap.end()) {
						//cout << "Not in the map" << endl;;
						// Not in the map - easy!
						pathPtr = new a_path;
						pathsCreated++;
						*pathPtr = *pathMap[nPtr->nodeID];	// *copy* the path
						pathPtr->path.push_back(nPtr->arcs[i]);
						pathPtr->path.push_back(network[end_nodeID]);
						pathPtr->cost += nPtr->arcs[i]->distance;
						pathMap[end_nodeID] = pathPtr;
						nodesLeft.push_back(network[end_nodeID]);	// By definition this can't be in the list already, or
						// it would also have been in the map and hence OR'd with this one.
						if(end_nodeID == B->nodeID) {
							//cout << "Solution found!!!" << endl;
							// Since this node wasn't in the map this is by definition the first solution
							solution_cost = pathPtr->cost;
							solution_path = *pathPtr;
							solution_found = true;
						}
					} else {
						//cout << "Already in the map" << endl;
						// In the map - not so easy - need to get rid of an arc from the higher cost one.
						//cout << "Current cost of node " << end_nodeID << " is " << pathMap[end_nodeID]->cost << endl;
						int newCost = pathMap[nPtr->nodeID]->cost + nPtr->arcs[i]->distance;
						//cout << "New cost is of node " << nPtr->nodeID << " is " << newCost << endl;
						if(newCost >= pathMap[end_nodeID]->cost) {
							// No need to do anything.
							//cout << "Not doing anything!" << endl;
						} else {
							delete pathMap[end_nodeID];
							pathsCreated--;
							
							pathPtr = new a_path;
							pathsCreated++;
							*pathPtr = *pathMap[nPtr->nodeID];	// *copy* the path
							pathPtr->path.push_back(nPtr->arcs[i]);
							pathPtr->path.push_back(network[end_nodeID]);
							pathPtr->cost += nPtr->arcs[i]->distance;
							pathMap[end_nodeID] = pathPtr;
							
							// We need to add this node to the list-to-do again to force a recalculation 
							// onwards from this node with the new lower cost to node cost.
							nodesLeft.push_back(network[end_nodeID]);
							
							if(end_nodeID == B->nodeID) {
								//cout << "Solution found!!!" << endl;
								// Need to check if there is a previous better solution
								if((solution_cost < 0) || (pathPtr->cost < solution_cost)) {
									solution_cost = pathPtr->cost;
									solution_path = *pathPtr;
									solution_found = true;
								}
							}
						}
					}
				}
			}
		}
	}
	
	// delete all the paths before returning
	shortest_path_map_iterator spItr = pathMap.begin();
	while(spItr != pathMap.end()) {
		if(spItr->second != NULL) {
			delete spItr->second;
			--pathsCreated;
		}
		++spItr;
	}
	
	//cout << "pathsCreated = " << pathsCreated << '\n';
	if(pathsCreated > 0) {
		SG_LOG(SG_ATC, SG_ALERT, "WARNING - Possible memory leak in FGGround::GetShortestPath\n\
									  Please report to flightgear-devel@flightgear.org\n");
	}
	
	//cout << (solution_found ? "Result: solution found\n" : "Result: no solution found\n");
	return(solution_path.path);		// TODO - we really ought to have a fallback position incase a solution isn't found.
}
		


// Randomly or otherwise populate some of the gates with parked planes
// (might eventually be done by the AIMgr if and when lots of AI traffic is generated)

// Return a list of exits from a given runway
// It is up to the calling function to check for non-zero size of returned array before use
node_array_type FGGround::GetExits(string rwyID) {
	// FIXME - get a 07L or similar in here and we're stuffed!!!
	return(runways[atoi(rwyID.c_str())].exits);
}

void FGGround::RequestDeparture(PlaneRec plane, FGAIEntity* requestee) {
	// For now we'll just automatically clear all planes to the runway hold.
	// This communication needs to be delayed 20 sec or so from receiving the request.
	// Even if display=false we still need to start the timer in case display=true when communication starts.
	// We also need to bear in mind we also might have other outstanding communications, although for now we'll punt that issue!
	// FIXME - sort the above!
	
	// HACK - assume that anything requesting departure is new for now - FIXME LATER
	GroundRec* g = new GroundRec;
	g->plane = plane;
	g->planePtr = requestee;
	g->taxiRequestOutstanding = true;
	g->clearanceCounter = 0;
	g->cleared = false;
	g->incoming = false;
	// TODO - need to handle the next 3 as well
    //Point3D current_pos;
    //node* destination;
    //node* last_clearance;
	
	ground_traffic.push_back(g);
}

#if 0
void FGGround::NewArrival(plane_rec plane) {
	// What are we going to do here?
	// We need to start a new ground_rec and add the plane_rec to it
	// We need to decide what gate we are going to clear it to.
	// Then we need to add clearing it to that gate to the pending transmissions queue? - or simply transmit?
	// Probably simply transmit for now and think about a transmission queue later if we need one.
	// We might need one though in order to add a little delay for response time.
	ground_rec* g = new ground_rec;
	g->plane_rec = plane;
	g->current_pos = ConvertWGS84ToXY(plane.pos);
	g->node = GetNode(g->current_pos);  // TODO - might need to sort out node/arc here
	AssignGate(g);
	g->cleared = false;
	ground_traffic.push_back(g);
	NextClearance(g);
}

void FGGround::NewContact(plane_rec plane) {
	// This is a bit of a convienience function at the moment and is likely to change.
	if(at a gate or apron)
		NewDeparture(plane);
	else
		NewArrival(plane);
}

void FGGround::NextClearance(ground_rec &g) {
	// Need to work out where we can clear g to.
	// Assume the pilot doesn't need progressive instructions
	// We *should* already have a gate or holding point assigned by the time we get here
	// but it wouldn't do any harm to check.
	
	// For now though we will hardwire it to clear to the final destination.
}

void FGGround::AssignGate(ground_rec &g) {
	// We'll cheat for now - since we only have the user's aircraft and a couple of airports implemented
	// we'll hardwire the gate!
	// In the long run the logic of which gate or area to send the plane to could be somewhat non-trivial.
}
#endif //0