Fixed distance calculation, re-written unoptimal code