Wrote dijkstra's path. Need to be more efficient