import ij.*;
import ij.process.*;
import ij.gui.*;
import java.awt.*;
import ij.plugin.PlugIn;
import ij.WindowManager;
//import edu.emory.mathcs.jtransforms.fft.*;
//import edu.emory.mathcs.utils.*;
import org.jtransforms.fft.*;
import org.jtransforms.utils.*;
import flanagan.complex.*;
import flanagan.math.*;
import ij.plugin.frame.RoiManager;
import ij.gui.Roi;


public class Align_ComputeShifts implements PlugIn {
    protected ImagePlus imp_r, imp_i;
    protected int reference_slide;
    protected Roi roi;
    protected int usfac;
    protected boolean debug = true;
    double[][] shifts;
    boolean allShifts;
    
    static boolean isPowerOf2(int n) {
        if (n == 0)
            return false;

        while (n != 1) {
            if (n % 2 != 0)
                return false;
            n = n / 2;
        }
        return true;
    }
    

     public void setup(int upscaleFactor, boolean allShifts, ImagePlus imp_r, ImagePlus imp_i, 
            int reference_slide,  Roi roi) {
         if (imp_r==null){
            throw new RuntimeException("Real part image must exist!");
         }

         if (roi==null){
            roi = new Roi(0,0,imp_r.getWidth(), imp_r.getHeight());
         }
        Rectangle box = roi.getBounds();
        //if (!ConcurrencyUtils.isPowerOf2(box.height) || !ConcurrencyUtils.isPowerOf2(box.width)) {
        if (!isPowerOf2(box.height) || !isPowerOf2(box.width)) {
            throw new RuntimeException("The selected ROI height and with must be a power of 2");
        }

         this.usfac = upscaleFactor;
         this.allShifts = allShifts;
         this.imp_r = imp_r;
         this.imp_i = imp_i;
         this.reference_slide=reference_slide;
         this.roi = roi;
        
     }

    public void run(String arg) {   
            if (allShifts) {
                calculateAllShiftsRun();
                
            } else {
                calculateShiftsRun();
            }
            return;
    }

    private void calculateShiftsRun() {
        // perform the FFT of each slice
        
        IJ.showStatus("1/2 Perform FFT of each slice");
        ComplexMatrix[] ffts = computeFFT();
        
        // calculate shifts
        IJ.showStatus("2/2 Calculate shifts between slices");
        shifts = calculateShifts(ffts);

        /*
        // save shifts
        ShiftsIO sio = new ShiftsIO();
        sio.save(shifts, "directshifts");
       */
    }

   public double[][] getShifts(){
        return shifts;
   }

    private void calculateAllShiftsRun() {
        // perform the FFT of each slice
        IJ.showStatus("1/2 Perform FFT of each slice");
        ComplexMatrix[] ffts = computeFFT();

        // calculate shifts
        IJ.showStatus("2/2 Calculate shifts between slices");
        shifts = calculateAllShifts(ffts);

        /*
        // save shifts
        ShiftsIO sio = new ShiftsIO();
        sio.save(shifts, "allshifts");
        */
    }

    // perform the FFT of each slice
    private ComplexMatrix[] computeFFT() {
        
        int slices = imp_r.getStackSize();
        
        ComplexMatrix[] ffts = new ComplexMatrix[slices];
        for (int i=1; i <= slices; i++) {
            if (imp_i == null) {
                ImageProcessor ip = imp_r.getStack().getProcessor(i);
                ip.setRoi(roi);
                ImageProcessor curr = ip.crop().convertToFloat();
                double[][] data = ImageProcessor_to_FFTArray2D(curr);   
                ffts[i-1] = fft2(data);
            } else {
                ImageProcessor ip1, ip2;
                ip1 = imp_r.getStack().getProcessor(i);
                ip1.setRoi(roi);
                ImageProcessor curr_r = ip1.crop().convertToFloat();
                ip2 = imp_i.getStack().getProcessor(i);
                ip2.setRoi(roi);
                ImageProcessor curr_i = ip2.crop().convertToFloat();
                double[][] data = ImageProcessor_to_FFTComplexArray2D(curr_r, curr_i);
                ffts[i-1] = cfft2(data);
            }
            IJ.showProgress(i, slices);
        }

        return ffts;
    }

    //calculate the shifts between ffts
    private double[][] calculateShifts(ComplexMatrix[] ffts) {
        
        double[][] shifts = new double[ffts.length][6];        
        for (int i = 0; i < ffts.length; i++) {
            shifts[i][0] = reference_slide; shifts[i][1] = i+1;
            double[] temp = DFTRegistration(ffts[reference_slide - 1], ffts[i]);
            shifts[i][2] = temp[2]; shifts[i][3] = temp[3];
            shifts[i][4] = temp[0]; shifts[i][5] = temp[1];            
            IJ.showProgress(i + 1, ffts.length);
        }
        return shifts; // [ref, drifted, dr, dc, error, diffphase]
    
    }

    //calculate all the shifts between ffts
    private double[][] calculateAllShifts(ComplexMatrix[] ffts) {

        double[][] shifts = new double[ffts.length*(ffts.length-1)/2][6];
        int id = 0;
        for (int i = 0; i < ffts.length-1; i++) {
            for (int j = i+1; j < ffts.length; j++) {
                shifts[id][0] = i+1; shifts[id][1] = j+1;
                double[] temp = DFTRegistration(ffts[i], ffts[j]);
                shifts[id][2] = temp[2]; shifts[id][3] = temp[3];
                shifts[id][4] = temp[0]; shifts[id][5] = temp[1];
                id = id + 1;
                IJ.showProgress(id + 1, ffts.length*(ffts.length-1)/2);
            }
        }
        
        return shifts; // [ref,drifted,dr,dc,error, diffphase]
    }
    
    // compute 2D fft from an image
    private ComplexMatrix fft2(double[][] data) {

        int h = data.length;
        int w = data[0].length;
        DoubleFFT_2D fft = new DoubleFFT_2D(h, w);
        
        fft.realForward(data);
        ComplexMatrix m = FFTArray2D_to_ComplexMatrix(data, h, w);

        return m;
    }

    // compute complex 2D fft from an image
    private ComplexMatrix cfft2(double[][] data) {

        int h = data.length;
        int w = data[0].length;
        DoubleFFT_2D fft = new DoubleFFT_2D(h, w/2);
        
        fft.complexForward(data);
        ComplexMatrix m = FFTComplexArray2D_to_ComplexMatrix(data, h, w/2);

        return m;
    }
    
    // compute inverse 2D fft from a complex matrix
    private double[][] ifft2(ComplexMatrix m) {
        int w = m.getNcol();
        int h = m.getNrow();
        
        DoubleFFT_2D fft = new DoubleFFT_2D(h, w);
        
        double[][] data = ComplexMatrix_to_FFTArray2D(m);   
        fft.realInverse(data, true);

        return data;
    }
    
    // compute complex inverse 2D fft from a complex matrix
    private ComplexMatrix cifft2(ComplexMatrix m) {
        int w = m.getNcol();
        int h = m.getNrow();
        
        DoubleFFT_2D fft = new DoubleFFT_2D(h, w);
        
        double[][] data = new double[h][2*w];
        for (int j=0; j<h; j++) {
            for (int i=0; i<w; i++) {
                data[j][2*i] = m.getElementReference(j, i).getReal();
                data[j][2*i+1] = m.getElementReference(j, i).getImag();             
            }
        }

        fft.complexInverse(data, true);

        ComplexMatrix out = new ComplexMatrix(h, w);
        for (int j=0; j<h; j++) {
            for (int i=0; i<w; i++) {
                out.setElement(j, i, data[j][2*i], data[j][2*i+1]);         
            }
        }

        return out;
    }
    
    private double[] DFTRegistration(ComplexMatrix ref, ComplexMatrix drifted) {
        int m = ref.getNrow();
        int n = ref.getNcol();
        double[] output = new double[4];
        
        // First upsample by a factor of 2 to obtain initial estimate
            // Embed Fourier data in a 2x larger array
            int mlarge = m*2;
            int nlarge = n*2;
            ComplexMatrix large = new ComplexMatrix(mlarge, nlarge);
            ComplexMatrix c = fftshift(ElementProduct(ref, drifted.conjugate()));

            for (int j = 0; j < m; j++) {
                for (int i = 0; i < n; i++) {
                    large.setElement((int)(j + m - Math.floor(m/2.0)), (int)(i + n - Math.floor(n/2.0)), c.getElementReference(j, i));
                }
            }
  
            // Compute crosscorrelation and locate the peak
            ComplexMatrix CC = cifft2(ifftshift(large));
            double[] peak = cFindPeak(CC); //max, r, c, max_r, max_c
                
            // Obtain shift in original pixel grid from the position of the
            // crosscorrelation peak 
            if (peak[1] > m) { 
                peak[1] = peak[1] - mlarge;
            }
        if (peak[2] > n) {
            peak[2] = peak[2] - nlarge;
        }

            //% If upsampling > 2, then refine estimate with matrix multiply DFT
            if (this.usfac > 2) {
                //   %%% DFT computation %%%
                //   % Initial shift estimate in upsampled grid
                double row_shift = Math.round(peak[1]/2.0*this.usfac)/this.usfac; 
                double col_shift = Math.round(peak[2]/2.0*this.usfac)/this.usfac;     
                int dftshift = (int)Math.floor(Math.ceil(this.usfac*1.5)/2); // Center of output array at dftshift+1
                //   % Matrix multiply DFT around the current shift estimate
                ComplexMatrix in = ElementProduct(drifted, ref.conjugate());
                ComplexMatrix nCC = dftups(in, (int)Math.ceil(this.usfac*1.5), (int)Math.ceil(this.usfac*1.5),
                        dftshift-row_shift*this.usfac, dftshift-col_shift*this.usfac);
                    nCC = nCC.times(1.0/(m*n*this.usfac*this.usfac)).conjugate();
                //   % Locate maximum and map back to original pixel grid 
                double[] npeak = cFindPeak(nCC); //max_r, max_i, r, c
    
            ComplexMatrix mrg00 = dftups(ElementProduct(ref, ref.conjugate()),1,1,0,0);
            double rg00 = mrg00.getElementReference(0, 0).abs()/(m*n*this.usfac*this.usfac);
                ComplexMatrix mrf00 = dftups(ElementProduct(drifted, drifted.conjugate()),1,1,0,0);
                double rf00 = mrf00.getElementReference(0, 0).abs()/(m*n*this.usfac*this.usfac);
                
                npeak[1] = npeak[1] - dftshift;
                npeak[2] = npeak[2] - dftshift;
                output[0] = Math.sqrt(Math.abs(1.0 - npeak[0]*npeak[0]/(rg00*rf00)));   //error
            output[1] = Math.atan2(npeak[4], npeak[3]);             //diffphase     
                output[2] = row_shift + npeak[1]/this.usfac;            //delta row
                output[3] = col_shift + npeak[2]/this.usfac;            //delta col
                
            } else {
            // % If upsampling = 2, no additional pixel shift refinement
            double rg00 = SumSquareAbs(ref)/(mlarge*nlarge);
            double rf00 = SumSquareAbs(drifted)/(mlarge*nlarge);
            
            output[0] = Math.sqrt(Math.abs(1.0 - peak[0]*peak[0]/(rg00*rf00))); //error
            output[1] = Math.atan2(peak[4], peak[3]);               //diffphase     
            output[2] = peak[1]/2.0;                    //delta row
                output[3] = peak[2]/2.0;                    //delta col
            }
            
        return output;  
    }

    private double SumSquareAbs(ComplexMatrix m) {
        double sum = 0.0;

        for (int j = 0; j < m.getNrow(); j ++){
            for (int i = 0; i < m.getNcol(); i++) {
                sum += m.getElementReference(j, i).squareAbs();
            }
        }

        return sum;
    }
    
    private double[] cFindPeak(ComplexMatrix m) {
        double max = 0.0;
        double realmax = 0.0;
        double imagmax = 0.0;
        int cmax = 0, rmax = 0;
        
        for (int j = 0; j < m.getNrow(); j ++){
            for (int i = 0; i < m.getNcol(); i++) {
                if (m.getElementReference(j, i).abs() > max) {
                    max = m.getElementReference(j, i).abs();
                    realmax = m.getElementReference(j, i).getReal();
                    imagmax = m.getElementReference(j, i).getImag();
                    rmax = j;
                    cmax = i;
                }
            }
        }

        double[] res = new double[5];
        res[0] = Math.sqrt(realmax*realmax+imagmax*imagmax); res[1] = rmax; res[2] = cmax;
        res[3] = realmax; res[4] = imagmax;
        return res;
    }
    
    private ComplexMatrix fftshift(ComplexMatrix in) {
        int nc = in.getNcol();
        int nr = in.getNrow();

        ComplexMatrix out = new ComplexMatrix (nr, nc);
        
        int midi = (int)Math.floor(nc/2.0);
        int offi = (int)Math.ceil(nc/2.0);
        int midj = (int)Math.floor(nr/2.0);
        int offj = (int)Math.ceil(nr/2.0);
        
        for (int j = 0; j < nr; j ++){
            for (int i = 0; i < nc; i++) {
                if (j < midj) {
                    if (i < midi) {
                        out.setElement(j, i, in.getElementReference(j+offj, i+offi));
                    } else {
                        out.setElement(j, i, in.getElementReference(j+offj, i-midi));       
                    }
                } else {
                    if (i < midi) {
                        out.setElement(j, i, in.getElementReference(j-midj, i+offi));
                    } else {
                        out.setElement(j, i, in.getElementReference(j-midj, i-midi));       
                    }
                }
            }
        }

        return out;
    }

    private ComplexMatrix ifftshift(ComplexMatrix in) {
        int nc = in.getNcol();
        int nr = in.getNrow();

        ComplexMatrix out = new ComplexMatrix (nr, nc);
        
        int midi = (int)Math.ceil(nc/2.0);
        int offi = (int)Math.floor(nc/2.0);
        int midj = (int)Math.ceil(nr/2.0);
        int offj = (int)Math.floor(nr/2.0);
        
        for (int j = 0; j < nr; j ++){
            for (int i = 0; i < nc; i++) {
                if (j < midj) {
                    if (i < midi) {
                        out.setElement(j, i, in.getElementReference(j+offj, i+offi));
                    } else {
                        out.setElement(j, i, in.getElementReference(j+offj, i-midi));       
                    }
                } else {
                    if (i < midi) {
                        out.setElement(j, i, in.getElementReference(j-midj, i+offi));
                    } else {
                        out.setElement(j, i, in.getElementReference(j-midj, i-midi));       
                    }
                }
            }
        }

        return out;
    }
    
    private Matrix ifftshift(Matrix in) {
        int nc = in.getNcol();
        int nr = in.getNrow();

        Matrix out = new Matrix (nr, nc);
        
        int midi = (int)Math.ceil(nc/2.0);
        int offi = (int)Math.floor(nc/2.0);
        int midj = (int)Math.ceil(nr/2.0);
        int offj = (int)Math.floor(nr/2.0);
        
        for (int j = 0; j < nr; j ++){
            for (int i = 0; i < nc; i++) {
                if (j < midj) {
                    if (i < midi) {
                        out.setElement(j, i, in.getElement(j+offj, i+offi));
                    } else {
                        out.setElement(j, i, in.getElement(j+offj, i-midi));        
                    }
                } else {
                    if (i < midi) {
                        out.setElement(j, i, in.getElement(j-midj, i+offi));
                    } else {
                        out.setElement(j, i, in.getElement(j-midj, i-midi));        
                    }
                }
            }
        }

        return out;
    }

        public Matrix times(Matrix amat, Matrix bmat){
            
            if(amat.getNumberOfColumns()!=bmat.getNumberOfRows())throw new IllegalArgumentException("Nonconformable matrices");

            Matrix cmat = new Matrix(amat.getNumberOfRows(), bmat.getNumberOfColumns());
            double [][] aarray = amat.getArrayReference();
            double [][] barray = bmat.getArrayReference();
            double [][] carray = cmat.getArrayReference();
            double sum = 0.0D;

            for(int i=0; i<amat.getNumberOfRows(); i++){
                    for(int j=0; j<bmat.getNumberOfColumns(); j++){
                        sum=0.0D;
                        for(int k=0; k<amat.getNumberOfColumns(); k++){
                                sum += aarray[i][k]*barray[k][j];
                        }
                        carray[i][j]=sum;
                    }
            }
            return cmat;
        }   
    
    private ComplexMatrix dftups(ComplexMatrix in, int nor, int noc, double roff, double coff) {
        // function out=dftups(in,nor,noc,usfac,roff,coff);
        // Upsampled DFT by matrix multiplies, can compute an upsampled DFT in just
        // a small region.
        // usfac         Upsampling factor (default usfac = 1)
        // [nor,noc]     Number of pixels in the output upsampled DFT, in
        //               units of upsampled pixels (default = size(in))
        // roff, coff    Row and column offsets, allow to shift the output array to
        //               a region of interest on the DFT (default = 0)
        // Recieves DC in upper left corner, image center must be in (1,1)
        // Loïc Le Guyader - Jun 11, 2011 Java version for ImageJ plugin 
        // Manuel Guizar - Dec 13, 2007
        // Modified from dftus, by J.R. Fienup 7/31/06

        // This code is intended to provide the same result as if the following
        // operations were performed
        //   - Embed the array "in" in an array that is usfac times larger in each
        //     dimension. ifftshift to bring the center of the image to (1,1).
        //   - Take the FFT of the larger array
        //   - Extract an [nor, noc] region of the result. Starting with the 
        //     [roff+1 coff+1] element.

        // It achieves this result by computing the DFT in the output array without
        // the need to zeropad. Much faster and memory efficient than the
        // zero-padded FFT approach if [nor noc] are much smaller than [nr*usfac nc*usfac]

        int nr = in.getNrow();
        int nc = in.getNcol();
        // Compute kernels and obtain DFT by matrix products
        double amplitude = -2.0*Math.PI/(nc*usfac);
        
        Matrix u = new Matrix(nc, 1);
        for (int i = 0; i < nc; i++) {
            u.setElement(i, 0, i - Math.floor(nc/2.0));
        }
        u = ifftshift(u);

        Matrix v = new Matrix(1, noc);
        for (int i = 0; i < noc; i++) {
            v.setElement(0, i, i-coff);
        }
        Matrix phase = u.times(v);
        //Matrix phase = times(u,v);
      
        ComplexMatrix kernc = new ComplexMatrix(nc, noc);
        for (int j = 0; j < nc; j++) {
            for (int i = 0; i < noc; i++)  {
                Complex t = new Complex();
                t.polar(1.0, amplitude*phase.getElement(j, i));
                kernc.setElement(j, i, t);
            }       
        }
        //ComplexMatrixPrint(kernc);
        
        amplitude = -2.0*Math.PI/(nr*usfac);
        
        Matrix w = new Matrix(nor, 1); 
        for (int i = 0; i < nor; i++) {
            w.setElement(i, 0, i - roff);
        }
        
        Matrix x = new Matrix(1, nr);
        for (int i = 0; i < nr; i++) {
            x.setElement(0, i, i - Math.floor(nr/2.0));
        }
        x = ifftshift(x);

        Matrix nphase = w.times(x);
       //Matrix nphase = times(w,x);
        ComplexMatrix kernr = new ComplexMatrix(nor, nr);
        for (int j = 0; j < nor; j++) {
            for (int i = 0; i < nr; i++) {
                Complex t = new Complex();
                t.polar(1.0, amplitude*nphase.getElement(j, i));
                kernr.setElement(j, i, t);
            }       
        }
        //ComplexMatrixPrint(kernr);
        
        ComplexMatrix out = kernr.times(in.times(kernc));     
        return out;
    }
    
    private double[][] CrossCorrelation(ComplexMatrix ref, ComplexMatrix drifted) {
        int h = ref.getNrow();
        int w = ref.getNcol();
        ComplexMatrix b = drifted.conjugate();
        ComplexMatrix res = ElementProduct(ref, b);
        
        double[][] data = ComplexMatrix_to_FFTArray2D(res);
        DoubleFFT_2D fft = new DoubleFFT_2D(h, w);
        fft.realInverse(data, true);
        
        return data;
    }

    private ComplexMatrix ElementProduct(ComplexMatrix a, ComplexMatrix b) {
        int nr = a.getNrow();
        int nc = a.getNcol();
        
        ComplexMatrix res = new ComplexMatrix(nr, nc);

        for(int j = 0; j < nr; j++) {
            for(int i = 0; i < nc; i++) {
                res.setElement(j, i, a.getElementReference(j, i).times(b.getElementReference(j, i)));
            }
        }

        return res;
    }
    
    private double[][] ImageProcessor_to_FFTArray2D(ImageProcessor ip) {

        float[] pixels = (float[])ip.getPixels();
        int w = ip.getWidth();
        int h = ip.getHeight();
        double[][] data = new double[h][w];
        
        for (int j = 0; j < h; j++) { 
            for (int i = 0; i < w; i++) {
                data[j][i] = (double)pixels[j*w + i];
                }
        }

        return data;
    }
    
    private double[][] ImageProcessor_to_FFTComplexArray2D(ImageProcessor ip_r, ImageProcessor ip_i) {

        float[] pixels_r = (float[])ip_r.getPixels();
        float[] pixels_i = (float[])ip_i.getPixels();
        int w = ip_r.getWidth();
        int h = ip_r.getHeight();
        double[][] data = new double[h][2*w];
        
        for (int j = 0; j < h; j++) { 
            for (int i = 0; i < w; i++) {
                data[j][2*i] = (double)pixels_r[j*w + i];
                data[j][2*i+1] = (double)pixels_i[j*w + i];
                }
        }

        return data;
    }
    
    private ComplexMatrix FFTArray2D_to_ComplexMatrix(double[][] data, int h, int w) {
    
        ComplexMatrix m = new ComplexMatrix(h,w);

        for (int j = 0; j < h; j++) {
            for (int i = 0; i <= w/2; i++) {
                if (j > 0 && i > 0 && i < w/2) { 
                        m.setElement(j,i, new Complex(data[j][2*i], data[j][2*i+1]));
                        m.setElement(h-j, w-i, new Complex(data[j][2*i], -data[j][2*i+1]));
                }
                if (j == 0 && i > 0 && i < w/2) {
                    m.setElement(0, i, new Complex(data[0][2*i], data[0][2*i+1]));
                        m.setElement(0, w-i, new Complex(data[0][2*i], -data[0][2*i+1]));
                }
                if (i == 0 && j > 0 && j < h/2) {
                    m.setElement(j,0, new Complex(data[j][0], data[j][1]));
                        m.setElement(h-j, 0, new Complex(data[j][0], -data[j][1]));
                        m.setElement(j, w/2, new Complex(data[h-j][1], -data[h-j][0]));
                        m.setElement(h-j, w/2, new Complex(data[h-j][1], data[h-j][0]));    
                }
                if (j == 0 && i == 0) {
                    m.setElement(0, 0, new Complex(data[0][0], 0));
                }
                if (j == 0 && i == w/2) {
                    m.setElement(0, w/2, new Complex(data[0][1], 0));
                }
                if (j == h/2 && i == 0) {
                    m.setElement(h/2, 0, new Complex(data[h/2][0], 0));
                }
                if (j == h/2 && i == w/2) {
                    m.setElement(h/2, w/2, new Complex(data[h/2][1], 0));
                }
            }
        }

        return m;
    }

    private ComplexMatrix FFTComplexArray2D_to_ComplexMatrix(double[][] data, int h, int w) {
    
        ComplexMatrix m = new ComplexMatrix(h,w);

        for (int j = 0; j < h; j++) {
            for (int i = 0; i < w; i++) {
                m.setElement(j,i, new Complex(data[j][2*i], data[j][2*i+1]));
            }
        }

        return m;
    }

    private double[][] ComplexMatrix_to_FFTArray2D(ComplexMatrix m) {
        int w = m.getNcol();
        int h = m.getNrow();
        double[][] data = new double[h][w];

        for (int j = 0; j < h; j++) {
            for (int i = 0; i <= w/2; i++) {
                if (j > 0 && i > 0 && i < w/2) {
                        data[j][2*i] = m.getElementReference(j,i).getReal();
                        data[j][2*i+1] = m.getElementReference(j,i).getImag();
                }
                if (j == 0 && i > 0 && i < w/2) {
                    data[0][2*i] = m.getElementReference(0,i).getReal();
                        data[0][2*i+1] = m.getElementReference(0,i).getImag();
                }
                if (i == 0 && j > 0 && j < h/2) {
                    data[j][0] = m.getElementReference(j,0).getReal();
                        data[j][1] = m.getElementReference(j,0).getImag();
                        data[h-j][1] = m.getElementReference(j,w/2).getReal();
                        data[h-j][0] = m.getElementReference(h-j,w/2).getImag();    
                }
                if (j == 0 && i == 0) {
                    data[0][0] = m.getElementReference(0,0).getReal();
                }
                if (j == 0 && i == w/2) {
                    data[0][1] = m.getElementReference(0,w/2).getReal();
                }
                if (j == h/2 && i == 0) {
                    data[h/2][0] = m.getElementReference(h/2,0).getReal();
                }
                if (j == h/2 && i == w/2) {
                    data[h/2][1] = m.getElementReference(h/2,w/2).getReal();
                }
            }
        }

        return data;
    }

    // convert a Complex Matrix into an 2d real part array data[0][][]
    // and 2d imaginary part data[1][][]
    private double[][][] ComplexMatrix_to_RealArray2D(ComplexMatrix m) {
        int w = m.getNcol();
        int h = m.getNrow();
        double[][][] data = new double[2][h][w];

        for (int j = 0; j < h; j++) {
            for (int i = 0; i < w; i++) {
                data[0][j][i] = m.getElementReference(j,i).getReal();
                    data[1][j][i] = m.getElementReference(j,i).getImag();
            }
        }
        
        return data;
    }
}