Changed distance implementation, Still needs more tests, Seems to be working
Simple distance cases working