/////////////////////////////////////////////////////////////
// Procedure de calcul pour le suivi d'objets de petite
// taille. Initialement concue pour le suivi de centrioles
// fluorescent
/////////////////////////////////////////////////////////////
// Date    : 16/02/2002
// Auteur  : Olivier Cardoso
// Email   : ocardoso@ccr.jussieu.fr
// Adresse : LBHP - Universit  Paris 7 - Denis Diderot
//           Tour 33-43 2 me  tage - Case 7056
//           2 place Jussieu
//           F-75251 Paris Cedex 05
/////////////////////////////////////////////////////////////
import ij.*;
import ij.process.*;
import ij.gui.*;
import java.awt.*;
import ij.plugin.filter.*;
import ij.text.*;
import ij.plugin.frame.*;
import ij.measure.*;

public class TrackCentri_ implements PlugInFilter {
    ImagePlus imp;
    private ResultsTable rt;
    private int objectSize = 6;
    private int correlSize = 3;
    private int tabSize = objectSize*2+1;
    private int tab[][][];
    private double posX[],posY[];
    private ImageProcessor theIP;
    private String st;	
    private double mat[][];
    private double temp[];

    public int setup(String arg, ImagePlus imp) {
        this.imp = imp;
        if (arg.equals("about"))
            {showAbout(); return DONE;}
//	return DOES_8G+SUPPORTS_MASKING;
        return DOES_ALL;
    }

    public void run(ImageProcessor ip) {
        int tmp1,tmp2;
        ImageStack is;

        TrackerParams.params();
        objectSize = TrackerParams.objectSize;
        correlSize = TrackerParams.correlSize;
        tabSize = TrackerParams.tabSize;
        
// On récupère la position des centrioles stocké dans l'image ouverte...
        Analyzer anal =  new Analyzer(imp); 
        rt = anal.getResultsTable(); 
        int cx = rt.getColumnIndex("X"); 
        int cy = rt.getColumnIndex("Y"); 
        int nObjects = rt.getCounter();
        theIP = ip;

// le tableau des images initiales de l'objet à chercher.	
        tab = new int[nObjects][tabSize][tabSize];
        posX = new double[nObjects];
        posY = new double[nObjects];
        imp.setSlice(1);
        for(int i=0;i<nObjects;i++){	
    // on récupère la position initiale de l'objet décalée de la taille de l'objet
            posX[i] = rt.getValue(cx,i)-objectSize;
            posY[i]= rt.getValue(cy,i)-objectSize; 

// On devra ici ajouter un test pour vérifier qu'on ne sort pas des limites permises par
// la bienséance...
// On récupère la forme initiale de l'objet

            for (int dx = 0; dx<tabSize;dx++){
                for(int dy=0;dy<tabSize;dy++){
                    tab[i][dx][dy] = ip.getPixel((int)posX[i]+dx,(int)posY[i]+dy);
                }
            }
        }
        
        int nFrames = imp.getStackSize();
        is = imp.getStack();

        int ox[][] = new int[nObjects][nFrames+1], oy[][]=new int [nObjects][nFrames+1];
        for (int Slice=1;Slice<=nFrames; Slice++) {
            IJ.showProgress((double)Slice/nFrames);
            imp.setSlice(Slice);
            st = is.getSliceLabel(Slice);
            if (st == null) st="";

            tmp1 = st.indexOf("<track");
            if (tmp1 != -1) {
                tmp2 = st.indexOf("track>");
                st = st.substring(0,tmp1) + st.substring(tmp2+6);		
            }
            st += "<track\t";
            for(int i=0;i<nObjects;i++){
                avance(i);
                ox[i][Slice]=(int)(posX[i]+objectSize);
                oy[i][Slice]=(int)(posY[i]+objectSize);
                st += IJ.d2s(posX[i]+objectSize,2)+"\t"+IJ.d2s(posY[i]+objectSize,2)+"\t";
            }
            st += "track>";
            is.setSliceLabel(st,Slice);
        }
        ip.reset();
        imp.setSlice(1);

//      System.out.println("Do Unlock");
        //IJ.run("Unlock Image");
        imp.unlock();
//      System.out.println("Do DrawDivergence");
        IJ.run("DrawDivergence ");
        imp.lockSilently();
    }






// cherche à déterminer la nouvelle position de l'objet i
// Cette procédure modifier posX et posY...
// C'est par posX et posY que sont transférés les nouvelles positions
    void avance(int i){
        double corr;
        double maxcorr=0;
        double theX,theY;
        int tX,tY;


        tX = (int)Math.round(posX[i]);
        tY = (int)Math.round(posY[i]);
        theX = 0;
        theY = 0;

    //ii et jj sont des les déplacements test
        for(int ii=-correlSize;ii<=correlSize;ii++){
            for(int jj=-correlSize;jj<=correlSize;jj++){
                corr = correlation(tab[i],tX,tY,ii,jj);

                if (corr>maxcorr) {
                    maxcorr = corr;
                    theX = ii;
                    theY = jj;
                }
            } // for ii
        } // for jj

// On a trouvé le décalage sur le réseau entier
//On cherche maintenant en interpoint

        double a,b,c,d,e,f;
        double det;
        double xp,xp2,xp3,xp4,yp,yp2,yp3,yp4,zp;

        double x4 =0;
        double x3y =0;
        double x2y2 =0;
        double xy3 =0;
        double y4 =0;
        double x3 =0;
        double x2y =0;
        double xy2 =0;
        double y3 =0;
        double x2 =0;
        double xy =0;
        double y2 =0;
        double x =0;
        double y =0;
        double un =0;
        double zx2 =0;
        double zxy =0;
        double zy2 =0;
        double zx =0;
        double zy =0;
        double z =0;

        mat = new double[7][7];
        temp = new double[7];

        double dX,dY;

        for(int ii=(int)theX-1;ii<=theX+1;ii++){
            for(int jj=(int)theY-1;jj<=theY+1;jj++){

// On se place autour du maximum
                xp = ii-theX;
                yp = jj-theY;
                zp = correlation(tab[i],tX,tY,ii,jj);
                
                xp2 = xp    *   xp;
                xp3 = xp2   *   xp;
                xp4 = xp3   *   xp;
                
                yp2 = yp    *   yp;
                yp3 = yp2   *   yp;
                yp4 = yp3   *   yp;
                
                x4  +=  xp4;
                x3y +=  xp3 *   yp ;
                x2y2+=  xp2 *   yp2;
                xy3 +=  xp  *   yp3;
                y4  +=          yp4;
                
                x3  += xp3;
                x2y += xp2  *   yp;
                xy2 += xp   *   yp2;
                y3  +=          yp3;
                
                x2  += xp2;
                xy  += xp   *   yp;
                y2  +=          yp2;
                
                x   += xp;
                y   +=          yp;
                
                un++;
                
                zx2 += zp   *   xp2;
                zxy += zp   *   xp  *   yp;
                zy2 += zp           *   yp2;
                
                zx  += zp   *   xp;
                zy  += zp           *   yp;
                
                z   += zp;
                
            } // for ii
        } // for jj
        
        mat[1][1] = x4;
        mat[1][2] = x3y;
        mat[1][3] = x2y2;
        mat[1][4] = x3;
        mat[1][5] = x2y;
        mat[1][6] = x2;
        
        mat[2][1] = x3y;
        mat[2][2] = x2y2;
        mat[2][3] = xy3;
        mat[2][4] = x2y;
        mat[2][5] = xy2;
        mat[2][6] = xy;
        
        mat[3][1] = x2y2;
        mat[3][2] = xy3;
        mat[3][3] = y4;
        mat[3][4] = xy2;
        mat[3][5] = y3;
        mat[3][6] = y2;
        
        mat[4][1] = x3;
        mat[4][2] = x2y;
        mat[4][3] = xy2;
        mat[4][4] = x2;
        mat[4][5] = xy;
        mat[4][6] = x;
        
        mat[5][1] = x2y;
        mat[5][2] = xy2;
        mat[5][3] = y3;
        mat[5][4] = xy;
        mat[5][5] = y2;
        mat[5][6] = y;
        
        mat[6][1] = x2;
        mat[6][2] = xy;
        mat[6][3] = y2;
        mat[6][4] = x;
        mat[6][5] = y;
        mat[6][6] = un;
        
        temp[1] = zx2;
        temp[2] = zxy;
        temp[3] = zy2;
        temp[4] = zx;
        temp[5] = zy;
        temp[6] = z;
        
        temp = linearsolve(6);
        
        a = temp[1];
        b = temp[2];
        c = temp[3];
        d = temp[4];
        e = temp[5];
        f = temp[6];
        
        det = b*b-4*a*c;
                
        dX = (2*d*c-e*b)/det;
        dY = (2*e*a-d*b)/det;
        
        posX[i] = (tX+theX+dX);
        posY[i] = (tY+theY+dY);
        
        if ( (dX<-1) || (dX>1)  || (dY<-1) || (dY>1) ) {
            IJ.log("---------------coefficients");
            IJ.log("a="+a);
            IJ.log("b="+b);
            IJ.log("c="+c);
            IJ.log("d="+d);
            IJ.log("e="+e);
            IJ.log("f="+f);
            IJ.log("fonc(x,y)=a*x*x+b*x*y+c*y*y+d*x+e*y+f");
            IJ.log("---------------données");  
            for(int ii=(int)theX-1;ii<=theX+1;ii++){
                for(int jj=(int)theY-1;jj<=theY+1;jj++){
                    zp = correlation(tab[i],tX,tY,ii,jj);
                    IJ.log(""+(ii-theX)+" "+(jj-theY)+" "+zp);
                }
            }
            IJ.log(""+(dX)+" "+(dY)+" "+(a*dX*dX+b*dX*dY+c*dY*dY+d*dX+e*dY+f));
            IJ.log("---------------données image");
            for(int ii=(int)theX-1;ii<=theX+1;ii++){
                String st = "";
                for(int jj=(int)theY-1;jj<=theY+1;jj++){
                    zp = correlation(tab[i],tX,tY,ii,jj);
                    st = st + "  "+IJ.d2s(zp,3);
                }
                IJ.log(st);
            }
        
        
        } 
        
    } // avance;
    
    
    double [] linearsolve(int n){
        double d;
        
        int imax=0;
        double big,dum,sum,tmp;
        double vv[],indx[];
        int ii=0;
        int ip;
        
        vv = new double[n+1];
        indx = new double[n+1];
    
        d=1.0;
        for (int i=1;i<=n;i++) {
            big=0.0;
            for (int j=1;j<=n;j++)
                if ((tmp=Math.abs(mat[i][j])) > big) big=tmp;
            if (big == 0.0) IJ.error("Singular matrix in routine ludcmp");
            vv[i]=1.0/big;
        } // for i
        for (int j=1;j<=n;j++) {
            for (int i=1;i<j;i++) {
                sum=mat[i][j];
                for (int k=1;k<i;k++) sum -= mat[i][k]*mat[k][j];
                mat[i][j]=sum;
            } // for i
            big=0.0;
            for (int i=j;i<=n;i++) {
                sum=mat[i][j];
                for (int k=1;k<j;k++)
                sum -= mat[i][k]*mat[k][j];
                mat[i][j]=sum;
                if ( (dum=vv[i]*Math.abs(sum)) >= big) {
                    big=dum;
                    imax=i;
                } // if
            } // for i
            if (j != imax) {
                for (int k=1;k<=n;k++) {
                    dum=mat[imax][k];
                    mat[imax][k]=mat[j][k];
                    mat[j][k]=dum;
                }// for k
                d = -(d);
                vv[imax]=vv[j];
            }// if j
            indx[j]=imax;
            if (mat[j][j] == 0.0) mat[j][j]=1.0e-20;
            if (j != n) {
                dum=1.0/(mat[j][j]);
                for (int i=j+1;i<=n;i++) mat[i][j] *= dum;
            }// if
        } // for j


        for (int i=1;i<=n;i++) {
            ip=(int)indx[i];
            sum=temp[ip];
            temp[ip]=temp[i];
            if (ii==0) 
                for (int j=ii;j<=i-1;j++) sum -= mat[i][j]*temp[j];
            else 
                if (sum==0) ii=i;
            temp[i]=sum;
        } // for i
        for (int i=n;i>=1;i--) {
            sum=temp[i];
            for (int j=i+1;j<=n;j++) sum -= mat[i][j]*temp[j];
            temp[i]=sum/mat[i][i];
        } // for i
        

        return(temp);	
    }  //linearsolve
    
    double correlation(int tab[][], int px, int py,int ii, int jj){
        double Ex,Ey,Exx,Eyy,Exy;
        double sx,sy;
        double x,y;
        int n;
        
        Ex =0;
        Ey =0;
        Exy =0;
        Exx = 0;
        Eyy = 0;
        n = 0;
        for(int dx=0;dx<tabSize;dx++){
            for(int dy=0;dy<tabSize;dy++){
                x = theIP.getPixel(px+dx+ii,py+dy+jj);
                y = tab[dx][dy];
                n++;
                Ex += x;
                Ey += y;
                Exx += x*x;
                Eyy += y*y;
                Exy += x*y;
            } //for dy
        } // for dx
        Ex /= n;
        Ey /= n;
        Exx /= n;
        Eyy /=n;
        Exy /= n;
                
        sx = Math.sqrt(Exx - Ex * Ex);
        sy = Math.sqrt(Eyy - Ey *Ey);
                
        if (sx ==0) sx =1;
        if (sy == 0) sy =1;
                
        return ((Exy -Ex*Ey)/(sx*sy));
    }

    
    void showAbout() {
        IJ.showMessage("About TrackCentri_...",
            "Ce PlugIn sert à déterminer les différentes\n" +
            "positions des centrioles."
        );
    }
}


// Lecture des paramètres pour le calcul
    class TrackerParams{
        public static int objectSize = 6;
        public static int correlSize = 3;
        public static int tabSize = objectSize*2+1;
    
        static public void params(){
            GenericDialog gd = new GenericDialog("Tacker settings...");
            gd.addNumericField("Object Size :",objectSize,0);
            gd.addNumericField("correlSize :",correlSize,0);
    
            // show the dialog and quit, if the user clicks "cancel"
            gd.showDialog();
            if (gd.wasCanceled()) {
                return;
            }
            objectSize = (int) gd.getNextNumber();
            correlSize= (int) gd.getNextNumber();
            tabSize = objectSize*2+1;
        }// params
    } // class TrackerParams;


