Added proper calculation for distance between two points