   , const double threshold
   ) {
   cudaError_t err;
+  place * peakp;
+  place
+      found_place_psf
+    , found_place_res
+    ;
+  const int totsize = linsize * linsize;
+
+  // working space to find peaks
+  err = cudaMalloc((void **) &peakp, __NBLOCKS(totsize, 1024) * sizeof(place));
+  __CHECK
+
+#undef __CHECK
+#define __CHECK if (err != cudaSuccess) goto fin;
+
+  err = findPeak(psf_p, peakp, &found_place_psf, totsize);
+  __CHECK
 
   for (unsigned int i = 0; i < niters; ++i) {
-    err = findPeaks(res_p, psf_p, linsize * linsize);
-    if (err != cudaSuccess) return err;
+    err = findPeak(res_p, peakp, &found_place_res, totsize);
+    __CHECK
 
     if (abs(found_place_res.val) < threshold) break;
 
-    subtractPSF(res_p, psf_p, linsize, gain);
+    subtractPSF(res_p, psf_p, found_place_res.pos, found_place_psf.pos, linsize, found_place_res.val * gain);
     mod_p[found_place_res.pos] += found_place_res.val * gain;
     // Wait for the PSF subtraction to finish
     err = cudaDeviceSynchronize();
   }
 
+  fin:;
+  cudaFree(peakp);
   return err;
 }


#define __BLOCK_DIM 16

inline
__host__
static void subtractPSF(
          double * res_p
  , const double * psf_p
  , const int linsize
  , const double gain
  ) {
  typedef long long int lli;
  const lli diff = (lli)found_place_psf.pos - (lli)found_place_res.pos;
  lldiv_t
      resxy = div(found_place_res.pos, (lli)linsize)
    , psfxy = div(found_place_psf.pos, (lli)linsize)
    ;
  const int
      stopx = linsize - abs (psfxy.rem - resxy.rem)

  dim3 threadsPerBlock(__BLOCK_DIM, __BLOCK_DIM);

  if (diff >= 0)
    subtract_psf_kernel<<<numBlocks, threadsPerBlock>>>(res_p, psf_p + diff, stopx, stopy, diff, linsize, peak_x_gain);
  else
    subtract_psf_kernel<<<numBlocks, threadsPerBlock>>>(res_p - diff, psf_p, stopx, stopy, diff, linsize, peak_x_gain);
}

// All pointers are device pointers
