Fixed, improved, and tested distance calculation