Add a distance function
Fixed warnings
Changed operators a bit