Implemented Pythagoras way finding

o based on craynors calculateDistanceArray function