Time Distance Calculation implemented

Large distances are even handled, though the sensor sweep still does not
handle them.