

static char *rcsid = "split-merge.c";


/*   split-merge.c:  3/26/93 by Greg Klanderman

     Based on Eric Jaquith's "pbmtoptl.c" from 3/26/93. (see his comment below)
     Hacked to use my new (wjr-like) library and work as a C-callable routine.

     TODO: kill sqrt in Distance()
           version for FloatImage
*/


/*----------------------------------------------------------------------------
   From the original:
   
                           Eric Jaquith                          
                       Summer '91 - Fall '92
                                                            
  This program creates image edges from a pbm file.

  It does this by reading in the array from the pbm file which represents an
  x_max*y_max grid of pixels, which have values either 0 or 1.  The program
  finds paths by finding a pixel with a value of 1, and tracing a path from it
  through other pixels = 1.  Edges are made by taking the endpts of each path,
  and finding the pt. furthest from the line the segment is part of.  If it is
  not within a certain tolerance, the path is split in two, and we check again
  recursively until each path is approximated by "straight" lines.

  command line arguments:                                   
    infile:  filename of input file (file of unsigned char) 
    max_x:   the width of the picture (usu. 512)            
    max_y:   the height of the picture (usu. 512)           

----------------------------------------------------------------------------*/


#include "split-merge.h"
#include "panic.h"


double  sqrt(double x);

#define SEGMENT_CHUNK_SIZE   100



/* AREA(): Finds the area of the triangle made by the three points */

inline static float TwiceArea(Point *a, Point *b, Point *c)
{
  float ax, ay, bx, by, cx, cy;

  ax =  a->x;
  ay =  a->y;
  bx =  b->x;
  by =  b->y;
  cx =  c->x;
  cy =  c->y;

  return ABS((c->x - b->x)*a->y + (a->x - c->x)*b->y + (b->x - a->x)*c->y);
}


/* DISTANCE():  Finds the distance between the two points */

inline static float Distance(Point *a, Point *b)
{
  return sqrt(SQUARE(((float) (b->x - a->x))) + SQUARE(((float) (b->y - a->y))));
}



typedef struct { Point pt; boolean valid; } PointV;


/* NEIGHBORS():  returns the number of neighbors of a given point.  If the
                 number is 2, then we return the locations of the two neighbors */

inline static int Neighbors(BinaryImage img, int x, int y,
			    PointV *neighbor1, PointV *neighbor2)
{
  int numNeighbors = 0;
  PointV pt[2];

  /* to ensure we don't report neighbors that may screw up later calculations */
  pt[0].valid = pt[1].valid = FALSE;

#define CHECKPT(I, J)      if (imRef(img, I, J)) {                                     \
			      if (numNeighbors < 2) {                                  \
				pt[numNeighbors] = ((PointV) {{I, J}, TRUE});          \
			      }                                                        \
			      numNeighbors++;                                          \
			    }

  CHECKPT(x-1, y-1);
  CHECKPT(x-1, y);
  CHECKPT(x-1, y+1);
  CHECKPT(x,   y-1);
  CHECKPT(x,   y+1);
  CHECKPT(x+1, y-1);
  CHECKPT(x+1, y);
  CHECKPT(x+1, y+1);

#undef CHECKPT

  *neighbor1 = pt[0];
  *neighbor2 = pt[1];

  return numNeighbors;
}


/* EQUALPT():  returns whether or not 2 points are the same */

inline static boolean EqualPtV(PointV a, PointV b)
{
  return (a.valid  &&  b.valid  &&  a.pt.x == b.pt.x  &&  a.pt.y == b.pt.y);
}


/* ADJACENT():  returns whether or not two points are adjacent to each other
                in the array. */

inline static boolean Adjacent(Point a, Point b)
{
  return (ABS(b.x - a.x) + ABS(b.y - a.y) <= 1);
}


/* ENDPOINT():  returns whether or not the point at x,y in the array is an
                endpoint (i.e. if it is the end of some component) */

inline static boolean EndPoint(BinaryImage img, int x, int y)
{

  int numN;      /* # of neighbors & location of 2 neighbors to (x,y) */
  PointV n1, n2;
  int numN1;     /* # of neighbors & location of 2 neighbors to (n1.x,n1.y) */
  PointV n3, n4;
  int numN2;     /* # of neighbors & location of 2 neighbors to (n2.x,n2.y) */
  PointV n5, n6;

  numN = Neighbors(img, x, y, &n1, &n2);
  
  if (numN < 2)
    return TRUE;
  
  if (numN > 2  ||  (numN == 2  &&  !Adjacent(n1.pt, n2.pt)))
    return FALSE;

  imRef(img, n1.pt.x, n1.pt.y) = 0;
  imRef(img, n2.pt.x, n2.pt.y) = 0;
  numN1 = Neighbors(img, n1.pt.x, n1.pt.y, &n3, &n4);
  numN2 = Neighbors(img, n2.pt.x, n2.pt.y, &n5, &n6);
  imRef(img, n1.pt.x, n1.pt.y) = 1;
  imRef(img, n2.pt.x, n2.pt.y) = 1;

  /* get rid of cases where the two neighborhoods share common neighbors */
  if (EqualPtV(n3, n5)  ||  EqualPtV(n3, n6))
    numN1--;

  if (EqualPtV(n4, n5)  ||  EqualPtV(n4, n6))
    numN1--;

  if (numN1 + numN2 > 2)
    return FALSE;
  else if ((numN1 + numN2 < 2) ||
	   (numN1 == 2 && Adjacent(n3.pt, n4.pt)) ||
	   (numN2 == 2 && Adjacent(n5.pt, n6.pt)) ||
	   (numN1 == 1 && Adjacent(n3.pt, n5.pt))) {
    /* we're returning TRUE iff:
         the number of neighbors to both n1 and n2 is 0 or 1;
         there are two adjacent neighbors to n1 and none to n2;
         there are two adjacent neighbors to n2 and none to n1;
         both n1 and n2 have one neighbor and they are adjacent; */
    return TRUE;
  }

  return FALSE;
}


/* GETNEXT():  returns the coords of the next found "on" point given the current
               point.  If none found (point sent was endpoint) then return NULL.
  
	       I use a 3x3 window to look at eight points surrounding the 
	       center point.  I choose the first point found "on" with
	       a specific ordering, picking the first on point closest to
	       where the previous point was found.

	       +-----+
	       |0 1 2|
	       |3   4|    <= window with grid array ordering
	       |5 6 7|
	       +-----+  */

inline static boolean GetNext(BinaryImage img, Point *prev, Point *curr, Point *next)
{
  static Point ordering[9][8] =
      {{{0,-1},  {-1,0},  {1,-1},  {-1,1},  {1,0},   {0,1},   {1,1},   {0,0}},  /* TL */
       {{1,-1},  {-1,-1}, {1,0},   {-1,0},  {1,1},   {-1,1},  {0,1},   {0,0}},  /* TM */
       {{1,0},   {0,-1},  {1,1},   {-1,-1}, {0,1},   {-1,0},  {-1,1},  {0,0}},  /* TR */
       {{-1,-1}, {-1,1},  {0,-1},  {0,1},   {1,-1},  {1,1},   {1,0},   {0,0}},  /* ML */
       {{0,-1},  {-1,0},  {1,-1},  {-1,1},  {1,0},   {0,1},   {1,1}, {-1,-1}},  /* AL */
       {{1,1},   {1,-1},  {0,1},   {0,-1},  {-1,1},  {-1,-1}, {-1,0},  {0,0}},  /* MR */
       {{-1,0},  {0,1},   {-1,-1}, {1,1},   {0,-1},  {1,0},   {1,-1},  {0,0}},  /* BL */
       {{-1,1},  {1,1},   {-1,0},  {1,0},   {-1,-1}, {1,-1},  {0,-1},  {0,0}},  /* BM */
       {{0,1},   {1,0},   {-1,1},  {1,-1},  {-1,0},  {0,-1},  {-1,-1}, {0,0}}}; /* BR */
  
  int i;
  Point *check;

  
  if (prev) {				     /* there is a point and a previous one */
    check = ordering[3*(prev->y - curr->y + 1) + (prev->x - curr->x + 1)];
    
    for (i = 0; i < 7; i++)
      if (imRef(img, curr->x + check[i].x, curr->y + check[i].y)) {
	next->x = curr->x + check[i].x;
	next->y = curr->y + check[i].y;
	return TRUE;
      }

    /* no next point was found at CURRent point, so check for ANY point
       adjacent to the PREVious point. */

    check = ordering[4];

    for (i = 0; i < 8; i++)
      if (imRef(img, prev->x + check[i].x, prev->y + check[i].y)) {
	next->x = prev->x + check[i].x;
	next->y = prev->y + check[i].y;
	curr->x = prev->x;
	curr->y = prev->y;
	return TRUE;
      }

    return FALSE;			     /* no next point to be found */
  }
  else {				     /* we are at the start of a component */
    check = ordering[4];

    for (i = 0; i < 8; i++)
      if (imRef(img, curr->x + check[i].x, curr->y + check[i].y)) {
	next->x = curr->x + check[i].x;
	next->y = curr->y + check[i].y;
	return TRUE;
      }

    return FALSE;
  }
}


inline static int addSegment(SegmentVector *segtail, Segment seg)
{
  SegmentVector tmp;

  if (vecUserData(*segtail).used >= SEGMENT_CHUNK_SIZE) {
    if ((tmp = vecNewStruct(SegmentVector, SEGMENT_CHUNK_SIZE)) == NULL) {
      return -1;
    }
    
    vecUserData(*segtail).ptr = tmp;
    vecUserData(tmp) = ((SegVecUserData) {used: 0, ptr: (void *) NULL});
    *segtail = tmp;
  }

  vecRef(*segtail, vecUserData(*segtail).used++) = seg;
  return 0;
}



typedef struct {
  Point *first, *last;
} *BreakStack;


/* BREAKANDWRITE():  take path (list of points) and look at 2 endpoints.
                     Find point in the path that is furthest from the line
		     the endpoints are in.  If furthest point is over bendSlop
		     away from line, cut path (at furthest point) into
		     2 inter paths.  But, if pt < bendSlop from line, have
		     found an almost perfectly straight segment, so write it
		     to file as an image edge */


static int BreakPath(SegmentVector *segtail, float bendSlop, float minLength,
		     BreakStack breakstk, int *compcnt)
{
  BreakStack bstop = breakstk + 1;
  Point *first, *last;
  float furthest;
  float height, base;
  Point *check, *worst;


  while (bstop > breakstk) {
    bstop--;                                 /* pop a path off stack */
    first = bstop->first;
    last  = bstop->last;

  already_have_one_to_do:
    worst = (Point *) NULL;                  /* find furthest from straight line path */
    base  = Distance(first, last);

    for (furthest = bendSlop, check = first + 1; check < last; check++) {
      height = TwiceArea(first, last, check) / base;

      if (height > furthest) {
	furthest = height;
	worst    = check;
      }
    }

    if (worst) {                             /* straight line not good enough => split */
      if (worst - first <= last - worst) {   /* push larger and continue with smaller */
        *(bstop++) = ((typeof (*bstop)) {first: worst, last: last}); 
        last  = worst - 1;                   /* first = first; -- implicit */
      }
      else {
        *(bstop++) = ((typeof (*bstop)) {first: first, last: worst - 1}); 
        first = worst;                       /* last = last; -- implicit */
      }

      goto already_have_one_to_do;         
    }
    else if (base >= minLength) {            /* straight line ok & long enough => add */
      if (compcnt) {
        if (addSegment(segtail, ((Segment) {a: ((Point) {0, *compcnt}),
                                            b: ((Point) {0, *compcnt})}))) {
          return -1;
        }
        (*compcnt)++;
        compcnt = NULL;
      }
      
      if (addSegment(segtail, ((Segment) {a: *first, b: *last}))) {
        return -1;
      }
    }
  }

  return 0;
}


/* SplitAndMerge():  this function makes image edges from the picture by making
                     passes.  The first looks only for "endpoints" of lines and
		     curves, and then gets the path it belongs to.  The next
		     passes are used to find any remaining "on" points belonging to
		     either cycles or paths which were part of forks that were split
		     arbitrarily in the first pass. */

SegmentVector splitAndMerge_(BinaryImage img, float bendSlop, float minLength,
                             kwArgDecl(SplitMergeKW))
{
  kwDefault(comment, 0);                     /* do commenting??? */
  int passcnt;
  Point *pathlist, *path, *pathend;
  boolean gotone;
  int x, y;
  boolean clear;
  int x_max, y_max, x_min, y_min;
  SegmentVector segtail, segments;
  BreakStack breakstk;
  int compcnt;
  int *compcntptr = (comment & SM_COMPONENT) ? &compcnt : NULL;
  

  x_min = imGetXBase(img);
  y_min = imGetYBase(img);
  x_max = imGetXMax(img);
  y_max = imGetYMax(img);

  bendSlop  = MAX(bendSlop,  1.0);           /* less is silly */
  minLength = MAX(minLength, 1.0);           /* never output any single points */

  /* clear all "on" pixels from outer borders so we can ignore the borders */
  for (x = x_min; x <= x_max; x++)    imRef(img, x, 0) = imRef(img, x, y_max) = 0;
  for (y = y_min; y <= y_max; y++)    imRef(img, 0, y) = imRef(img, x_max, y) = 0;

  { int mxlen = 5*(imGetWidth(img) + imGetHeight(img)); /* vector to store a pathlist */
    int stklen = (int) (log((double) mxlen)/log(2) + 10); /* plus a little slop */

    if ((pathlist = (Point *) malloc(mxlen * sizeof(Point))) == NULL) {
      return (SegmentVector) NULL;
    }

    path     = pathlist;
    pathend  = pathlist + mxlen;
    
    if ((breakstk = (BreakStack) malloc(stklen * sizeof(*breakstk))) == NULL) {
      free(pathlist);
      return (SegmentVector) NULL;
    }
  }

  /* linked list of Segment Vectors to store the segments */
  if ((segments = vecNewStruct(SegmentVector, SEGMENT_CHUNK_SIZE)) == NULL) {
    free(pathlist);
    free(breakstk);
    return (SegmentVector) NULL;
  }
  vecUserData(segments) = ((SegVecUserData) {used: 0, ptr: NULL});
  segtail = segments;


  /* first pass finds endpoints, additional passes find everything else*/
  passcnt = 1;
  clear   = FALSE;
  compcnt = 1;

  while (!clear) {
    clear = TRUE;

    if (comment & SM_PASS) {                 /* mark pass */
      if (addSegment(&segtail, ((Segment) {a: ((Point) {1, passcnt}),
                                           b: ((Point) {1, passcnt})}))) {
        goto error;
      }
    }
    
    for (y = y_min + 1; y < y_max; y++) {
      for (x = x_min + 1; x < x_max; x++) {
        if (imRef(img, x, y)) {
	  clear = FALSE;
	  imRef(img, x, y) = 0;

	  /* only start at endpoints in first pass */
          if (passcnt == 1  &&  !EndPoint(img, x, y)) {
	    imRef(img, x, y) = 1;
	    continue;
	  }

          /* point is "on", so find path it is part of */

	  *(path++) = ((Point) {x: x, y: y});
	  
	  /* keep adding on to path until endpoint is reached */

	  gotone = GetNext(img, (Point *) NULL, path - 1, path);
	  while (gotone) {
            imRef(img, path->x, path->y) = 0;
	    path++;

	    if (path < pathend) {
	      gotone = GetNext(img, path - 2, path - 1, path);
	    }
	    else {
	      gotone = FALSE;
	      warn("SplitAndMerge: Pathlist overflow!!!");
	    }
	  }

	  if (path - pathlist > 2) {	     /* skip 1 and 2 pt paths */
            *breakstk = ((typeof (*breakstk)) {first: pathlist, last: path - 1});

	    if (BreakPath(&segtail, bendSlop, minLength, breakstk, compcntptr)) {
              goto error;
	    }
	  }

	  path = pathlist;		     /* start a new path */
	}
      }
    }
    passcnt++;
  }

  free(pathlist);
  free(breakstk);

  
  { SegmentVector tmp, segs;                 /* then copy segments to one large */
    int nsegs, i;			     /* segment vector, SEGS, and return */
				
    for (tmp = segments, nsegs = 0; tmp != NULL; tmp = vecUserData(tmp).ptr) {
      nsegs += vecUserData(tmp).used;
    }

    if ((segs = vecNewStruct(SegmentVector, nsegs)) == NULL) {
      return (SegmentVector) NULL;
    }
    vecUserData(segs) = ((SegVecUserData) {used: nsegs, ptr: NULL});

    nsegs = 0;
    for ( ; segments != NULL; segments = tmp) {
      for (i = 0; i < vecUserData(segments).used; i++) {
	vecRef(segs, nsegs++) = vecRef(segments, i);
      }

      tmp = (SegmentVector) vecUserData(segments).ptr;
      vecFree(segments);
    }
    assert(vecGetLength(segs) == nsegs);

    return segs;
  }


 /* got an error => free storage and return NULL */
  
 error:
  free(pathlist);  
  free(breakstk);
  
  while (segments != (SegmentVector) NULL) {
    SegmentVector tmp = (SegmentVector) vecUserData(segments).ptr;
    vecFree(segments);
    segments = tmp;
  }

  return (SegmentVector) NULL;
}




/* translated to c from welg's ~welg/stereo/segment-stereo-stable.lisp */
/* (lisp function is "compute-edge-intensities")                       */
/* computes the average intensity on the left and right side of each   */
/* segment in the segment vector, in a strip of the specified width    */
/* this could probably be made much faster.                            */

EdgeIntensVector getEdgeIntensities(GrayImage img, SegmentVector segs, int width)
{
  int n, off, i, l;
  double tx, ty, d;
  double lx, ly, rx, ry;
  int lcount, rcount, lsum, rsum;
  Segment seg;
  const int xmin = imGetXBase(img);
  const int xmax = imGetXMax(img);
  const int ymin = imGetYBase(img);
  const int ymax = imGetYMax(img);
  EdgeIntensVector rslt;


  rslt = (EdgeIntensVector) vecNewStruct(EdgeIntensVector, vecGetLength(segs));

  if (rslt == (EdgeIntensVector) NULL) {
    return (EdgeIntensVector) NULL;
  }

  for (n = 0; n < vecGetLength(segs); n++) {
    seg = vecRef(segs, n);

    tx  = seg.b.x - seg.a.x;
    ty  = seg.b.y - seg.a.y;
    d   = sqrt(tx*tx + ty*ty);
    tx /= d;
    ty /= d;
    l   = (int) (d + 0.5);
          
    lcount = rcount = lsum = rsum = 0;

    for (off = 1; off <= width; off++) {
      lx = seg.a.x  +  ty * off;
      ly = seg.a.y  -  tx * off;
      
      rx = seg.a.x  -  ty * off;
      ry = seg.a.y  +  tx * off;      
      
      for (i = 0; i < l; i++, lx += tx, ly += ty, rx += tx, ry += ty) {
        int ilx = (int) (lx + 0.5);
        int ily = (int) (ly + 0.5);
        int irx = (int) (rx + 0.5);
        int iry = (int) (ry + 0.5);

        if (ilx >= xmin  &&  ilx <= xmax  &&
            ily >= ymin  &&  ily <= ymax) {
          lcount++;
          lsum += imRef(img, ilx, ily);
        }
        
        if (irx >= xmin  &&  irx <= xmax  &&
            iry >= ymin  &&  iry <= ymax) {
          rcount++;
          rsum += imRef(img, irx, iry);
        }
      }
    }

    vecRef(rslt, n).left  = lcount ? (uchar) (lsum / lcount) : 0;
    vecRef(rslt, n).right = rcount ? (uchar) (rsum / rcount) : 0;
  }

  return rslt;
}
