Replaced redundant variables for the time spent each state with one variable with he time spent in current state.
The old version only tested points that where new candidats to be the totaly closest point. Therefore it failed if the closest reachable point was behind a closer unreachable one.
Note to myself: build before commit