Refactoring: move direction math into a single function. Includes some fixes and adjustments