Add code to find optimal path.
Improve optimal path algorithm.