import ij.IJ;
import org.micromanager.utils.ImageUtils;
import ij.plugin.FFTMath;
import java.awt.Point;
import ij.process.ImageProcessor;
import ij.process.FloatProcessor;
import ij.ImagePlus;
import ij.process.FHT;
import java.awt.geom.Point2D;
import ij.process.ShortProcessor;
import java.lang.Math;
import java.awt.geom.AffineTransform;
import org.micromanager.utils.MathFunctions;

// Performs a 2D cross-correlation between two images)


ImageProcessor theSlide = null;

ImageProcessor crossCorrelate(ImageProcessor proc1, ImageProcessor proc2) {
	h1 = new FHT(proc1);
	h2 = new FHT(proc2);
	h1.transform();
	h2.transform();
	result = h1.conjugateMultiply(h2);
	result.inverseTransform();
	result.swapQuadrants();
	result.resetMinAndMax();
	return result;
}

// Measures the displacement between two images by cross-correlating, and then finding the maximum value.
// Accurate to one pixel only.

Point2D.Double measureDisplacement(ImageProcessor proc1, ImageProcessor proc2, boolean display) {
	result = crossCorrelate(proc1,proc2);
	resultCenter = getSubImage(result,result.getWidth()/2-8,result.getHeight()/2-8,16,16);
	resultCenter.setInterpolationMethod(ImageProcessor.BICUBIC);
	resultCenterScaled = resultCenter.resize(resultCenter.getWidth()*10);
	img = new ImagePlus("",resultCenterScaled);
	p = ImageUtils.findMaxPixel(img);
	d = new Point(p.x - img.getWidth()/2, p.y - img.getHeight()/2);
	d2 = new Point2D.Double(d.x/10.,d.y/10.);
	if (display)
		img.show();
	return d2;
}

moveRelative(dx, dy) {
	xystage = mmc.getXYStageDevice();
	mmc.waitForDevice(xystage);
	startPos = gui.getXYStagePosition();
	mmc.waitForDevice(xystage);
	gui.setXYStagePosition(startPos.x + dx, startPos.y + dy);
	mmc.waitForDevice(xystage);
	curPos = gui.getXYStagePosition();
	return new Point2D.Double(curPos.x - startPos.x, curPos.y - startPos.y);
}

ImageProcessor getSubImage(ImageProcessor proc, int x, int y, int w, int h) {
	proc2 = new FloatProcessor(w,h);
	proc2.insert(proc,-x,-y);
	return proc2;
}

ImageProcessor simulateAcquire(slideProc, x, y) {
	int w = slideProc.getWidth();
	int h = slideProc.getHeight();
	return getSubImage(slideProc, x, y, w, h);
}

ImageProcessor snapImageAt(double x, double y, boolean simulate) {
	if (simulate) {
		return simulateAcquire(theSlide,(int) (x+(3*Math.random()-1.5)),(int) (y+(3*Math.random()-1.5)));
	} else {
		gui.setXYStagePosition(x,y);
		mmc.waitForDevice(mmc.getXYStageDevice());
		mmc.snapImage();
		pix = mmc.getImage();
		gui.displayImage(pix);
		return ImageUtils.makeProcessor(mmc,pix);
	}
}


Point2D.Double measureDisplacement(double x1, double y1, Point2D.Double d, boolean display, boolean sim) {
		ImageProcessor snap = (ImageProcessor) snapImageAt(x1,y1,sim);
		ImageProcessor foundImage = getSubImage(snap,(int) ((w-w_small)/2),(int) ((h-h_small)/2),w_small,h_small);
		//new ImagePlus("found at "+dx+","+dy,foundImage).show();
		ImageProcessor simulatedImage = getSubImage(baseImage,(int) (d.x-w_small/2+w/2),(int) (d.y-h_small/2+h/2),w_small,h_small);
		//new ImagePlus("simulated at "+dx+","+dy,simulatedImage).show();
		dChange = measureDisplacement(simulatedImage, foundImage, display);
		return new Point2D.Double(d.x + dChange.x,d.y + dChange.y);
}

pointPairs = new Hashtable();

Point2D.Double runSearch(double dxi, double dyi, boolean sim) {

	double dx = dxi;
	double dy = dyi;
	d = new Point2D.Double(0.,0.);

	// Now continue to double displacements and match acquired half-size images with expected half-size images

	for (i=0;i<25;i++) {

		print(dx+","+dy+","+d);
		if ((2*d.x+w_small/2)>=w/2 || (2*d.y+h_small/2)>=h/2 || (2*d.x-w_small/2)<-(w/2) || (2*d.y-h_small/2)<-(h/2))
			break;

		dx = dx*2;
		dy = dy*2;
		d.x = d.x*2;
		d.y = d.y*2;

		d = measureDisplacement(x+dx, y+dy, d, false, sim);

	}
	stagePos = gui.getXYStagePosition();
	pointPairs.put(new Point2D.Double(d.x, d.y),stagePos);
	return stagePos;
	
}

int smallestPowerOf2LessThanOrEqualTo(int x) {
	return 1 << ((int) Math.floor(Math.log(x)/Math.log(2)));
}

double x;
double y;
ImageProcessor baseImage;
int w;
int h;
int w_small;
int h_small;

AffineTransform getFirstApprox(boolean sim) {
	if (sim && theSlide == null) {
		theSlide = IJ.getImage().getProcessor();
	}


	if (sim) {
		x = 0.;
		y = 0.;
	} else {
		p = gui.getXYStagePosition();
		x = p.x;
		y = p.y;
	}

	// First find the smallest detectable displacement.
	baseImage = snapImageAt(x,y,sim);
	
	w = baseImage.getWidth();
	h = baseImage.getHeight();
	w_small = smallestPowerOf2LessThanOrEqualTo(w/4);
	h_small = smallestPowerOf2LessThanOrEqualTo(h/4);


	pointPairs.clear();
	pointPairs.put(new Point2D.Double(0.,0.),new Point2D.Double(x,y));
	runSearch(0.1,0,sim);
	runSearch(0,0.1,sim);
	
	return MathFunctions.generateAffineTransformFromPointPairs(pointPairs);
}

measureCorner(AffineTransform firstApprox, Point c1, sim) {
	c1d = new Point2D.Double(c1.x, c1.y);
	s1 = firstApprox.transform(c1d,null);
	c2 = measureDisplacement(s1.x, s1.y, c1d, false, sim);
	s2 = gui.getXYStagePosition();
	pointPairs.put(new Point2D.Double(c2.x, c2.y), s2);
}

getSecondApprox(AffineTransform firstApprox, boolean sim) {
	pointPairs.clear();
	s1 = new Point2D.Double();
	ax = w/2 - w_small/2;
	ay = h/2 - h_small/2;
	
	c1 = new Point(-ax,-ay);
	measureCorner(firstApprox, c1, sim);
	c1 = new Point(-ax,ay);
	measureCorner(firstApprox, c1, sim);
	c1 = new Point(ax,ay);
	measureCorner(firstApprox, c1, sim);
	c1 = new Point(ax,-ay);
	measureCorner(firstApprox, c1, sim);

	return MathFunctions.generateAffineTransformFromPointPairs(pointPairs);
}

double getPixelSize(AffineTransform cameraToStage) {
	return Math.sqrt(Math.abs(cameraToStage.getDeterminant()));
}

AffineTransform firstApprox;
AffineTransform secondApprox;
double pixelSize;

runCalibration(boolean sim) {
	xy0 = gui.getXYStagePosition();
	firstApprox = getFirstApprox(sim);
	secondApprox = getSecondApprox(firstApprox, sim);
	pixelSize = getPixelSize(secondApprox);
	print(secondApprox);
	print("Pixel size: "+pixelSize);
	gui.setXYStagePosition(xy0.x, xy0.y);
	gui.snapSingleImage();
}

runCalibration() {
	runCalibration(false);
}

update();