Use atan2(y,x) for calculationg angle from position

Also code refactoring and some renaming