Fixed dijkstra search and added node distance penalty