Incomplete Restructuring for A* Algorithm

#include "MyMap.h"
#include "Provided.h"
#include "Support.h"

#include <string>
#include <vector>
#include <queue>
#include <list>
#include <iostream>
#include <algorithm>

using namespace std;

struct Node {
	GeoCoord coord;
	double g_cost; // Distance from starting point
	double h_cost; // Distance from end point
	double f_cost; // Total cost (g_cost + h_cost)
	string name;

	// Reverse operators because we want to be ordered with min distance at the top
	bool operator <(const Node& other) const
	{
		return f_cost > other.f_cost;
	}
};

class NavigatorImpl {
public:
	NavigatorImpl();
	~NavigatorImpl();
	bool load_map_data(string map_file);
	NavResult navigate(string start, string end, vector<NavSegment>& directions) const;
private:
	MapLoader map_load_;
	AttractionMapper attr_map_;
	SegmentMapper seg_map_;
	string direction(double angle) const;
};

string NavigatorImpl::direction(double angle) const
{
	if (0 <= angle && angle <= 22.5) return "east";
	else if (22.5 <= angle && angle <= 67.5) return "northeast";
	else if (67.5 <= angle && angle <= 112.5) return "north";
	else if (112.5 <= angle && angle <= 157.5) return "northwest";
	else if (157.5 <= angle && angle <= 202.5) return "west";
	else if (202.5 <= angle && angle <= 247.5) return "southwest";
	else if (247.5 <= angle && angle <= 292.5) return "south";
	else return "east";
}

NavigatorImpl::NavigatorImpl()
{

}

NavigatorImpl::~NavigatorImpl()
{

}

bool NavigatorImpl::load_map_data(string map_file)
{
	// This statement actually checks to see if the map file is loaded already and if not it loads it
	if (!map_load_.load(map_file)) return false;

	attr_map_.init(map_load_);
	seg_map_.init(map_load_);
	return true;
}

NavResult NavigatorImpl::navigate(string start, string end, vector<NavSegment>& directions) const
{
	if (start == end) return NAV_SUCCESS;

	bool found = false;
	Node start_node, end_node;

	// We are actually getting the geocoord of the start and storing it in start_point.coord
	// If the inputted start doesn't actually have corresponding start geocoord, return bad source
	if (!attr_map_.getGeoCoord(start, start_node.coord)) return NAV_BAD_SOURCE;
	if (!attr_map_.getGeoCoord(end, end_node.coord)) return NAV_BAD_DESTINATION;

	start_node.g_cost = 0;
	start_node.h_cost = distanceEarthMiles(start_node.coord, end_node.coord);
	start_node.f_cost = start_node.g_cost + start_node.h_cost;

	// In case the start or end is on an intersection
	vector<StreetSegment> segs_associated_with_start = seg_map_.getSegments(start_node.coord);
	vector<StreetSegment> segs_associated_with_end = seg_map_.getSegments(end_node.coord);

	MyMap<GeoCoord, double> geocoord_and_f_cost; // Used for priority
	MyMap<GeoCoord, GeoCoord> came_from; // Keep track of path
	priority_queue<Node> prio_queue_of_node;

	geocoord_and_f_cost.associate(start_node.coord, start_node.f_cost);
	came_from.associate(start_node.coord, start_node.coord);
	prio_queue_of_node.push(start_node);

	while (!prio_queue_of_node.empty())
	{
		// The very first time we enter this while loop, we input the start_node
		// We initialized the start_point.coord earlier
		Node current_node = prio_queue_of_node.top();
		prio_queue_of_node.pop();

		vector<StreetSegment> segs_associated_with_current = seg_map_.getSegments(current_node.coord);
		for (int i = 0; i < segs_associated_with_current.size(); i++) {
			for (int j = 0; j < segs_associated_with_end.size(); j++) {
				if (segs_associated_with_current[i] == segs_associated_with_end[j]) {
					found = true;
					came_from.associate(end_node.coord, current_node.coord);
				}
			}
		}
		if (found) break;

		// If it is NOT found still, we will check neighboring segments on the Geocoord
		Node next_node;
		for (int k = 0; k < segs_associated_with_current.size(); k++) {
			const StreetSegment next_street_seg = segs_associated_with_current[k];
			const GeoSegment next_geo_seg = next_street_seg.segment;

			// If our current node is at the start of a geoseg, we need to also explore its end
			// Likewise if the current node is at the end, we need to explore its start
			if (next_geo_seg.start == current_node.coord || next_geo_seg.end == current_node.coord) {
				if (next_geo_seg.start == current_node.coord) next_node.coord = next_geo_seg.end;
				else next_node.coord = next_geo_seg.start;

				// Check if where it's coming from originally isn't the same as the next coord
				//if (came_from.find(current_coord) != nullptr && *(came_from.find(current_coord)) != next_coord) {
				if (*(came_from.find(current_node.coord)) != next_node.coord) {
					const double potential_g_cost = current_node.g_cost + distanceEarthMiles(current_node.coord, next_node.coord);
					const double potential_h_cost = distanceEarthMiles(next_node.coord, end_node.coord);
					const double potential_f_cost = potential_g_cost + potential_h_cost;

					// We will see if a cost for the next coord already exists
					// If it doesn't, we will just associate it or if it has one but the new cost is smaller, we will update it
					if (geocoord_and_f_cost.find(next_node.coord) == nullptr || *(geocoord_and_f_cost.find(next_node.coord)) > potential_f_cost) {
						next_node.g_cost = potential_g_cost;
						next_node.h_cost = potential_h_cost;
						next_node.f_cost = potential_f_cost;

						geocoord_and_f_cost.associate(next_node.coord, next_node.f_cost);
						prio_queue_of_node.push(next_node);
						came_from.associate(next_node.coord, current_node.coord);
					}
				}
			}

			// If our current node is in the middle of a geosegment
			else {
				Node start_geo_seg, end_geo_seg;
				start_geo_seg.coord = next_geo_seg.start;
				end_geo_seg.coord = next_geo_seg.end;

				const double potential_start_g_cost = current_node.g_cost + distanceEarthMiles(current_node.coord, start_geo_seg.coord);
				const double potential_start_h_cost = distanceEarthMiles(start_geo_seg.coord, end_node.coord);
				const double potential_start_f_cost = potential_start_g_cost + potential_start_f_cost;
				const double potential_end_g_cost = current_node.g_cost + distanceEarthMiles(current_node.coord, end_geo_seg.coord);
				const double potential_end_h_cost = distanceEarthMiles(end_geo_seg.coord, end_node.coord);
				const double potential_end_f_cost = potential_start_g_cost + potential_start_f_cost;

				// if (geocoord_and_f_cost.find(start_geo_seg) == nullptr || *(geocoord_and_f_cost.find(next_node.coord)) > potential_f_cost) {
				if (geocoord_and_f_cost.find(start_geo_seg.coord) != nullptr || *(geocoord_and_f_cost.find(start_geo_seg.coord)) > potential_start_f_cost) {

				}
			}
		}
	}
}