Refactoring, optimization and transfer into own method of th angle adjustment to goal