From 528406137f6746eafd1ad4c7f87c876c145f0277 Mon Sep 17 00:00:00 2001
From: Joe Knapper <jaknapper@hotmail.com>
Date: Mon, 29 Jan 2024 18:10:11 +0000
Subject: [PATCH] Start autofocus from base of range for speed

---
 .../things/auto_recentre_stage.py                    | 12 ++++++------
 .../things/autofocus.py                              |  6 ++++--
 .../things/smart_scan.py                             | 12 ++++++------
 3 files changed, 16 insertions(+), 14 deletions(-)

diff --git a/src/openflexure_microscope_server/things/auto_recentre_stage.py b/src/openflexure_microscope_server/things/auto_recentre_stage.py
index 0e46e97b..ba11a900 100644
--- a/src/openflexure_microscope_server/things/auto_recentre_stage.py
+++ b/src/openflexure_microscope_server/things/auto_recentre_stage.py
@@ -33,14 +33,14 @@ def unpack_autofocus(scan_data):
 
     jpeg_heights = np.interp(jpeg_times, stage_times, stage_height)
 
-    turning = np.where(turningpoints(jpeg_heights))[0] + 1
+    turning = np.where(turningpoints(jpeg_heights))[0]
 
     return jpeg_heights[turning[0] : turning[1]], jpeg_sizes_MB[turning[0] : turning[1]]
 
 
 class RecentringThing(Thing):
     @thing_action
-    def looping_autofocus(self, autofocus: AutofocusDep, stage: StageDep, dz=2000):
+    def looping_autofocus(self, autofocus: AutofocusDep, stage: StageDep, dz=2000, start='centre'):
         """Repeatedly autofocus the stage until it looks focused.
         
         This action will run the `fast_autofocus` action until it settles on a point
@@ -51,17 +51,17 @@ class RecentringThing(Thing):
         repeat = True
         attempts = 0
         while repeat and attempts < 10:
-            height_min = stage.position["z"] - dz / 2
-            height_max = stage.position["z"] + dz / 2
-            data = autofocus.fast_autofocus(dz=dz)
+            data = autofocus.fast_autofocus(dz=dz, start=start)
             heights, _ = unpack_autofocus(data)
-            time.sleep(0.3)
+            height_min = np.min(heights)
+            height_max = np.max(heights)
             # TODO: max heights seems badly wrong! Something about turning?
             if (
                 stage.position["z"] - height_min < dz / 5
                 or height_max - stage.position["z"] < dz / 5
             ):
                 attempts += 1
+                start = 'centre'
             else:
                 repeat = False
 
diff --git a/src/openflexure_microscope_server/things/autofocus.py b/src/openflexure_microscope_server/things/autofocus.py
index 426fcdac..b70f8a44 100644
--- a/src/openflexure_microscope_server/things/autofocus.py
+++ b/src/openflexure_microscope_server/things/autofocus.py
@@ -143,7 +143,8 @@ class AutofocusThing(Thing):
     def fast_autofocus(
         self,
         m: SharpnessMonitorDep,
-        dz: int=2000
+        dz: int=2000,
+        start: str='centre',
     ) -> SharpnessDataArrays:
         """Sweep the stage up and down, then move to the sharpest point
         
@@ -153,7 +154,8 @@ class AutofocusThing(Thing):
         """
         with m.run():
             # Move to (-dz / 2)
-            m.focus_rel(-dz / 2)
+            if start == 'centre':
+                m.focus_rel(-dz / 2)
             # Move to dz while monitoring sharpness
             # i: Sharpness monitor index for this move
             # z: Final z position after move
diff --git a/src/openflexure_microscope_server/things/smart_scan.py b/src/openflexure_microscope_server/things/smart_scan.py
index e3a75083..0230d16b 100644
--- a/src/openflexure_microscope_server/things/smart_scan.py
+++ b/src/openflexure_microscope_server/things/smart_scan.py
@@ -480,15 +480,15 @@ class SmartScanThing(Thing):
 
                 # TODO: combine this with the move below for speed (I think this could just be "else")
                 logger.info(f"Moving to {loc}")
-                stage.move_absolute(x=int(loc[0]), y=int(loc[1]), z=int(loc[2]))
 
                 if len(focused_path) > 1:
                     z_index = closest(loc, focused_path)
+                    z=int(focused_path[z_index][2])
                     # print('Moving to {0}'.format([coords[0], coords[1], focused_path[z_index][2]]))
                     # print(focused_path)
-                    stage.move_absolute(
-                        x=int(loc[0]), y=int(loc[1]), z=int(focused_path[z_index][2])
-                    )
+                stage.move_absolute(
+                    x=int(loc[0]), y=int(loc[1]), z = z - self.autofocus_dz / 2
+                )
 
                 # Check if the image is background
                 if self.skip_background:
@@ -517,7 +517,7 @@ class SmartScanThing(Thing):
                     attempts = 0
                     if self.autofocus_dz > 200:
                         while True:
-                            recentre.looping_autofocus(dz=self.autofocus_dz)
+                            recentre.looping_autofocus(dz=self.autofocus_dz, start = 'base')
                             current_height = stage.position["z"]
 
                             # if there have been successful autofocuses in this scan, find the closest one in x-y
@@ -542,7 +542,7 @@ class SmartScanThing(Thing):
                                 focused_path.append(loc)
                                 break
                             if attempts >= 3:
-                                logger.warning("Could not autofocus after 5 attempts.")
+                                logger.warning("Could not autofocus after 3 attempts.")
                                 break
                             # if the autofocus was rejected, we return to the height of the closest successful autofocus. not perfect, but better than wandering out of focus
                             logger.info(