In dire need of refactoring. Implement priority queue for Dijkstra and redo in higher performance language