Final fixed calculate distance

Signed-off-by: Konctantin <gawrilyako@gmail.com>