Shortest path working pretty well. Need to clean up
GUI cleaned up and header comments inserted