Find shortast distace

Made the damn thing work..  DIn't comment anything though