Moved logic into directions, fixes

Fixed bugs in bearing
More fixes to movement calcs