Add new autofocus algorithm