import ij.*;
import ij.gui.*;
import ij.plugin.filter.*;
import ij.process.*;
import ij.text.*;
import ij.measure.*;
import java.awt.*;
import java.text.*;
import java.math.*;

public class newCORtrackOrigin_ implements PlugInFilter
 {
	boolean BoW = false;  		//black on white image?
	boolean interp = true;		//do interpolation of correlation peak?
	boolean parabolic = false;		//do the interpolation by parabolic interpolation?  Otherwise centroid
	double centroidpeak = 0.2;		//what fraction of the correlation peak to use in centroid calculation
	boolean showCOR = false;		//show the correlation matrix in the corner?  Good for diagnostics
	boolean showOverallVelocity = false; 	//show a window with the overall velocity at the end

	ImagePlus imp;
	int mcount = 0;
	double[][] CORmatrix = new double[256][256];
        	double peakx, peaky;
	TextWindow textWindow;
	TextPanel textPanel;
	ResultsTable myResults;
	Rectangle KernelRect;
	Rectangle CurrentRect;
	int [][] ROImatrix = new int [256][256];

	public int setup(String arg, ImagePlus imp)
	{
		this.imp = imp;
		if (showOverallVelocity) 
		{
			textWindow = new TextWindow("CORtrackOrigin Results", "", 320, 300);
			textPanel = textWindow.getTextPanel();
		}
		return DOES_ALL;
	}

	public void run(ImageProcessor ip) 
	{
		CORFollowAlong2();
	}
    
	private void setupKernel()
	{
		int row, column;
		for (column = 0; column < KernelRect.width; column++)
		{
			for (row = 0; row < KernelRect.height; row++) 
			{
				ROImatrix[column][row] = imp.getProcessor().getPixel(column+KernelRect.x, row+KernelRect.y);
				if (!BoW)
				{
					ROImatrix[column][row] = 255-ROImatrix[column][row];
				}
		                  }
		}
	}

	private void zeroCORmatrix()
	{
		int row, column;
		for (column = 0; column < KernelRect.width; column++)
		{
			for (row = 0; row < KernelRect.height; row++) 
			{
				CORmatrix[column][row] = 0;		
		                  }
		}
	}


	private void drawCORmatrix(double max)
	{
		int row,column;
		int thevalue;
		for (row=0; row<KernelRect.height; row++)  // draws the COR matrix in the corner
		{
			for (column=0; column<KernelRect.width; column++)
				{
					thevalue = (int)Math.floor(CORmatrix[column][row]*255/max);
					if (BoW) 
						{imp.getProcessor().putPixel(column,row,thevalue);
					}else{
						imp.getProcessor().putPixel(column,row,255-thevalue);}
				}
		} 
	}


	private double COR() 
	{
		int row, column, index;
		int RoiWidth, RoiHeight;
		double SumX, SumY, SumAll;
		double ComX, ComY;
		String str, str2; 
		int OriginX, OriginY;
		int x, y, i, j;
		double CORmax;
		int CORmaxX, CORmaxY;
                		double a,b,c,d,e;
		double CORx, CORy;
		double pixelvalue;
                		double temppix, tempnorm;


		zeroCORmatrix();

		for (x=0; x < CurrentRect.width; x++)
		{
			for (y=0; y < CurrentRect.height; y++)
			{
				for (column = 0; column < KernelRect.width; column++)
				{
					for (row = 0; row < KernelRect.height; row++) 
					{
						pixelvalue = imp.getProcessor().getPixel(column+(CurrentRect.x-(CurrentRect.width/2))+x, row+(CurrentRect.y-(CurrentRect.height/2))+y);
						if (BoW)
						{
							CORmatrix[x][y] = CORmatrix[x][y] + pixelvalue*ROImatrix[column][row];
						}
						else
						{
							CORmatrix[x][y] = CORmatrix[x][y] + (255-pixelvalue)*ROImatrix[column][row];
						}
					}
				}
			}
		}
		CORmax = CORmatrix[0][0];
		double CORmin = CORmax;
		CORmaxX = 0;
		CORmaxY = 0;
		for (x=0; x<CurrentRect.width; x++)
		{
			for (y=0; y<CurrentRect.height; y++)
			{
				if (CORmatrix[x][y] >= CORmax)
				{
					CORmax = CORmatrix[x][y];
					CORmaxX=x;
					CORmaxY=y;
				}
				if (CORmatrix[x][y] < CORmin)
				{
					CORmin = CORmatrix[x][y];
				}
			}
		}
		CORx = (double)CORmaxX - ((double)CurrentRect.width/2)+(double)0.5;
		CORy = (double)CORmaxY - ((double)CurrentRect.height/2)+(double)0.5;
                
               if ((CORmaxX > 1) & (CORmaxX < CurrentRect.width-1) & (CORmaxY > 1) & (CORmaxY < CurrentRect.height-1) & interp)  
	//if it's got room and it is supposed to, do the interpolation
                   {
		if (parabolic) 
		{
	     		a = CORmatrix[CORmaxX][CORmaxY];		//this is parabolic interpolation, ala cheezum et al
                        		b = (CORmatrix[CORmaxX+1][CORmaxY]-CORmatrix[CORmaxX-1][CORmaxY])/2;  
                       		 c = (CORmatrix[CORmaxX][CORmaxY+1]-CORmatrix[CORmaxX][CORmaxY-1])/2;
                        		d = -CORmatrix[CORmaxX][CORmaxY]+0.5*CORmatrix[CORmaxX+1][CORmaxY]+0.5*CORmatrix[CORmaxX-1][CORmaxY];
                        		e = -CORmatrix[CORmaxX][CORmaxY]+0.5*CORmatrix[CORmaxX][CORmaxY+1]+0.5*CORmatrix[CORmaxX][CORmaxY-1];
                        		peakx= b/(2*d)+CORx;
                        		peaky= c/(2*e)+CORy;
		} else {
	     		//CENTROID INTERPOLATION
	    		double CORrange = CORmax-CORmin;
	    		double CORthresh = CORrange*(1-centroidpeak)+CORmin;  //keep top centroidpeak%
	    		if (!(CORrange<1))
	   		 {		
				double CORsumX = 0;
				double CORsumY = 0;
				int thismany = 0;
				for (x=0; x<CurrentRect.width; x++)
				{
					for (y=0; y<CurrentRect.height; y++)
					{
						if (CORmatrix[x][y] >= CORthresh)
						{
							CORsumX = CORsumX + x;
							CORsumY = CORsumY + y;
							thismany++;
						}
					}
				}
				peakx = CORsumX/(double)thismany - ((double)CurrentRect.width/2)+(double)0.5;
				peaky = CORsumY/(double)thismany - ((double)CurrentRect.height/2)+(double)0.5;
			}
		}
	    } else {
		peakx=CORx;
		peaky=CORy;
	}
                
                	return CORmax;
	}

        
        
	public void CORFollowAlong2()
	{
		int i, beginslice;
		double CurrentX, CurrentY;
		double LastX, LastY;
		Point Move = new Point();
		
		double tempreal, velocity, sx, sx2, sd, mean;
		double cor_value = 0;
		double myInterval, stddev;
                		double cor_self;
		double totalx = 0;
		double totaly = 0;
		int row,column;
		int thevalue = 0;

		sx=0;
		sx2=0;
		mcount=0;
		myInterval = imp.getCalibration().frameInterval; 

		KernelRect=imp.getRoi().getBoundingRect();
		beginslice = imp.getCurrentSlice();

		myResults = Analyzer.getResultsTable(); // set up the results table
		myResults.reset();

		int locx = myResults.getFreeColumn("x_displacement"); // find the first free column
		int locy = myResults.getFreeColumn("y_displacement"); 
		int locv = myResults.getFreeColumn("velocity"); 
		int locc = myResults.getFreeColumn("correlation"); 
                
         
                		if ((KernelRect.width % 2)==0) // make the width and height odd numbers                		
		{
			KernelRect.width ++;
                 		}
                		if ((KernelRect.height % 2)==0)                 		
                		 {
			KernelRect.height ++;
                		 }	
	                	imp.setRoi(KernelRect);     // make ROI selection region
		
		CurrentRect = new Rectangle();
		CurrentRect.x=KernelRect.x;
		CurrentRect.y=KernelRect.y;
		CurrentRect.width=KernelRect.width;
		CurrentRect.height=KernelRect.height;

                		imp.setSlice(beginslice);
		imp.updateAndRepaintWindow();
		setupKernel();
		
                		cor_self = COR();  // get self correlation as a reference value
		if (showCOR) {drawCORmatrix(cor_self);}
		LastX = 0;
		LastY = 0;
		double skewX = 0;  //original offset of correlation peak from center, to be subrated from every cross-cor
		double skewY = 0;
                
		for (i=beginslice; i<(imp.getStackSize()+1); i++)
		{	
			imp.setSlice(i); 
			imp.updateAndRepaintWindow();

			if (imp.getCurrentSlice()<imp.getStackSize())
			{
				cor_value = COR();
			}

			if (showCOR) {drawCORmatrix(cor_self);}

			if (i==beginslice) 
			{
				skewX = peakx;
				skewY = peaky;
			}
			if (imp.getCalibration().scaled())
			{
				CurrentX = (peakx-skewX+CurrentRect.x-KernelRect.x)* imp.getCalibration().pixelWidth;
				CurrentY = (peaky-skewY+CurrentRect.y-KernelRect.y)*imp.getCalibration().pixelHeight;
			}
			else
			{
				CurrentX = (peakx-skewX+CurrentRect.x-KernelRect.x);
				CurrentY = (peaky-skewY+CurrentRect.y-KernelRect.y);
			}
			
			velocity = Math.sqrt(((CurrentX-LastX)*(CurrentX-LastX))+((CurrentY-LastY)*(CurrentY-LastY)));
			if (myInterval>0)
			{
				velocity/=myInterval;
			}
			sx += velocity;
			sx2+= velocity*velocity;
			mcount++;

			LastX = CurrentX;
			LastY = CurrentY;

			CurrentRect.translate((int)Math.round(peakx),(int)Math.round(peaky));   // shift the ROI by the integer displacement reported by COR

			imp.setRoi(CurrentRect);     //MAKE REGION

			myResults.incrementCounter();
			myResults.addValue(locx,CurrentX);
			myResults.addValue(locy,CurrentY);
			myResults.addValue(locv,velocity);
			myResults.addValue(locc,(cor_value/cor_self));

			if (imp.getWidth()<CurrentRect.x+CurrentRect.width || imp.getHeight()<CurrentRect.y+CurrentRect.height || CurrentRect.x<0 || CurrentRect.y<0 /*||CommandPeriod*/ )
			{
				IJ.beep();
				i = imp.getStackSize();
				// End early if out of bounds
			}

			
		}	
		if (showOverallVelocity)
		{
			StringBuffer buf = new StringBuffer();
			mean = sx / mcount;
			stddev = Math.sqrt((sx2 - sx*sx/mcount)/mcount);
			buf.append("Mean = ");
			buf.append(IJ.d2s(mean));
			buf.append("+/-");
			buf.append(IJ.d2s(stddev));	
			buf.append("s.d. (");
			buf.append(IJ.d2s(stddev/Math.sqrt(mcount)));
			buf.append(" s.e.m.)");
			textPanel.appendLine(buf.toString());
		}
		myResults.show("Positions");

	}

}