/*****************************************************************************
 -                                                                           -
 -                    POLONY SEQUENCER ACQUISITION SUITE                     -
 -                                Church Lab                                 -
 -                          Harvard Medical School                           -
 -                                                                           -
 -    Free software for running a Polony Sequencing automated microscope     -
 -                                                                           -
 - ========================================================================= -
 -                                                                           -
 -                                                                           -
 - seq_primer.cpp                                                            -
 -                                                                           -
 -   Program to acquire Cy3 fluorescent primer images                        -
 -                                                                           -
 - Written by Greg Porreca, 10-26-2005                                       -
 -                                                                           -
 - Revised                                                                   -
 -                                                                           -
 -                                                                           -
 - 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"

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

#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 2

#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 = "c:\\test1";
char log_filename[500];
char list_filename[500];
char log_string[500];
char img_output_dir[500];
char fluorophore[10];
char cycle_name[50];

int fl_x, fl_m, fl_block;

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;

FILE *poslist_file, *img_file[4];

Reporter r(DEBUG_LEVEL);

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


bool save_image(short unsigned int*, int, char*);


void main(int argc, char *argv[]){
	int num_frames=0;
	int i, j;
	double new_offset_pifoc=0;
	int success;

	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;

	double *x, *y, *z;

	double ex1, sens1;
	

	short unsigned int *curr_img_stack;

	if(argc<4){
		fprintf(stderr, "ERROR: wrong number of input arguments (expecting 3 or 4):\n");
		fprintf(stderr, " .\\seq_primer exposure sensitivity fluorophore [directory]\n");
		exit(42);
	}

	ex1 = atof(argv[1]) / 1000;
	sens1 = atof(argv[2]);
	strcpy(fluorophore, argv[3]);
	if(argc==5){
		strcpy(cycle_name, argv[4]);
	}
	else{
		strcpy(cycle_name, "000");
	}

	//SET FLUOROPHORE
	//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, "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);
	}


	/*************************************************/
	/* 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(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(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(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 primer (%s) images using the following settings: (%.3f,%.0f)", fluorophore, ex1, sens1);
	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);
	}

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


	/*************************************************/
	/* 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);
	}
	/*                                               */
	/*************************************************/


	//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(_BLOCK_DUAL)){
		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);
	if(!this_filter.SetPosition(2, _CY5_EX, _CY5_EM)){
		r.error("set filter wheels failed", 1);
	}
	

	//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 - 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 - init_pifoc_z;
	}
	autof.setFocusAxis(0);

	if(acq.acquire_expose_noshutter(_BRIGHTFIELD_EXPOSURETIME, _BRIGHTFIELD_SENSITIVITY)){
		sprintf(log_string, "C:\\test1\\SEQ-PRIMER_%s-%s", cycle_name, fluorophore);
		save_image(acq.acquire_getdata(_AUTOFOCUS_BF_CSTEPS), 0, log_string);
	}
	else{
		r.error("unable to acquire starting image", 1);
	}
	this_camera.EndMultiple();
	this_te2000.CloseUniblitz();

	//NOW FLUORESCENT AUTOFOCUS
	if(!this_te2000.SetFilterBlock(fl_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, fl_x, fl_m);
		offset_z = curr_te2000_z - init_te2000_z;
	}
	else{
		//FINE FOCUS USING TE2000
		curr_te2000_z = autof.autofocus_fl(_AUTOFOCUS_FL_CSTEPS, _AUTOFOCUS_FL_CSPACING, ex1, sens1, fl_x, fl_m);
		//FINE FOCUS USING PIFOC
		autof.setFocusAxis(1);
		curr_pifoc_z = autof.autofocus_fl(_AUTOFOCUS_FL_CSTEPS, _AUTOFOCUS_FL_CSPACING, ex1, sens1, fl_x, fl_m);
		offset_z = curr_pifoc_z - init_pifoc_z;
	}
	if(acq.acquire_expose_shutter(ex1, sens1)){
		sprintf(log_string, "C:\\test1\\SEQ-PRIMER_%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();


	//autoe.autoexpose(&ex1, &sens1);
	sprintf(log_string, "Acquiring images using exposure/sensitivity %f/%f", 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);


	//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), i, 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()){;}

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

	for(j=0; j<1; j++){
		save_image(acq.acquire_getdata(j), i, 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);
}

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