Rewrote vision code - needs testing