Simplified Dijkstra's algorithm
Moved fast input
Updated TODO
Cleaned up geometry library.
Removed comment from geometry code.