#include <stdio.h>
#include <stdlib.h>
#include "..\cstage.h"
#include "..\cscope.h"
#include "..\cfilter.h"
#include "..\chamcam.h"
#include "..\cpifoc.h"
#include "..\acquisition.h"
#include "..\reporter.h"
#include "..\config_reader.h"
//#include "..\tiff.h"
//#include "..\tiffio.h"
//#include "..\tiffconf.h"
//#include "..\tiffvers.h"


//#define _FRAMESIZE_X_STAGEUNITS 400
//#define _FRAMESIZE_Y_STAGEUNITS 400
//#define _RASTERSIZE_X_FRAMES 26
//#define _RASTERSIZE_Y_FRAMES 26
//#define _INITIAL_OFFSET_X -10322
#define _INITIAL_OFFSET_Y 0

#define _BRIGHTFIELD_EXPOSURETIME 0.02
#define _BRIGHTFIELD_SENSITIVITY 0
#define _BRIGHTFIELD_INTENS_THRESH 11000

//steps must be an odd integer
#define _AUTOFOCUS_BF_CSTEPS 38
#define _AUTOFOCUS_BF_CSPACING 0.15

#define DEBUG_LEVEL 2


//UNCOMMENT THIS IF A PIFOC IS NOT PRESENT ON THE ACQUISITION INSTRUMENT
//
//#define FINE_FOCUS_TE2000


char *run_name="C:\\test1";
char list_filename[500];
char log_filename[500];
char runinfo_filename[500];
char log_string[500];
char img_output_dir[500];
int focus_axis=0;//autofocus_brightfield uses TE2000 if 0, pifoc if 1; DO NOT SET HERE!!!

int _RASTERSIZE_X_FRAMES=0;
int _RASTERSIZE_Y_FRAMES=0;
int _INITIAL_OFFSET_X=0;
int _FRAMESIZE_X_STAGEUNITS=1;
int _FRAMESIZE_Y_STAGEUNITS=1;


short unsigned int *temp_img;

FILE *poslist_file;
FILE *runinfo_file;

Reporter r(DEBUG_LEVEL);

CPifoc this_pifoc(&r);
CScope this_te2000(&r);
CStage this_stage(&r);
CHamCam this_camera(&r);
CFilter this_filter(&r);
Acquisition acq(&r, &this_camera, &this_filter);
Config_reader cfg("..\\..\\master.cfg");


double compute_complexity(short unsigned int*);
bool save_image(short unsigned int*, int, char*);
double autofocus_brightfield(int, double);
double compute_complexity_autofocus(short unsigned int*);
short unsigned int *acquire_getdata(int framenum);
double autofocus_fl(int num_steps_coarse, double coarse_interval, double exposure, double sensitivity, int ex, int em);


void main(int argc, char* argv[]){

	int num_xcols, num_yrows; //number of raster positions
	int col_size=_FRAMESIZE_X_STAGEUNITS, row_size=_FRAMESIZE_Y_STAGEUNITS; //size of each frame in stage units
	double initial_offset_x, initial_offset_y=_INITIAL_OFFSET_Y; //position where rectangle starts

	double curr_x, curr_y, curr_pifoc_z, curr_te2000_z, initial_x, initial_y, initial_te2000_z, initial_pifoc_z;
	//double curr_complexity, complexity_thresh=1100;
	double curr_complexity, complexity_thresh=475;
	int num_frames=0;

	int i,j;
	short unsigned int *curr_img;
	bool success;
	char config_val[80];

	double last_zpos;


	//PARSE COMMAND LINE ARGS
	fprintf(stderr, "%d\n", argc);
	if(argc < 4){
		fprintf(stderr, "ERROR: you did not start start_seq_run.exe with the correct number of arguments.\n");
		fprintf(stderr, " (should be .\\start_seq_run num_xcols num_yrows initial_x_offset)\n");
		exit(42);
	}
	_RASTERSIZE_X_FRAMES = atoi(argv[1]);
	_RASTERSIZE_Y_FRAMES = atoi(argv[2]);
	_INITIAL_OFFSET_X = atoi(argv[3]);
	if(argc == 5){
		complexity_thresh = double(atoi(argv[4]));
	}
//	_INITIAL_OFFSET_X = _INITIAL_OFFSET_X * -1;
	num_xcols=_RASTERSIZE_X_FRAMES;
	num_yrows=_RASTERSIZE_Y_FRAMES; 
	initial_offset_x = (double) _INITIAL_OFFSET_X;

	/*************************************************/
	/* BUILD OUTPUT FILENAMES AND OPEN FILES         */
	strcpy(log_filename, "");
	strcat(log_filename, run_name);
	strcat(log_filename, "\\");
	strcat(log_filename, "start_seq_run.log");
	r.open(log_filename, 'w');

	sprintf(log_string, "Using %s for log file", log_filename); 
	r.log(log_string, 1);

	strcpy(runinfo_filename, "");
	strcat(runinfo_filename, run_name);
	strcat(runinfo_filename, "\\");
	strcat(runinfo_filename, "runinfo.dat");
	sprintf(log_string, "Using %s for runinfo file", runinfo_filename);
	r.log(log_string,1);

	if((runinfo_file = fopen(runinfo_filename, "w")) == NULL){
		r.error("cannot open runinfo output file",1);
	}

	strcpy(list_filename, "");
	strcat(list_filename, run_name);
	strcat(list_filename, "\\");
	strcat(list_filename, "position_list.dat");
	sprintf(log_string, "Using %s for list file.\n", list_filename);
	r.log(log_string, 1);
	if((poslist_file = fopen(list_filename, "w")) == NULL){
		r.error("cannot open position list file", 1);
	}

	
	strcpy(img_output_dir, "mkdir ");
	strcat(img_output_dir, run_name);
	strcat(img_output_dir, "\\");
	strcat(img_output_dir, "999");
	strcat(img_output_dir, "\\");
	system(img_output_dir);

	strcpy(img_output_dir, "");
	strcat(img_output_dir, run_name);
	strcat(img_output_dir, "\\999\\");
	strcat(img_output_dir, "WL_");
	sprintf(log_string, "Using %s for image output directory.\n", img_output_dir);
	r.log(log_string, 1);
	/*                                               */
	/*************************************************/


	/*************************************************/
	/* LOAD PARAMS FROM CFG FILE                     */
	if(!(cfg.getTag(config_val, "framesize_x_stageunits"))) r.error("config file does not include tag framesize_x_stageunits",1);
	_FRAMESIZE_X_STAGEUNITS = atoi(config_val); sprintf(log_string, "setting _FRAMESIZE_X_STAGEUNITS = %d", _FRAMESIZE_X_STAGEUNITS); r.log(log_string, 1); 
	if(!(cfg.getTag(config_val, "framesize_y_stageunits"))) r.error("config file does not include tag framesize_y_stageunits",1);
	_FRAMESIZE_Y_STAGEUNITS = atoi(config_val); sprintf(log_string, "setting _FRAMESIZE_Y_STAGEUNITS = %d", _FRAMESIZE_Y_STAGEUNITS); r.log(log_string, 1); 
	/*                                               */
	/*************************************************/


	
	/*************************************************/
	/* INITIALIZE HARDWARE                           */
	r.log("Initializing scope", 1);
	if(!(success=this_te2000.Init())){
		r.error("TE2000 initialization failed", 1);
	}

	r.log("Initializing stage", 1);
	if(!(success=this_stage.Init())){
		r.error("stage initialization failed", 1);
	}

	r.log("Initializing camera", 1);
	if(!(success=this_camera.Init())){
		r.error("camera initialization failed", 1);
	}

	r.log("Initializing filter wheels and shutters", 1);
	if(!(success=this_filter.Init())){
		r.error("filter wheel initialization failed", 1);
	}

#ifndef FINE_FOCUS_TE2000
	r.log("Initializing pifoc", 1);
	if(!(success=this_pifoc.Init())){
		r.error("pifoc initialization failed", 1);
	}
#endif
	/*                                               */
	/*************************************************/




	/*************************************************/
	/* ALLOCATE MEMORY                               */
	r.log("Allocating memory for image storage", 1);
	if((curr_img = (short unsigned int*) malloc(1000000 * sizeof(short unsigned int))) == NULL){
		r.error("memory allocation for current image failed", 1);
	}

	if((temp_img = (short unsigned int*) malloc(1000000 * sizeof(short unsigned int))) == NULL){
		r.error("memory allocation for temp image failed", 1);
	}
	/*                                               */
	/*************************************************/



	//ASSUME WE ARE AT THE ORIGIN; GET X, Y AND RECORD AS INITIAL POSITIONS
	curr_x = this_stage.GetPosX();
	curr_y = this_stage.GetPosY();
	curr_te2000_z = this_te2000.GetPos();
#ifdef FINE_FOCUS_TE2000
	r.log("**NO PIFOC USED; ANY PIFOC POSITIONS REPORTED ARE MEANINGLESS", 1);
	curr_pifoc_z = 0;
#else
	curr_pifoc_z = this_pifoc.GetPos();
#endif
	
	
	sprintf(log_string, "Starting sequencing run; instrument at %f, %f, %f, %f, (X, Y, Z1, Z2)", curr_x, curr_y, curr_te2000_z, curr_pifoc_z);
	r.log(log_string, 1);

	//PROPER ILLUMINATION FOR BRIGHTFIELD IMAGING
	//triple cube
	//	sprintf(log_string, "Setting filter wheels for brightfield illumination: %d, %d, %d (speed, wheelA, wheelC)", 2, 5, 5);
	sprintf(log_string, "Setting filter wheels for brightfield illumination: %d, %d, %d (speed, wheelA, wheelC)", 2, 1, 1);
	this_te2000.OpenUniblitz();
	r.log(log_string, 1);
//	if(!this_filter.SetPosition(2, 5, 5)){
	if(!this_filter.SetPosition(2, 1, 1)){
		r.error("set filter wheels failed", 1);
	}

//triple cube
//	sprintf(log_string, "Setting TE2000 to filter block %d", 1);
	sprintf(log_string, "Setting TE2000 to filter block %d", 5);
	r.log(log_string, 1);
//	if(!this_te2000.SetFilterBlock(1)){
	if(!this_te2000.SetFilterBlock(5)){
	r.error("set TE2000 filter block failed", 1);
	}


//	sprintf(log_string, "Setting camera for 'snap' acquisition in %d-frame blocks", _AUTOFOCUS_BF_CSTEPS + 1); 
	sprintf(log_string, "Setting camera for 'snap' acquisition in %d-frame blocks", 21);
	r.log(log_string, 1);
//	if(!(success=this_camera.SetupMultiple(_AUTOFOCUS_BF_CSTEPS + 1))){
	if(!(success=this_camera.SetupMultiple(21))){
		r.error("could not setup camera for acquisition", 1);
	}

	this_camera.SetExposure(0.020);
	this_camera.SetSensitivity(0);


	//AUTOFOCUS TO GET THE INITIAL TE2000 POSITION
	//QUEUE MULTI-FRAME CAPTURE
//	curr_te2000_z = autofocus_brightfield(_AUTOFOCUS_BF_CSTEPS, _AUTOFOCUS_BF_CSPACING);
	curr_te2000_z = autofocus_brightfield(21, 0.5);
	this_camera.EndMultiple();

	sprintf(log_string, "Setting camera for 'snap' acquisition in %d-frame blocks", _AUTOFOCUS_BF_CSTEPS + 1); 
	r.log(log_string, 1);
	if(!(success=this_camera.SetupMultiple(_AUTOFOCUS_BF_CSTEPS + 1))){
		r.error("could not setup camera for acquisition", 1);
	}

#ifdef FINE_FOCUS_TE2000
	fprintf(runinfo_file, "**NO PIFOC USED; FINE FOCUS IS TE2000\n");
	curr_te2000_z = autofocus_brightfield(_AUTOFOCUS_BF_CSTEPS, _AUTOFOCUS_BF_CSPACING);

#else
	focus_axis = 1;
//	acq.acquire_expose_noshutter(_BRIGHTFIELD_EXPOSURETIME, _BRIGHTFIELD_SENSITIVITY);
//	curr_img = acq.acquire_getdata(_AUTOFOCUS_BF_CSTEPS);
	curr_pifoc_z = autofocus_brightfield(_AUTOFOCUS_BF_CSTEPS, _AUTOFOCUS_BF_CSPACING);
#endif


	//WE NOW HAVE INITIAL POSITION FOR X, Y, TE2000_Z, AND MAYBE PIFOC_Z;
	//REPORT THEM TO THE EXPERIMENT LOG FILE AND TO THE STDOUT IF DESIRED
	initial_x = curr_x;
	initial_y = curr_y;
	initial_te2000_z = curr_te2000_z;
	initial_pifoc_z = curr_pifoc_z;
	sprintf(log_string, "Autofocus completed.  Instrument at %f, %f, %f, %f (X, Y, Z1, Z2)", initial_x, initial_y, initial_te2000_z, initial_pifoc_z);
	r.log(log_string, 1);

	fprintf(runinfo_file, "#TIME RUN STARTED:\n");
	fprintf(runinfo_file, "%s\n", "0/0/00 00:00:00");
	fprintf(runinfo_file, "#EXPERIMENT ID:\n");
	fprintf(runinfo_file, "%s\n", "DEFAULT");
	fprintf(runinfo_file, "#FLOWCELL:\n");
	fprintf(runinfo_file, "%s\n", "R/L");
	fprintf(runinfo_file, "#COLS IN RASTER:\n");
	fprintf(runinfo_file, "%d\n", num_xcols);
	fprintf(runinfo_file, "#ROWS IN RASTER:\n");
	fprintf(runinfo_file, "%d\n", num_yrows);
	fprintf(runinfo_file, "#COL_SIZE:\n");
	fprintf(runinfo_file, "%d\n", col_size);
	fprintf(runinfo_file, "#ROW_SIZE:\n");
	fprintf(runinfo_file, "%d\n", row_size);
	fprintf(runinfo_file, "#INITIAL X:\n");
	fprintf(runinfo_file, "%.0f\n", initial_x);
	fprintf(runinfo_file, "#INITIAL Y:\n");
	fprintf(runinfo_file, "%.0f\n", initial_y);
	fprintf(runinfo_file, "#INITIAL Z-TE2000:\n");
	fprintf(runinfo_file, "%f\n", initial_te2000_z);
	fprintf(runinfo_file, "#INITIAL Z-PIFOC:\n");
	fprintf(runinfo_file, "%f\n", initial_pifoc_z);
	fflush(runinfo_file);


	//Output initial position to position_log file
#ifdef FINE_FOCUS_TE2000
	fprintf(poslist_file, "%f\t%f\t%f\t%f\n", curr_x, curr_y, curr_te2000_z, curr_te2000_z);
#else
	fprintf(poslist_file, "%f\t%f\t%f\t%f\n", curr_x, curr_y, curr_te2000_z, curr_pifoc_z);
#endif
	fflush(poslist_file);

	
	//ACQUIRE AND SAVE BASE IMAGE
	r.log("Acquiring base image", 1);
	if(acq.acquire_expose_noshutter(_BRIGHTFIELD_EXPOSURETIME, _BRIGHTFIELD_SENSITIVITY)){
		curr_img = acq.acquire_getdata(_AUTOFOCUS_BF_CSTEPS);
		save_image(curr_img, 0, "C:\\test1\\BASE");
	}
	else{
		r.error("no image was acquired.  Saving blank image", 0);
		save_image(curr_img, 0, "C:\\test1\\BASE");
	}

	//MOVE TO X,Y INITIAL (initial_offset_x, initial_offset_y)
	initial_offset_x = curr_x + initial_offset_x;
	initial_offset_y = curr_y + initial_offset_y;
	this_stage.SetPosition((int)initial_offset_x, (int)initial_offset_y);
	while(!this_stage.ConfirmPosition()){;}

	curr_x = this_stage.GetPosX();
	curr_y = this_stage.GetPosY();

//FROM THIS POINT FORWARD, WE WILL USE INITIAL_PIFOC_Z TO SPECIFY
//THE FINE FOCUS AXIS, REGARDLESS OF WHAT THE ACTUAL HARDWARE IS
//(EITHER TE2000 OR PIFOC)
#ifdef FINE_FOCUS_TE2000
	initial_pifoc_z = curr_te2000_z;
#endif

	last_zpos = initial_pifoc_z;

	for(i=0; i<num_yrows; i++){
		for(j=0; j<num_xcols; j++){
			sprintf(log_string, "Frame %d of %d", i * num_xcols + j + 1, num_yrows*num_xcols); 
			r.log(log_string, 1);

			//move stage to position
			if(i%2==0){ // move right to left on even rows
				this_stage.SetPosition((int)initial_offset_x + (col_size * (j)), (int)initial_offset_y + (row_size * i));
			}
			else{ // move left to right on odd rows
				this_stage.SetPosition( (int)(((int)initial_offset_x+(int)((num_xcols-1)*col_size)) - (j*col_size)), (int)initial_offset_y + (row_size * i)); 
			}

#ifdef FINE_FOCUS_TE2000
			sprintf(log_string, "--Last good frame position: %f; Z at start (from hardware): %f; autofocus acquire...", last_zpos, this_te2000.GetPos()); 
			r.log(log_string, 2);
#else
			sprintf(log_string, "--Last good frame position: %f; Z at start (from hardware): %f; autofocus acquire...", last_zpos, this_pifoc.GetPos()); 
			r.log(log_string, 2);
#endif


			while(!(this_stage.ConfirmPosition())){;}//wait for stage to move into position for the next iteration

			//UPDATE POSITIONS
			curr_x = this_stage.GetPosX();
			curr_y = this_stage.GetPosY();	
			curr_pifoc_z = autofocus_brightfield(_AUTOFOCUS_BF_CSTEPS, _AUTOFOCUS_BF_CSPACING);			
//			curr_pifoc_z = autofocus_fl(_AUTOFOCUS_BF_CSTEPS, _AUTOFOCUS_BF_CSPACING*2, 0.030, 100, 6, 6);			


			//ACQUIRE CURR_IMAGE
			if(acq.acquire_expose_noshutter(_BRIGHTFIELD_EXPOSURETIME, _BRIGHTFIELD_SENSITIVITY)){
//			if(acq.acquire_expose_shutter(0.030, 100)){
				curr_img = acq.acquire_getdata(_AUTOFOCUS_BF_CSTEPS);
				sprintf(log_string, "--Candidate image was acquired at plane %f", curr_pifoc_z);
				r.log(log_string, 2);
			}
			else{
				r.error("no image was acquired.  Saving blank image", 0);
			}
	

			//GOOD BEAD IMAGE?
			if( (curr_complexity=compute_complexity(curr_img)) > complexity_thresh){
				num_frames++;

				sprintf(log_string, "--Good autofocus found; logging position %d: %f, %f, %f\n", num_frames-1, curr_x, curr_y, curr_pifoc_z); 
				r.log(log_string, 2);

				fprintf(poslist_file, "%i\t%f\t%f\t%f\t%f\n", num_frames - 1, curr_x, curr_y, curr_pifoc_z, curr_complexity);
//				fprintf(poslist_file, "%i\t%f\t%f\t%f\t%f\n", num_frames - 1, curr_x, curr_y, curr_pifoc_z, 1);//---------------------
				fflush(poslist_file);

				last_zpos = curr_pifoc_z;

				if(!(save_image(curr_img, num_frames, img_output_dir))){
					sprintf(log_string, "writing image %d to file failed", num_frames);
					r.error(log_string, 0);
				}
#ifndef FINE_FOCUS_TE2000
				this_te2000.SetPosition(initial_te2000_z); //MAKE SURE TE2000 STAYS AT THE SAME POSITION
				while(!(this_te2000.ConfirmPosition())){;}
#endif
			}

			else{ //NO GOOD IMAGE; MOVE BACK TO MEMORIZED Z
				sprintf(log_string, "--No good autofocus found (contrast %f); Z at end: %f, returned Z to %f for next frame.\n", curr_complexity, curr_pifoc_z, last_zpos); 
				r.log(log_string, 2);

#ifdef FINE_FOCUS_TE2000
				this_te2000.SetPosition(last_zpos);
				while(!(this_te2000.ConfirmPosition())){;}
#else
				this_te2000.SetPosition(initial_te2000_z);
				this_pifoc.SetPosition(last_zpos);
				while(!(this_pifoc.ConfirmPosition())){;}
				while(!(this_te2000.ConfirmPosition())){;}
#endif
			}
		}
	}

	//POSITION LOG GENERATED; MOVE BACK TO INITIAL POS
	this_stage.SetPosition((int)initial_x, (int)initial_y);
#ifdef FINE_FOCUS_TE2000		
	this_te2000.SetPosition(initial_pifoc_z);
	while(!(this_te2000.ConfirmPosition())){;}
#else
	this_te2000.SetPosition(initial_te2000_z);
	while(!(this_te2000.ConfirmPosition())){;}
	this_pifoc.SetPosition(initial_pifoc_z);
	while(!(this_pifoc.ConfirmPosition())){;}
#endif

	sprintf(log_string, "start_seq_run finished.  Visited %d positions and found %d good images", num_yrows*num_xcols, num_frames);
	r.log(log_string, 1);

	this_te2000.CloseUniblitz();

	this_stage.Close();
	this_te2000.Close();
	this_camera.Close();
	this_filter.Close();
#ifndef FINE_FOCUS_TE2000
	this_pifoc.Close();
#endif

	fclose(runinfo_file);
	fclose(poslist_file);
}


/*
bool save_image(short unsigned int *curr_img, int curr_framenum, char *img_output_dirname){
	TIFF *image;
	tsize_t strip_size=8000;
	unsigned long image_offset=0;
	int strip_max=250, strip_count, result;
	char img_output_fullname[500];
	char str_framenum[500];


	strcpy(img_output_fullname, "");
	strcat(img_output_fullname, img_output_dirname);
	sprintf(str_framenum, "%04d", curr_framenum);
	strcat(img_output_fullname, str_framenum);
	strcat(img_output_fullname, ".tif");

	if((image = TIFFOpen(img_output_fullname,"w")) == NULL){
		return false;
	}
	else{
		TIFFSetField(image, TIFFTAG_IMAGEWIDTH, 1000);
		TIFFSetField(image, TIFFTAG_IMAGELENGTH, 1000);
		TIFFSetField(image, TIFFTAG_BITSPERSAMPLE, 16);
		TIFFSetField(image, TIFFTAG_ROWSPERSTRIP, 4);
		TIFFSetField(image, TIFFTAG_PHOTOMETRIC, PHOTOMETRIC_MINISBLACK);
		TIFFSetField(image, TIFFTAG_PLANARCONFIG, PLANARCONFIG_CONTIG);
		TIFFSetField(image, TIFFTAG_XRESOLUTION, 72.0);
		TIFFSetField(image, TIFFTAG_YRESOLUTION, 72.0);
		TIFFSetField(image, TIFFTAG_RESOLUTIONUNIT, RESUNIT_INCH);

		for (strip_count = 0; strip_count < strip_max; strip_count++){
    
		    if((result = TIFFWriteEncodedStrip (image, strip_count, curr_img + image_offset, strip_size)) == -1){
				fprintf(stderr, "Write error on input strip number %d\n", strip_count);
				TIFFClose(image);
				return false;
			}
			image_offset += result;
		}
	}
	TIFFClose(image);
	return true;
}*/

bool save_image(short unsigned int *curr_img, int curr_framenum, char *img_output_dirname){
	char img_output_fullname[500];
	char str_framenum[500];
	FILE *img_outfile;

	strcpy(img_output_fullname, "");
	strcat(img_output_fullname, img_output_dirname);
	sprintf(str_framenum, "%04d", curr_framenum);
	strcat(img_output_fullname, str_framenum);
	strcat(img_output_fullname, ".raw");

	if((img_outfile = fopen(img_output_fullname, "w+b"))==NULL){
		sprintf(log_string, "ERROR opening raw output file %s", img_output_fullname);
		r.error(log_string, 0);
		return false;
	}
			
	fwrite(curr_img, 2, 1000000, img_outfile);
	fclose(img_outfile);
	return true;
}

double compute_complexity(unsigned short int *curr_img){
	double delta_sum, delta_count, delta_mean;
	int i;

	delta_count = 0;
	delta_sum = 0;
	for(i=0; i<999999; i++){
		if(((i+1)%1000)){
			if(((i)%1000)){
				delta_sum = delta_sum + (double)abs( (*(curr_img + i) - *(curr_img + i + 1)));
				delta_count++;
			}
		}
	}

	if(delta_count > 0){
		delta_mean = delta_sum / delta_count;
	}
	else{
		r.error("--pixel_count==0", 0);
		delta_mean = 0;
	}

	sprintf(log_string, "--Contrast = %.0f", delta_mean);
	r.log(log_string, 2);

	return delta_mean;
}


double compute_complexity_autofocus(short unsigned int* curr_img){
	double above_thresh = 0;
	double thresh = _BRIGHTFIELD_INTENS_THRESH;
	int i;
	for(i=0; i<1000000; i++){
		if((double)*(curr_img + i) > thresh) above_thresh++;
	}
	
	sprintf(log_string, "Pixels above threshold: %f",above_thresh);
	r.log(log_string, 3);

	return above_thresh;
}

	
double autofocus_brightfield(int num_steps_coarse, double coarse_interval){
	int i;
	double initial_zpos, start_focus_pos, curr_focus_pos;
	short unsigned int **image_ptr_stack;
	double max_complexity=0, max_complexity_zpos, curr_complexity;
	double *focus_pos;

	if((image_ptr_stack = (short unsigned int**)malloc(num_steps_coarse * sizeof(short unsigned int*)))==NULL){
		r.error("allocate memory for autofocus pointer stack failed", 1);
	}
	
	if((focus_pos = (double*) malloc( (num_steps_coarse + (int) ((coarse_interval / 0.05)+1) ) * sizeof(double) ))==NULL){
		r.error("allocate memory for focus position list failed", 1);
	}

	if(focus_axis==1){//PIFOC USED
		initial_zpos = this_pifoc.GetPos();
	}
	else{
		initial_zpos = this_te2000.GetPos();
	}

	start_focus_pos = initial_zpos - (((num_steps_coarse - 1)/2)*coarse_interval);
	max_complexity_zpos = initial_zpos;

	sprintf(log_string, "----Initial autofocus plane: %f", initial_zpos);
	r.log(log_string, 3);

	if(!this_camera.QueueCapture()){
		r.error("queue capture of multiple frames failed", 0);
		return initial_zpos;
	}

	for(i=0; i< num_steps_coarse; i++){
		
		curr_focus_pos = start_focus_pos + (i*coarse_interval);
		sprintf(log_string, "---- %d %f", i, curr_focus_pos);
		r.log(log_string, 3);

		if(focus_axis==1){
			this_pifoc.SetPosition(curr_focus_pos);
			while(!(this_pifoc.ConfirmPosition())){;}
			*(focus_pos + i) = this_pifoc.GetPos();
		}
		else{
			this_te2000.SetPosition(curr_focus_pos);
			while(!(this_te2000.ConfirmPosition())){;}
			*(focus_pos + i) = this_te2000.GetPos();
		}

		r.log("----Acquire start", 4);
		if(!acq.acquire_expose_noshutter(_BRIGHTFIELD_EXPOSURETIME, _BRIGHTFIELD_SENSITIVITY)){
			sprintf(log_string, "Error acquiring image %d in brightfield autofocus stack", i);
			r.error(log_string, 0);
		}
		else{
			image_ptr_stack[i] = acq.acquire_getdata(i);  
		}
		r.log("----Acquire end", 4);
	}
	
	max_complexity = 0;
	for(i=0; i<num_steps_coarse; i++){
		curr_complexity = compute_complexity_autofocus(image_ptr_stack[i]);

		sprintf(log_string, "---- Pixels above threshold for position %02d (z plane %f) = %f\n", i, *(focus_pos + i), curr_complexity);
		r.log(log_string, 3);

		if(curr_complexity > max_complexity){
			sprintf(log_string, "CURR_PIXELCOUNT %f > MAX_PIXELCOUNT %f", curr_complexity, max_complexity);
			r.log(log_string, 4);
			max_complexity = curr_complexity;
			max_complexity_zpos = *(focus_pos + i);
		}
	}
	sprintf(log_string, "----Best position: %f, score: %f", max_complexity_zpos, max_complexity); 
	r.log(log_string, 3);

//WE'VE FOUND THE RIGHT POSITION IN THE STACK, NOW MOVE THERE
	if(focus_axis==1){
		this_pifoc.SetPosition(max_complexity_zpos);
		while(!(this_pifoc.ConfirmPosition())){;}
	}
	else{
		sprintf(log_string, "Setting focus to position %f (started at %f)", max_complexity_zpos, initial_zpos);
		r.log(log_string, 2);
		this_te2000.SetPosition(max_complexity_zpos);
		while(!(this_te2000.ConfirmPosition())){;}
	}

	free(image_ptr_stack);
	free(focus_pos);
	return max_complexity_zpos;
}


double autofocus_fl(int num_steps_coarse, double coarse_interval, double exposure, double sensitivity, int ex, int em){
	int i;
	double initial_zpos, start_focus_pos, curr_focus_pos;
	short unsigned int **image_ptr_stack;
	double max_complexity=0, max_complexity_zpos, curr_complexity;
	double *focus_pos;

	if(!this_filter.SetPosition(2, ex, em)){
		r.error("set filter positions failed", 1);
	}


	if((image_ptr_stack = (short unsigned int**)malloc(num_steps_coarse * sizeof(short unsigned int*)))==NULL){
		r.error("allocate memory for autofocus pointer stack failed", 1);
	}
	
	if((focus_pos = (double*) malloc( (num_steps_coarse + (int) ((coarse_interval / 0.05)+1) ) * sizeof(double) ))==NULL){
		r.error("allocate memory for focus position list failed", 1);
	}

	if(focus_axis==1){//PIFOC USED
		initial_zpos = this_pifoc.GetPos();
	}
	else{
		initial_zpos = this_te2000.GetPos();
	}

	start_focus_pos = initial_zpos - (((num_steps_coarse - 1)/2)*coarse_interval);
	max_complexity_zpos = initial_zpos;

	sprintf(log_string, "----Initial autofocus plane: %f", initial_zpos);
	r.log(log_string, 3);

	if(!this_camera.QueueCapture()){
		r.error("queue capture of multiple frames failed", 0);
		return initial_zpos;
	}


	for(i=0; i< num_steps_coarse; i++){
		
		curr_focus_pos = start_focus_pos + (i*coarse_interval);
		sprintf(log_string, "---- %d %f", i, curr_focus_pos);
		r.log(log_string, 3);

		if(focus_axis==1){
			this_pifoc.SetPosition(curr_focus_pos);
			while(!(this_pifoc.ConfirmPosition())){;}
			*(focus_pos + i) = this_pifoc.GetPos();
		}
		else{
			this_te2000.SetPosition(curr_focus_pos);
			while(!(this_te2000.ConfirmPosition())){;}
			*(focus_pos + i) = this_te2000.GetPos();
		}

		r.log("----Acquire start", 4);
		if(!acq.acquire_expose_shutter(exposure, sensitivity)){
			sprintf(log_string, "Error acquiring image %d in fluorescence autofocus stack", i);
			r.error(log_string, 0);
		}
		else{
			image_ptr_stack[i] = acq.acquire_getdata(i);  
		}
		r.log("----Acquire end", 4);
	}
	
	max_complexity = 0;
	for(i=0; i<num_steps_coarse; i++){
		curr_complexity = compute_complexity(image_ptr_stack[i]);

		sprintf(log_string, "---- Contrast for position %02d (z plane %f) = %f\n", i, *(focus_pos + i), curr_complexity);
		r.log(log_string, 3);

		if(curr_complexity > max_complexity){
			sprintf(log_string, "CURR_CONTRAST %f > MAX_CONTRAST %f", curr_complexity, max_complexity);
			r.log(log_string, 4);
			max_complexity = curr_complexity;
			max_complexity_zpos = *(focus_pos + i);
		}
	}
	sprintf(log_string, "----Best position: %f, score: %f", max_complexity_zpos, max_complexity); 
	r.log(log_string, 3);

//WE'VE FOUND THE RIGHT POSITION IN THE STACK, NOW MOVE THERE
	if(focus_axis==1){
		this_pifoc.SetPosition(max_complexity_zpos);
		while(!(this_pifoc.ConfirmPosition())){;}
	}
	else{
		sprintf(log_string, "Setting focus to position %f (started at %f)", max_complexity_zpos, initial_zpos);
		r.log(log_string, 2);
		this_te2000.SetPosition(max_complexity_zpos);
		while(!(this_te2000.ConfirmPosition())){;}
	}

	free(image_ptr_stack);
	free(focus_pos);
	return max_complexity_zpos;
}