Implemented distance algorithm and update of (some) state variables