Added autofocus

Added autofocus.
TODO: Change structure to open and close camera imediately before/after
capture operations. Get rid of image in dynamic data structure. Sort out
error handling.