double AOTF488_intensity = 2;
double AOTF561_intensity = 2;

double mir_start = -0.25;  // use Y-axis along is good enough
double mir_step_size = 0.026;   //0.05
int mir_steps = 51;

double etl_start_dpt = 0.6736;
double etl_step_size = -0.071;
int etl_steps = 31;

double mir_x_start = mir_start * -1 + 1.5;
double mir_y_start = mir_start + 2.77;

double z_start = 991.30;
double z_step_size = 1;
int z_steps = 21;


// Constant
double etl_min_dpt = -12.03;
double etl_max_dpt = 16.45;

import mmcorej.TaggedImage;
import org.micromanager.utils.ImageUtils;
import org.micromanager.acquisition.MMAcquisition;
import ij.process.ImageProcessor;
import ij.process.FloatProcessor;
import ij.process.ShortProcessor;
import ij.process.ByteProcessor;
import ij.process.ImageProcessor;
import ij.WindowManager;
import ij.*;
import ij.gui.*;

import Differentials_;
import ij.process.ImageConverter;
import cismm.ETLControlForm;
import java.lang.Float;
import java.lang.Math;
import java.time.LocalDate;
import java.lang.String;

import java.io.File;
import java.io.FileNotFoundException;
import java.io.PrintWriter;

// import for find_peak()
import ij.IJ;
import ij.ImagePlus;
import ij.WindowManager;
import ij.gui.GenericDialog;
import ij.gui.ImageWindow;
import ij.gui.Plot;
import ij.gui.PlotWindow;
import ij.plugin.filter.MaximumFinder;
import ij.util.ArrayUtil;
import ij.util.Tools;


void move_ls(double offset) {
	double target_x = mir_x_start + (offset * -1);
	double target_y = mir_y_start + offset;
	mmc.setProperty("MirrorX", "Volts", String.valueOf(target_x));
	mmc.setProperty("MirrorY", "Volts", String.valueOf(target_y) );
}

void move_etl(double dpt) {
	double v = ((etl_start_dpt + dpt) - etl_min_dpt) / (etl_max_dpt - etl_min_dpt) * 10;
	mmc.setProperty("ETL3", "Volts", String.valueOf(v));
	mmc.waitForSystem();
}

void set_etl(double dpt) {
	double v = ((dpt - etl_min_dpt) / (etl_max_dpt - etl_min_dpt)) * 10;
	mmc.setProperty("ETL3", "Volts", String.valueOf(v));
	mmc.waitForSystem();
}

void set_etl_v(double v) {
	mmc.setProperty("ETL3", "Volts", String.valueOf(v));
	mmc.waitForSystem();
}

void move_z(double offset) {
	double new_z = z_start + offset;
	mmc.setPosition("ZStage", new_z);
	mmc.waitForSystem();
}

void move_mir(double offset) {
	mmc.setProperty("MirrorX", "Volts", String.valueOf(-mir_start - offset));
	mmc.setProperty("MirrorY", "Volts", String.valueOf(mir_start + offset));
	mmc.waitForSystem();
}

void set_z(double z) {
	mmc.setPosition("ZStage", z);
	mmc.waitForSystem();
}

void set_mir(double volt) {
	mmc.setProperty("MirrorX", "Volts", String.valueOf(-volt));
	mmc.setProperty("MirrorY", "Volts", String.valueOf(volt));
	mmc.waitForSystem();
}

double dpt_to_v(double dpt) {
	return (dpt - etl_min_dpt) / (etl_max_dpt - etl_min_dpt) * 10;	
}

void set_AOTF(int wavelength, double v) {
	if (wavelength == 488)
		mmc.setProperty("AOTF488", "Volts", String.valueOf(v));
	else if (wavelength == 561)
		mmc.setProperty("AOTF561", "Volts", String.valueOf(v));
	mmc.waitForSystem();
}

int[] get_date() {
	Date your_date = new Date();
	Calendar cal = Calendar.getInstance(); 
	cal.setTime(your_date);
	int[] ret = new int[5];
	ret[0] = cal.get(Calendar.YEAR);
	ret[1] = cal.get(Calendar.MONTH);
	ret[2] = cal.get(Calendar.DATE);
	ret[3] = cal.get(Calendar.HOUR);
	ret[4] = cal.get(Calendar.MINUTE);
	return ret;
}

String get_save_dir() {
	String year  = String.valueOf(get_date()[0]);
	String month = String.format("%02d", new Object[]{get_date()[1]+1}); 
	//String.valueOf(get_date()[1]+1);
	String date = String.format("%02d", new Object[]{get_date()[2]}); 
	//String date  = String.valueOf(get_date()[2]);
	String dir_str = "F:/Joe_2017/volumetric_scan/"+year+"_"+month+"_"+date;
	new File(dir_str).mkdirs();
	return dir_str;
}

String get_volumetric_file_pre_path() {
	String dir = "C:/Users/phsiao/Desktop/volumetric_launcher";
	String year  = String.valueOf(get_date()[0]);
	String month = String.format("%02d", new Object[]{get_date()[1]+1}); 
	String date = String.format("%02d", new Object[]{get_date()[2]});
	String hour = String.format("%02d", new Object[]{get_date()[3]});
	String min = String.format("%02d", new Object[]{get_date()[4]});
	return dir + "/" + year +"_" + month +"_" + date +"_" + hour +"_" + min;
}

import ij.measure.CurveFitter;

void curve_fit_and_plot(double[] xtofit, double[] ytofit) {
	CurveFitter fitter = new CurveFitter(xtofit, ytofit);
	fitter.doFit(CurveFitter.POLY3);
	ij.plugin.frame.Fitter.plot(fitter);
}
/*
 * find_gaussian_peak is simplied from
 * https://github.com/tferr/Scripts/blob/master/BAR/src/main/resources/scripts/BAR/Data_Analysis/Find_Peaks.bsh
 */
int[] findMaxima(double[] values, double tolerance) {
	MaximumFinder maxFinder = new MaximumFinder();
	int[] positions = maxFinder.findMaxima(values, tolerance, false);
	return positions;
}

double[] getCoordinates(double[] values, int[] positions) {
	int size = positions.length;
	double[] cc = new double[size];
	for (int i=0; i<size; i++)
		cc[i] = values[ positions[i] ];
	return cc;
}

double find_gaussian_peak_x() {
	PlotWindow pw;
	ImagePlus imp = WindowManager.getCurrentImage();
	if (imp==null) {
		IJ.error("There are no plots open.");
		exit("There are no plots open.");
	}
	
	ImageWindow win = imp.getWindow();
	if (win == null || !(win instanceof PlotWindow)) {
		IJ.error(imp.getTitle() +" is not a plot window.");
		return;
	}
	
	pw = (PlotWindow)win;
	float[] fyvalues = pw.getYValues();
	ArrayUtil stats = new ArrayUtil(fyvalues);
	tolerance = Math.sqrt(stats.getVariance());
	yvalues = Tools.toDouble(fyvalues);
	xvalues = Tools.toDouble(pw.getXValues());
	int[] positions = findMaxima(yvalues, tolerance);
	double[] xMaxima = getCoordinates(xvalues, positions);

	win.close();
	return xMaxima[0];
}

/*
ImageProcessor to_imageprocessor(int w, int h, short[] short_arr) {
	ImageProcessor ret = new ShortProcessor(w, h, short_arr, null);
	return ret;
}
*/

/*
 * Image sharpness measure functions
 * ==================================
 */
double func1_tenengrad(ImageProcessor proc) {
	int h = proc.getHeight();
   int w = proc.getWidth();
   double sum = 0.0;
   int[] ken1 = {-1, 0, 1, -2, 0, 2, -1, 0, 1};
   int[] ken2 = {1, 2, 1, 0, 0, 0, -1, -2, -1};

   ImageProcessor proc1 = proc.duplicate();
   ImageProcessor proc2 = proc.duplicate();
   proc1.convolve3x3(ken1);
   proc2.convolve3x3(ken2);
   for (int i=0; i<w; i++){
      for (int j=0; j<h; j++){
         sum += Math.pow(proc1.getPixel(i,j),2) + Math.pow(proc2.getPixel(i, j), 2);
      }
   }
   return sum;
}

/* ==================================
 * End of sharpness measure functions
 */

double snap_and_measure() {
	mmc.snapImage();
	short[] short_arr = mmc.getImage();
	int w = mmc.getImageWidth();
	int h = mmc.getImageHeight();
	ImageProcessor ip = new ShortProcessor(w, h, short_arr, null);
	return func1_tenengrad(ip);
}

double snap_and_measure_and_show() {
	mmc.snapImage();
	TaggedImage ti = mmc.getTaggedImage();
	short[] short_arr = ti.pix;
	int w = mmc.getImageWidth();
	int h = mmc.getImageHeight();
	ImageProcessor ip = new ShortProcessor(w, h, short_arr, null);
	double sharpness = func1_tenengrad(ip);

	gui.closeAllAcquisitions();
	gui.openAcquisition("temp", get_save_dir(), 1, 1, 1, 1, true, false);   
	gui.addImageToAcquisition("temp", 0, 0, 0, 0, ti);
	gui.closeAllAcquisitions();

	return sharpness;
}

/**
 * return:
 * Object[0] = Double(), sharpness meausre
 * Object[1] = TaggedImage(), the most recent image      
 */
Object[] snap_add_acq_measure(String acq_name, int frame_ind,
										int wavelength, double intensity) {
	set_AOTF(wavelength, intensity);
	mmc.snapImage();
	set_AOTF(wavelength, 0);
	TaggedImage ti = mmc.getTaggedImage();
	gui.addImageToAcquisition(acq_name, frame_ind, 0, 0, 0, ti);
	short[] short_arr = ti.pix;
	int w = mmc.getImageWidth();
	int h = mmc.getImageHeight();
	ImageProcessor ip = new ShortProcessor(w, h, short_arr, null);
	double sharpness = func1_tenengrad(ip);

	Object ret = new Object[2];
	ret[0] = Double.valueOf(sharpness);
	ret[1] = ti;
	
	return ret;
}

TaggedImage snap(int wavelength, double intensity) {
	set_AOTF(wavelength, intensity);
	mmc.snapImage();
	set_AOTF(wavelength, 0);
	return mmc.getTaggedImage();
}

void set_roi_from_selection() {
	ImagePlus imp = WindowManager.getCurrentImage();
	Roi roi = imp.getRoi();
	if (roi == null) {
		gui.message("Selection of a region is required.");
		return;
	}
	java.awt.Rectangle bounds = roi.getBounds();
	int x = bounds.getX();
	int y = bounds.getY();
	int w = bounds.getWidth();
	int h = bounds.getHeight();
	mmc.setROI(x, y, w, h);
}

void restore_roi(int w, int h) {
	mmc.setROI(0, 0, w, h);
}

/*  With everything else fixed, run a ETL scan from a specified range, and get a focus
 *  measurement at each ETL position. Results will be written to a file.
 */
void etl_scan_with_output(String scan_name, double start, double end, int steps) {
	String csv_file = get_save_dir() + "/" + scan_name + ".csv";
	PrintWriter pw = new PrintWriter(new File(csv_file));
	StringBuilder sb = new StringBuilder();

	sb.append("");
	double step_size = (end - start) / steps;
	
	for (int i=0; i<steps; i++) {
		double etl_value =  step_size * i + start;
		sb.append("," + String.valueOf(etl_value));
		if (i == steps-1)
			sb.append("\n");
	}

	gui.closeAllAcquisitions();
	gui.openAcquisition(scan_name, get_save_dir(), steps, 1, 1, 1, true, true);
	//sb.append("Focus");
	for (int i = 0; i < steps; i++) {
		double cur = start + (step_size * i);
		//set_etl(cur);
		set_etl_v(cur);
		mmc.snapImage();
		TaggedImage ti = mmc.getTaggedImage();
		gui.addImageToAcquisition(scan_name, i, 0, 0, 0, ti);
		short[] short_arr = ti.pix;
		int w = mmc.getImageWidth();
		int h = mmc.getImageHeight();
		ImageProcessor ip = new ShortProcessor(w, h, short_arr, null);
		double sharpness = func1_tenengrad(ip);
		sb.append("," + String.valueOf(sharpness));
		if (i == steps-1)
			sb.append("\n");   
	}
	gui.closeAllAcquisitions();
	pw.write(sb.toString());
   pw.close();
}

void ETL3_distance_test() {
	String acq_pre = "ETL3Test";
	move_z(0);
	move_etl(0);
	gui.closeAllAcquisitions();

	for (int i=0; i < z_steps; i++) {
		double z_offset = z_step_size * i;
		move_z(z_offset);
		mmc.waitForSystem();

		String acq_name = acq_prefix + String.valueOf(i) + "_z_" + String.valueOf(z_offset);
		gui.openAcquisition(acq_name, get_save_dir(), etl_steps, 1, 1, 1, true, true);
		double[] focus_arr = new double[etl_steps];
		for (int j=0; j < etl_steps; j++) {
			double etl_offset = etl_step_size * j;
			move_etl(etl_offset);
			mmc.waitForSystem();
			
			mmc.snapImage();
			
			short[] short_arr = mmc.getImage();
			int w = mmc.getImageWidth();
			int h = mmc.getImageHeight();
			ImageProcessor ip = new ShortProcessor(w, h, short_arr, null);
			focus_arr[j] = func1_tenengrad(ip);
			
			TaggedImage ti = new TaggedImage(short_arr, null);
			gui.addImageToAcquisition(acq_name, j, 0, 0, 0, ti);
		}
		gui.closeAllAcquisitions();
	}
}

void plainview_scan() {
	String acq_pre = "PlainView_4_";
	gui.closeAllAcquisitions();

	String csv_file = get_save_dir() + "/" + acq_pre + ".csv";
	PrintWriter pw = new PrintWriter(new File(csv_file));
	StringBuilder sb = new StringBuilder();

	// csv column titles
	sb.append("dpt");
	for (int i=0; i<etl_steps; i++) {
		double etl_value = etl_step_size * i + etl_start_dpt;
		sb.append("," + String.valueOf(etl_value));
		if (i == etl_steps-1)
			sb.append("\n");
	}
	
	for (int i=0; i<z_steps; i++) {
		double z_offset = z_step_size * i;
		move_z(z_offset);
		String acq_name = acq_pre + String.valueOf(i) + "_z_" + String.valueOf(z_offset+z_start);
		gui.openAcquisition(acq_name, get_save_dir(), etl_steps, 1, 1, 1, true, true);
		sb.append(String.valueOf(z_offset + z_start));
		
		for (int j=0; j<etl_steps; j++) {
			double etl_offset = etl_step_size * j;
			move_etl(etl_offset);
			mmc.snapImage();
			TaggedImage ti = mmc.getTaggedImage();
			gui.addImageToAcquisition(acq_name, j, 0, 0, 0, ti);
			short[] short_arr = ti.pix;
			int w = mmc.getImageWidth();
			int h = mmc.getImageHeight();
			ImageProcessor ip = new ShortProcessor(w, h, short_arr, null);
			double sharpness = func1_tenengrad(ip);
			sb.append("," + String.valueOf(sharpness));
			if (j == etl_steps-1)
				sb.append("\n");   
		}
		gui.closeAllAcquisitions();
	}
	pw.write(sb.toString());
   pw.close();
}

/*
 * Take a full volume scan with LS, and for each LS we'd save a whole range of focus.
 * The stacks can be further used for autofocus analyzation.
 */
void sideview_scan() {
	String acq_pre = "liveCell_10_50steps";
	String csv_file = get_save_dir() + "/" + acq_pre + ".csv";
	PrintWriter pw = new PrintWriter(new File(csv_file));
	StringBuilder sb = new StringBuilder();

	// csv column titles
	sb.append("mir_row_etl_col");
	for (int i=0; i<etl_steps; i++) {
		double etl_value = etl_step_size * i + etl_start_dpt;
		sb.append("," + String.valueOf(etl_value));
		if (i == etl_steps-1)
			sb.append("\n");
	}

	gui.closeAllAcquisitions();
	for (int i=0; i<mir_steps; i++) {
		double mir_offset = mir_step_size * i;
		move_mir(mir_offset);
		String acq_name = acq_pre + String.format("%03d", i) + "_mir_" + String.valueOf(mir_offset+mir_start);
		gui.openAcquisition(acq_name, get_save_dir(), etl_steps, 1, 1, 1, true, true);
		sb.append(String.valueOf(mir_offset + mir_start));
		
		for (int j=0; j<etl_steps; j++) {
			double etl_offset = etl_step_size * j;
			move_etl(etl_offset);
			mmc.snapImage();
			TaggedImage ti = mmc.getTaggedImage();
			gui.addImageToAcquisition(acq_name, j, 0, 0, 0, ti);
			short[] short_arr = ti.pix;
			int w = mmc.getImageWidth();
			int h = mmc.getImageHeight();
			ImageProcessor ip = new ShortProcessor(w, h, short_arr, null);
			double sharpness = func1_tenengrad(ip);
			sb.append("," + String.valueOf(sharpness));
			if (j == etl_steps-1)
				sb.append("\n");   
		}
		gui.closeAllAcquisitions();
	}
	pw.write(sb.toString());
   pw.close();
}

double find_best_focus_etl(double etl_center, double scan_step_size, int amplitude_steps)
{
	double max_sharpness = 0;
	double most_sharp_etl = etl_center;
	double etl_start = etl_center - (scan_step_size * amplitude_steps);
	for (int i = 0 ; i < amplitude_steps * 2 + 1; i++) {
			double cur_etl = etl_start + scan_step_size * i;
			set_etl_v(cur_etl);
			double sharp_mea = snap_and_measure();
			print(String.valueOf(cur_etl) + " : " + String.valueOf(sharp_mea));
			if (sharp_mea > max_sharpness) {
				max_sharpness = sharp_mea;
				most_sharp_etl = cur_etl;
			}
	}
	return most_sharp_etl;
}

/**
 Given start and end positions of mirror(v) and ETL(v), the function will
 interpolate @num_slices mirror positions and ETL voltages.
 
 @mir_start: mirror start voltage
 @mir_end: mirror end voltage
 @elt_start: etl start voltage
 @etl_end: etl_end_voltage
 @num_slices: total number of mirro positions in a scan
 @etl_amplitude: centered on a interpolated etl value, how far a focus scan would go in one side.
**/

void side_scan_interpolation(String acq_pre, double mir_start, double mir_end,
				  			 double etl_start, double etl_end,
				  			 int num_slices, double etl_step_size, int amplitude_steps)
{
	acq_pre = acq_pre + "_" + String.valueOf(num_slices) + "_slices";
	String csv_file = get_save_dir() + "/" + acq_pre + ".csv";
	PrintWriter pw = new PrintWriter(new File(csv_file));
	StringBuilder sb = new StringBuilder();
	sb.append("Mir_Pos, ELT_v, Sharpness\n");

	/*
	set_mir(mir_end);
	etl_end = find_best_focus_etl(etl_end, 0.05, 5);
	print("etl end = " + String.valueOf(etl_end));
	set_mir(mir_start);
	etl_start = find_best_focus_etl(etl_start, 0.05, 5);
	print("etl start = " + String.valueOf(etl_start));
	*/
	
	double[] mir_pos_arr = new double[num_slices];
	double[] etl_init_pos_arr = new double[num_slices];
	double[][] etl_volt_arr = new double[num_slices][amplitude_steps * 2 + 1];
	
	double mir_step_size = (mir_end - mir_start) / (num_slices - 1);
	double etl_step_size = (etl_end - etl_start) / (num_slices - 1);
	
	for (int i = 0 ; i < num_slices; i++) {
		mir_pos_arr[i] = mir_start + mir_step_size * i;
		etl_init_pos_arr[i] = etl_start + etl_step_size * i;
		
		double etl_local_start = etl_init_pos_arr[i] - (etl_step_size * amplitude_steps);
		for (int j = 0 ; j < amplitude_steps * 2 + 1; j++) {
			etl_volt_arr[i][j] = etl_local_start + etl_step_size * j;
		}
	}
	
	gui.closeAllAcquisitions();
	String acq_best_focus = acq_pre + "_best_foucs_";
	gui.openAcquisition(acq_best_focus, get_save_dir(), mir_pos_arr.length, 1, 1, 1, true, true);
	String best_csv_file = get_save_dir() + "/" + acq_best_focus + ".csv";
	PrintWriter best_pw = new PrintWriter(new File(best_csv_file));
	StringBuilder best_sb = new StringBuilder();
	for (int i = 0 ; i < mir_pos_arr.length; i++) {
		set_mir(mir_pos_arr[i]);
		String acq_name = acq_pre + "_" + String.format("%03d", new Object[]{i});
		double max_sharpness = 0;
		double most_sharp_etl = 0;
		TaggedImage most_sharp_img = null;
		gui.openAcquisition(acq_name, get_save_dir(), etl_volt_arr[i].length, 1, 1, 1, true, true);
		for (int j = 0 ; j < etl_volt_arr[i].length; j++) {
			set_etl_v(etl_volt_arr[i][j]);
			Object[] sharpness_obj = snap_add_acq_measure(acq_name, j);
			sb.append(String.valueOf(mir_pos_arr[i]) + "," +
					  String.valueOf(etl_volt_arr[i][j]) + "," +
					  ((Double)sharpness_obj[0]).toString() +
					  "\n");
			if (((Double)sharpness_obj[0]).doubleValue() > max_sharpness) {
				max_sharpness = ((Double)sharpness_obj[0]).doubleValue();
				most_sharp_img = (TaggedImage)sharpness_obj[1];
				most_sharp_etl = etl_volt_arr[i][j];
			}
		}
		gui.closeAcquisition(acq_name);  // close etl local scan acq
		
		if (most_sharp_img == null)
			most_sharp_img = TaggedImage(sharpness_obj[1]);
		
		gui.addImageToAcquisition(acq_best_focus, i, 0, 0, 0, most_sharp_img);
		best_sb.append(String.valueOf(mir_pos_arr[i]) + "," + String.valueOf(most_sharp_etl) + "," + String.valueOf(max_sharpness) + "\n");
	}
	gui.closeAcquisition(acq_best_focus); // close best focus acq
	pw.write(sb.toString());
	pw.close();
	best_pw.write(best_sb.toString());
	best_pw.close();
}

double get_slope(double x1, double x2, double y1, double y2)
{
	if (x1 == x2)
		return 0.000001;
	return (y2 - y1) / (x2 - x1); 
}

void save_2_color_volumetric_file(double[] mir_pos_arr, double[] etl_final_arr,
							      int readout, int exposure, double laser_intensity_1, double laser_intensity_2) {
	String dir = "C:/Users/efn/Desktop/volumetric_launcher";
	new File(dir).mkdirs();
	String year  = String.valueOf(get_date()[0]);
	String month = String.format("%02d", new Object[]{get_date()[1]+1}); 
	String date = String.format("%02d", new Object[]{get_date()[2]});
	String hour = String.format("%02d", new Object[]{get_date()[3]});
	String min = String.format("%02d", new Object[]{get_date()[4]});
	
	String out_file = dir + "/" + year +"_" + month +"_" + date +"_" + hour +"_" + min +
					  "_" + "readout_" + String.valueOf(readout) + "_" + "exp_" + String.valueOf(exposure) + ".txt";
	PrintWriter pw = new PrintWriter(new File(out_file));
	StringBuilder sb = new StringBuilder();
	
	sb.append(String.valueOf(readout) + " ");
	sb.append(String.valueOf(exposure) + " ");
	sb.append("3" + " ");
	
	int larger = readout > exposure ? readout : exposure;
	int smaller = readout < exposure ? readout : exposure;
	
	int ratio = larger / smaller;

	if (larger == readout) {
		for (int i=0 ; i<ratio-1; i++)
			sb.append("0 ");
		sb.append(String.valueOf(laser_intensity_1) + " ");
		sb.append("0 ");
	} else {
		sb.append("0 ");
		for (int i=0 ; i<ratio-1; i++)
			sb.append(String.valueOf(laser_intensity_1) + " ");
		sb.append("0 ");
	}
	
	for (int i=0; i<(ratio+1)*2; i++) {
		sb.append("0 ");
	}
	
	if (larger == readout) {
		for (int i=0 ; i<ratio-1; i++)
			sb.append("0 ");
		sb.append(String.valueOf(laser_intensity_2) + " ");
		sb.append("0 ");
	} else {
		sb.append("0 ");
		for (int i=0 ; i<ratio-1; i++)
			sb.append(String.valueOf(laser_intensity_2) + " ");
		sb.append("0 ");
	}

	for (int i=0; i<(ratio+1)*2; i++) {
		sb.append("10 ");
	}
	
	// Use N+1 frames to catch the last exposure.
	// We need to repeat the last voltage twice for each array.
	//sb.append(String.valueOf(mir_pos_arr.length+1) + " ");
	sb.append(String.valueOf(mir_pos_arr.length) + " ");
	sb.append("3 ");
	
	for (int i=0; i<mir_pos_arr.length; i++) {
		sb.append(String.valueOf(-mir_pos_arr[i]) + " ");	
	}
	//sb.append(String.valueOf(-mir_pos_arr[mir_pos_arr.length-1] + " "));
	
	for (int i=0; i<mir_pos_arr.length; i++) {
		sb.append(String.valueOf(mir_pos_arr[i]) + " ");	
	}
	//sb.append(String.valueOf(mir_pos_arr[mir_pos_arr.length-1] + " "));
	
	for (int i=0; i<etl_final_arr.length; i++) {
		sb.append(String.valueOf(etl_final_arr[i]) + " ");	
	}
	//sb.append(String.valueOf(etl_final_arr[etl_final_arr.length-1] + " "));
	
	pw.write(sb.toString());
	pw.close();
}

void save_mm_2_color_volumetric_file(double[] mir_pos_arr, double[] etl_final_arr,
							         int exposure, double laser_intensity_1, double laser_intensity_2)
{
	String dir = "C:/Users/efn/Desktop/volumetric_launcher";
	new File(dir).mkdirs();
	String year  = String.valueOf(get_date()[0]);
	String month = String.format("%02d", new Object[]{get_date()[1]+1}); 
	String date = String.format("%02d", new Object[]{get_date()[2]});
	String hour = String.format("%02d", new Object[]{get_date()[3]});
	String min = String.format("%02d", new Object[]{get_date()[4]});
	
	String out_file = dir + "/mm_" + year +"_" + month +"_" + date +"_" + hour +"_" + min +
					  "_" + "exp_" + String.valueOf(exposure) + "_laser" +
					  String.valueOf(laser_intensity_1) + "_" + String.valueOf(laser_intensity_2) +
					  ".txt";
	PrintWriter pw = new PrintWriter(new File(out_file));
	StringBuilder sb = new StringBuilder();
	sb.append(String.valueOf(exposure) + "," + 
			  String.valueOf(laser_intensity_1) +
			  String.valueOf(laser_intensity_2) + "\n");
	
	sb.append(String.valueOf(mir_pos_arr.length));
	
	for (int i = 0 ; i < mir_pos_arr.length; i++) {
		sb.append(String.valueOf(mir_pos_arr[i]) + "," + String.valueOf(etl_final_arr[i]) + "\n");
	}
	pw.write(sb.toString());
	pw.close();
}

void twenty_frames_side_scan(String acq_pre, double mir_start, double mir_end,
			  			 double landmark_mir_1, double landmark_mir_2,
			  			 double landmark_etl_1, double landmark_etl_2,
			  			 int num_slices, double etl_local_step_size, int amplitude_steps)
{
	acq_pre = acq_pre + "_" + String.valueOf(num_slices) + "_slices";
	String csv_file = get_save_dir() + "/" + acq_pre + ".csv";
	PrintWriter pw = new PrintWriter(new File(csv_file));
	StringBuilder sb = new StringBuilder();
	sb.append("Mir_Pos, ELT_v, Sharpness\n");
	
	// Get an esimation of what elt_start and elt_end going to be
	// using the two landmarks. 
	// Equatino: y = Ax + B

	/*
	set_mir(landmark_mir_1);
	landmark_etl_1 = find_best_focus_etl(landmark_etl_1, 0.05, 2);
	print("landmark_etl_1 = " + String.valueOf(landmark_etl_1));
	set_mir(landmark_mir_2);
	landmark_etl_2 = find_best_focus_etl(landmark_etl_2, 0.05, 2);
	print("landmark_etl_2 = " + String.valueOf(landmark_etl_2));
	*/
	
	double slope = get_slope(landmark_mir_1, landmark_mir_2,
							 landmark_etl_1, landmark_etl_2);
	
	double etl_start = (mir_start - landmark_mir_1) * slope + landmark_etl_1;
	double etl_end =   (mir_end - landmark_mir_2)   * slope + landmark_etl_2;

	/*
	set_mir(mir_start);
	etl_start = find_best_focus_etl(etl_start, 0.05, 5);
	set_mir(mir_end);
	elt_end = find_best_focus_etl(etl_end, 0.05, 5);
	*/
	
	double[] mir_pos_arr = new double[num_slices];
	double[] etl_init_pos_arr = new double[num_slices];
	double[][] etl_volt_arr = new double[num_slices][amplitude_steps * 2 + 1];
	double[] etl_final_arr = new double[num_slices];
	
	double mir_step_size = (mir_end - mir_start) / (num_slices - 1);
	double etl_step_size = (etl_end - etl_start) / (num_slices - 1);
	
	for (int i = 0 ; i < num_slices; i++) {
		mir_pos_arr[i] = mir_start + mir_step_size * i;
		etl_init_pos_arr[i] = etl_start + etl_step_size * i;
		
		double etl_local_start = etl_init_pos_arr[i] - (etl_local_step_size * amplitude_steps);
		for (int j = 0 ; j < amplitude_steps * 2 + 1; j++) {
			etl_volt_arr[i][j] = etl_local_start + etl_local_step_size * j;
		}
	}
	
	gui.closeAllAcquisitions();
	String acq_best_focus = acq_pre + "_best_foucs";
	gui.openAcquisition(acq_best_focus, get_save_dir(), mir_pos_arr.length, 1, 1, 1, true, true);
	String best_csv_file = get_save_dir() + "/" + acq_best_focus + ".csv";
	PrintWriter best_pw = new PrintWriter(new File(best_csv_file));
	StringBuilder best_sb = new StringBuilder();
	for (int i = 0 ; i < mir_pos_arr.length; i++) {
		set_mir(mir_pos_arr[i]);
		String acq_name = acq_pre + "_" + String.format("%03d", new Object[]{i});
		double max_sharpness = 0;
		double most_sharp_etl = 0;
		TaggedImage most_sharp_img = null;
		gui.openAcquisition(acq_name, get_save_dir(), etl_volt_arr[i].length, 1, 1, 1, true, true);
		for (int j = 0 ; j < etl_volt_arr[i].length; j++) {
			set_etl_v(etl_volt_arr[i][j]);
			Object[] sharpness_obj = snap_add_acq_measure(acq_name, j);
			sb.append(String.valueOf(mir_pos_arr[i]) + "," +
					  String.valueOf(etl_volt_arr[i][j]) + "," +
					  ((Double)sharpness_obj[0]).toString() +
					  "\n");
			if (((Double)sharpness_obj[0]).doubleValue() > max_sharpness) {
				max_sharpness = ((Double)sharpness_obj[0]).doubleValue();
				most_sharp_img = (TaggedImage)sharpness_obj[1];
				most_sharp_etl = etl_volt_arr[i][j];
				etl_final_arr[i] = most_sharp_etl;
			}
		}
		gui.closeAcquisition(acq_name);  // close etl local scan acq
		
		if (most_sharp_img == null)
			most_sharp_img = TaggedImage(sharpness_obj[1]);
		
		gui.addImageToAcquisition(acq_best_focus, i, 0, 0, 0, most_sharp_img);
		best_sb.append(String.valueOf(mir_pos_arr[i]) + "," + String.valueOf(most_sharp_etl) + "," + String.valueOf(max_sharpness) + "\n");
	}
	gui.closeAcquisition(acq_best_focus); // close best focus acq
	pw.write(sb.toString());
	pw.close();
	best_pw.write(best_sb.toString());
	best_pw.close();
	
	save_volumetric_scan_file(mir_pos_arr, etl_final_arr, 10, 90, AOTF488_intensity);
}

void twenty_frames_side_scan_2_colors(String acq_pre, double mir_start, double mir_end,
			  			 double landmark_mir_1, double landmark_mir_2,
			  			 double landmark_etl_1, double landmark_etl_2,
			  			 int num_slices, double etl_local_step_size, int amplitude_steps)
{
	acq_pre = acq_pre + "_" + String.valueOf(num_slices) + "_slices";
	String csv_file = get_save_dir() + "/" + acq_pre + ".csv";
	PrintWriter pw = new PrintWriter(new File(csv_file));
	StringBuilder sb = new StringBuilder();
	sb.append("Mir_Pos, ELT_v, Sharpness\n");
	
	// Get an esimation of what elt_start and elt_end going to be
	// using the two landmarks. 
	// Equatino: y = Ax + B

	/*
	set_mir(landmark_mir_1);
	landmark_etl_1 = find_best_focus_etl(landmark_etl_1, 0.05, 2);
	print("landmark_etl_1 = " + String.valueOf(landmark_etl_1));
	set_mir(landmark_mir_2);
	landmark_etl_2 = find_best_focus_etl(landmark_etl_2, 0.05, 2);
	print("landmark_etl_2 = " + String.valueOf(landmark_etl_2));
	*/
	
	double slope = get_slope(landmark_mir_1, landmark_mir_2,
							 landmark_etl_1, landmark_etl_2);
	
	double etl_start = (mir_start - landmark_mir_1) * slope + landmark_etl_1;
	double etl_end =   (mir_end - landmark_mir_2)   * slope + landmark_etl_2;

	/*
	set_mir(mir_start);
	etl_start = find_best_focus_etl(etl_start, 0.05, 5);
	set_mir(mir_end);
	elt_end = find_best_focus_etl(etl_end, 0.05, 5);
	*/
	
	double[] mir_pos_arr = new double[num_slices];
	double[] etl_init_pos_arr = new double[num_slices];
	double[][] etl_volt_arr = new double[num_slices][amplitude_steps * 2 + 1];
	double[] etl_final_arr = new double[num_slices];
	
	double mir_step_size = (mir_end - mir_start) / (num_slices - 1);
	double etl_step_size = (etl_end - etl_start) / (num_slices - 1);
	
	for (int i = 0 ; i < num_slices; i++) {
		mir_pos_arr[i] = mir_start + mir_step_size * i;
		etl_init_pos_arr[i] = etl_start + etl_step_size * i;
		
		double etl_local_start = etl_init_pos_arr[i] - (etl_local_step_size * amplitude_steps);
		for (int j = 0 ; j < amplitude_steps * 2 + 1; j++) {
			etl_volt_arr[i][j] = etl_local_start + etl_local_step_size * j;
		}
	}
	
	gui.closeAllAcquisitions();
	String acq_best_focus_488 = acq_pre + "_best_foucs_488";
	String acq_best_focus_561 = acq_pre + "_best_foucs_561";
	
	gui.openAcquisition(acq_best_focus_488, get_save_dir(), mir_pos_arr.length, 1, 1, 1, true, true);
	gui.openAcquisition(acq_best_focus_561, get_save_dir(), mir_pos_arr.length, 1, 1, 1, true, true);
	
	String best_csv_file = get_save_dir() + "/" + acq_best_focus + ".csv";
	PrintWriter best_pw = new PrintWriter(new File(best_csv_file));
	StringBuilder best_sb = new StringBuilder();
	for (int i = 0 ; i < mir_pos_arr.length; i++) {
		set_mir(mir_pos_arr[i]);
		String acq_name = acq_pre + "_" + String.format("%03d", new Object[]{i});
		double max_sharpness = 0;
		double most_sharp_etl = 0;
		TaggedImage most_sharp_img_488 = null;
		TaggedImage most_sharp_img_561 = null;
		gui.openAcquisition(acq_name, get_save_dir(), etl_volt_arr[i].length, 1, 1, 1, true, true);
		
		for (int j = 0 ; j < etl_volt_arr[i].length; j++) {
			set_etl_v(etl_volt_arr[i][j]);
			//Object[] sharpness_obj = snap_add_acq_measure(acq_name, j, 488, AOTF488_intensity);
			Object[] sharpness_obj = snap_add_acq_measure(acq_name, j, 561, AOTF561_intensity);
			sb.append(String.valueOf(mir_pos_arr[i]) + "," +
					  String.valueOf(etl_volt_arr[i][j]) + "," +
					  ((Double)sharpness_obj[0]).toString() +
					  "\n");
			if (((Double)sharpness_obj[0]).doubleValue() > max_sharpness) {
				max_sharpness = ((Double)sharpness_obj[0]).doubleValue();
				//most_sharp_img_488 = (TaggedImage)sharpness_obj[1];
				most_sharp_img_561 = (TaggedImage)sharpness_obj[1];
				most_sharp_etl = etl_volt_arr[i][j];
				etl_final_arr[i] = most_sharp_etl;
				most_sharp_img_488 = snap(488, AOTF488_intensity);
			}
		}
		gui.closeAcquisition(acq_name);  // close etl local scan acq
		
		if (most_sharp_img_488 == null) {
			gui.message("ERROR: 488 image is null");
			most_sharp_img_488 = TaggedImage(sharpness_obj[1]);
		}
		
		if (most_sharp_img_561 == null) {
			gui.message("ERROR: 561 image is null");
			most_sharp_img_561 = snap(561, AOTF561_intensity);
		}
		
		gui.addImageToAcquisition(acq_best_focus_488, i, 0, 0, 0, most_sharp_img_488);
		gui.addImageToAcquisition(acq_best_focus_561, i, 0, 0, 0, most_sharp_img_561);
		
		best_sb.append(String.valueOf(mir_pos_arr[i]) + "," + String.valueOf(most_sharp_etl) + "," + String.valueOf(max_sharpness) + "\n");
	}
	gui.closeAcquisition(acq_best_focus_488); // close best focus acq
	gui.closeAcquisition(acq_best_focus_561); 
	pw.write(sb.toString());
	pw.close();
	best_pw.write(best_sb.toString());
	best_pw.close();
	
	save_2_color_volumetric_file(mir_pos_arr, etl_final_arr, 1, 9, AOTF488_intensity+2, AOTF561_intensity+2);
	save_mm_2_color_volumetric_file(mir_pos_arr, etl_final_arr, 20, AOTF488_intensity, AOTF561_intensity);
}

double gaussian_fit_and_find_peak(double[] xtofit, double[] ytofit) {
	curve_fit_and_plot(xtofit, ytofit);
	return find_gaussian_peak_x();	
}

void twenty_frames_side_scan_2_colors_with_fit(String acq_pre, double mir_start, double mir_end,
			  			 double landmark_mir_1, double landmark_mir_2,
			  			 double landmark_etl_1, double landmark_etl_2,
			  			 int num_slices, double etl_local_step_size, int amplitude_steps)
{
	acq_pre = acq_pre + "_" + String.valueOf(num_slices) + "_slices";
	String csv_file = get_save_dir() + "/" + acq_pre + ".csv";
	PrintWriter pw = new PrintWriter(new File(csv_file));
	StringBuilder sb = new StringBuilder();
	sb.append("Mir_Pos, ELT_v, Sharpness\n");
	
	// Get an esimation of what elt_start and elt_end going to be
	// using the two landmarks. 
	// Equatino: y = Ax + B

	/*
	set_mir(landmark_mir_1);
	landmark_etl_1 = find_best_focus_etl(landmark_etl_1, 0.05, 2);
	print("landmark_etl_1 = " + String.valueOf(landmark_etl_1));
	set_mir(landmark_mir_2);
	landmark_etl_2 = find_best_focus_etl(landmark_etl_2, 0.05, 2);
	print("landmark_etl_2 = " + String.valueOf(landmark_etl_2));
	*/
	
	double slope = get_slope(landmark_mir_1, landmark_mir_2,
							 landmark_etl_1, landmark_etl_2);
	
	double etl_start = (mir_start - landmark_mir_1) * slope + landmark_etl_1;
	double etl_end =   (mir_end - landmark_mir_2)   * slope + landmark_etl_2;

	/*
	set_mir(mir_start);
	etl_start = find_best_focus_etl(etl_start, 0.05, 5);
	set_mir(mir_end);
	elt_end = find_best_focus_etl(etl_end, 0.05, 5);
	*/
	
	double[] mir_pos_arr = new double[num_slices];
	double[] etl_init_pos_arr = new double[num_slices];
	double[][] etl_volt_arr = new double[num_slices][amplitude_steps * 2 + 1];
	double[] etl_final_arr = new double[num_slices];
	double[] etl_gaussian_arr = new double[num_slices];
	
	double mir_step_size = (mir_end - mir_start) / (num_slices - 1);
	double etl_step_size = (etl_end - etl_start) / (num_slices - 1);
	
	for (int i = 0 ; i < num_slices; i++) {
		mir_pos_arr[i] = mir_start + mir_step_size * i;
		etl_init_pos_arr[i] = etl_start + etl_step_size * i;
		
		double etl_local_start = etl_init_pos_arr[i] - (etl_local_step_size * amplitude_steps);
		for (int j = 0 ; j < amplitude_steps * 2 + 1; j++) {
			etl_volt_arr[i][j] = etl_local_start + etl_local_step_size * j;
		}
	}
	
	gui.closeAllAcquisitions();
	String acq_best_focus_488 = acq_pre + "_best_foucs_488";
	String acq_best_focus_561 = acq_pre + "_best_foucs_561";
	
	gui.openAcquisition(acq_best_focus_488, get_save_dir(), mir_pos_arr.length, 1, 1, 1, true, true);
	gui.openAcquisition(acq_best_focus_561, get_save_dir(), mir_pos_arr.length, 1, 1, 1, true, true);
	
	String best_csv_file = get_save_dir() + "/" + acq_best_focus + ".csv";
	PrintWriter best_pw = new PrintWriter(new File(best_csv_file));
	StringBuilder best_sb = new StringBuilder();
	for (int i = 0 ; i < mir_pos_arr.length; i++) {
		set_mir(mir_pos_arr[i]);
		String acq_name = acq_pre + "_" + String.format("%03d", new Object[]{i});
		double max_sharpness = 0;
		double most_sharp_etl = 0;
		TaggedImage most_sharp_img_488 = null;
		TaggedImage most_sharp_img_561 = null;
		gui.openAcquisition(acq_name, get_save_dir(), etl_volt_arr[i].length, 1, 1, 1, true, true);
		double[] sharpness_arr = new double[etl_volt_arr[i].length];
		for (int j = 0 ; j < etl_volt_arr[i].length; j++) {
			set_etl_v(etl_volt_arr[i][j]);
			Object[] sharpness_obj = snap_add_acq_measure(acq_name, j, 488, AOTF488_intensity);
			sb.append(String.valueOf(mir_pos_arr[i]) + "," +
					  String.valueOf(etl_volt_arr[i][j]) + "," +
					  ((Double)sharpness_obj[0]).toString() +
					  "\n");
			if (((Double)sharpness_obj[0]).doubleValue() > max_sharpness) {
				max_sharpness = ((Double)sharpness_obj[0]).doubleValue();
				most_sharp_img_488 = (TaggedImage)sharpness_obj[1];
				most_sharp_etl = etl_volt_arr[i][j];
				etl_final_arr[i] = most_sharp_etl;
				most_sharp_img_561 = snap(561, AOTF561_intensity);
				sharpness_arr[j] = max_sharpness;
			}
			
		}
		gui.closeAcquisition(acq_name);  // close etl local scan acq
		
		
		if (most_sharp_img_488 == null) {
			gui.message("ERROR: 488 image is null");
			most_sharp_img_488 = TaggedImage(sharpness_obj[1]);
		}
		
		if (most_sharp_img_561 == null) {
			gui.message("ERROR: 561 image is null");
			most_sharp_img_561 = snap(561, AOTF561_intensity);
		}
		
		gui.addImageToAcquisition(acq_best_focus_488, i, 0, 0, 0, most_sharp_img_488);
		gui.addImageToAcquisition(acq_best_focus_561, i, 0, 0, 0, most_sharp_img_561);
		
		best_sb.append(String.valueOf(mir_pos_arr[i]) + "," + String.valueOf(most_sharp_etl) + "," + String.valueOf(max_sharpness) + "\n");
		
		double gaussian_best_etl = gaussian_fit_and_find_peak(etl_volt_arr[i], sharpness_arr);
		etl_gaussian_arr[i] = gaussian_best_etl;
	}
	
	gui.closeAcquisition(acq_best_focus_488); // close best focus acq
	gui.closeAcquisition(acq_best_focus_561); 
	pw.write(sb.toString());
	pw.close();
	best_pw.write(best_sb.toString());
	best_pw.close();
	
	save_2_color_volumetric_file(mir_pos_arr, etl_gaussian_arr, 1, 9, AOTF488_intensity+3, AOTF561_intensity+3);
}

void main() {
	/*
	restore_roi(2048, 2048);
	return;
	*/
	
	set_roi_from_selection();
	mmc.setExposure(200);
	AOTF488_intensity = 2;
	AOTF561_intensity = 2;
	
	/**
	 * name
	 * mir_start, mir_end
	 * lm_mir_1, lm_mir_2
	 * lm_etl_1, lm_etl_2
	 * num_slice
	 * etl_local_step_size
	 * etl_amplitude_steps
	 **/
	twenty_frames_side_scan_2_colors("fixedCell_1_3",
						0.9, 2.5,
			  			1.2, 2,
			  			4.45, 4.15,
			  			50,
			  			0.1,
			  			3);
	
	//plainview_scan();
	//sideview_scan();
	//side_scan_interpolation("livecell_1_8", -0.4, 0.9, 4.9, 4.4, 50, 0.05, 10);
	//set_etl_v(7.3);
	//etl_scan_with_output("fixed_ls_side3_scan_2", 5.5, 7.3,28);
	restore_roi(2048, 2048);
}

main();

/**** devices 
core.setProperty("AOTF488", "Volts", "0");
core.setProperty("AOTF561", "Volts", "0");
core.setProperty("AOTF_Blanking", "Volts", "0");
core.setProperty("MirrorX", "Volts", "0");
core.setProperty("MirrorY", "Volts", "0");
core.setProperty("ETL3", "Volts", "0");
****/




/**** focus measure ****/
/*
double ret = snap_and_measure();
gui.message(ret);

TaggedImage global_ti;

double snap_and_measure() {
	mmc.snapImage();
	global_ti = mmc.getTaggedImage();
	//gui.displayImage(ti);

	proc = makeMonochromeProcessor(global_ti.pix);
	if (proc == null) {
		gui.message("makeMonochromeProcessor() returns null.");
		return -1;
	}
	return computeTenengrad(proc);
}

public static ImageProcessor makeMonochromeProcessor(Object pixels) {
      int w = mmc.getImageWidth();
      int h = mmc.getImageHeight();
      if (pixels instanceof byte[]) {
         return new ByteProcessor(w, h, (byte[]) pixels, null);
      } else if (pixels instanceof short[]) {
         return new ShortProcessor(w, h, (short[]) pixels, null);
      } else {
         return null;
      }
   }

double computeTenengrad(ImageProcessor proc) {
	int h = proc.getHeight();
   int w = proc.getWidth();
   double sum = 0.0;
   int[] ken1 = {-1, 0, 1, -2, 0, 2, -1, 0, 1};
   int[] ken2 = {1, 2, 1, 0, 0, 0, -1, -2, -1};

	ImageProcessor proc1 = proc.duplicate();
   ImageProcessor proc2 = proc.duplicate();
   
   proc1.convolve3x3(ken1);
   proc2.convolve3x3(ken2);
   for (int i=0; i<w; i++){
      for (int j=0; j<h; j++){
			double p1 = proc1.getPixel(i,j);
      	double p2 = proc2.getPixel(i,j);
      	double pow1 = Math.pow(p1, 2.0);
      	double pow2 = Math.pow(p2, 2.0);

         sum += pow1 + pow2;
      }
   }
   return sum;
}
*/