Optimize distance function

Signed-off-by: Eli Skeggs <eskeggs@globesherpa.com>
Minor updates, add tests

Signed-off-by: Eli Skeggs <eskeggs@globesherpa.com>