Clean up class and implement shortest path search