static void push_neighbors_on_queue(PixelQueue &pq, /*double dist,*/ double spatial_dist,
                        double *image,
                        unsigned int i, unsigned int j,
                        unsigned int m, unsigned int n,
                        int radius, double ref_intensity,  double lambda, double cutoff_dist,
                        double label, double *seeds_out, mxLogical* mask_in)
{
   double d, sd;
  
   /* 26-connected */
   int di, dj, idi, jdj;
   for(di = -1; di <= 1; di++) {
      for (dj = -1; dj <= 1; dj++) {  
            idi = i +di; jdj = j + dj;
            if (idi>=0 && idi < m && jdj >= 0 && jdj < n &&
               mask_in[IJ(idi,jdj)] &&  0 == seeds_out[IJ(idi,jdj)]) {
               //only push if neighbours are within resonable distance
               d = difference(image, i, j, idi, jdj, m, n, radius, ref_intensity, lambda, spatial_dist, sd);
               //cout << "i,j,k=" << i << "," << j << "," << k << endl;
               //cout << "di,j,k=" << idi << "," << jdj << "," << kdk << endl;
               //cout << "d=" << d << " sp_dist=" << spatial_dist << " sd=" << sd << endl;
               //cout << "ref_intensity=" << ref_intensity << endl;
               if (d < cutoff_dist) {
                  pq.push(Pixel(d, sd, idi, jdj, label));
               }
            }
      }
   }          
};
static void
push_neighbors_on_queue(PixelQueue &pq, double dist,
                        double *image,
                        unsigned int i, unsigned int j,
                        unsigned int m, unsigned int n,
                        double lambda, double label,
                        double *labels_out)
{
  /* TODO: Check if the neighbor is already labelled. If so, skip pushing. 
   */    
    
  /* 4-connected */
  if (i > 0) {
    if ( 0 == labels_out[IJ(i-1,j)] ) // if the neighbor was not labeled, do pushing
      pq.push(Pixel(dist + Difference(image, i, j, i-1, j, m, n, lambda), i-1, j, label));
  }                                                                   
  if (j > 0) {                                                        
    if ( 0 == labels_out[IJ(i,j-1)] )   
      pq.push(Pixel(dist + Difference(image, i, j, i, j-1, m, n, lambda), i, j-1, label));
  }                                                                   
  if (i < (m-1)) {
    if ( 0 == labels_out[IJ(i+1,j)] ) 
      pq.push(Pixel(dist + Difference(image, i, j, i+1, j, m, n, lambda), i+1, j, label));
  }                                                                   
  if (j < (n-1)) {              
    if ( 0 == labels_out[IJ(i,j+1)] )   
      pq.push(Pixel(dist + Difference(image, i, j, i, j+1, m, n, lambda), i, j+1, label));
  } 

  /* 8-connected */
  if ((i > 0) && (j > 0)) {
    if ( 0 == labels_out[IJ(i-1,j-1)] )   
      pq.push(Pixel(dist + Difference(image, i, j, i-1, j-1, m, n, lambda), i-1, j-1, label));
  }                                                                       
  if ((i < (m-1)) && (j > 0)) {                                           
    if ( 0 == labels_out[IJ(i+1,j-1)] )   
      pq.push(Pixel(dist + Difference(image, i, j, i+1, j-1, m, n, lambda), i+1, j-1, label));
  }                                                                       
  if ((i > 0) && (j < (n-1))) {                                           
    if ( 0 == labels_out[IJ(i-1,j+1)] )   
      pq.push(Pixel(dist + Difference(image, i, j, i-1, j+1, m, n, lambda), i-1, j+1, label));
  }                                                                       
  if ((i < (m-1)) && (j < (n-1))) {
    if ( 0 == labels_out[IJ(i+1,j+1)] )   
      pq.push(Pixel(dist + Difference(image, i, j, i+1, j+1, m, n, lambda), i+1, j+1, label));
  }
  
}