******************************************************************************
 * 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"
#include "map.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)) {
            Map<Loc,Info> allInformation;
            Vector<Loc> wayYouMetHer;
            double wayLength=0;
            TrailblazerPQueue<Loc> discuss;
            makeList(allInformation,world);
            Info inform=allInformation.get(start);
            inform.color="YELLOW";
            colorCell(world,start,YELLOW);
            double possible=0;
            discuss.enqueue(start,possible);
            wayYouMetHer.add(start);
            while(!discuss.isEmpty()){
                Loc loc=discuss.dequeueMin();
                wayYouMetHer.add(loc);
                wayLength+=costFn(start,loc,world);
                Info inform=allInformation.get(loc);
                inform.color="GREEN";
                colorCell(world,loc,GREEN);
                if(loc==end){
                    break;
                }
                Set<Loc> neighbours=makeNeighbours(loc,world);
                foreach(Loc loca in neighbours){
                    if(allInformation.containsKey(loca)){
                        Info inf=allInformation.get(loca);
                        int ownDistance=inf.way;
                        double k=wayLength+costFn(loc,loca,world);
                        if(inf.color=="GRAY"){
                            inf.color="YELLOW";
                            colorCell(world,loca,YELLOW);
                            inf.way=k;
                            inf.parent=loc;
                            discuss.enqueue(loc,k);
                            allInformation.put(loca,inf);
                        }else if(inf.color=="YELLOW" && (ownDistance>k)){
                            inf.way=k;
                            inf.parent=loc;
                            discuss.decreaseKey(loca,k);
                            allInformation.put(loca,inf);

                        }
                        colorCell(world,loca,GREEN);
                    }
                }
            }

                 error("shortestPath is not implemented yet.");
    return wayYouMetHer;
}




Set<Loc> makeNeighbours(Loc & loc, Grid<double> & world){
    Set<Loc> neighbours;
    for(int i=-1; i<=1; i++){
        for(int j=-1; j<=1; j++){
            Loc neighbour;
            int x=loc.col+i;
            int y=loc.row+j;
            neighbour.col=x;
            neighbour.row=y;
            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;
            loca.col=i;
            loca.row=j;
            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>();
}