/*
* file: trailblazer.cpp
* ---------------------
* author: Nahva Techlu and Jeremy Bao
* date: Dec. 6
* This is a file that implements route searching algorithms in
* Trailblazer. It includes BFS algorithm, Dijkstras algorithm and
* A* algorithm.
*/

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

using namespace std;

// this makes Path an alias for the type Vector<Vertex*>
typedef Vector<Vertex*> Path;

//Self defined functions
Path algorithmHelper(RoadGraph& graph, Vertex* start, Vertex* end, int type, Vertex* ignore1, Vertex* ignore2);
double getPathCost(RoadGraph &graph, Path& targetPath, Vertex* end, int type);
double heuristic(RoadGraph& graph, Vertex* current, Vertex* end);
double getDifference(Path& P, Path& B);

// the minimum difference for an alternate route
const double SUFFICIENT_DIFFERENCE = 0.2;

//This function carries out Bread First Search algorithm.
Path breadthFirstSearch(RoadGraph& graph, Vertex* start, Vertex* end) {
    Queue<Path> todoList;
    Set<Vertex* > history;
    Path temp;
    temp.add(start);
    todoList.enqueue(temp);
    start->setColor(YELLOW);

    while (!todoList.isEmpty()){
        Path current = todoList.dequeue();
        Vertex* currNode = current[current.size() - 1];

        if (currNode == end){
            currNode->setColor(GREEN);
            return current;
        }

        if (history.contains(currNode)){
            continue;
        } else {
            currNode->setColor(GREEN);
            history.add(currNode);
            Set<Vertex* >neighborNodes = graph.getNeighbors(currNode);
            for (Vertex* each : neighborNodes){
                if (!history.contains(each)){
                    Path tempPath = current;
                    tempPath.add(each);
                    todoList.enqueue(tempPath);
                    each->setColor(YELLOW);
                }
            }
        }
    }
    Path emptyPath;
    return emptyPath;
}

//This function carries out Dijkstras Algorithm, which is actually implemented
//by the function algorithmHelper.
Path dijkstrasAlgorithm(RoadGraph& graph, Vertex* start, Vertex* end) {
    return algorithmHelper(graph, start, end, 0, NULL, NULL);
}

//This function carries out A* Algorithm, which is actually implemented
//by the function algorithmHelper.
Path aStar(RoadGraph& graph, Vertex* start, Vertex* end) {
    return algorithmHelper(graph, start, end, 1, NULL, NULL);
}

//This function carries out alternative route Algorithm, which is not very efficient.
//It will find the best route first, and ignore edges in that best route one at a time
//to find lots of alternative routes. Then it selects one with significant difference
//from the best route and has the lowest cost.
Path alternativeRoute(RoadGraph& graph, Vertex* start, Vertex* end) {
    Path bestPath = aStar(graph, start, end);//Use A* algorithm to find the shortest route
    Path bestAlternative;
    double lowestCost = DBL_MAX;//Initially, the lowest cost is maximum infinity, so the first qualified
                                //route is sure to have lower cost than it and can replace it

    if (bestPath.size() == 0){
        return bestAlternative;
    }
    for (int i = 0; i < bestPath.size() - 1; i++){
        Path temp = algorithmHelper(graph, start, end, 1, bestPath[i], bestPath[i+1]);//Find alternative route by ignoring edges
        double difference = getDifference(temp, bestPath);
        double cost = getPathCost(graph, temp, end, 0);
        if ((difference > SUFFICIENT_DIFFERENCE)&&(lowestCost > cost)){
            bestAlternative = temp;
            lowestCost = cost;
        }//Select the path with significant difference and has the lowest cost at the same time
    }
    return bestAlternative;
}

//This is a combined function which can carry out Dijkstras and A* algorithm at the same time.
//Type == 0 means dijkstras; Type == 1 means A*; this function can also ignore an edge between
//specified nodes (ignore1 and ignore2). If we don't want to ignore any edge, we can just put ignore1=NULL
//and ignore2=NULL
Path algorithmHelper(RoadGraph& graph, Vertex* start, Vertex* end, int type, Vertex* ignore1, Vertex* ignore2){
    PriorityQueue<Path> todoList;
    Set<Vertex* > history;
    Path temp;
    temp.add(start);
    todoList.enqueue(temp, 0.0);
    start->setColor(YELLOW);

    while (!todoList.isEmpty()){
        Path current = todoList.dequeue();
        Vertex* currNode = current[current.size() - 1];
        currNode->setColor(GREEN);
        if (currNode == end){
            return current;
        }
        if (history.contains(currNode)){
            continue;
        } else {
            history.add(currNode);
            Set<Vertex* >neighborNodes = graph.getNeighbors(currNode);
            for (Vertex* each : neighborNodes){
                if (!history.contains(each)){
                    if (!((currNode == ignore1) && (each == ignore2))){//Ignore an edge if needed
                        Path tempPath = current;
                        tempPath.add(each);
                        double tempCost = getPathCost(graph, tempPath, end, type);//Get dijkstras cost or A* cost
                        todoList.enqueue(tempPath, tempCost);
                        each->setColor(YELLOW);
                    }
                }
            }
        }
    }
    Path emptyPath;
    return emptyPath;
}

//This is a combined function that calculates the cost of dijkstras or A* algorithm.
//Type == 0 means dijkstras; Type == 1 means A*.
double getPathCost(RoadGraph& graph, Path& targetPath, Vertex* end, int type){
    if (targetPath.size() == 0){
        return DBL_MAX;
    } else if (targetPath.size() == 1) {
        return 0.0 + type * heuristic(graph, targetPath[0], end);
    } else {
        double costSum = 0;
        for (int i = 0; i < targetPath.size() - 1; i++){
            Edge* tempEdge = graph.getEdge(targetPath[i], targetPath[i+1]);
            costSum += tempEdge->cost;
        }
        if (type == 0){
            return costSum;//Dijkstras cost
        } else {
            return costSum + heuristic(graph, targetPath[targetPath.size()-1], end);//Dijkstras cost + heuristic means A* cost
        }
    }
}

//This function calculates the heuristic for A* algorithm
double heuristic(RoadGraph& graph, Vertex* current, Vertex* end){
    return (graph.getCrowFlyDistance(current, end)/graph.getMaxRoadSpeed());
}

//This function calculates the difference as defined in the spec
double getDifference(Path& P, Path& B){
    int count = 0;
    int temp = 1;
    for (Vertex* each : B){
        for (Vertex* each2 : P){
            if (each == each2){
                temp = 0;
                break;
            }
        }
        count = count + temp;
        temp = 1;
    }
    return (double)count/B.size();
}
