before merge with robot version
works with angle as approached goal criterium