Improve speed by using two sets for State management:
states: Contains all the states we need to visit indexed by state - Cost is ignored.
costs: Contains all the states we need to visit indexed by cost then state.
This then means the lookup for the minimum cost is fast.
