Fixed atan2 bug. Parameters were reversed. Unittest wasn't testing the correct behavior either.