// This is the CPP file you will edit and turn in.
// Also remove these comments here and add your own, along with
// comments on every function and on complex code sections.
// TODO: write comment header for this file; remove this comment

#include "trailblazer.h"
#include "queue.h"
#include "set.h"
#include "pqueue.h"
#include "point.h"


using namespace std;

typedef Vector<Vertex*> Path;

const double SUFFICIENT_DIFFERENCE = 0.2;

Path aStarExclude(RoadGraph &graph, Vertex*start, Vertex*end, Edge * exclude);

double pathCost(RoadGraph & g, Path & p) {
    double cost = 0;
    for(int i = 1; i < p.size(); i++) {
        Vertex * a = p[i-1];
        Vertex * b = p[i];
        cost += g.getEdge(a, b)->cost;
    }
    return cost;
}

Path breadthFirstSearch(RoadGraph& graph, Vertex* start, Vertex* end) {
    Queue<Path> fringe;
    Path startPath;
    startPath.add(start);
    fringe.enqueue(startPath);
    Set<Vertex *> seen;

    while(!fringe.isEmpty()) {
        Path currPath = fringe.dequeue();
        Vertex * last = currPath[currPath.size() - 1];
        last->setColor(GREEN);
        if(last == end) {
            return currPath;
        }
        seen.add(last);
        for(Vertex * neighbor : graph.getNeighbors(last)) {
            if(!seen.contains(neighbor)) {
                neighbor->setColor(YELLOW);
                Path newPath = currPath;
                newPath.add(neighbor);
                fringe.add(newPath);
            }
        }
    }
    Path emptyPath;
    return emptyPath;
}

Path dijkstrasAlgorithm(RoadGraph& graph, Vertex* start, Vertex* end) {
    PriorityQueue<Path> fringe;
    Path startPath;
    startPath.add(start);
    fringe.enqueue(startPath, 0);
    Set<Vertex *> seen;

    while(!fringe.isEmpty()) {
        Path currPath = fringe.dequeue();
        Vertex * last = currPath[currPath.size() - 1];
        if(seen.contains(last)) continue;
        last->setColor(GREEN);
        if(last == end) {
            return currPath;
        }
        seen.add(last);
        for(Vertex * neighbor : graph.getNeighbors(last)) {
            if(!seen.contains(neighbor)) {
                neighbor->setColor(YELLOW);
                Path newPath = currPath;
                newPath.add(neighbor);
                fringe.add(newPath, pathCost(graph, newPath));
            }
        }
    }
    Path emptyPath;
    return emptyPath;
}

double heuristic(RoadGraph & g, Vertex * last, Vertex * goal) {
    double maxRate = g.getMaxRoadSpeed();
    double dist = g.getCrowFlyDistance(last, goal);
    return dist / maxRate;
}

double getAStarCost(RoadGraph& graph, Map<Vertex*, double> & cache, Path & path, Vertex * goal) {
    Vertex * last = path[path.size() - 1];
    double cost = pathCost(graph, path);
    /*if(cache.containsKey(last)) {
        cost = cache[last];
    } else {
        cost = pathCost(graph, path);
        cache[last] = cost;
    }*/

    return cost + heuristic(graph, last, goal);
}

Path aStar(RoadGraph& graph, Vertex* start, Vertex* end) {
    return aStarExclude(graph, start, end, NULL);
}

bool sufficientlyDifferent(Path a, Path b) {
    Set<Node *> nodesInA;
    Set<Node *> nodesInB;
    for(Vertex * v : a) nodesInA.add(v);
    for(Vertex * v : b) nodesInB.add(v);
    int numUnique = nodesInB.removeAll(nodesInA).size();
    double numInA = a.size();
    return (numUnique / numInA) > SUFFICIENT_DIFFERENCE;
}

bool pathContains(Path & a, Vertex * node) {
    for(Vertex * v : a) {
        if(v == node) return true;
    }
    return false;
}

Path aStarExclude(RoadGraph &graph, Vertex*start, Vertex*end, Edge * exclude) {
    PriorityQueue<Path> fringe;
    Path startPath;
    startPath.add(start);
    fringe.enqueue(startPath, 0);
    Set<Vertex *> seen;
    Map<Vertex *, double> cache;

    while(!fringe.isEmpty()) {
        Path currPath = fringe.dequeue();

        Vertex * last = currPath[currPath.size() - 1];
        if(seen.contains(last)) continue;
        last->setColor(GREEN);
        if(last == end) {
            return currPath;
        }
        seen.add(last);
        for(Vertex * neighbor : graph.getNeighbors(last)) {
            Edge * edge = graph.getEdge(last, neighbor);
            if(edge != exclude && !seen.contains(neighbor)) {
                neighbor->setColor(YELLOW);
                Path newPath = currPath;
                newPath.add(neighbor);
                fringe.add(newPath, getAStarCost(graph, cache, newPath, end));
            }
        }
    }
    Path emptyPath;
    return emptyPath;
}

Path alternativeRoute(RoadGraph& graph, Vertex* start, Vertex* end) {
    Path best = aStarExclude(graph, start, end, NULL);
    PriorityQueue<Path> alternatives;
    for(int i = 1; i < best.size(); i++) {
        Edge * edge = graph.getEdge(best[i-1], best[i]);
        Path alt = aStarExclude(graph, start, end, edge);
        alternatives.add(alt, pathCost(graph, alt));
    }

    while(!alternatives.isEmpty()) {
        Path alt = alternatives.dequeue();
        if(sufficientlyDifferent(best, alt)) {
            return alt;
        }
    }

    Path emptyPath;
    return emptyPath;
}

