/*****************************************************************************
 -                                                                           -
 -                    POLONY SEQUENCER ACQUISITION SUITE                     -
 -                                Church Lab                                 -
 -                          Harvard Medical School                           -
 -                                                                           -
 -    Free software for running a Polony Sequencing automated microscope     -
 -                                                                           -
 - ========================================================================= -
 -                                                                           -
 -                                                                           -
 - seq_cycle_single.cpp                                                      -
 -                                                                           -
 -   Program to acquire sequencing cycle images in a single color (TxRed)    -
 -                                                                           -
 -   Written by Greg Porreca, 10-26-2005                                     -
 -                                                                           -
 - Revised                                                                   -
 -   03-22-06 GP - added parameter-setting by config file                    -
 -                                                                           -
 - This software may be used, modified, and distributed freely, but this     -
 - header may not be modified and must appear at the top of this file.       -
 -                                                                           -
 -                                                                           -
 *****************************************************************************/

#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 "..\autofocus.h"
//#include "..\autoexpose.h"
#include "..\config_reader.h"

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

int _AUTOFOCUS_BF_CSTEPS;
double _AUTOFOCUS_BF_CSPACING;
int _AUTOFOCUS_FL_CSTEPS;
double _AUTOFOCUS_FL_CSPACING;

int DEBUG_LEVEL = 5;

int _TXRED_EX; 
int _TXRED_EM; 
int _CY3_EX; 
int _CY3_EM; 
int _CY5_EX; 
int _CY5_EM; 
int _FRET_EX; 
int _FRET_EM; 
int _FITC_EX; 
int _FITC_EM; 

int _BLOCK_DUAL; 
int _BLOCK_TXRED;
int _BLOCK_FITC; 


//#define _AUTOFOCUS_BF_CSTEPS 19
//#define _AUTOFOCUS_BF_CSPACING 0.15
//#define _AUTOFOCUS_FL_CSTEPS 19
//#define _AUTOFOCUS_FL_CSPACING 0.15

//#define DEBUG_LEVEL 5

//#define _TXRED_EX 9
//#define _TXRED_EM 9
//#define _CY3_EX 0
//#define _CY3_EM 0
//#define _CY5_EX 1
//#define _CY5_EM 1
//#define _FRET_EX 0
//#define _FRET_EM 1
//#define _FITC_EX 7
//#define _FITC_EM 7

//#define _BLOCK_DUAL 5
//#define _BLOCK_TXRED 6
//#define _BLOCK_FITC 1

/*FILTER CONFIGURATION:
0	Cy3
1	Cy5
2	OPEN
3	---
4	---
5	3Cy5
6	3Cy3
7	3FITC
8	FITC
9	TxRed
*/

char cycle_dir_root[500];
char log_filename[500];
char list_filename[500];
char log_string[500];
char img_output_dir[500];
char cycle_name[500];
char fluorophore[10];

int FINE_FOCUS_TE2000=0;

int focus_axis=0; //autofocus brightfield uses TE2000 if 0, pifoc if 1; DO NOT SET HERE!!!

short unsigned int *temp_img_stack;
short unsigned int *temp_img;
short unsigned int *brightfield_img;

FILE *poslist_file, *img_file[4];

Reporter r(DEBUG_LEVEL);

//Config_reader cfg("..\\..\\master.cfg");
Config_reader cfg("scope\\master.cfg");


CScope this_te2000(&r);
CStage this_stage(&r);
CHamCam this_camera(&r);
CFilter this_filter(&r);
CPifoc this_pifoc(&r);

Acquisition acq(&r, &this_camera, &this_filter);
Autofocus autof(&r, &this_camera, &this_filter, &this_te2000, &this_pifoc, &acq);
//Autoexpose autoe(&r, &this_camera, &this_filter, &this_te2000, &this_pifoc, &acq, brightfield_img);

bool save_image(short unsigned int*, int, char*);
void q_sort(double numbers[], int left, int right);
void quickSort(double numbers[], int array_size);


void main(int argc, char *argv[]){
	int num_frames=0;
	int i, j;
	int rand_start_frame;
	double new_offset_pifoc=0;
	int success;
	int fl_x, fl_m, fl_block;
	int af_x, af_m, af_block;
	int numpos;

	char config_val[80];
	char af_color[80];

	char temp1[50], temp2[50], temp3[50], temp4[50], temp5[50];


	double curr_x=0, curr_y=0, curr_z=0, curr_te2000_z, curr_pifoc_z;
	double init_x, init_y, init_te2000_z, init_pifoc_z;
	double offset_z, z_brightfield_off;//, offset_difference;

	double *x, *y, *z;

	double ex1, sens1;
	double *aex1, *asens1;
	double *aex1_s;
	
	int autoexpose_flag;

	short unsigned int *curr_img_stack;

	if(!( (argc==6) || (argc==7) )){
		fprintf(stderr, "ERROR: wrong number of arguments (expecting 5 for autoexpose or 6 for no autoexpose):\n");
		fprintf(stderr, " .\\seq_cycle_single fluorophore numpos exposure sensitivity image_dirname\n");
		exit(42);
	}

	//AUTOEXPOSE IF 6TH ARG IS NOT PRESENT
	if(argc==6){
		strcpy(fluorophore, argv[1]);
		numpos = atoi(argv[2]);
		ex1 = atof(argv[3]) / 1000;
		sens1 = atof(argv[4]);
		strcpy(cycle_name, argv[5]);
		autoexpose_flag = 1;
	}

	//DO NOT AUTOEXPOSE IF 6TH ARG IS PRESENT
	else{
		strcpy(fluorophore, argv[1]);
		numpos = atoi(argv[2]);
		ex1 = atof(argv[3]) / 1000;
		sens1 = atof(argv[4]);
		strcpy(cycle_name, argv[5]);
		autoexpose_flag = 0;
	}

	if(!(cfg.getTag(cycle_dir_root, "image_basedir"))){
		fprintf(stderr, "config file does not include tag image_basedir");
		exit(42);
	}

	/***************************************************************************/
	/* BUILD OUTPUT FILENAMES AND OPEN FILES                                   */
	strcpy(img_output_dir, "mkdir ");
	strcat(img_output_dir, cycle_dir_root);
	strcat(img_output_dir, "\\");
	strcat(img_output_dir, cycle_name);
	strcat(img_output_dir, "\\");
	system(img_output_dir);
	
	strcpy(log_filename, "");
	strcat(log_filename, cycle_dir_root);
	strcat(log_filename, "\\");
	strcat(log_filename, cycle_name);
	strcat(log_filename, "\\");
	strcat(log_filename, "seq_cycle.log");
	r.open(log_filename, 'a');

	strcpy(img_output_dir, "");
	strcat(img_output_dir, cycle_dir_root);
	strcat(img_output_dir, "\\");
	strcat(img_output_dir, cycle_name);
	strcat(img_output_dir, "\\");
	strcat(img_output_dir, "SC_");

	strcpy(list_filename, "");
	strcat(list_filename, cycle_dir_root);
	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, "r")) == NULL){
		r.error("cannot open position list file", 1);
	}

	sprintf(log_string, "Using %s for image output directory.\n", img_output_dir);
	r.log(log_string, 1);
	sprintf(log_string, "Acquiring images using the following settings: (%.3f,%.0f)", ex1, sens1);
	r.log(log_string, 1);
	/*                                                                         */
	/***************************************************************************/



	//**************************************************************************
	// LOAD PARAMS FROM CFG FILE
	//
	if(!(cfg.getTag(config_val, "autofocus_bf_csteps"))) r.error("config file does not include tag autofocus_bf_csteps",1);
	_AUTOFOCUS_BF_CSTEPS = atoi(config_val); sprintf(log_string, "setting _AUTOFOCUS_BF_CSTEPS = %d", _AUTOFOCUS_BF_CSTEPS); r.log(log_string, 1); 
	if(!(cfg.getTag(config_val, "autofocus_bf_cspacing"))) r.error("config file does not include tag autofocus_bf_cspacing",1);
	_AUTOFOCUS_BF_CSPACING = atof(config_val);sprintf(log_string, "setting _AUTOFOCUS_BF_CSPACING = %f", _AUTOFOCUS_BF_CSPACING); r.log(log_string, 1);
	if(!(cfg.getTag(config_val, "autofocus_fl_csteps"))) r.error("config file does not include tag autofocus_fl_csteps",1);
	_AUTOFOCUS_FL_CSTEPS = atoi(config_val);sprintf(log_string, "setting _AUTOFOCUS_FL_CSTEPS = %d", _AUTOFOCUS_FL_CSTEPS); r.log(log_string, 1);
	if(!(cfg.getTag(config_val, "autofocus_fl_cspacing"))) r.error("config file does not include tag autofocus_fl_cspacing",1);
	_AUTOFOCUS_FL_CSPACING = atof(config_val);sprintf(log_string, "setting _AUTOFOCUS_FL_CSPACING = %f", _AUTOFOCUS_FL_CSPACING); r.log(log_string, 1);

	if(!(cfg.getTag(config_val, "debug_seqcyc_single"))) r.error("config file does not include tag debug_seqcyc_single",1);
	DEBUG_LEVEL = atoi(config_val);sprintf(log_string, "setting DEBUG_LEVEL = %d", DEBUG_LEVEL); r.log(log_string, 1);
	r.set_debuglevel(DEBUG_LEVEL);

	if(!(cfg.getTag(config_val, "ex_txred"))) r.error("config file does not include tag ex_txred",1);
	_TXRED_EX = atoi(config_val);sprintf(log_string, "setting _TXRED_EX = %d", _TXRED_EX); r.log(log_string, 1);
	if(!(cfg.getTag(config_val, "em_txred"))) r.error("config file does not include tag em_txred",1);
	_TXRED_EM = atoi(config_val);sprintf(log_string, "setting _TXRED_EM = %d", _TXRED_EM); r.log(log_string, 1);
	if(!(cfg.getTag(config_val, "ex_cy5"))) r.error("config file does not include tag ex_cy5",1);
	_CY5_EX = atoi(config_val);sprintf(log_string, "setting _CY5_EX = %d", _CY5_EX); r.log(log_string, 1);
	if(!(cfg.getTag(config_val, "em_cy5"))) r.error("config file does not include tag em_cy5",1);
	_CY5_EM = atoi(config_val);sprintf(log_string, "setting _CY5_EM = %d", _CY5_EM); r.log(log_string, 1);
	if(!(cfg.getTag(config_val, "ex_cy3"))) r.error("config file does not include tag ex_cy3",1);
	_CY3_EX = atoi(config_val);sprintf(log_string, "setting _CY3_EX = %d", _CY3_EX); r.log(log_string, 1);
	if(!(cfg.getTag(config_val, "em_cy3"))) r.error("config file does not include tag em_cy3",1);
	_CY3_EM = atoi(config_val);sprintf(log_string, "setting _CY3_EM = %d", _CY3_EM); r.log(log_string, 1);
	if(!(cfg.getTag(config_val, "ex_fret"))) r.error("config file does not include tag ex_fret",1);
	_FRET_EX = atoi(config_val);sprintf(log_string, "setting _FRET_EX = %d", _FRET_EX); r.log(log_string, 1);
	if(!(cfg.getTag(config_val, "em_fret"))) r.error("config file does not include tag em_fret",1);
	_FRET_EM = atoi(config_val);sprintf(log_string, "setting _FRET_EM = %d", _FRET_EM); r.log(log_string, 1);
	if(!(cfg.getTag(config_val, "ex_fitc"))) r.error("config file does not include tag ex_fitc",1);
	_FITC_EX = atoi(config_val);sprintf(log_string, "setting _FITC_EX = %d", _FITC_EX); r.log(log_string, 1);
	if(!(cfg.getTag(config_val, "em_fitc"))) r.error("config file does not include tag em_fitc",1);
	_FITC_EM = atoi(config_val);sprintf(log_string, "setting _FITC_EM = %d", _FITC_EM); r.log(log_string, 1);

	if(!(cfg.getTag(config_val, "block_dual"))) r.error("config file does not include tag block_dual",1);
	_BLOCK_DUAL = atoi(config_val);sprintf(log_string, "setting _BLOCK_DUAL = %d", _BLOCK_DUAL); r.log(log_string, 1);
	if(!(cfg.getTag(config_val, "block_txred"))) r.error("config file does not include tag block_txred",1);
	_BLOCK_TXRED = atoi(config_val);sprintf(log_string, "setting _BLOCK_TXRED= %d", _BLOCK_TXRED); r.log(log_string, 1);
	if(!(cfg.getTag(config_val, "block_fitc"))) r.error("config file does not include tag block_fitc",1);
	_BLOCK_FITC = atoi(config_val);sprintf(log_string, "setting _BLOCK_FITC = %d", _BLOCK_FITC); r.log(log_string, 1);
	if(!(cfg.getTag(config_val, "single_af_color"))) r.error("config file does not include tag single_af_color",1);
	strcpy(af_color, config_val);sprintf(log_string, "setting autofocus color = %s", af_color); r.log(log_string, 1);
	//
	//****************************************************************************


	//MAKE SURE A VALID FLUOROPHORE WAS SPECIFIED
	if(!strcmp(fluorophore, "cy5")){
		fl_x = _CY5_EX;
		fl_m = _CY5_EM;
		fl_block = _BLOCK_DUAL;
	}
	else if(!strcmp(fluorophore, "cy3")){
		fl_x = _CY3_EX;
		fl_m = _CY3_EM;
		fl_block = _BLOCK_DUAL;
	}
	else if(!strcmp(fluorophore, "fret")){
		fl_x = _FRET_EX;
		fl_m = _FRET_EM;
		fl_block = _BLOCK_DUAL;
	}
	else if(!strcmp(fluorophore, "txred")){
		fl_x = _TXRED_EX;
		fl_m = _TXRED_EM;
		fl_block = _BLOCK_TXRED;
	}
	else if(!strcmp(fluorophore, "2fitc")){
//		fl_x = _FITC_EX;
//		fl_m = _FITC_EM;
//		fl_block = _BLOCK_FITC;
		fl_x = 8;
		fl_m = 8;
		fl_block = _BLOCK_TXRED;
	}
	else if(!strcmp(fluorophore, "fitc")){
		fl_x = _FITC_EX;
		fl_m = _FITC_EM;
		fl_block = _BLOCK_FITC;
	}
	else{
		fprintf(stderr, "ERROR: unrecognized fluorophore ->%s<- (must be cy5, cy3, fret, txred, fitc)\n", fluorophore);
		exit(42);
	}


	//SET AUTOFOCUS COLOR
	//IF FITC WAS THE FLUOROPHORE, WE CAN SPECIFY THE FILTERS
	//TO USE WHILE AUTOFOCUSING (BRIGHTFIELD AND FLUORESCENT).  THIS
	//SHOULD BE EITHER CY5 OR TXRED FOR BEST PERFORMANCE; CAN BE FITC IF
	//THE FITC SIGNAL IS STRONG AND WILL NOT PHOTOBLEACH DURING
	//THE FLUORESCENCE AUTOFOCUS.  THE AUTOFOCUS COLOR IS SET BY THE
	//'single_af_color' TAG IN THE CONFIG FILE.
	if(!strcmp(fluorophore, "fitc")){ 
		if(!strcmp(af_color, "fitc")){
			af_x = _CY5_EX;
			af_m = _CY5_EM;
			af_block = _BLOCK_FITC;
		}
		else if(!strcmp(af_color, "txred")){
			af_x = _TXRED_EX;
			af_m = _TXRED_EM;
			af_block = _BLOCK_TXRED;
		}
		else if(!strcmp(af_color, "cy5")){
			af_x = _CY5_EX;
			af_m = _CY5_EM;
			af_block = _BLOCK_DUAL;
		}
		else if(!strcmp(af_color, "cy3")){
			af_x = _CY3_EX;
			af_m = _CY3_EM;
			af_block = _BLOCK_DUAL;
		}
	}
	else{
		if(!strcmp(fluorophore, "cy3")){
			af_x = _CY5_EX;
			af_m = _CY5_EM;
			af_block = _BLOCK_DUAL;
		}
		else{
			af_x = fl_x;
			af_m = fl_m;
			af_block = fl_block;
		}
	}


	//MAKE SURE VALID NUMPOS WAS SPECIFIED
	if( (numpos < 1) || (numpos > 4)){
		fprintf(stderr, "ERROR: invalid numpos: image set must be either 1, 2, 3, or 4\n");
		exit(42);
	}

	//SEED RANDOM NUMBER GENERATOR
	srand( (int) time(NULL));


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

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

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

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

	/*                                               */
	/*************************************************/


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

	if((temp_img_stack = (short unsigned int*) malloc(1000000 * sizeof(short unsigned int) * 4)) == NULL){
		r.error("memory allocation for temp image stack failed", 1);
	}
	if((brightfield_img = (short unsigned int*) malloc(1000000 * sizeof(short unsigned int))) == NULL){
		r.error("memory allocation for brightfield image failed", 1);
	}
	if(autoexpose_flag){
		if((aex1 = (double*) malloc(3 * sizeof(double))) == NULL){
			r.error("memory allocation failed", 1);
		}
		if((asens1 = (double*) malloc(3 * sizeof(double))) == NULL){
			r.error("memory allocation failed", 1);
		}
		if((aex1_s = (double*) malloc(3 * sizeof(double))) == NULL){
			r.error("memory allocation failed", 1);
		}
	}
	/*                                               */
	/*************************************************/


	//DETERMINE NUMBER OF FRAMES TO VISIT
	if( (fscanf(poslist_file, "%s\t%s\t%s\t%s\n", temp1, temp2, temp3, temp4))==EOF){
		r.error("position list file does not appear to be valid", 1);
	}
	while( (fscanf(poslist_file, "%s\t%s\t%s\t%s\t%s\n", temp1, temp2, temp3, temp4, temp5))!=EOF){
		num_frames++; //frame index starts at 0 in poslist_file
	}
	sprintf(log_string, "Position file has %d positions", num_frames);
	r.log(log_string, 1);
	fclose(poslist_file);
	
	if((poslist_file = fopen(list_filename, "r")) == NULL){
		r.error("cannot open position list file", 1);
	}


	//ALLOCATE MEMORY FOR POSITION LIST
	if((x = (double*) malloc(num_frames * sizeof(double))) == NULL){
		r.error("memory allocation for list of x positions failed", 1);
	}
	if((y = (double*) malloc(num_frames * sizeof(double))) == NULL){
		r.error("memory allocation for list of y positions failed", 1);
	}
	if((z = (double*) malloc(num_frames * sizeof(double))) == NULL){
		r.error("memory allocation for list of z positions failed", 1);
	}


	
	//READ INITIAL POSITION FROM FILE AND DETERMINE FINE FOCUS METHOD
	if( (success = fscanf(poslist_file, "%s\t%s\t%s\t%s\n", temp1, temp2, temp3, temp4))<4){
		r.error("read first line from position list file failed", 1);
	}
	init_x = atof(temp1);
	init_y = atof(temp2);
	init_te2000_z = atof(temp3);
	init_pifoc_z = atof(temp4);


	//READ REST OF POSITIONS IN FILE
	for(i=0; i<num_frames; i++){
		if( (success = fscanf(poslist_file, "%s\t%s\t%s\t%s\t%s\n", temp1, temp2, temp3, temp4, temp5)) < 5){
			r.error("read line from position list file failed", 1);
		}
		*(x+i) = atof(temp2);
		*(y+i) = atof(temp3);
		*(z+i) = atof(temp4);
	}
	fclose(poslist_file);


	//ARE WE USING PIFOC OR NOT?
	if(init_te2000_z == init_pifoc_z){
		r.log("**THIS POSITION FILE SPECIFIES TE2000 WAS USED FOR FINE FOCUS", 1);
		FINE_FOCUS_TE2000 = 1;
	}
	else{
		r.log("**THIS POSITION FILE SPECIFIES PIFOC WAS USED FOR FINE FOCUS", 1);
		r.log("Initializing pifoc", 1);
		if(!(success=this_pifoc.Init())){
			r.error("pifoc initialization failed", 1);
		}
		FINE_FOCUS_TE2000 = 0;
	}


	//MOVE AXES TO INITIAL POSITION
	if(!this_te2000.SetFilterBlock(af_block)){
		r.error("set TE2000 filter block failed", 1);
	}
	sprintf(log_string, "Moving instrument to initial position: %f, %f, %f, %f (X, Y, Z1, Z2)", init_x, init_y, init_te2000_z, init_pifoc_z);
	r.log(log_string, 1);
	this_stage.SetPosition((int) init_x, (int) init_y);
	this_te2000.SetPosition(init_te2000_z);
	while(!(this_stage.ConfirmPosition())){;}
	while(!(this_te2000.ConfirmPosition())){;}
	if(!FINE_FOCUS_TE2000){
		this_pifoc.SetPosition(init_pifoc_z);
		while(!this_pifoc.ConfirmPosition()){;}
	}


	//CONFIGURE HARDWARE FOR BRIGHTFIELD ACQUISITION
	this_te2000.OpenUniblitz();
	this_camera.SetExposure(0.020);
	this_camera.SetSensitivity(0);
	//WE WANT TO BRIGHTFIELD AUTOFOCUS WITH CY5 FILTERS FOR PROPER BRIGHTFIELD IMAGE
	if(!this_filter.SetPosition(2, _CY5_EX, _CY5_EM)){
		r.error("set filter wheels failed", 1);
	}

	/*
	//DETERMINE RANDOM FRAME TO VISIT (WE DON'T WANT TO DO AUTOFOCUS & AUTOEXPOSE AT FRAME 0)
	while( (rand_start_frame = (int) ( (rand()/(double)RAND_MAX) * (double) num_frames))==num_frames){;}
	sprintf(log_string, "Moving instrument to random position %d: %f, %f, %f, %f (X, Y, Z1, Z2)", rand_start_frame, *(x+rand_start_frame), *(y+rand_start_frame), init_te2000_z, *(z+rand_start_frame));
	r.log(log_string, 1);
	this_stage.SetPosition((int) *(x+rand_start_frame), (int) *(y+rand_start_frame));
	while(!(this_stage.ConfirmPosition())){;}
	if(!FINE_FOCUS_TE2000){
		this_pifoc.SetPosition(*(z+rand_start_frame));
		while(!this_pifoc.ConfirmPosition()){;}
	}
	else{
		this_te2000.SetPosition(*(z+rand_start_frame));
		while(!(this_te2000.ConfirmPosition())){;}
	}
	*/

	

	//FIND INITIAL Z OFFSET TO APPLY TO ALL VALS IN FILE
	sprintf(log_string, "Setting camera for 'snap' acquisition in %d-frame blocks", 81);
	r.log(log_string, 1);
	if(!this_camera.SetupMultiple(81)){
		r.error("could not setup camera for acquisition", 1);
	}
	curr_te2000_z = autof.autofocus_brightfield(81, 0.25);
	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(!this_camera.SetupMultiple(_AUTOFOCUS_BF_CSTEPS + 1)){
		r.error("could not setup camera for acquisition", 1);
	}
	if(FINE_FOCUS_TE2000){
		curr_te2000_z = autof.autofocus_brightfield(_AUTOFOCUS_BF_CSTEPS, _AUTOFOCUS_BF_CSPACING);
//		offset_z = curr_te2000_z - *(z+rand_start_frame);
		/*
		//z_brightfield_off = curr_te2000_z - *(z+rand_start_frame);
		*/
		z_brightfield_off = curr_te2000_z - init_te2000_z;
	}
	else{
		//FINE FOCUS USING TE2000
		curr_te2000_z = autof.autofocus_brightfield(_AUTOFOCUS_BF_CSTEPS, _AUTOFOCUS_BF_CSPACING);
		//FINE FOCUS USING PIFOC
		autof.setFocusAxis(1);
		curr_pifoc_z = autof.autofocus_brightfield(_AUTOFOCUS_BF_CSTEPS, _AUTOFOCUS_BF_CSPACING);
//		offset_z = curr_pifoc_z - *(z+rand_start_frame);
		/*
		//z_brightfield_off = curr_pifoc_z - *(z+rand_start_frame);
		*/
		z_brightfield_off = curr_pifoc_z - init_pifoc_z;
	}
	autof.setFocusAxis(0);

	if(acq.acquire_expose_noshutter(_BRIGHTFIELD_EXPOSURETIME, _BRIGHTFIELD_SENSITIVITY)){
		temp_img = acq.acquire_getdata(_AUTOFOCUS_BF_CSTEPS); //NEW
		sprintf(log_string, "C:\\test1\\SEQ-CYCLESINGLE_%s-%s", cycle_name, fluorophore);
		save_image(temp_img, 0, log_string);//NEW
	}
	else{
		r.error("unable to acquire starting image", 1);
	}

	//KEEP IMAGE FOR USE DURING AUTOEXPOSE
	for(i=0; i<1000000; i++){
		*(brightfield_img + i) = *(temp_img + i);
	}

	this_camera.EndMultiple();
	this_te2000.CloseUniblitz();

	//NOW FLUORESCENT AUTOFOCUS
	if(!this_te2000.SetFilterBlock(af_block)){
		r.error("set TE2000 filter block failed", 1);
	}
	sprintf(log_string, "Setting camera for 'snap' acquisition in %d-frame blocks", _AUTOFOCUS_BF_CSTEPS + 1); 
	r.log(log_string, 1);
	if(!this_camera.SetupMultiple(_AUTOFOCUS_FL_CSTEPS + 1)){
		r.error("could not setup camera for acquisition", 1);
	}
	if(FINE_FOCUS_TE2000){
		curr_te2000_z = autof.autofocus_fl(_AUTOFOCUS_FL_CSTEPS, _AUTOFOCUS_FL_CSPACING, ex1, sens1, af_x, af_m);
		/*
		//offset_z = curr_te2000_z - *(z+rand_start_frame);
		*/
		offset_z = curr_te2000_z - init_te2000_z;
	}
	else{
		curr_te2000_z = autof.autofocus_fl(_AUTOFOCUS_FL_CSTEPS, _AUTOFOCUS_FL_CSPACING, ex1, sens1, af_x, af_m);
		autof.setFocusAxis(1); //NOW FINE FOCUS WITH PIFOC; ALL FUTURE Z-AXIS MOVEMENT WILL BE PIFOC
		curr_pifoc_z = autof.autofocus_fl(_AUTOFOCUS_FL_CSTEPS, _AUTOFOCUS_FL_CSPACING, ex1, sens1, af_x, af_m);
		/*
		//offset_z = curr_pifoc_z - *(z+rand_start_frame);
		*/
		offset_z = curr_pifoc_z - init_pifoc_z;
	}
	if(acq.acquire_expose_shutter(ex1, sens1)){
		sprintf(log_string, "C:\\test1\\SEQ-CYCLESINGLE_%s-%s", cycle_name, fluorophore);
		save_image(acq.acquire_getdata(_AUTOFOCUS_FL_CSTEPS), 1, log_string);
	}
	else{
		r.error("unable to acquire starting image", 1);
	}
	this_camera.EndMultiple();
	//offset_difference = (*(z+rand_start_frame) + z_brightfield_off) - (*(z+rand_start_frame) + offset_z);

/*
	//AUTOEXPOSE 3 TIMES AND TAKE THE MEDIAN VALUES
	//--HAVE NOT FIXED THIS CODE BLOCK TO USE THE RIGHT FLUOROPHORE - ALWAYS WILL USE TXRED
	if(autoexpose_flag){
		for(i=0; i<3; i++){
			while((rand_start_frame=(int)((rand()/(double)RAND_MAX)*(double)num_frames))==num_frames){;}
			sprintf(log_string, "Moving instrument to position %d: %f, %f, %f, %f (X, Y, Z1, Z2)", rand_start_frame, *(x+rand_start_frame), *(y+rand_start_frame), *(z+rand_start_frame)+offset_z, *(z+rand_start_frame)+offset_z);
			r.log(log_string, 1);

			this_stage.SetPosition((int) *(x+rand_start_frame), (int) *(y+rand_start_frame));
			while(!(this_stage.ConfirmPosition())){;}
	
			if(!FINE_FOCUS_TE2000){
				this_pifoc.SetPosition(*(z+rand_start_frame)+z_brightfield_off);
				while(!this_pifoc.ConfirmPosition()){;}
			}
			else{
				this_te2000.SetPosition(*(z+rand_start_frame)+z_brightfield_off);
				while(!(this_te2000.ConfirmPosition())){;}
			}

			//setup system for brightfield acquisition
			this_te2000.OpenUniblitz();
			this_camera.SetExposure(0.020);
			this_camera.SetSensitivity(0);
			if(!this_camera.SetupMultiple(1)){
				r.error("Could not setup camera for acquisition", 1);
			}
			if(!this_camera.QueueCapture()){
				r.error("Queue image capture failed", 1);
			}
			if(acq.acquire_expose_noshutter(_BRIGHTFIELD_EXPOSURETIME, _BRIGHTFIELD_SENSITIVITY)){
				temp_img = acq.acquire_getdata(0);
				for(j=0; j<1000000; j++){
					*(brightfield_img + j) = *(temp_img + j);
				}
			}
			else{
				r.error("unable to acquire brightfield image",1);
			}
			this_camera.EndMultiple();
			this_te2000.CloseUniblitz();

			if(!FINE_FOCUS_TE2000){
				this_pifoc.SetPosition(*(z+rand_start_frame)+offset_z);
				while(!this_pifoc.ConfirmPosition()){;}
			}
			else{
				this_te2000.SetPosition(*(z+rand_start_frame)+offset_z);
				while(!this_te2000.ConfirmPosition()){;}
			}

			autoe.autoexpose(aex1 + i, asens1 + i, brightfield_img, 4, cycle_name);
			*(aex1_s+i) = *(aex1+i);
			sprintf(log_string, "Best settings found were %f/%f (exp/sens)", *(aex1+i), *(asens1+i));
			r.log(log_string, 1);
		}

		quickSort(aex1_s, 3);
		for(i=0; i<3; i++){
			if(*(aex1+i) == *(aex1_s+2)){
				ex1 = *(aex1+i);
				sens1 = *(asens1+i);
			}
		}
	}
*/
	sprintf(log_string, "Acquiring images using the following settings: %s(%.3f, %.0f)", fluorophore, ex1, sens1);
	r.log(log_string, 1);


	if(!this_camera.SetupMultiple(1)){
		r.error("could not setup camera for acquisition", 1);
	}
	
	sprintf(log_string, "APPLYING OFFSET OF %f MICRONS TO EACH ZPOS IN FILE", offset_z);
	r.log(log_string, 1);


	//SET FILTER WHEELS AND BLOCK FOR ACQUISITION
	if(!this_filter.SetPosition(2, fl_x, fl_m)){
		r.error("set filter wheels failed", 1);
	}
	if(!this_te2000.SetFilterBlock(fl_block)){
		r.error("set TE2000 filter block failed", 1);
	}

	
	//NOW ITERATE THROUGH FILE, VISIT EACH POSITION AND ACQUIRE IMAGES
	for(i=0; i<num_frames; i++){

		curr_x = *(x+i);
		curr_y = *(y+i);
		curr_z = *(z+i);
	
		sprintf(log_string, "Moving to position %d, %.1f %.1f %.2f %.2f", i+1, curr_x, curr_y, curr_z, curr_z + offset_z);
		r.log(log_string, 1);

		//SET STAGE
		if(!this_stage.SetPosition((int)curr_x, (int)curr_y)){
			sprintf(log_string, "frame %d: cannot set stage to %f %f", i, curr_x, curr_y);
			r.error(log_string, 1);
		}

		//IF THIS IS NOT THE FIRST TIME THROUGH THE LOOP, SAVE IMAGES FROM LAST ROUND
		if(i>0){
			for(j=0; j<1; j++){
				save_image(acq.acquire_getdata(j), (4*(i-1))+j+numpos, img_output_dir);

			}
			this_camera.QueueCapture();
		}
		//NO DATA TO SAVE YET, BUT STILL MUST QUEUE THE CAMERA
		else if(i==0){
			this_camera.QueueCapture();
		}


		//SET Z AXIS
		if(FINE_FOCUS_TE2000){
			if(!this_te2000.SetPosition(curr_z + offset_z)){
				sprintf(log_string, "frame %d: cannot set te2000 to z position %f", i, curr_z + offset_z);
				r.error(log_string, 1);
			}
			//BLOCK UNTIL TE2000 IS IN POSITION
			while(!this_te2000.ConfirmPosition()){;}
		}
		else{
			if(!this_pifoc.SetPosition(curr_z + offset_z)){
				sprintf(log_string, "frame %d: cannot set pifoc to z position %f", i, curr_z + offset_z);
				r.error(log_string, 1);
			}
			//BLOCK UNTIL PIFOC IS IN POSITION
			while(!this_pifoc.ConfirmPosition()){;}
		}

		//BLOCK UNTIL STAGE IS IN POSITION
		while(!this_stage.ConfirmPosition()){;}

		//OUTPUT ACTUAL AXIS POSITIONS IF WE ARE DEBUGGING
		if(DEBUG_LEVEL > 4){
			sprintf(log_string, "Positions read from instrument: %f\t%f\t%f\t%f\n", this_stage.GetPosX(), this_stage.GetPosY(), this_te2000.GetPos(), this_pifoc.GetPos());
			r.log(log_string, 2);
		}

		//ACQUIRE THE IMAGE
		acq.acquire_expose_shutter(ex1, sens1);
	}

	for(j=0; j<1; j++){
		save_image(acq.acquire_getdata(j), (4*(i-1))+j+numpos, img_output_dir);
	}


	if(!this_camera.EndMultiple()){
		r.error("unable to end multiple acquisition", 0);
	}

	//MOVE BACK TO START POSITION
	this_stage.SetPosition((int) init_x, (int) init_y);
	this_te2000.SetPosition(init_te2000_z);
	while(!(this_stage.ConfirmPosition())){;}
	while(!(this_te2000.ConfirmPosition())){;}
	if(!FINE_FOCUS_TE2000){
		this_pifoc.SetPosition(init_pifoc_z + offset_z);
		while(!this_pifoc.ConfirmPosition()){;}
	}
	else{
		this_te2000.SetPosition(init_te2000_z + offset_z);
		while(!this_te2000.ConfirmPosition()){;}
	}


	//CLOSE ALL DEVICES
	this_stage.Close();
	this_te2000.Close();
	this_camera.Close();
	this_filter.Close();
	if(!FINE_FOCUS_TE2000){
		this_pifoc.Close();
	}

	free(temp_img_stack);
	free(curr_img_stack);
	free(brightfield_img);
	if(autoexpose_flag){
		free(aex1);
		free(asens1);
		free(aex1_s);
	}
}

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;
}

void q_sort(double numbers[], int left, int right){
	double pivot;
	int p2;
	int l_hold;
	int r_hold;

	l_hold = left;
	r_hold = right;
	pivot = numbers[left];

	while(left < right){
		while((numbers[right] >= pivot) && (left < right))
			right--;
		if(left != right){
			numbers[left] = numbers[right];
			left++;
		}
		while((numbers[left] <= pivot) && (left < right))
			left++;
		if(left != right){
			numbers[right] = numbers[left];
			right--;
		}
	}
	numbers[left] = (double) pivot;
	p2 = left;
	left = l_hold;
	right = r_hold;
	if(left < p2)
		q_sort(numbers, left, p2-1);
	if(right > p2)
		q_sort(numbers, p2+1, right);
}


void quickSort(double numbers[], int array_size){
	q_sort(numbers, 0, array_size - 1);
}