/* Fourier_DM_loop.c: 
 
   Compilation from Matlab:
   mex  -lfftw3 Fourier_DM_loop.c
   mex -L. -lfftw3  Fourier_DM_loop.c
   maybe a tiny bit faster code is generated by 
   mex -O COPTIMFLAGS='-O2' LDOPTIMFLAGS='-O2' -L. -lfftw3 Fourier_DM_loop.c
   mex -O -v CFLAGS="\$CFLAGS -fopenmp -Wall -O3" -L. -lfftw3 -lgomp Fourier_DM_loop_par2.c
 
   compilation tested 1.2017
   mex -lfftw3  Fourier_DM_loop_par2.c  CFLAGS="\$CFLAGS -fopenmp" LDFLAGS="\$LDFLAGS -fopenmp"
 
   Usage from Matlab:
   Fourier_DM_loop_par2(iter,probe,ob,p1,p2,f,ph,df,double(fmask),fmag,p.power_bound,er2,rf,rf_nrm,af, fdev, fdev2, fmaski,int32(positions),int32(numpts),int32(fmask_per_scan),int32(compute_rfact));

 This code in matlab:
for i=1:numpts
    if fmask_per_scan
        fmaski2 = fmask(:,:,i);
    end
    Indy = positions(i,1) + (1:asize(1));
    Indx = positions(i,2) + (1:asize(2));
    p1 = probe .* ob(Indy, Indx);
    f = fft2( 2*p1 - iter(:,:,i) )/fnorm;
    af = abs(f);
    ph = f ./ (af+1e-10);
    fdev = af - fmag(:,:,i);
    fdev2 = fmaski.*fdev.^2;
    power = sum(sum(fdev2))/a2;
    if power > p.power_bound
        renorm = sqrt(p.power_bound / power);
        af = af.*(1-fmaski) + fmaski.*(fmag(:,:,i) + fdev * renorm);
    end
    p2 = fnorm*ifft2(af .* ph);
    df = p2 - p1;
    iter(:,:,i) = iter(:,:,i) + df;
    er2 = er2 + sum(sum(abs(df).^2));
    if compute_rfact
        rf = rf + sum(sum(abs(abs(fft2(p1))/fnorm - fmag(:,:,i))));
        rf_nrm = rf_nrm + sum(sum(fmag(:,:,i)));
    end
end
 
Academic License Agreement

Source Code

 Introduction 
 •	This license agreement sets forth the terms and conditions under which the PAUL SCHERRER INSTITUT (PSI), CH-5232 Villigen-PSI, Switzerland (hereafter "LICENSOR") 
   will grant you (hereafter "LICENSEE") a royalty-free, non-exclusive license for academic, non-commercial purposes only (hereafter "LICENSE") to use the cSAXS 
   ptychography MATLAB package computer software program and associated documentation furnished hereunder (hereafter "PROGRAM").

 Terms and Conditions of the LICENSE
 1.	LICENSOR grants to LICENSEE a royalty-free, non-exclusive license to use the PROGRAM for academic, non-commercial purposes, upon the terms and conditions 
       hereinafter set out and until termination of this license as set forth below.
 2.	LICENSEE acknowledges that the PROGRAM is a research tool still in the development stage. The PROGRAM is provided without any related services, improvements 
       or warranties from LICENSOR and that the LICENSE is entered into in order to enable others to utilize the PROGRAM in their academic activities. It is the 
       LICENSEE’s responsibility to ensure its proper use and the correctness of the results.”
 3.	THE PROGRAM IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR 
       A PARTICULAR PURPOSE AND NONINFRINGEMENT OF ANY PATENTS, COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS. IN NO EVENT SHALL THE LICENSOR, THE AUTHORS OR THE COPYRIGHT 
       HOLDERS BE LIABLE FOR ANY CLAIM, DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES OR OTHER LIABILITY ARISING FROM, OUT OF OR IN CONNECTION WITH THE PROGRAM OR THE USE 
       OF THE PROGRAM OR OTHER DEALINGS IN THE PROGRAM.
 4.	LICENSEE agrees that it will use the PROGRAM and any modifications, improvements, or derivatives of PROGRAM that LICENSEE may create (collectively, 
       "IMPROVEMENTS") solely for academic, non-commercial purposes and that any copy of PROGRAM or derivatives thereof shall be distributed only under the same 
       license as PROGRAM. The terms "academic, non-commercial", as used in this Agreement, mean academic or other scholarly research which (a) is not undertaken for 
       profit, or (b) is not intended to produce works, services, or data for commercial use, or (c) is neither conducted, nor funded, by a person or an entity engaged 
       in the commercial use, application or exploitation of works similar to the PROGRAM.
 5.	LICENSEE agrees that it shall make the following acknowledgement in any publication resulting from the use of the PROGRAM or any translation of the code into 
       another computing language:
       "Data processing was carried out using the cSAXS ptychography MATLAB package developed by the Science IT and the coherent X-ray scattering (CXS) groups, Paul 
       Scherrer Institut, Switzerland."

 Additionally, any publication using the package, or any translation of the code into another computing language should cite for difference map:
 P. Thibault, M. Dierolf, A. Menzel, O. Bunk, C. David, F. Pfeiffer, High-resolution scanning X-ray diffraction microscopy, Science 321, 379–382 (2008). 
   (doi: 10.1126/science.1158573),
 for maximum likelihood:
 P. Thibault and M. Guizar-Sicairos, Maximum-likelihood refinement for coherent diffractive imaging, New J. Phys. 14, 063004 (2012). 
   (doi: 10.1088/1367-2630/14/6/063004),
 for mixed coherent modes:
 P. Thibault and A. Menzel, Reconstructing state mixtures from diffraction measurements, Nature 494, 68–71 (2013). (doi: 10.1038/nature11806),
 and/or for multislice:
 E. H. R. Tsai, I. Usov, A. Diaz, A. Menzel, and M. Guizar-Sicairos, X-ray ptychography with extended depth of field, Opt. Express 24, 29089–29108 (2016). 
   (doi: 10.1364/OE.24.029089).
 6.	Except for the above-mentioned acknowledgment, LICENSEE shall not use the PROGRAM title or the names or logos of LICENSOR, nor any adaptation thereof, nor the 
       names of any of its employees or laboratories, in any advertising, promotional or sales material without prior written consent obtained from LICENSOR in each case.
 7.	Ownership of all rights, including copyright in the PROGRAM and in any material associated therewith, shall at all times remain with LICENSOR, and LICENSEE 
       agrees to preserve same. LICENSEE agrees not to use any portion of the PROGRAM or of any IMPROVEMENTS in any machine-readable form outside the PROGRAM, nor to 
       make any copies except for its internal use, without prior written consent of LICENSOR. LICENSEE agrees to place the following copyright notice on any such copies: 
       © All rights reserved. PAUL SCHERRER INSTITUT, Switzerland, Laboratory for Macromolecules and Bioimaging, 2017. 
 8.	The LICENSE shall not be construed to confer any rights upon LICENSEE by implication or otherwise except as specifically set forth herein.
 9.	DISCLAIMER: LICENSEE shall be aware that Phase Focus Limited of Sheffield, UK has an international portfolio of patents and pending applications which relate 
       to ptychography and that the PROGRAM may be capable of being used in circumstances which may fall within the claims of one or more of the Phase Focus patents, 
       in particular of patent with international application number PCT/GB2005/001464. The LICENSOR explicitly declares not to indemnify the users of the software 
       in case Phase Focus or any other third party will open a legal action against the LICENSEE due to the use of the program.
 10.	This Agreement shall be governed by the material laws of Switzerland and any dispute arising out of this Agreement or use of the PROGRAM shall be brought before 
       the courts of Zürich, Switzerland. 
 */

#include <omp.h>
#include "mex.h"
#include <string.h>
#include <math.h> 
#include <stdio.h> 
#include <complex.h>
#include "fftw3.h"
#include <sys/time.h>


void mexFunction(int nlhs, mxArray *plhs[],
                 int nrhs, const mxArray *prhs[])
{
  int i;
  
 
  double t_start, t_end;


  
      /* Check for proper number of arguments. */
  if (nrhs != 22) 
    mexErrMsgIdAndTxt("MexError:ptycho","22 input arguments required: Fourier_DM_loop(iter,probe,ob,p1,p2,f,ph,df,fmask,fmag,power_bound,er2,rf,rf_nrm,af,int32(positions),int32(numpts),int32(fmask_per_scan),int32(compute_rfact))");
  else if (nlhs != 0) 
    mexErrMsgIdAndTxt("MexError:ptycho","No output argument has to be specified.");

      /* Inputs that must be of type double. */
  for (i=0; i < 18; i++) {
      if ((mxIsDouble(prhs[i]) != 1) && (i!=9)){
          printf("Input %d is not double",i+1);
          mexErrMsgIdAndTxt("MexError:ptycho","Inputs must be of correct type.");
      }
  }

  
      /* Input must be of type int32. */
  for (i=18; i<nrhs; i++){
    if (mxIsInt32(prhs[i]) != 1){
          printf("Input %d is not integer",i+1);
          mexErrMsgIdAndTxt("MexError:ptycho","Inputs must be of correct type.");
    }
  }
      /* iter, fmag and fmask cannot be one-dimensional */
  if(mxGetNumberOfDimensions(prhs[0]) < 2) {
    printf("iter must have at least two dimensions.");
    mexErrMsgIdAndTxt("MexError:ptycho","wrong number of dimensions");
  }
  if(mxGetNumberOfDimensions(prhs[9]) < 2) {
    printf("fmag must have at least two dimensions.");
    mexErrMsgIdAndTxt("MexError:ptycho","wrong number of dimensions");
  }
  if(mxGetNumberOfDimensions(prhs[8]) < 2) {
      printf("fmask must have at least two dimensions.");
      mexErrMsgIdAndTxt("MexError:ptycho","wrong number of dimensions");
  }
  
    /* iter fmag and fmask cannot be more than 3-dimensional */
   if(mxGetNumberOfDimensions(prhs[0]) > 3) {
    printf("iter argument must have at most three dimensions.");
    mexErrMsgIdAndTxt("MexError:ptycho","wrong number of dimensions");
  }
   if(mxGetNumberOfDimensions(prhs[9]) > 3) {
    printf("fmag argument must have at most three dimensions.");
    mexErrMsgIdAndTxt("MexError:ptycho","wrong number of dimensions");
  }
  if(mxGetNumberOfDimensions(prhs[8]) > 3) {
    printf("fmask argument must have at most three dimensions.");
    mexErrMsgIdAndTxt("MexError:ptycho","wrong number of dimensions");
  }
    
      /* allocate memory for image data, to be returned 
  plhs[0] = 
    mxCreateNumericMatrix(dim1, dim2, mxDOUBLE_CLASS, mxREAL);
  if (plhs[0] == NULL)
    mexErrMsgIdAndTxt("MexError:ptycho","Could not allocate memory for return data.");*/
   
  /*% Variables
% Double complex    - { 0- 7} iter, probe, ob, p1, p2, f, ph, df
% Double real       - { 8-17} fmask, fmag, power_bound, er2, rf, rf_nrm, af, fdev, fdev2, fmaski
% Integer           - {18-21} positions, numpts, fmask_per_scan, compute_rfact */
  double *iter_real, *iter_imag, *probe_real, *probe_imag, *ob_real, *ob_imag;
  double *p1_real, *p1_imag, *p2_real, *p2_imag, *f_real, *f_imag;
  double *ph_real, *ph_imag, *df_real, *df_imag, *fmask, *fmag, *power_bound;
  double *er2_m, *rf_m, *rf_nrm_m, *af_m, *fdev_m, *fdev2_m, *fmaski;
  int *positions, *numpts, *fmask_per_scan, *compute_rfact;
  
  /* Check that arrays are complex */
  for (i=0; i<8; i++){
      if(mxIsComplex(prhs[i]) != 1) {
          printf("Input %d is not complex\n",i+1);
          mexErrMsgIdAndTxt("MexError:ptycho","Expected complex arrays are real-valued");
      }
  }

  /* get pointers to input data 
   (iter,probe,ob,p1,p2,f,ph,df,fmask,fmag,power_bound,er2,rf,rf_nrm,af,
   * int32(positions),int32(numpts),int32(fmask_per_scan),int32(compute_rfact))*/
  iter_real =   mxGetPr(prhs[0]);
  iter_imag =   mxGetPi(prhs[0]);
  probe_real =  mxGetPr(prhs[1]);
  probe_imag =  mxGetPi(prhs[1]);  
  ob_real =     mxGetPr(prhs[2]);
  ob_imag =     mxGetPi(prhs[2]); 
  p1_real =     mxGetPr(prhs[3]);
  p1_imag =     mxGetPi(prhs[3]); 
  p2_real =     mxGetPr(prhs[4]);
  p2_imag =     mxGetPi(prhs[4]); 
  f_real =      mxGetPr(prhs[5]);
  f_imag =      mxGetPi(prhs[5]); 
  ph_real =     mxGetPr(prhs[6]);
  ph_imag =     mxGetPi(prhs[6]); 
  df_real =     mxGetPr(prhs[7]);
  df_imag =     mxGetPi(prhs[7]); 
  fmask =       mxGetPr(prhs[8]);
  fmag =        mxGetPr(prhs[9]);
  power_bound = mxGetPr(prhs[10]);
  er2_m =         mxGetPr(prhs[11]);
  rf_m =          mxGetPr(prhs[12]);
  rf_nrm_m =      mxGetPr(prhs[13]);
  af_m =          mxGetPr(prhs[14]);
  fdev_m =        mxGetPr(prhs[15]);
  fdev2_m =       mxGetPr(prhs[16]);   
  fmaski =      mxGetPr(prhs[17]);  
  positions =   mxGetData(prhs[18]);
  numpts =      mxGetData(prhs[19]);
  fmask_per_scan = mxGetData(prhs[20]);
  compute_rfact = mxGetData(prhs[21]);
  
  /* Get dimension of probe and object */
  int no_rows_p, no_cols_p, no_rows_o, no_cols_o;
  no_rows_p = mxGetM(prhs[1]);
  no_cols_p = mxGetN(prhs[1]);
  no_rows_o = mxGetM(prhs[2]);
  no_cols_o = mxGetN(prhs[2]);
  
  /* Check 3D arrays */
  /* Check that iter agrees with probe dimensions and numpts */
  if(mxGetM(prhs[0]) != no_rows_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","iter and probe do not have the same number of rows");
  } 
  if(mxGetN(prhs[0]) != numpts[0]*no_cols_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","iter size does not agree with columns in probe and numpts");
  }   
  /* Check that fmag agrees with probe dimensions and numpts */
  if(mxGetM(prhs[9]) != no_rows_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","iter and probe do not have the same number of rows");
  } 
  if(mxGetN(prhs[9]) != numpts[0]*no_cols_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","iter size does not agree with columns in probe and numpts");
  } 
  

  
  if(fmask_per_scan[0]==0){
      /* Check that fmask agrees with probe dimensions*/
      if(mxGetM(prhs[8]) != no_rows_p) {
          mexErrMsgIdAndTxt("MexError:ptycho","fmask and probe do not have the same number of rows");
      }
      if(mxGetN(prhs[8]) != no_cols_p) {
          mexErrMsgIdAndTxt("MexError:ptycho","fmask and probe do not have the same number of cols");
      }
  }
  else{
      /* Check that fmask agrees with probe dimensions and numpts */
      if(mxGetM(prhs[8]) != no_rows_p) {
          mexErrMsgIdAndTxt("MexError:ptycho","fmask and probe do not have the same number of rows");
      }
      if(mxGetN(prhs[8]) != numpts[0]*no_cols_p) {
          mexErrMsgIdAndTxt("MexError:ptycho","fmask size does not agree with columns in probe and numpts");
      }
  }
  
  /* Check 2D arrays */
  /* Check that p1 agrees with probe dimensions */
  if(mxGetM(prhs[3]) != no_rows_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","p1 and probe do not have the same number of rows");
  } 
  if(mxGetN(prhs[3]) != no_cols_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","p1 and probe do not have the same number of cols");
  }
  /* Check that p2 agrees with probe dimensions */
  if(mxGetM(prhs[4]) != no_rows_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","p2 and probe do not have the same number of rows");
  } 
  if(mxGetN(prhs[4]) != no_cols_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","p2 and probe do not have the same number of cols");
  }
  /* Check that f agrees with probe dimensions */
  if(mxGetM(prhs[5]) != no_rows_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","f and probe do not have the same number of rows");
  } 
  if(mxGetN(prhs[5]) != no_cols_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","f and probe do not have the same number of cols");
  }
  /* Check that ph agrees with probe dimensions */
  if(mxGetM(prhs[6]) != no_rows_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","ph and probe do not have the same number of rows");
  } 
  if(mxGetN(prhs[6]) != no_cols_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","ph and probe do not have the same number of cols");
  }
   /* Check that fdev agrees with probe dimensions */
  if(mxGetM(prhs[7]) != no_rows_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","df and probe do not have the same number of rows");
  } 
  if(mxGetN(prhs[7]) != no_cols_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","df and probe do not have the same number of cols");
  }
  /* Check that af agrees with probe dimensions */
  if(mxGetM(prhs[14]) != no_rows_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","af and probe do not have the same number of rows");
  } 
  if(mxGetN(prhs[14]) != no_cols_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","af and probe do not have the same number of cols");
  }
  /* Check that fdev agrees with probe dimensions */
  if(mxGetM(prhs[15]) != no_rows_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","fdev and probe do not have the same number of rows");
  } 
  if(mxGetN(prhs[15]) != no_cols_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","fdev and probe do not have the same number of cols");
  }
  /* Check that fdev2 agrees with probe dimensions */
  if(mxGetM(prhs[16]) != no_rows_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","fdev2 and probe do not have the same number of rows");
  } 
  if(mxGetN(prhs[16]) != no_cols_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","fdev2 and probe do not have the same number of cols");
  }
  /* Check that fmaski agrees with probe dimensions */
  if(mxGetM(prhs[17]) != no_rows_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","fmaski and probe do not have the same number of rows");
  } 
  if(mxGetN(prhs[17]) != no_cols_p) {
      mexErrMsgIdAndTxt("MexError:ptycho","fmaski and probe do not have the same number of cols");
  }
  
  
  /* Check that positions has the right size */
  if(mxGetN(prhs[18]) != 2) {
      mexErrMsgIdAndTxt("MexError:ptycho","positions must have 2 columns");
  } 
  if(mxGetM(prhs[18]) != numpts[0]) {
      mexErrMsgIdAndTxt("MexError:ptycho","positions must have numpts of rows");
  }  
  /* Find maximum and minimum of positions */
  int min_pos_row = positions[0], max_pos_row = positions[0];
  int min_pos_col = positions[numpts[0]], max_pos_col = positions[numpts[0]];
  for (i = 1; i < numpts[0] ; i++) {
      if (min_pos_row > positions[i]) {
          min_pos_row = positions[i];
      }
      if (max_pos_row < positions[i]) {
          max_pos_row = positions[i];
      }     
      if (min_pos_col > positions[i+numpts[0]]) {
          min_pos_col = positions[i+numpts[0]];
      }
      if (max_pos_col < positions[i+numpts[0]]) {
          max_pos_col = positions[i+numpts[0]];
      }         
  }
  
  /*printf ("min_pos_row=%d max_pos_row=%d min_pos_col=%d max_pos_col=%d\nno_row_o=%d no_col_o=%d no_row_p=%d no_col_p=%d\n", 
          min_pos_row, max_pos_row, min_pos_col, max_pos_col, no_rows_o, no_cols_o, no_rows_p, no_cols_p);/*
  /* Check that indices will not exceed matrix dimensions */
  if ((min_pos_col < 0)||(min_pos_row < 0))  {
      mexErrMsgIdAndTxt("MexError:ptycho","Minimum of positions is less than zero. Subscript indices must either be real positive integers\n");
  }
  if ((max_pos_col + no_cols_p > no_cols_o)||(max_pos_row + no_rows_p > no_rows_o))  {
      mexErrMsgIdAndTxt("MexError:ptycho","Maximum of positions is too large. Subscript exceeds matrix dimensions\n");
  }  

  
  
  /* The actual code */
  
  int max_threads = omp_get_max_threads();
  int threads_used;


  /* Build fft plan */
  /*int N = 2;*/
  fftw_complex *out, *p1, *p2, *f, *ph, *df, *out_gl, *p1_gl, *p2_gl, *f_gl, *ph_gl, *df_gl;
  fftw_plan *planft, *planfti, *planftp;
  double *af_gl, *af, *fdev_gl, *fdev, *fdev2_gl, *fdev2, *er2_gl, *rf_gl, *rf_nrm_gl, *er2, *rf, *rf_nrm;

#define alloc_test(var,typ,size)   var = (typ*) fftw_malloc(sizeof(typ) * size);\
  if (var == NULL)\
  {\
      mexErrMsgIdAndTxt("MexError:ptycho","Alloc = NULL\n");\
      return;\
  }

  
  alloc_test(out_gl,fftw_complex,no_cols_p*no_rows_p*max_threads);
  alloc_test(p1_gl,fftw_complex,no_cols_p*no_rows_p*max_threads);
  alloc_test(p2_gl,fftw_complex,no_cols_p*no_rows_p*max_threads);
  alloc_test(f_gl,fftw_complex,no_cols_p*no_rows_p*max_threads);
  alloc_test(ph_gl,fftw_complex,no_cols_p*no_rows_p*max_threads);
  alloc_test(df_gl,fftw_complex,no_cols_p*no_rows_p*max_threads);
  alloc_test(af_gl,double,no_cols_p*no_rows_p*max_threads);
  alloc_test(fdev_gl,double,no_cols_p*no_rows_p*max_threads);
  alloc_test(fdev2_gl,double,no_cols_p*no_rows_p*max_threads);
  alloc_test(er2_gl,double,max_threads);
  alloc_test(rf_gl,double,max_threads);
  alloc_test(rf_nrm_gl,double,max_threads);

  alloc_test(planft,fftw_plan,max_threads);
  alloc_test(planfti,fftw_plan,max_threads);
  alloc_test(planftp,fftw_plan,max_threads);

  
  for (i=0; i<max_threads;i++)
  {
      planft[i] = fftw_plan_dft_2d(no_cols_p,no_rows_p,f_gl+no_cols_p*no_rows_p*i,f_gl+no_cols_p*no_rows_p*i,FFTW_FORWARD, FFTW_ESTIMATE);
      planfti[i] = fftw_plan_dft_2d(no_cols_p,no_rows_p,p2_gl+no_cols_p*no_rows_p*i,p2_gl+no_cols_p*no_rows_p*i,FFTW_BACKWARD, FFTW_ESTIMATE);
      planftp[i] = fftw_plan_dft_2d(no_cols_p,no_rows_p,p1_gl+no_cols_p*no_rows_p*i,out_gl+no_cols_p*no_rows_p*i,FFTW_FORWARD, FFTW_ESTIMATE);
  }

 
  //printf ("Number of threads %d \n", max_threads);
  
  double fnorm = sqrt (no_cols_p * no_rows_p);
  double a2 = no_cols_p * no_rows_p;
  double power, renorm;
  

  t_start=omp_get_wtime();
   
  /* main loop */
  int col=0, row=0, view=0, fmask_index=0, thread_id, last_id=0;
  
 #pragma omp parallel default(shared) \
         private(view, col, row, power, renorm, thread_id,fmask_index, er2, rf, rf_nrm, out, p1, p2, f, ph, df, fdev, fdev2, af) 
 {
  thread_id = omp_get_thread_num();
  if (thread_id==0)
  {
      threads_used = omp_get_num_threads();
  }
  
  out = out_gl +no_cols_p*no_rows_p*thread_id;
  p1 = p1_gl +no_cols_p*no_rows_p*thread_id;
  p2 = p2_gl +no_cols_p*no_rows_p*thread_id;
  f = f_gl +no_cols_p*no_rows_p*thread_id;
  ph = ph_gl+no_cols_p*no_rows_p*thread_id;
  df = df_gl+no_cols_p*no_rows_p*thread_id;
  af = af_gl+no_cols_p*no_rows_p*thread_id;
  fdev = fdev_gl+no_cols_p*no_rows_p*thread_id;
  fdev2 = fdev2_gl+no_cols_p*no_rows_p*thread_id;

  er2 = er2_gl+thread_id;
  rf = rf_gl + thread_id;
  rf_nrm = rf_nrm_gl + thread_id;
  
  *er2 = 0;
  *rf = 0;
  *rf_nrm =0;
  
  fmask_index=0;

/*  #pragma omp critical
  {
  printf ("thread %d started of %d\n", thread_id, omp_get_num_threads());
  }  */
  
  #pragma omp for schedule(dynamic) 
  for (view=0;view<numpts[0];view++) {
      /*fmaski = fmask(:,:,i);*/
      if(fmask_per_scan[0]!=0){
          fmask_index = view;
      }
      if (view == (numpts[0]-1))
          last_id=thread_id;
      
    /*p1 = probe .* ob(Indy, Indx);*/
      for (col=0;col<no_cols_p;col++) {
          for (row=0;row<no_rows_p;row++) {
              p1[row + col*no_rows_p] = probe_real[row + col*no_rows_p]*ob_real[row + positions[view] + (col+positions[view+numpts[0]])*no_rows_o]-
                      probe_imag[row + col*no_rows_p]*ob_imag[row + positions[view] + (col+positions[view+numpts[0]])*no_rows_o] + 
                      I*(probe_real[row + col*no_rows_p]*ob_imag[row + positions[view] + (col+positions[view+numpts[0]])*no_rows_o] +
                      probe_imag[row + col*no_rows_p]*ob_real[row + positions[view] + (col+positions[view+numpts[0]])*no_rows_o]);
              f [row + col*no_rows_p] = (2 * p1[row + col*no_rows_p] - iter_real[row + col*no_rows_p + view*no_cols_p*no_rows_p]-
                      I*iter_imag[row + col*no_rows_p + view*no_cols_p*no_rows_p]);  /*/fnorm;*/
          }
      }

      fftw_execute (planft[thread_id]); 
 
      power = 0;
   
      for (col=0;col<(no_cols_p*no_rows_p);col++) {
              f [col] = f[col] / fnorm; 
              af[col] = sqrt(creal(f[col])*creal(f[col])+cimag(f[col])*cimag(f[col]));   /*cabs (f[col]); */
              ph[col] = f[col] / (af[col] + 1e-10);
              fdev[col] = af[col] - fmag [col + view*no_cols_p*no_rows_p];
              fdev2[col] = fmask[col+fmask_index*no_cols_p*no_rows_p] * fdev [col] * fdev [col];
              power += fdev2[col];
              p2[col] = af[col] * ph[col];  
          }
      
      power /= a2;
     
      if (power > *power_bound)
      {
          renorm = sqrt (*power_bound / power);
          for (col=0;col<(no_cols_p*no_rows_p);col++) {
             af[col]=af[col]*(1-fmask[col+fmask_index*no_cols_p*no_rows_p]) + 
                      fmask[col+fmask_index*no_cols_p*no_rows_p]*(fmag [col + view*no_cols_p*no_rows_p] + fdev[col] * renorm);  
              p2[col] = af[col] * ph[col];
          }
      }
 
      fftw_execute (planfti[thread_id]);
      
      for (col=0;col<(no_cols_p*no_rows_p);col++) {
          p2[col] *= (fnorm / (no_cols_p * no_rows_p));
          df[col] = p2[col] - p1[col];
          iter_real[col + view*no_cols_p*no_rows_p] += creal(df [col]);
          iter_imag[col + view*no_cols_p*no_rows_p] += cimag(df [col]);
          *er2 += creal(df[col])*creal(df[col]) + cimag(df[col])*cimag(df[col]);
      }

      if (compute_rfact[0]) {
          fftw_execute (planftp[thread_id]);
          for (col=0;col<(no_cols_p*no_rows_p);col++) {
              *rf += fabs(cabs(out[col])/fnorm - fmag[col + view*no_cols_p*no_rows_p]);
              *rf_nrm += fmag[col + view*no_cols_p*no_rows_p];
          }
      }

  } /* mainloop */
  } /* end parallel threads */
  
  t_end=omp_get_wtime();
  /*printf ("  loop time=%e\n", t_end-t_start);*/
  
  for (i=0; i<threads_used; i++)
  {
      *er2_m += er2_gl[i];
      *rf_m += rf_gl[i];
      *rf_nrm_m += rf_nrm_gl[i];
  }
/*
  for (i=0; i<(no_rows_p * no_cols_p);i++)
  {
 
    p1_real[i] = creal(p1_gl[i+no_rows_p * no_cols_p * last_id]);
    p1_imag[i] = cimag(p1_gl[i+no_rows_p * no_cols_p * last_id]);
    p2_real[i] = creal(p2_gl[i+no_rows_p * no_cols_p * last_id]);
    p2_imag[i] = cimag(p2_gl[i+no_rows_p * no_cols_p * last_id]);
    f_real[i] = creal(f_gl[i+no_rows_p * no_cols_p * last_id]);
    f_imag[i] = cimag(f_gl[i+no_rows_p * no_cols_p * last_id]);
    ph_real[i] = creal(ph_gl[i+no_rows_p * no_cols_p * last_id]);
    ph_imag[i] = cimag(ph_gl[i+no_rows_p * no_cols_p * last_id]);
    df_real[i] = creal(df_gl[i+no_rows_p * no_cols_p * last_id]);
    df_imag[i] = cimag(df_gl[i+no_rows_p * no_cols_p * last_id]);
    af_m[i] = af_gl[i+no_rows_p * no_cols_p * last_id];

  }
  */
/*
  for (i=0; i<(no_rows_o * no_cols_o);i++)
  {
    ob_real[i] = creal(ob[i]);
    ob_imag[i] = cimag(ob[i]);
  }

*/  
  
  fftw_free(out_gl);
  fftw_free(p1_gl);
  fftw_free(p2_gl);
  fftw_free(f_gl);
  fftw_free(ph_gl);
  fftw_free(df_gl);
  fftw_free(af_gl);
  fftw_free(fdev_gl);
  fftw_free(fdev2_gl);
  fftw_free(er2_gl);
  fftw_free(rf_gl);
  fftw_free(rf_nrm_gl);

  for (i=0; i<max_threads; i++)
  {
    fftw_destroy_plan(planft[i]);
    fftw_destroy_plan(planfti[i]); 
    fftw_destroy_plan(planftp[i]);
  }

  fftw_free(planft);
  fftw_free(planfti);
  fftw_free(planftp);

  

  
  

  /* Check fmask - Special because it can be 2D or 3D */
  
  
}

