//Path Node Class
// Author: Andrew Garrett
// Description:
//  Represents a node, a system of which can be used
//  to calculate the shortest route between two nodes,
//  If the nodes, and the relationships between them,
//  are established

#include "stdafx.h"

PathNode::PathNode() {
	
}

bool PathNode::Connect( PathNode *targetNode, float cost, bool bidirectional ) {
	this->Connections[targetNode] = cost;

	if (bidirectional) {
		targetNode->Connect( this, cost, false );
	}

	return targetNode->BackConnect(this, cost);
}

bool PathNode::BackConnect( PathNode *srcNode, float cost ) {
	this->BackConnections[srcNode] = cost;
	return true;
}

bool PathNode::FindPath( PathNode* targetNode, std::vector<PathNode*> *route ) {
	if (route) {
		route->push_back( this );
	}

	if (targetNode == this) {
		return true;
	}

	std::vector<PathNode*> triedNodes;

	this->FindBestCost( targetNode, &triedNodes );

	if ( this->RememberedRoutes.find( targetNode ) != this->RememberedRoutes.end() ) {
		// Cache HIT
		std::map<PathNode *, PathNode *>::iterator iter;
		iter = this->RememberedRoutes.find( targetNode );

		return iter->second->FindPath( targetNode, route );
	} else if ( this->Connections.find( targetNode ) != this->Connections.end() ) {
		// We have a direct connection - WHEE - add it to the route so we have a full trace.

		return targetNode->FindPath( targetNode, route );
	} else {
		// Cache MISS
		return false;
	}
}

float PathNode::FindBestCost( PathNode* targetNode, std::vector<PathNode *> *triedNodes ) {

	bool isRecursion = false;

	// Have we already been tried?
	for( std::vector<PathNode *>::iterator iter = triedNodes->begin(); iter != triedNodes->end(); iter++ ) {
		PathNode* thisNode = *(iter);

		if (thisNode == this) {
			// Recursing. Stop it now by returning a massive value.

			isRecursion = true;
		}
	}

	if (!isRecursion)
		triedNodes->push_back( this );

	// First, is it us we're looking for?
	if (targetNode == this) {
		return 0;
	// Now, see if we have a cached one
	} else if ( this->RememberedRoutes.find( targetNode ) != this->RememberedRoutes.end() ) {
		// Cache HIT - wheee!
		std::map<PathNode *, PathNode *>::iterator iter;
		iter = this->RememberedRoutes.find( targetNode );

		float cost;

		if (!isRecursion)
			cost = iter->second->FindBestCost( targetNode, triedNodes ) + this->Connections[ iter->second ];
		else
			cost = 999 * (10^100);

		return cost;
	// Cache MISS, check each one we're connected to, and find the best score
	} else {
		std::map<PathNode *, float>::iterator iter;

		float bestCost = 0;
		PathNode* bestNode = NULL;
		float thisCost = 0;

		for ( iter = this->Connections.begin(); iter != this->Connections.end(); iter++ ) {
			if (!bestNode) {
				bestNode = iter->first;
				bestCost = iter->second;
			}

			//Short out if the cost is worse than the best overall cost
			// It makes no sense to check that.
			if (iter->second <= bestCost && !isRecursion) {
				thisCost = iter->first->FindBestCost( targetNode, triedNodes ) + iter->second;

				if (thisCost < bestCost) {
					// We have a winrar
					bestCost = thisCost;
					bestNode = iter->first;
				}
			}
		}

		// BELOW CODE IS DEPRECATED
		////if ( this->Connections.find( targetNode ) != this->Connections.end() ) {
		////	// Connections HIT - celebrate!

		////	float thisCost = this->Connections[targetNode];

		////	if (thisCost <= bestCost) {
		////		bestCost = thisCost;
		////		bestNode = targetNode;
		////	}
		////}
		//// We've figured out the best path
		// ABOVE CODE IS DEPRECATED

		this->RememberedRoutes[targetNode] = bestNode ;

		return bestCost;
	}
}