Refactored fast autofocus

    ### FAST AUTOFOCUS

    #JPEGSharpnessMonitor = JPEGSharpnessMonitor # make the class available

    def sharpness_monitor(self):

        return JPEGSharpnessMonitor(self.microscope)

    @contextmanager

    def monitor_sharpness(self):

        m = self.sharpness_monitor()

        m = JPEGSharpnessMonitor(self.microscope)

        m.start()

        try:

            yield m

                i, z = m.focus_rel(fz - z - backlash)

            m.focus_rel(fz - z)

            return m.data_dict()

    def fast_up_down_up_autofocus(self, dz=2000, target_z=0, initial_move_up=True, mini_backlash=150):

        """Autofocus by measuring on the way down, and moving back up with feedback.

        This autofocus method is very efficient, as it only passes the peak once.

        The sequence of moves it performs is:

        1. Move to the top of the range `dz/2` (can be disabled)

        2. Move down by `dz` while monitoring JPEG size to find the focus.

        3. Move back up to the `target_z` position, relative to the sharpest image.

        4. Measure the sharpness, and compare against the curve recorded in (2) to 

           estimate how much further we need to go.  Make this move, to reach our

           target position.

        Moving back to the target position in two steps allows us to correct for

        backlash, by using the sharpness-vs-z curve as a rough encoder for Z.

        Parameters:

        dz: number of steps over which to scan (optional, default 2000)

        target_z: we aim to finish at this position, relative to focus.  This may

           be useful if, for example, you want to acquire a stack of images in Z.

           It is optional, and the default value of 0 will finish at the focus.

        initial_move_up: (optional, default True) set this to `False` to move down

           from the starting position.  Mostly useful if you're able to combine

           the initial move with something else, e.g. moving to the next scan point.

        mini_backlash: (optional, default 50) is a small extra move made in step

           3 to help counteract backlash.  It should be small enough that you

           would always expect there to be greater backlash than this.  Too small

           might slightly hurt accuracy, but is unlikely to be a big issue.  Too big

           may cause you to overshoot, which is a problem.

        """

        with self.monitor_sharpness() as m:

            df = dz #TODO: refactor so I actually use dz in the code below!

            if initial_move_up:

                m.focus_rel(df/2)

            # move down

            i, z = m.focus_rel(-df)

            # now inspect where the sharpest point is, and estimate the sharpness

            # (JPEG size) that we should find at the start of the Z stack

            jt, jz, js = m.move_data(i)

            best_z = jz[np.argmax(js)]

            target_s = np.interp([best_z+target_z], jz[::-1], js[::-1]) #NB jz is decreasing

            # now move to the start of the z stack

            i, z = m.focus_rel(best_z + target_z - z + mini_backlash) # takes us to the start of the stack

            # We've deliberately undershot - figure out how much further we should move based on the curve

            current_js = m.jpeg_size()

            imax = np.argmax(js) # we want to crop out just the bit below the peak

            js = js[imax:] # NB z is in DECREASING order

            jz = jz[imax:]

            inow = np.argmax(js < current_js) # use the curve we recorded to estimate our position

            # TODO: fancy interpolation stuff

            # So, the Z position corresponding to our current sharpness value is zs[inow]

            # That means we should move forwards, by best_z - zs[inow]

            correction_move = best_z + target_z - jz[inow]

            logging.debug("Fast autofocus scan: correcting backlash by moving {} steps".format(correction_move))

            m.focus_rel(correction_move)

            return m.data_dict()

        else:

            autofocus_enabled = False

        if fast_autofocus and not hasattr(self.microscope.plugin, 'default_fast_autofocus'):

            logging.error("Can't use fast autofocus in the scan - the plugin is missing or disabled.")

        if fast_autofocus and not hasattr(self.microscope.plugin.default_autofocus, 'monitor_sharpness'):

            logging.error("Can't use fast autofocus in the scan - the default plugin doesn't support monitor_sharpness; maybe it is too old?")

            fast_autofocus = False

        z_stack_dz = grid[2] * step_size[2] if grid[2] > 1 else 0 # shorthand for Z stack range

        sweep_to_scan_offset = 50 #TODO: make this a parameter, or calibrate it better! too small isn't a big problem, too big causes issues.

        # Construct an x-y grid (worry about z later)

        x_y_grid = construct_grid(

                # Refocus

                if autofocus_enabled:

                    if fast_autofocus:

                        # TODO: put this in the fast autofocus plugin!

                        with self.microscope.plugin.default_fast_autofocus.monitor_sharpness() as m:

                            df = autofocus_dz

                            # move down

                            i, z = m.focus_rel(-df)

                            # now inspect where the sharpest point is, and estimate the sharpness

                            # (JPEG size) that we should find at the start of the Z stack

                            jt, jz, js = m.move_data(i)

                            best_z = jz[np.argmax(js)]

                            target_s = np.interp([best_z+z_stack_dz/2.0], jz[::-1], js[::-1]) #NB jz is decreasing

                            # now move to the start of the z stack

                            i, z = m.focus_rel(best_z - z + z_stack_dz/2.0 + sweep_to_scan_offset) # takes us to the start of the stack

                            # We've deliberately undershot - figure out how much further we should move based on the curve

                            current_js = m.jpeg_size()

                            imax = np.argmax(js) # we want to crop out just the bit below the peak

                            js = js[imax:] # NB z is in DECREASING order

                            jz = jz[imax:]

                            inow = np.argmax(js < current_js) # use the curve we recorded to estimate our position

                            # TODO: fancy interpolation stuff

                            # So, the Z position corresponding to our current sharpness value is zs[inow]

                            # That means we should move forwards, by best_z - zs[inow]

                            correction_move = best_z - jz[inow] - z_stack_dz/2.0

                            logging.debug("Fast autofocus scan: correcting backlash by moving {} steps".format(correction_move))

                            m.focus_rel(correction_move)

                        self.microscope.plugin.default_autofocus.fast_up_down_up_autofocus(

                                dz=autofocus_dz,

                                target_z=-z_stack_dz/2.0, # Finish below the focus

                                initial_move_up=False, # We're already at the top of the scan

                                )

                        #TODO: save the focus data for future reference? Use it for diagnostics?

                    else:

                        logging.debug("Running autofocus")

                        self.microscope.plugin.default_autofocus.autofocus(