/*****************************************************************************/
/*
 * Handle pmFootprints
 */
#include <assert.h>
#include <string.h>
#include <pslib.h>
#include <psmodules.h>
#include "psphot.h"
#include "pmFootprint.h"

static void spanFree(pmSpan *tmp) {
   ;
}

/*
 * pmSpanAlloc()
 */
pmSpan *pmSpanAlloc(int y, int x0, int x1)
{
    pmSpan *span = (pmSpan *)psAlloc(sizeof(pmSpan));
    psMemSetDeallocator(span, (psFreeFunc) spanFree);

    span->y = y;
    span->x0 = x0;
    span->x1 = x1;

    return(span);
}

bool pmSpanTest(const psPtr ptr)
{
    return (psMemGetDeallocator(ptr) == (psFreeFunc)spanFree);
}

//
// Sort pmSpans by y, then x0, then x1
//
int pmSpanSortByYX(const void **a, const void **b) {
    const pmSpan *sa = *(const pmSpan **)a;
    const pmSpan *sb = *(const pmSpan **)b;

    if (sa->y < sb->y) {
	return -1;
    } else if (sa->y == sb->y) {
	if (sa->x0 < sb->x0) {
	    return -1;
	} else if (sa->x0 == sb->x0) {
	    if (sa->x1 < sb->x1) {
		return -1;
	    } else if (sa->x1 == sb->x1) {
		return 0;
	    } else {
		return 1;
	    }
	} else {
	    return 1;
	}
    } else {
	return 1;
    }
}

/************************************************************************************************************/

static void footprintFree(pmFootprint *tmp)
{
   if (!tmp) {
        return;
   }

   psTrace("psModules.objects", 5, "---- begin ----\n");

   psFree(tmp->spans);
   psFree(tmp->peaks);

   psTrace("psModules.objects", 5, "---- end ----\n");
}

/*
 * pmFootprintAlloc(): Allocate the pmFootprint structure to NULL.
 */
pmFootprint *pmFootprintAlloc(int nspan, // number of spans expected in footprint
			      const psImage *image) // region footprint lives in
{
    psTrace("psModules.objects", 5, "---- begin ----\n");

    static int id = 1;
    pmFootprint *footprint = (pmFootprint *)psAlloc(sizeof(pmFootprint));
    *(int *)&footprint->id = id++;
    psMemSetDeallocator(footprint, (psFreeFunc) footprintFree);

    footprint->normalized = false;

    assert(nspan >= 0);
    footprint->npix = 0;
    footprint->spans = psArrayAllocEmpty(nspan);
    footprint->peaks = psArrayAlloc(0);

    footprint->bbox.x0 = footprint->bbox.y0 = 0;
    footprint->bbox.x1 = footprint->bbox.y1 = -1;

    if (image == NULL) {
	footprint->region.x0 = footprint->region.y0 = 0;
	footprint->region.x1 = footprint->region.y1 = -1;
    } else {
	footprint->region.x0 = image->col0;
	footprint->region.x1 = image->col0 + image->numCols - 1;
	footprint->region.y0 = image->row0;
	footprint->region.y1 = image->row0 + image->numRows - 1;
    }

    psTrace("psModules.objects", 5, "---- end ----\n");
    return(footprint);
}

bool pmFootprintTest(const psPtr ptr) {
    return (psMemGetDeallocator(ptr) == (psFreeFunc)footprintFree);
}

pmFootprint *pmFootprintNormalize(pmFootprint *fp) {
    if (fp != NULL && !fp->normalized) {
	fp->peaks = psArraySort(fp->peaks, pmPeakSortBySN);
	fp->normalized = true;
    }

    return fp;
}

//
// Add a span to a footprint, returning the new span
//
static pmSpan *pmFootprintAddSpan(pmFootprint *fp,	// the footprint to add to
				  const int y, // row to add
				  int x0,      // range of
				  int x1) {    //          columns

    if (x1 < x0) {
	int tmp = x0;
	x0 = x1;
	x1 = tmp;
    }

    pmSpan *sp = pmSpanAlloc(y, x0, x1);
    psArrayAdd(fp->spans, 1, sp);
    psFree(sp);
    
    fp->npix += x1 - x0 + 1;

    if (fp->spans->n == 1) {
	fp->bbox.x0 = x0;
	fp->bbox.x1 = x1;
	fp->bbox.y0 = y;
	fp->bbox.y1 = y;
    } else {
	if (x0 < fp->bbox.x0) fp->bbox.x0 = x0;
	if (x1 > fp->bbox.x1) fp->bbox.x1 = x1;
	if (y < fp->bbox.y0) fp->bbox.y0 = y;
	if (y > fp->bbox.y1) fp->bbox.y1 = y;
    }

    return sp;
}

void pmFootprintSetBBox(pmFootprint *fp) {
    assert (fp != NULL);
    if (fp->spans->n == 0) {
	return;
    }
    pmSpan *sp = fp->spans->data[0];
    int x0 = sp->x0;
    int x1 = sp->x1;
    int y0 = sp->y;
    int y1 = sp->y;

    for (int i = 1; i < fp->spans->n; i++) {
	sp = fp->spans->data[i];
	
	if (sp->x0 < x0) x0 = sp->x0;
	if (sp->x1 > x1) x1 = sp->x1;
	if (sp->y < y0) y0 = sp->y;
	if (sp->y > y1) y1 = sp->y;
    }

    fp->bbox.x0 = x0;
    fp->bbox.x1 = x1;
    fp->bbox.y0 = y0;
    fp->bbox.y1 = y1;
}

int pmFootprintSetNpix(pmFootprint *fp) {
   assert (fp != NULL);
   int npix = 0;
   for (int i = 0; i < fp->spans->n; i++) {
       pmSpan *span = fp->spans->data[i];
       npix += span->x1 - span->x0 + 1;
   }
   fp->npix = npix;

   return npix;
}

/************************************************************************************************************/

typedef struct {			/* run-length code for part of object*/
   int id;				/* ID for object */
   int y;				/* Row wherein WSPAN dwells */
   int x0, x1;				/* inclusive range of columns */
} WSPAN;

/*
 * comparison function for qsort; sort by ID then row
 */
static int
compar(const void *va, const void *vb)
{
   const WSPAN *a = va;
   const WSPAN *b = vb;

   if(a->id < b->id) {
      return(-1);
   } else if(a->id > b->id) {
      return(1);
   } else {
      return(a->y - b->y);
   }
}

/*
 * Follow a chain of aliases, returning the final resolved value.
 */
static int
resolve_alias(const int *aliases,	/* list of aliases */
	      int id)			/* alias to look up */
{
   int resolved = id;			/* resolved alias */

   while(id != aliases[id]) {
      resolved = id = aliases[id];
   }

   return(resolved);
}

/*
 * Go through an image, finding sets of connected pixels above threshold
 * and assembling them into pmFootprints;  the resulting set of objects
 * is returned as a psArray
 */
psArray *
pmFindFootprints(const psImage *img,	// image to search
		 const float threshold,	// Threshold
		 const int npixMin)	// minimum number of pixels in an acceptable pmFootprint
{
   int i0;				/* initial value of i */
   int id;				/* object ID */
   int in_span;				/* object ID of current WSPAN */
   int nspan = 0;			/* number of spans */
   int nobj = 0;			/* number of objects found */
   int x0 = 0;			        /* unpacked from a WSPAN */
   int *tmp;				/* used in swapping idc/idp */

   assert(img != NULL);

   bool F32 = false;			// is this an F32 image?
   if (img->type.type == PS_TYPE_F32) {
       F32 = true;
   } else if (img->type.type == PS_TYPE_S32) {
       F32 = false;
   } else {				// N.b. You can't trivially add more cases here; F32 is just a bool
       psError(PS_ERR_UNKNOWN, true, "Unsupported psImage type: %d", img->type.type);
       return NULL;
   }
   psF32 *imgRowF32 = NULL;		// row pointer if F32
   psS32 *imgRowS32 = NULL;		//  "   "   "  "  !F32
   
   const int row0 = img->row0;
   const int col0 = img->col0;
   const int numRows = img->numRows;
   const int numCols = img->numCols;
/*
 * Storage for arrays that identify objects by ID. We want to be able to
 * refer to idp[-1] and idp[numCols], hence the (numCols + 2)
 */
   int *id_s = psAlloc(2*(numCols + 2)*sizeof(int));
   memset(id_s, '\0', 2*(numCols + 2)*sizeof(int)); assert(id_s[0] == 0);
   int *idc = id_s + 1;			// object IDs in current/
   int *idp = idc + (numCols + 2);	//                       previous row

   int size_aliases = 1 + numRows/20;	// size of aliases[] array
   int *aliases = psAlloc(size_aliases*sizeof(int)); // aliases for object IDs

   int size_spans = 1 + numRows/20;	// size of spans[] array
   WSPAN *spans = psAlloc(size_spans*sizeof(WSPAN)); // row:x0,x1 for objects
/*
 * Go through image identifying objects
 */
   for (int i = 0; i < numRows; i++) {
      int j;
      tmp = idc; idc = idp; idp = tmp;	/* swap ID pointers */
      memset(idc, '\0', numCols*sizeof(int));
      
      imgRowF32 = img->data.F32[i];	// only one of
      imgRowS32 = img->data.S32[i];	//      these is valid!

      in_span = 0;			/* not in a span */
      for (j = 0; j < numCols; j++) {
	 double pixVal = F32 ? imgRowF32[j] : imgRowS32[j];
	 if (pixVal < threshold) {
	    if (in_span) {
	       if(nspan >= size_spans) {
		  size_spans *= 2;
		  spans = psRealloc(spans, size_spans*sizeof(WSPAN));
	       }
	       spans[nspan].id = in_span;
	       spans[nspan].y = i;
	       spans[nspan].x0 = x0;
	       spans[nspan].x1 = j - 1;
	       
	       nspan++;

	       in_span = 0;
	    }
	 } else {			/* a pixel to fix */
	    if(idc[j - 1] != 0) {
	       id = idc[j - 1];
	    } else if(idp[j - 1] != 0) {
	       id = idp[j - 1];
	    } else if(idp[j] != 0) {
	       id = idp[j];
	    } else if(idp[j + 1] != 0) {
	       id = idp[j + 1];
	    } else {
	       id = ++nobj;

	       if(id >= size_aliases) {
		  size_aliases *= 2;
		  aliases = psRealloc(aliases, size_aliases*sizeof(int));
	       }
	       aliases[id] = id;
	    }

	    idc[j] = id;
	    if(!in_span) {
	       x0 = j; in_span = id;
	    }
/*
 * Do we need to merge ID numbers? If so, make suitable entries in aliases[]
 */
	    if(idp[j + 1] != 0 && idp[j + 1] != id) {
	       aliases[resolve_alias(aliases, idp[j + 1])] =
						    resolve_alias(aliases, id);
	       
	       idc[j] = id = idp[j + 1];
	    }
	 }
      }

      if(in_span) {
	 if(nspan >= size_spans) {
	    size_spans *= 2;
	    spans = psRealloc(spans, size_spans*sizeof(WSPAN));
	 }

	 assert(nspan < size_spans);	/* we checked for space above */
	 spans[nspan].id = in_span;
	 spans[nspan].y = i;
	 spans[nspan].x0 = x0;
	 spans[nspan].x1 = j - 1;
	 
	 nspan++;
      }
   }

   psFree(id_s);
/*
 * Resolve aliases; first alias chains, then the IDs in the spans
 */
   for (int i = 0; i < nspan; i++) {
      spans[i].id = resolve_alias(aliases, spans[i].id);
   }

   psFree(aliases);
/*
 * Sort spans by ID, so we can sweep through them once
 */
   if(nspan > 0) {
      qsort(spans, nspan, sizeof(WSPAN), compar);
   }
/*
 * Build pmFootprints from the spans
 */
   psArray *footprints = psArrayAlloc(nobj);
   int n = 0;			// number of pmFootprints

   if(nspan > 0) {
      id = spans[0].id;
      i0 = 0;
      for (int i = 0; i <= nspan; i++) {	/* nspan + 1 to catch last object */
	 if(i == nspan || spans[i].id != id) {
	    pmFootprint *fp = pmFootprintAlloc(i - i0, img);
	    
	    for(; i0 < i; i0++) {
		pmFootprintAddSpan(fp, spans[i0].y + row0,
				   spans[i0].x0 + col0, spans[i0].x1 + col0);
	    }

	    if (fp->npix < npixMin) {
	       psFree(fp);
	    } else {
	       footprints->data[n++] = fp;
	    }
	 }
	 
	 id = spans[i].id;
      }
   }

   footprints = psArrayRealloc(footprints, n);
/*
 * clean up
 */
   psFree(spans);

   return footprints;
}

/************************************************************************************************************/
/*
 * A data structure to hold the starting point for a search for pixels above threshold,
 * used by pmFindFootprintAtPoint
 *
 * We don't want to find this span again --- it's already part of the footprint ---
 * so we set appropriate mask bits
 */
//
// An enum for what we should do with a Startspan
//
typedef enum {PM_SSPAN_DOWN = 0,	// scan down from this span
	      PM_SSPAN_UP,		// scan up from this span
	      PM_SSPAN_RESTART,		// restart scanning from this span
	      PM_SSPAN_DONE		// this span is processed
} PM_SSPAN_DIR;				// How to continue searching
//
// An enum for mask's pixel values.  We're looking for pixels that are above threshold, and
// we keep extra book-keeping information in the PM_SSPAN_STOP plane.  It's simpler to be
// able to check for 
//
enum {
    PM_SSPAN_INITIAL = 0x0,		// initial state of pixels.
    PM_SSPAN_DETECTED = 0x1,		// we've seen this pixel
    PM_SSPAN_STOP = 0x2			// you may stop searching when you see this pixel
};
//
// The struct that remembers how to [re-]start scanning the image for pixels
//
typedef struct {
    const pmSpan *span;			// save the pixel range
    PM_SSPAN_DIR direction;		// How to continue searching
    bool stop;				// should we stop searching?
} Spartspan;

static void startspanFree(Spartspan *sspan) {
    psFree((void *)sspan->span);
}

static Spartspan *
SpartspanAlloc(const pmSpan *span,	// The span in question
	       psImage *mask,		// Pixels that we've already detected
	       const PM_SSPAN_DIR dir	// Should we continue searching towards the top of the image?
    ) {
    Spartspan *sspan = psAlloc(sizeof(Spartspan));
    psMemSetDeallocator(sspan, (psFreeFunc)startspanFree);

    sspan->span = psMemIncrRefCounter((void *)span);
    sspan->direction = dir;
    sspan->stop = false;
    
    if (mask != NULL) {			// remember that we've detected these pixels
	psMaskType *mpix = &mask->data.PS_TYPE_MASK_DATA[span->y - mask->row0][span->x0 - mask->col0];

	for (int i = 0; i <= span->x1 - span->x0; i++) {
	    mpix[i] |= PM_SSPAN_DETECTED;
	    if (mpix[i] & PM_SSPAN_STOP) {
		sspan->stop = true;
	    }
	}
    }
    
    return sspan;
}

//
// Add a new Spartspan to an array of Spartspans.  Iff we see a stop bit, return true
//
static bool add_startspan(psArray *startspans, // the saved Spartspans
			  const pmSpan *sp, // the span in question
			  psImage *mask, // mask of detected/stop pixels
			  const PM_SSPAN_DIR dir) { // the desired direction to search
    if (dir == PM_SSPAN_RESTART) {
	if (add_startspan(startspans, sp, mask,  PM_SSPAN_UP) ||
	    add_startspan(startspans, sp, NULL, PM_SSPAN_DOWN)) {
	    return true;
	}
    } else {
	Spartspan *sspan = SpartspanAlloc(sp, mask, dir);
	if (sspan->stop) {		// we detected a stop bit
	    psFree(sspan);		// don't allocate new span

	    return true;
	} else {
	    psArrayAdd(startspans, 1, sspan);
	    psFree(sspan);		// as it's now owned by startspans
	}
    }

    return false;
}

/************************************************************************************************************/
/*
 * Search the image for pixels above threshold, starting at a single Spartspan.
 * We search the array looking for one to process; it'd be better to move the
 * ones that we're done with to the end, but it probably isn't worth it for
 * the anticipated uses of this routine.
 *
 * This is the guts of pmFindFootprintAtPoint
 */
static bool do_startspan(pmFootprint *fp, // the footprint that we're building
			 const psImage *img, // the psImage we're working on
			 psImage *mask, // the associated masks
			 const float threshold,	// Threshold
			 psArray *startspans) {	// specify which span to process next
    bool F32 = false;			// is this an F32 image?
    if (img->type.type == PS_TYPE_F32) {
	F32 = true;
    } else if (img->type.type == PS_TYPE_S32) {
	F32 = false;
    } else {				// N.b. You can't trivially add more cases here; F32 is just a bool
	psError(PS_ERR_UNKNOWN, true, "Unsupported psImage type: %d", img->type.type);
	return NULL;
    }

    psF32 *imgRowF32 = NULL;		// row pointer if F32
    psS32 *imgRowS32 = NULL;		//  "   "   "  "  !F32
    psMaskType *maskRow = NULL;		//  masks's row pointer
    
    const int row0 = img->row0;
    const int col0 = img->col0;
    const int numRows = img->numRows;
    const int numCols = img->numCols;
    
    /********************************************************************************************************/
    
    Spartspan *sspan = NULL;
    for (int i = 0; i < startspans->n; i++) {
	sspan = startspans->data[i];
	if (sspan->direction != PM_SSPAN_DONE) {
	    break;
	}
	if (sspan->stop) {
	    break;
	}
    }
    if (sspan == NULL || sspan->direction == PM_SSPAN_DONE) { // no more Spartspans to process
	return false;
    }
    if (sspan->stop) {			// they don't want any more spans processed
	return false;
    }
    /*
     * Work
     */
    const PM_SSPAN_DIR dir = sspan->direction;
    /*
     * Set initial span to the startspan
     */
    int x0 = sspan->span->x0 - col0, x1 = sspan->span->x1 - col0;
    /*
     * Go through image identifying objects
     */
    int nx0, nx1 = -1;			// new values of x0, x1
    const int di = (dir == PM_SSPAN_UP) ? 1 : -1; // how much i changes to get to the next row
    bool stop = false;			// should I stop searching for spans?

    for (int i = sspan->span->y -row0 + di; i < numRows && i >= 0; i += di) {
	imgRowF32 = img->data.F32[i];	// only one of
	imgRowS32 = img->data.S32[i];	//      these is valid!
	maskRow = mask->data.PS_TYPE_MASK_DATA[i];
	//
	// Search left from the pixel diagonally to the left of (i - di, x0). If there's
	// a connected span there it may need to grow up and/or down, so push it onto
	// the stack for later consideration
	//
	nx0 = -1;
	for (int j = x0 - 1; j >= -1; j--) {
	    double pixVal = (j < 0) ? threshold - 100 : (F32 ? imgRowF32[j] : imgRowS32[j]);
	    if ((maskRow[j] & PM_SSPAN_DETECTED) || pixVal < threshold) {
		if (j < x0 - 1) {	// we found some pixels above threshold
		    nx0 = j + 1;
		}
		break;
	    }
	}

	if (nx0 < 0) {			// no span to the left
	    nx1 = x0 - 1;		// we're going to resume searching at nx1 + 1
	} else {
	    //
	    // Search right in leftmost span
	    //
	    //nx1 = 0;			// make gcc happy
	    for (int j = nx0 + 1; j <= numCols; j++) {
		double pixVal = (j >= numCols) ? threshold - 100 : (F32 ? imgRowF32[j] : imgRowS32[j]);
		if ((maskRow[j] & PM_SSPAN_DETECTED) || pixVal < threshold) {
		    nx1 = j - 1;
		    break;
		}
	    }
	    
	    const pmSpan *sp = pmFootprintAddSpan(fp, i + row0, nx0 + col0, nx1 + col0);
	    
	    if (add_startspan(startspans, sp, mask, PM_SSPAN_RESTART)) {
		stop = true;
		break;
	    }
	}
	//
	// Now look for spans connected to the old span.  The first of these we'll
	// simply process, but others will have to be deferred for later consideration.
	//
	// In fact, if the span overhangs to the right we'll have to defer the overhang
	// until later too, as it too can grow in both directions
	//
	// Note that column numCols exists virtually, and always ends the last span; this
	// is why we claim below that sx1 is always set
	//
	bool first = false;		// is this the first new span detected?
	for (int j = nx1 + 1; j <= x1 + 1; j++) {
	    double pixVal = (j >= numCols) ? threshold - 100 : (F32 ? imgRowF32[j] : imgRowS32[j]);
	    if (!(maskRow[j] & PM_SSPAN_DETECTED) && pixVal >= threshold) {
		int sx0 = j++;		// span that we're working on is sx0:sx1
		int sx1 = -1;		// We know that if we got here, we'll also set sx1
		for (; j <= numCols; j++) {
		    double pixVal = (j >= numCols) ? threshold - 100 : (F32 ? imgRowF32[j] : imgRowS32[j]);
		    if ((maskRow[j] & PM_SSPAN_DETECTED) || pixVal < threshold) { // end of span
			sx1 = j;
			break;
		    }
		}
		assert (sx1 >= 0);

		const pmSpan *sp;
		if (first) {
		    if (sx1 <= x1) {
			sp = pmFootprintAddSpan(fp, i + row0, sx0 + col0, sx1 + col0 - 1);
			if (add_startspan(startspans, sp, mask, PM_SSPAN_DONE)) {
			    stop = true;
			    break;
			}
		    } else {		// overhangs to right
			sp = pmFootprintAddSpan(fp, i + row0, sx0 + col0, x1 + col0);
			if (add_startspan(startspans, sp, mask, PM_SSPAN_DONE)) {
			    stop = true;
			    break;
			}
			sp = pmFootprintAddSpan(fp, i + row0, x1 + 1 + col0, sx1 + col0 - 1);
			if (add_startspan(startspans, sp, mask, PM_SSPAN_RESTART)) {
			    stop = true;
			    break;
			}
		    }
		    first = false;
		} else {
		    sp = pmFootprintAddSpan(fp, i + row0, sx0 + col0, sx1 + col0 - 1);
		    if (add_startspan(startspans, sp, mask, PM_SSPAN_RESTART)) {
			stop = true;
			break;
		    }
		}
	    }
	}

	if (stop || first == false) {	// we're done
	    break;
	}

	x0 = nx0; x1 = nx1;
    }
    /*
     * Cleanup
     */

    sspan->direction = PM_SSPAN_DONE;
    return stop ? false : true;
}

/*
 * Go through an image, starting at (row, col) and assembling all the pixels
 * that are connected to that point (in a chess kings-move sort of way) into
 * a pmFootprint.
 *
 * This is much slower than pmFindFootprints if you want to find lots of
 * footprints, but if you only want a small region about a given point it
 * can be much faster
 *
 * N.b. The returned pmFootprint is not in "normal form"; that is the pmSpans
 * are not sorted by increasing y, x0, x1.  If this matters to you, call
 * pmFootprintNormalize()
 */
pmFootprint *
pmFindFootprintAtPoint(const psImage *img,	// image to search
		       const float threshold,	// Threshold
		       const psArray *peaks, // array of peaks; finding one terminates search for footprint
		       int row, int col) { // starting position (in img's parent's coordinate system)
   assert(img != NULL);

   bool F32 = false;			// is this an F32 image?
   if (img->type.type == PS_TYPE_F32) {
       F32 = true;
   } else if (img->type.type == PS_TYPE_S32) {
       F32 = false;
   } else {				// N.b. You can't trivially add more cases here; F32 is just a bool
       psError(PS_ERR_UNKNOWN, true, "Unsupported psImage type: %d", img->type.type);
       return NULL;
   }
   psF32 *imgRowF32 = NULL;		// row pointer if F32
   psS32 *imgRowS32 = NULL;		//  "   "   "  "  !F32
   
   const int row0 = img->row0;
   const int col0 = img->col0;
   const int numRows = img->numRows;
   const int numCols = img->numCols;
/*
 * Is point in image, and above threshold?
 */
   row -= row0; col -= col0;
   if (row < 0 || row >= numRows ||
       col < 0 || col >= numCols) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true,
                "row/col == (%d, %d) are out of bounds [%d--%d, %d--%d]",
		row + row0, col + col0, row0, row0 + numRows - 1, col0, col0 + numCols - 1);
       return NULL;
   }

   double pixVal = F32 ? img->data.F32[row][col] : img->data.S32[row][col];
   if (pixVal < threshold) {
       return pmFootprintAlloc(0, img);
   }
   
   pmFootprint *fp = pmFootprintAlloc(1 + img->numRows/10, img);
/*
 * We need a mask for two purposes; to indicate which pixels are already detected,
 * and to store the "stop" pixels --- those that, once reached, should stop us
 * looking for the rest of the pmFootprint.  These are generally set from peaks.
 */
   psImage *mask = psImageAlloc(numCols, numRows, PS_TYPE_MASK);
   P_PSIMAGE_SET_ROW0(mask, row0);
   P_PSIMAGE_SET_COL0(mask, col0);
   psImageInit(mask, PM_SSPAN_INITIAL);
   //
   // Set stop bits from peaks list
   //
   assert (peaks == NULL || peaks->n == 0 || pmPeakTest(peaks->data[0]));
   if (peaks != NULL) {
       for (int i = 0; i < peaks->n; i++) {
	   pmPeak *peak = peaks->data[i];
	   mask->data.PS_TYPE_MASK_DATA[peak->y - mask->row0][peak->x - mask->col0] |= PM_SSPAN_STOP;
       }
   }
/*
 * Find starting span passing through (row, col)
 */
   psArray *startspans = psArrayAllocEmpty(1); // spans where we have to restart the search
   
   imgRowF32 = img->data.F32[row];	// only one of
   imgRowS32 = img->data.S32[row];	//      these is valid!
   psMaskType *maskRow = mask->data.PS_TYPE_MASK_DATA[row];
   {
       int i;
       for (i = col; i >= 0; i--) {
	   pixVal = F32 ? imgRowF32[i] : imgRowS32[i];
	   if ((maskRow[i] & PM_SSPAN_DETECTED) || pixVal < threshold) {
	       break;
	   }
       }
       int i0 = i;
       for (i = col; i < numCols; i++) {
	   pixVal = F32 ? imgRowF32[i] : imgRowS32[i];
	   if ((maskRow[i] & PM_SSPAN_DETECTED) || pixVal < threshold) {
	       break;
	   }
       }
       int i1 = i;
       const pmSpan *sp = pmFootprintAddSpan(fp, row + row0, i0 + col0 + 1, i1 + col0 - 1);

       (void)add_startspan(startspans, sp, mask, PM_SSPAN_RESTART);
   }
   /*
    * Now workout from those Spartspans, searching for pixels above threshold
    */
   while (do_startspan(fp, img, mask, threshold, startspans)) continue;
   /*
    * Cleanup
    */
   psFree(mask);
   psFree(startspans);			// restores the image pixel

   return fp;				// pmFootprint really
}

/************************************************************************************************************/
/*
 * Worker routine for the pmSetFootprintArrayIDs/pmSetFootprintID (and pmMergeFootprintArrays)
 */
static void
set_footprint_id(psImage *idImage,	// the image to set
		 const pmFootprint *fp, // the footprint to insert
		 const int id) {	// the desired ID
   const int col0 = fp->region.x0;
   const int row0 = fp->region.y0;

   for (int j = 0; j < fp->spans->n; j++) {
       const pmSpan *span = fp->spans->data[j];
       psS32 *imgRow = idImage->data.S32[span->y - row0];
       for(int k = span->x0 - col0; k <= span->x1 - col0; k++) {
	   imgRow[k] += id;
       }
   }
}

static void
set_footprint_array_ids(psImage *idImage,
			const psArray *footprints, // the footprints to insert
			const bool relativeIDs) { // show IDs starting at 0, not pmFootprint->id
   int id = 0;				// first index will be 1
   for (int i = 0; i < footprints->n; i++) {
       const pmFootprint *fp = footprints->data[i];
       if (relativeIDs) {
	   id++;
       } else {
	   id = fp->id;
       }
       
       set_footprint_id(idImage, fp, id);
   }
}

/*
 * Set an image to the value of footprint's ID whever they may fall
 */
psImage *pmSetFootprintArrayIDs(const psArray *footprints, // the footprints to insert
				const bool relativeIDs) { // show IDs starting at 1, not pmFootprint->id
   assert (footprints != NULL);

   if (footprints->n == 0) {
       psError(PS_ERR_BAD_PARAMETER_SIZE, true, "You didn't provide any footprints");
       return NULL;
   }
   const pmFootprint *fp = footprints->data[0];
   assert(pmFootprintTest((const psPtr)fp));
   const int numCols = fp->region.x1 - fp->region.x0 + 1;
   const int numRows = fp->region.y1 - fp->region.y0 + 1;
   const int col0 = fp->region.x0;
   const int row0 = fp->region.y0;
   assert (numCols >= 0 && numRows >= 0);
   
   psImage *idImage = psImageAlloc(numCols, numRows, PS_TYPE_S32);
   P_PSIMAGE_SET_ROW0(idImage, row0);
   P_PSIMAGE_SET_COL0(idImage, col0);
   psImageInit(idImage, 0);
   /*
    * do the work
    */
   set_footprint_array_ids(idImage, footprints, relativeIDs);

   return idImage;
   
}

/*
 * Set an image to the value of footprint's ID whever they may fall
 */
psImage *pmSetFootprintID(const pmFootprint *fp, // the footprint to insert
			  const int id) {	// the desired ID
   assert(fp != NULL && pmFootprintTest((const psPtr)fp));
   const int numCols = fp->region.x1 - fp->region.x0 + 1;
   const int numRows = fp->region.y1 - fp->region.y0 + 1;
   const int col0 = fp->region.x0;
   const int row0 = fp->region.y0;
   assert (numCols >= 0 && numRows >= 0);
   
   psImage *idImage = psImageAlloc(numCols, numRows, PS_TYPE_S32);
   P_PSIMAGE_SET_ROW0(idImage, row0);
   P_PSIMAGE_SET_COL0(idImage, col0);
   psImageInit(idImage, 0);
   /*
    * do the work
    */
   set_footprint_id(idImage, fp, id);

   return idImage;
   
}

/************************************************************************************************************/
/*
 * Grow a psArray of pmFootprints isotropically by r pixels, returning a new psArray of new pmFootprints
 */
psArray *pmGrowFootprintArray(const psArray *footprints, // footprints to grow
			      int r) {	// how much to grow each footprint
    assert (footprints->n == 0 || pmFootprintTest(footprints->data[0]));

    if (footprints->n == 0) {		// we don't know the size of the footprint's region
	return psArrayAlloc(0);
    }
    /*
     * We'll insert the footprints into an image, then convolve with a disk,
     * then extract a footprint from the result --- this is magically what we want.
     */
    psImage *idImage = pmSetFootprintArrayIDs(footprints, true);
    if (r <= 0) {
	r = 1;				// r == 1 => no grow
    }
    psKernel *circle = psKernelAlloc(-r, r, -r, r);
    assert (circle->image->numRows == 2*r + 1 && circle->image->numCols == circle->image->numRows);
    for (int i = 0; i <= r; i++) {
	for (int j = 0; j <= r; j++) {
	    if (i*i + j*j <= r*r) {
		circle->kernel[i][j] = 
		    circle->kernel[i][-j] = 
		    circle->kernel[-i][j] = 
		    circle->kernel[-i][-j] = 1;
	    }
	}
    }

    psImage *grownIdImage = psImageConvolveDirect(NULL, idImage, circle); // Here's the actual grow step
    psFree(circle);	

    psArray *grown = pmFindFootprints(grownIdImage, 0.5, 1); // and here we rebuild the grown footprints
    assert (grown != NULL);
    psFree(idImage); psFree(grownIdImage);
    /*
     * Now assign the peaks appropriately.  We could do this more efficiently
     * using grownIdImage (which we just freed), but this is easy and probably fast enough
     */
    const psArray *peaks = pmFootprintArrayToPeaks(footprints);
    pmPeaksAssignToFootprints(grown, peaks);
    psFree((psArray *)peaks);

    return grown;
    
}

/************************************************************************************************************/
/*
 * Merge together two psArrays of pmFootprints neither of which is damaged.
 *
 * The returned psArray may contain elements of the inital psArrays (with
 * their reference counters suitable incremented)
 */
psArray *pmMergeFootprintArrays(const psArray *footprints1, // one set of footprints
				const psArray *footprints2, // the other set
				const int includePeaks) { // which peaks to set? 0x1 => footprints1, 0x2 => 2
    assert (footprints1->n == 0 || pmFootprintTest(footprints1->data[0]));
    assert (footprints2->n == 0 || pmFootprintTest(footprints2->data[0]));

    if (footprints1->n == 0 || footprints2->n == 0) {		// nothing to do but put copies on merged
	const psArray *old = (footprints1->n == 0) ? footprints2 : footprints1;

	psArray *merged = psArrayAllocEmpty(old->n);
	for (int i = 0; i < old->n; i++) {
	    psArrayAdd(merged, 1, old->data[i]);
	}
	
	return merged;
    }
    /*
     * We have real work to do as some pmFootprints in footprints2 may overlap
     * with footprints1
     */
    {
	pmFootprint *fp1 = footprints1->data[0];
	pmFootprint *fp2 = footprints2->data[0];
	if (fp1->region.x0 != fp2->region.x0 ||
	    fp1->region.x1 != fp2->region.x1 ||
	    fp1->region.y0 != fp2->region.y0 ||
	    fp1->region.y1 != fp2->region.y1) {
	    psError(PS_ERR_BAD_PARAMETER_SIZE, true,
		    "The two pmFootprint arrays correspnond to different-sized regions");
	    return NULL;
	}
    }
    /*
     * We'll insert first one set of footprints then the other into an image, then
     * extract a footprint from the result --- this is magically what we want.
     */
    psImage *idImage = pmSetFootprintArrayIDs(footprints1, true);
    set_footprint_array_ids(idImage, footprints2, true);

    psArray *merged = pmFindFootprints(idImage, 0.5, 1);
    assert (merged != NULL);
    psFree(idImage);
    /*
     * Now assign the peaks appropriately.  We could do this more efficiently
     * using idImage (which we just freed), but this is easy and probably fast enough
     */ 
    if (includePeaks & 0x1) {
	const psArray *peaks = pmFootprintArrayToPeaks(footprints1);
	pmPeaksAssignToFootprints(merged, peaks);
	psFree((psArray *)peaks);
    }

    if (includePeaks & 0x2) {
	const psArray *peaks = pmFootprintArrayToPeaks(footprints2);
	pmPeaksAssignToFootprints(merged, peaks);
	psFree((psArray *)peaks);
    }
    
    return merged;
}

/************************************************************************************************************/
/*
 * Given a psArray of pmFootprints and another of pmPeaks, assign the peaks to the
 * footprints in which that fall; if they _don't_ fall in a footprint, add a suitable
 * one to the list.
 */
psErrorCode
pmPeaksAssignToFootprints(psArray *footprints,	// the pmFootprints
			  const psArray *peaks) { // the pmPeaks
    assert (footprints != NULL);
    assert (footprints->n == 0 || pmFootprintTest(footprints->data[0]));
    assert (peaks != NULL);
    assert (peaks->n == 0 || pmPeakTest(peaks->data[0]));
    
    if (footprints->n == 0) {
	if (peaks->n > 0) {
	    return psError(PS_ERR_BAD_PARAMETER_SIZE, true, "Your list of footprints is empty");
	}
	return PS_ERR_NONE;
    }
    /*
     * Create an image filled with the object IDs, and use it to assign pmPeaks to the
     * objects
     */
    psImage *ids = pmSetFootprintArrayIDs(footprints, true);
    assert (ids != NULL);
    assert (ids->type.type == PS_TYPE_S32);
    const int row0 = ids->row0;
    const int col0 = ids->col0;
    const int numRows = ids->numRows;
    const int numCols = ids->numCols;

    for (int i = 0; i < peaks->n; i++) {
	pmPeak *peak = peaks->data[i];
	const int x = peak->x - col0;
	const int y = peak->y - row0;
	
	assert (x >= 0 && x < numCols && y >= 0 && y < numRows);
	int id = ids->data.S32[y][x - col0];

	if (id == 0) {			// peak isn't in a footprint, so make one for it
	    pmFootprint *nfp = pmFootprintAlloc(1, ids);
	    pmFootprintAddSpan(nfp, y, x, x);
	    psArrayAdd(footprints, 1, nfp);
	    psFree(nfp);
	    id = footprints->n;
	}

	assert (id >= 1 && id <= footprints->n);
	pmFootprint *fp = footprints->data[id - 1];
	psArrayAdd(fp->peaks, 5, peak);
    }
    
    psFree(ids);
    //
    // Make sure that peaks within each footprint are sorted and unique
    //
    for (int i = 0; i < footprints->n; i++) {
	pmFootprint *fp = footprints->data[i];
        fp->peaks = psArraySort(fp->peaks, pmPeakSortBySN);

	for (int j = 1; j < fp->peaks->n; j++) { // check for duplicates
	    if (fp->peaks->data[j] == fp->peaks->data[j-1]) {
		(void)psArrayRemoveIndex(fp->peaks, j);
		j--;			// we moved everything down one
	    }
	}
    }

    return PS_ERR_NONE;
}

/************************************************************************************************************/
 /*
  * Examine the peaks in a pmFootprint, and throw away the ones that are not sufficiently
  * isolated.  More precisely, for each peak find the highest coll that you'd have to traverse
  * to reach a still higher peak --- and if that coll's more than nsigma DN below your
  * starting point, discard the peak.
  */
psErrorCode pmFootprintCullPeaks(const psImage *img, // the image wherein lives the footprint
				 const psImage *weight,	// corresponding variance image
				 pmFootprint *fp, // Footprint containing mortal peaks
				 const float nsigma_delta, // how many sigma above local background a peak
				 	// needs to be to survive
				 const float min_threshold) { // minimum permitted coll height
    assert (img != NULL); assert (img->type.type == PS_TYPE_F32);
    assert (weight != NULL); assert (weight->type.type == PS_TYPE_F32);
    assert (img->row0 == weight->row0 && img->col0 == weight->col0);
    assert (fp != NULL);

    if (fp->peaks == NULL || fp->peaks->n == 0) { // nothing to do
	return PS_ERR_NONE;
    }

    psRegion subRegion;			// desired subregion; 1 larger than bounding box (grr)
    subRegion.x0 = fp->bbox.x0; subRegion.x1 = fp->bbox.x1 + 1;
    subRegion.y0 = fp->bbox.y0; subRegion.y1 = fp->bbox.y1 + 1;
    const psImage *subImg = psImageSubset((psImage *)img, subRegion);
    const psImage *subWt = psImageSubset((psImage *)weight, subRegion);
    assert (subImg != NULL && subWt != NULL);
    //
    // We need a psArray of peaks brighter than the current peak.  We'll fake this
    // by reusing the fp->peaks but lying about n.
    //
    // We do this for efficiency (otherwise I'd need two peaks lists), and we are
    // rather too chummy with psArray in consequence.  But it works.
    //
    psArray *brightPeaks = psArrayAlloc(0);
    psFree(brightPeaks->data);
    brightPeaks->data = psMemIncrRefCounter(fp->peaks->data);// use the data from fp->peaks
    //
    // The brightest peak is always safe; go through other peaks trying to cull them
    //
    for (int i = 1; i < fp->peaks->n; i++) { // n.b. fp->peaks->n can change within the loop
	const pmPeak *peak = fp->peaks->data[i];
	int x = peak->x - subImg->col0;
	int y = peak->y - subImg->row0;
	//
	// Find the level nsigma below the peak that must separate the peak
	// from any of its friends
	//
	assert (x >= 0 && x < subImg->numCols && y >= 0 && y < subImg->numRows);
	const float stdev = sqrt(subWt->data.F32[y][x]);
	float threshold = subImg->data.F32[y][x] - nsigma_delta*stdev;
	if (isnan(threshold) || threshold < min_threshold) {
#if 1					// min_threshold is assumed to be below the detection threshold,
					// so all the peaks are pmFootprint, and this isn't the brightest
	    (void)psArrayRemoveIndex(fp->peaks, i);
	    i--;			// we moved everything down one
	    continue;
#else
#error n.b. We will be running LOTS of checks at this threshold, so only find the footprint once
	    threshold = min_threshold;
#endif
	}
	if (threshold > subImg->data.F32[y][x]) {
	    threshold = subImg->data.F32[y][x] - 10*FLT_EPSILON;
	}

	const int peak_id = 1;		// the ID for the peak of interest
	brightPeaks->n = i;		// only stop at a peak brighter than we are
	pmFootprint *peakFootprint = pmFindFootprintAtPoint(subImg, threshold, brightPeaks, peak->y, peak->x);
	brightPeaks->n = 0;		// don't double free
	psImage *idImg = pmSetFootprintID(peakFootprint, peak_id);
	psFree(peakFootprint);

	int j;
	for (j = 0; j < i; j++) {
	    const pmPeak *peak2 = fp->peaks->data[j];
	    int x2 = peak2->x - subImg->col0;
	    int y2 = peak2->y - subImg->row0;
	    const int peak2_id = idImg->data.S32[y2][x2]; // the ID for some other peak

	    if (peak2_id == peak_id) {	// There's a brighter peak within the footprint above
		;			// threshold; so cull our initial peak
		(void)psArrayRemoveIndex(fp->peaks, i);
		i--;			// we moved everything down one
		break;
	    }
	}
	if (j == i) {
	    j++;
	}

	psFree(idImg);
    }

    brightPeaks->n = 0; psFree(brightPeaks);
    psFree((psImage *)subImg);
    psFree((psImage *)subWt);

    return PS_ERR_NONE;
}

/*
 * Cull an entire psArray of pmFootprints
 */
psErrorCode
pmFootprintArrayCullPeaks(const psImage *img, // the image wherein lives the footprint
			  const psImage *weight,	// corresponding variance image
			  psArray *footprints, // array of pmFootprints
			  const float nsigma_delta, // how many sigma above local background a peak
    					// needs to be to survive
			  const float min_threshold) { // minimum permitted coll height
    for (int i = 0; i < footprints->n; i++) {
	pmFootprint *fp = footprints->data[i];
	if (pmFootprintCullPeaks(img, weight, fp, nsigma_delta, min_threshold) != PS_ERR_NONE) {
	    return psError(PS_ERR_UNKNOWN, false, "Culling pmFootprint %d", fp->id);
	}
    }
    
    return PS_ERR_NONE;
}

/************************************************************************************************************/
/*
 * Extract the peaks in a psArray of pmFootprints, returning a psArray of pmPeaks
 */
psArray *pmFootprintArrayToPeaks(const psArray *footprints) {
   assert(footprints != NULL);
   assert(footprints->n == 0 || pmFootprintTest(footprints->data[0]));

   int npeak = 0;
   for (int i = 0; i < footprints->n; i++) {
      const pmFootprint *fp = footprints->data[i];
      npeak += fp->peaks->n;
   }

   psArray *peaks = psArrayAllocEmpty(npeak);
   
   for (int i = 0; i < footprints->n; i++) {
      const pmFootprint *fp = footprints->data[i];
      for (int j = 0; j < fp->peaks->n; j++) {
	 psArrayAdd(peaks, 1, fp->peaks->data[j]);
      }
   }

   return peaks;
}
