diff --git a/src/process_pipeline.c b/src/process_pipeline.c
index 77dcdc7..fab6d04 100644
--- a/src/process_pipeline.c
+++ b/src/process_pipeline.c
@@ -374,6 +374,95 @@ clamp_control(controlstate *control)
         }
 }
 
+static int focus;
+static int focus_phase;
+
+static void auto_focus_start(void)
+{
+	focus_phase = 0;
+	focus = 0;
+}
+
+struct focus_stats {
+	unsigned long long sharp;
+};
+
+#define PH_SWEEP 5
+#define PH_DONE 6
+
+static void auto_focus_step(const struct focus_stats *stats)
+{
+	static uint64_t best_sharp, best_focus;
+	static const bool debug = true;
+
+	if (debug) printf("Phase %d, sharp %d best %d ", focus_phase, (int)(stats->sharp / 10000), (int)(best_sharp/ 10000));	
+	if (focus_phase >= PH_DONE) {
+		focus_phase++;
+		if (stats->sharp < (best_sharp * 6) / 10) {
+			if (debug) printf("Lost, restart.\n");
+			auto_focus_start();
+			return;
+		}
+		if (debug) printf("Idle @ %d.\n", focus);
+		return;
+	}
+	if (focus_phase < PH_SWEEP) {
+		best_sharp = 0;
+		focus = 200;
+		focus_phase ++;
+		best_focus = 0;
+		if (debug) printf("...prepare\n");
+		goto set;
+	}
+	if (stats->sharp > best_sharp) {
+		if (debug) printf("Still improving, focus %d\n", focus);
+		best_focus = focus;
+		best_sharp = stats->sharp;
+		focus += 10;
+		goto set;
+	}
+	if (stats->sharp < (best_sharp * 8) / 10) {
+		if (debug) printf("AF done?\n");
+		focus = best_focus - 10;
+		focus_phase = PH_DONE;
+		goto set;
+	}
+	if (focus > 1023) {
+		if (debug) printf("Finished range\n");
+		focus = best_focus - 10;
+		focus_phase = PH_DONE;
+		goto set;
+	}
+	if (debug) printf("Not improving\n");
+	focus += 10;
+ set:
+	state_proc.focus.value_req = focus;
+	clamp_control(&state_proc.focus);
+	mp_io_pipeline_set_control_int32(&state_proc.focus.control,
+                                                 state_proc.focus.value_req);
+	state_proc.focus.value = state_proc.focus.value_req;
+}
+
+static void
+focus_stats(struct focus_stats *stats, const unsigned int *frame, const int width,
+	const int height)
+{
+	unsigned long long sharp = 0;
+	unsigned int last_y = 0;
+
+	for (ssize_t p = 0; p < width * height; p++) {
+	        unsigned int r = (frame[p] >> 0) & 0xff;
+                unsigned int g = (frame[p] >> 8) & 0xff;
+       	        unsigned int b = (frame[p] >> 16) & 0xff;
+                unsigned int y = (r + g + b) / 3;
+		
+		sharp += (y-last_y) * (y-last_y);
+		
+		last_y = y;
+	}
+	stats->sharp = sharp;
+}
+
 static void
 process_aaa()
 {
@@ -400,6 +489,12 @@ process_aaa()
         state_proc.blacklevel -= (float)state_proc.stats.blacklevel * 0.001f;
         state_proc.blacklevel = clamp_float(state_proc.blacklevel, 0.0f, 0.07f);
 
+	if (auto_focus) {
+		struct focus_stats stats;
+		focus_stats(&stats, center, width, height);
+		auto_focus_step(&stats);
+	}        
+
         if (auto_exposure) {
                 int direction = state_proc.stats.exposure;
                 int step = 0;