Remove accidentall commited debugging code
Correct updating function, and use proper distances.