Added autofocus
Code cleanup