UNTESTED: Refactored vision code to subsystem