Fixed BFS search.
Fixed energy cost issues.
Merge remote-tracking branch 'origin/Andrew-v4'