/******************************************************************************
 * File: Trailblazer.cpp
 *
 * Implementation of the graph algorithms that comprise the Trailblazer
 * assignment.
 */
#include  "Trailblazer.h"
#include "TrailblazerGraphics.h"
#include "TrailblazerTypes.h"
#include "TrailblazerPQueue.h"

using namespace std;

/* Function: shortestPath
 *
 * Finds the shortest path between the locations given by start and end in the
 * specified world.  The cost of moving from one edge to the next is specified
 * by the given cost function.  The resulting path is then returned as a
 * Vector<Loc> containing the locations to visit in the order in which they
 * would be visited.    If no path is found, this function should report an
 * error.
 *
 * In Part Two of this assignment, you will need to add an additional parameter
 * to this function that represents the heuristic to use while performing the
 * search.  Make sure to update both this implementation prototype and the
 * function prototype in Trailblazer.h.
 */
Vector<Loc>
shortestPath(Loc start,
             Loc end,
             Grid<double>& world,
             double costFn(Loc from, Loc to, Grid<double>& world),
             double heuristic(Loc start, Loc end, Grid<double>& world)) {

            Map<Loc,Info> allInformation;
            Vector<Loc> wayYouMetHer;
             TrailblazerPQueue<Loc> discuss;
            makeList(allInformation,world);
            Info inform=allInformation.get(start);
            inform.color="YELLOW";
            colorCell(world,start,YELLOW);
            inform.way = 0;
            allInformation.put(start,inform);
            discuss.enqueue(start,heuristic(start,end,world));
            while(!discuss.isEmpty()){
                Loc loc=discuss.dequeueMin();
                Info inform=allInformation.get(loc);
                inform.color="GREEN";
                colorCell(world,loc,GREEN);
                allInformation.put(loc,inform);
                if(loc==end){
                    break;
                }
                Set<Loc> neighbours=makeNeighbours(loc,world);
                double ownDistance=allInformation.get(loc).way;
                foreach(Loc loca in neighbours){
                        Info inf=allInformation.get(loca);
                        double newDistance=ownDistance+costFn(loc,loca,world);
                        double newPr = newDistance + heuristic(loca,end,world);
                        if(inf.color=="GRAY"){
                            inf.color="YELLOW";
                            colorCell(world,loca,YELLOW);
                            inf.way=newDistance;
                            inf.parent=loc;
                            discuss.enqueue(loca,newPr);
                            allInformation.put(loca,inf);
                        }else if(inf.color=="YELLOW" && (inf.way>newDistance)){
                            inf.way=newDistance;
                            inf.parent=loc;
                            discuss.decreaseKey(loca,newPr);
                            allInformation.put(loca,inf);

                        }
                      }
                }
    wayYouMetHer =  findWay(start,end,allInformation);
    return wayYouMetHer;
}


Vector <Loc> findWay(Loc & start,Loc& end,Map<Loc,Info>& allInformation) {
    Vector <Loc> result;
    Loc curr = end;
    result.add(end);
    while(true) {
        curr = allInformation.get(curr).parent;
        result.add(curr);
        if(curr == start) {
            break;
        }
    }
    for(int i=0;i<result.size();i++) {
        Loc remainder = result[i];
        result[i] = result[result.size()-i-1];
        result[result.size()-i-1] = remainder;
    }
    return result;
}


Set<Loc> makeNeighbours(Loc & loc, Grid<double> & world){
    Set<Loc> neighbours;
    Loc neighbour;
    for(int i=-1; i<=1; i++){
        for(int j=-1; j<=1; j++){
            neighbour = makeLoc(loc.row+j,loc.col+i);
            if(neighbour.col!=-1 && neighbour.col!=world.numCols() && neighbour.row
                !=-1 && neighbour.row!=world.numRows()){
                    neighbours.add(neighbour);
            }
        }
    }
    return neighbours;
}


void makeList ( Map<Loc,Info> &allInf, Grid<double> & world){
    for(int i=0; i<world.numCols(); i++){
        for(int j=0; j<world.numRows(); j++){
            Loc loca = makeLoc(j,i);
            Info inform;
            inform.color="GRAY";
            inform.way=0;
            allInf.put(loca,inform);

        }
    }


}

Set<Edge> createMaze(int numRows, int numCols) {
    // TODO: Fill this in!
    error("createMaze is not implemented yet.");
    return Set<Edge>();
}