/****************************************************************
 * fbquant.c: FBM Release 1.0 25-Feb-90 Michael Mauldin
 *
 * Copyright (C) 1989,1990 by Michael Mauldin.  Permission is granted
 * to use this file in whole or in part for any purpose, educational,
 * recreational or commercial, provided that this copyright notice
 * is retained unchanged.  This software is available to all free of
 * charge by anonymous FTP and in the UUNET archives.
 *
 * fbquant: Convert an RGB color image to mapped color format (color
 *	    quantization step).  Floyd-Steinberg dithering is used
 *	    to reduce color banding.  The quantization used is a
 *	    modification of Heckbert's median cut.
 *
 * USAGE
 *	% fbquant [ -c<numcolors> ] [ -r<bits> ] [ -m<mapimage> ]
 *		  [ -n ] [ -<type> ] < rgb > mapped"
 *
 * EDITLOG
 *	LastEditDate = Mon Jun 25 00:04:18 1990 - Michael Mauldin
 *	LastFileName = /usr2/mlm/src/misc/fbm/fbquant.c
 *
 * HISTORY
 * 25-Jun-90  Michael Mauldin (mlm@cs.cmu.edu) Carnegie Mellon
 *	Package for Release 1.0
 *
 * 04-Sep-89  Michael Mauldin (mlm) at Carnegie Mellon University
 *	Add option for low-res colors, default assumes each RGB value
 *	can vary from 0 to 255.  The default is 8 bits for more than 32
 *	color images, and 4 bits for images with 32 or fewer colors.
 *	For example, the Amiga has 4bit DACs and can only display
 * 	4^3 == 4096 colors.
 *
 * 03-May-89  Michael Mauldin (mlm) at Carnegie Mellon University
 *	Clear FBM pointers before allocate
 *
 * 07-Apr-89  Michael Mauldin (mlm) at Carnegie Mellon University
 *	Add -m<image> flag for using colormap from another image.
 *
 * 07-Mar-89  Michael Mauldin (mlm) at Carnegie Mellon University
 *	Beta release (version 0.93) mlm@cs.cmu.edu
 *
 * 26-Feb-89  Michael Mauldin (mlm) at Carnegie Mellon University
 *	Changes for small color maps.  Fixed bug with unsigned
 *	arithmetic that ruined dithering for images with small
 *	colormaps.  Added error limiting in the Floyd-Steinberg
 *	code to prevent color "shadowing" that occurs with small
 *	numbers of colors.  Also change to use colors 0..n-1 instead
 *	of reserving colors 0 and n-1 for Sun foreground/background
 *	colors, unless output type is SUN.
 *
 * 11-Nov-88  Michael Mauldin (mlm) at Carnegie Mellon University
 *	Created.
 *
 * References: Uses a variant of Heckbert's adaptive partitioning
 *	       algorithm.  See Computer Graphics, v16n3 July 1982
 *
 ****************************************************************/

# include <stdio.h>
# include "fbm.h"

int cmp_red(), cmp_grn(), cmp_blu(), cmp_cmap(), cmp_int();

# define RD 0
# define GR 1
# define BL 2
# define REDMASK 0076000
# define REDSHFT 10
# define GRNMASK 0001740
# define GRNSHFT 5
# define BLUMASK 0000037
# define BLUSHFT 0
# define CUBITS  5
# define CUBIGN  (8-CUBITS)
# define CUBSID  32
# define CUBSIZ  32768
# define MAXSHRT 32767
# define MAXERR	 32

# define GETR(X) (((X) & REDMASK) >> REDSHFT)
# define GETG(X) (((X) & GRNMASK) >> GRNSHFT)
# define GETB(X)  ((X) & BLUMASK)

# define CLRINDEX(R,G,B)			\
	(((R) << REDSHFT) & REDMASK |		\
	 ((G) << GRNSHFT) & GRNMASK |		\
	 ((B)  & BLUMASK))

# define CLRINDEX8(R,G,B)			\
	(((R) << (REDSHFT-CUBIGN)) & REDMASK |	\
	 ((G) << (GRNSHFT-CUBIGN)) & GRNMASK |	\
	 ((B) >> (CUBIGN))  & BLUMASK)

# define GETR8(X) (((X) & REDMASK) >> (REDSHFT-CUBIGN))
# define GETG8(X) (((X) & GRNMASK) >> (GRNSHFT-CUBIGN))
# define GETB8(X) (((X) & BLUMASK) << CUBIGN)

# define abs(X) (((X)<0)?-(X):(X))

typedef struct cstruct {
	unsigned char rd, gr, bl, indx;
} COLOR;

COLOR *cmap = NULL;

typedef struct pix_struct {
	short cnt;
	short color;
} PIXEL;

int debug=0, verbose=0, colors=256, showcolor=0, dither=1;
int resbits = -1, resmask = 0xff;
double flesh = 1.0, atof();
int fr = 192, fg = 96, fb = 80;

/****************************************************************
 * main
 ****************************************************************/

# define USAGE \
"Usage:  fbquant [ -c<numcolors> ] [ -r<bits> ] [ -m<mapimage> ]\n\
		[ -f<flesh> ] [ -n ] [ -<type> ] < rgb > mapped"

#ifndef lint
static char *fbmid = 
"$FBM fbquant.c <1.0> 25-Jun-90  (C) 1989,1990 by Michael Mauldin, source \
code available free from MLM@CS.CMU.EDU and from UUNET archives$";
#endif

main (argc, argv)
char *argv[];
{ FBM input, output, mapimage;	/* Images */
  int hist[CUBSIZ];		/* Color cube 32x32x32 for histogram */
  int outtype = DEF_8BIT;	/* Output format desired */
  char *mapfile = NULL;		/* Name of file to copy map from */
  register int c;

  /* Clear pointers */
  input.bm = input.cm = (unsigned char *) NULL;
  output.bm = output.cm = (unsigned char *) NULL;
  mapimage.bm = mapimage.cm = (unsigned char *) NULL;

  /* Get the options */
  while (--argc > 0 && (*++argv)[0] == '-')
  { while (*++(*argv))
    { switch (**argv)
      { case 'c':	colors = atoi (*argv+1); SKIPARG; break;
        case 'f':	flesh = atof (*argv+1); SKIPARG; break;
        case 'r':	resbits = atoi (*argv+1); SKIPARG; break;
        case 'm':	mapfile = *argv+1; SKIPARG; break;
	case 'n':	dither = 0; break;
	case 'd':	debug++; break;
	case 'D':	showcolor++; break;
	case 'v':	verbose++; break;
	case 'A':	outtype = FMT_ATK; break;
	case 'B':	outtype = FMT_FACE; break;
	case 'F':	outtype = FMT_FBM; break;
	case 'G':	outtype = FMT_GIF; break;
	case 'I':	outtype = FMT_IFF; break;
	case 'L':	outtype = FMT_LEAF; break;
	case 'M':	outtype = FMT_MCP; break;
	case 'P':	outtype = FMT_PBM; break;
	case 'R':	outtype = FMT_RLE; break;
	case 'S':	outtype = FMT_SUN; break;
	case 'T':	outtype = FMT_TIFF; break;
	case 'X':	outtype = FMT_X11; break;
	case 'Z':	outtype = FMT_PCX; break;
	default:	fprintf (stderr, "%s\n", USAGE);
			exit (1);
      }
    }
  }
  
  /* Check arguments */
  if (colors > 256 || colors < 4)
  { fprintf (stderr, "fbquant can only handle 4..256 colors\n");
    exit (1);
  }

  /* Choose default resbits: if using few colors, assume low-res */
  if (resbits < 0)
  { if (colors <= 32)	resbits = 4;
    else		resbits = 8;
  }

  if (resbits > 8 || resbits < 1)
  { fprintf (stderr, "color resolution (-r) must be 1..8 bits\n");
    exit (1);
  }

  resmask = (0xff - (0xff >> resbits));

  /* Open file if name given as argument */
  if (! read_bitmap (&input, (argc > 0) ? argv[0] : (char *) NULL))
  { exit (1); }

  /* Read colormap from map image (if specified) */
  if (mapfile != NULL)
  { if (!read_bitmap (&mapimage, mapfile))
    { perror (mapfile); exit (1); }
    
    if (mapimage.hdr.clrlen == 0)
    { fprintf (stderr, "fbquant: %s: no map\n", mapfile); exit (1); }
    
    /* Dont care about the map image, just its colormap */
    free (mapimage.bm); mapimage.bm = NULL;
    
    colors = mapimage.hdr.clrlen / 3;
    cmap = (COLOR *) malloc ((unsigned) colors * sizeof (COLOR));
    
    for (c=0; c<colors; c++)
    { cmap[c].rd = mapimage.cm[c];
      cmap[c].gr = mapimage.cm[c+colors];
      cmap[c].bl = mapimage.cm[c+colors*2];
      cmap[c].indx = c;
    }
    
    fprintf (stderr, "Quantizing \"%s\" [%dx%d] with %d colors from %s\n",
	     input.hdr.title, input.hdr.cols, input.hdr.rows, colors, mapfile);
  }
  else
  { fprintf (stderr,
	     "Quantizing \"%s\" [%dx%d] with %d colors, %d bits/clr\n",
	     input.hdr.title, input.hdr.cols, input.hdr.rows, colors, resbits);
  }

  if (input.hdr.planes != 3 || input.hdr.bits != 8)
  { fprintf (stderr, "fbquant can only handle 24bit RGB inputs\n");
    exit (1);
  }

  /* Now build header for output bit map */
  output.hdr = input.hdr;
  output.hdr.planes = 1;
  output.hdr.clrlen = 3 * colors;
  
  /* Allocate space for output */
  alloc_fbm (&output);

  /* Build colormap by sampling and mcut if not specified */
  if (mapfile == NULL)
  {
    cmap = (COLOR *) malloc ((unsigned) colors * sizeof (COLOR));
    sample_image (&input, hist);
    build_colormap (hist, cmap, colors);
  }
  
  /* Use Floyd-Steinberg error diffusion to quantize using the new cmap */
  clr_quantize (&input, &output, cmap, colors);
  
  /* Write out the result */
  if (write_bitmap (&output, stdout, outtype))
  { exit (0); }

  exit (1);
}

/****************************************************************
 * sample_image:
 ****************************************************************/

sample_image (image, hist)
FBM *image;
int *hist;
{ register int i, j;
  register unsigned char *rp, *gp, *bp;
  int width = image->hdr.cols, height = image->hdr.rows;
  int rowlen = image->hdr.rowlen, plnlen = image->hdr.plnlen;
  int used=0;
  
  /* Clear the histogram */
  for (i=0; i<CUBSIZ; i++) hist[i] = 0;
  
  /* Now count colors */
  for (j=0; j<height; j++)
  { rp = &(image->bm[j*rowlen]);
    gp = rp+plnlen;
    bp = gp+plnlen;

    /* If resolution is greater than cube size, just count */
    if (resbits >= CUBITS)
    { for (i=0; i<width; i++)
      { if (++hist[ CLRINDEX8 (*rp++, *gp++, *bp++) ] == 1) used++; }
    }

    /* If resolution is less than cube size, ignore extra bits */
    else
    { for (i=0; i<width; i++)
      { if (++hist[ CLRINDEX8 (*rp++ & resmask,
			       *gp++ & resmask,
			       *bp++ & resmask) ] == 1)
	{ used++; }
      }
    }
  }
  
  /* Bonus for flesh color */
  if (flesh > 1.01 || flesh < 0.99)
  { register int diff = 0;
    int maxdiff = 0;
    
    maxdiff += (fr > 127) ? fr : 256 - fr;
    maxdiff += (fg > 127) ? fr : 256 - fr;
    maxdiff += (fb > 127) ? fr : 256 - fr;
    
    if (debug)
    { fprintf (stderr, "\nFlesh bonus %1.4lf, max diff %d\n",
	       flesh, maxdiff);
    }

    for (i=0; i<CUBSIZ; i++)
    { if (hist[i] > 0)
      { j = GETR8(i) - fr; j = abs (j); diff += j;
        j = GETG8(i) - fr; j = abs (j); diff += j;
	j = GETB8(i) - fr; j = abs (j); diff += j;
	
	if (maxdiff < 64)
        { hist[i] *= (flesh - ((flesh * diff) / maxdiff)); }
      }
    }
  }

  if (debug)
  { fprintf (stderr, "Done counting, used %d colors for %d pixels\n",
	     used, width*height);
  }
}

/****************************************************************
 * build_colormap:
 ****************************************************************/

# define SWAP(A,B) (t=A,A=B,B=t)

build_colormap (hist, cmap, colors)
int *hist;
COLOR *cmap;
int colors;
{ register int i, k;
  PIXEL box[CUBSIZ];
  register PIXEL *b;
  int used=0, t;

  /* Build the first box, encompassing all pixels */  
  for (b=box, i=0; i<CUBSIZ; i++)
  { b->color = i;
    k = hist[i];
    b->cnt = (k > MAXSHRT) ? MAXSHRT : k;
    b++;
  }
  
  /* Move all non-zero count pixels to the front of the list */
  for (i=0, used=0; i<CUBSIZ; i++)
  { if (box[i].cnt > 0) box[used++] = box[i]; }

  /*-------- Special case if we didnt need all colors --------*/
  if (used <= colors)
  {
    /* Copy the colors actually found */
    for (i=0; i<used; i++)
    { cmap[i].rd = GETR8 (box[i].color);
      cmap[i].gr = GETG8 (box[i].color);
      cmap[i].bl = GETB8 (box[i].color);
    }

    /* Set the rest to WHITE */
    for (; i<colors; i++)
    { cmap[i].rd = 255;
      cmap[i].gr = 255;
      cmap[i].bl = 255;
    }
  }
  
  else				/* Build sets all colors */
  { split_box (box, used, 0, colors, cmap); }
  
  /*----------------------------------------------------------------
   * Now arrange the colors in the desired order.  Sun convention is that
   * color 0 is white and color n-1 is black, so we sort by increasing
   * intensity (why not?) and then swap the lightest and darkest colors
   */

  /* Now sort 0..n-1 according to increasing intensity */
  qsort (cmap, colors, sizeof (* cmap), cmp_int);

  /* Make first color lightest and last color darkest */
  SWAP (cmap[0].rd, cmap[colors-1].rd);
  SWAP (cmap[0].gr, cmap[colors-1].gr);
  SWAP (cmap[0].bl, cmap[colors-1].bl);
  
  /* Set the output indices */
  for (i=0; i<colors; i++) { cmap[i].indx = i; }
}

/****************************************************************
 * split_box: Basic recursive part of Heckberts adaptive partitioning
 *	      algorithm.
 ****************************************************************/

split_box (box, boxlen, clr, numclr, cmap)
PIXEL *box;
int boxlen, clr, numclr;
COLOR *cmap;
{ int maxv[3], minv[3], numv[3];
  int pcnt[3][CUBSID];
  int sbox, snum, split, half, maxdif, dif;
  register PIXEL *top, *bot;
  int topw, botw;
  int red, grn, blu;
  register int i, c;

  /* If numclr exceeds boxlen, we are in trouble */
  if (numclr > boxlen)
  { fprintf (stderr, "boxlen %d is less numclr %d, panic!\n than",
	     boxlen, numclr);
    fflush (stderr);
    abort ();
  }

  /* Base case: only one color, assign the average for this cell */
  if (numclr <= 1)
  { red = box_avg_red (box, boxlen);
    grn = box_avg_grn (box, boxlen);
    blu = box_avg_blu (box, boxlen);
    
    /* Map x to x+4, because the histogram maps values to multiples of 8 */
    cmap[clr].rd = red + 4;
    cmap[clr].gr = grn + 4;
    cmap[clr].bl = blu + 4;
    
    /* Mask off unused bits for color resolution */
    cmap[clr].rd &= resmask;
    cmap[clr].gr &= resmask;
    cmap[clr].bl &= resmask;
    
    if (debug)
    { fprintf (stderr, "\t\tassigning color %d  <%d,%d,%d>  (%d)\n",
	       clr, cmap[clr].rd, cmap[clr].gr, cmap[clr].bl,
	       box_weight (box, boxlen));
    }
    
    return;
  }

  /* Gather statistics about the boxes contents */
  minv[RD] = minv[GR] = minv[BL] = CUBSID;
  maxv[RD] = maxv[GR] = maxv[BL] = 0;
  numv[RD] = numv[GR] = numv[BL] = 0;
  for (i=0; i<CUBSID; i++) { pcnt[RD][i] = pcnt[GR][i] = pcnt[BL][i] = 0; }
  
  for (i=0; i<boxlen; i++)
  { c = box[i].color;
    red = GETR(c); grn = GETG(c); blu = GETB(c);
    
    if (red < minv[RD]) minv[RD] = red;
    if (red > maxv[RD]) maxv[RD] = red;
    if (grn < minv[GR]) minv[GR] = grn;
    if (grn > maxv[GR]) maxv[GR] = grn;
    if (blu < minv[BL]) minv[BL] = blu;
    if (blu > maxv[BL]) maxv[BL] = blu;
    
    if (++pcnt[RD][red] == 1) numv[RD]++;
    if (++pcnt[GR][grn] == 1) numv[GR]++;
    if (++pcnt[BL][blu] == 1) numv[BL]++;
  }

  /* Special case, boxlen = numclr, just assign each box one color */
  if (boxlen == numclr)
  { for (i=0; i<boxlen; i++)
    { split_box (box+i, 1, clr+i, 1, cmap); }
    return;
  }

  /*
   * Pick a dimension to split.  Currently splits longest dimension
   * in RGB space.  Heckbert and other experts suggest splitting
   * dimension with greatest variance may work better.
   */

  split = -1; maxdif = -1;

  if ((dif = (maxv[RD] - minv[RD])) > maxdif) { maxdif = dif; split = RD; }
  if ((dif = (maxv[GR] - minv[GR])) > maxdif) { maxdif = dif; split = GR; }
  if ((dif = (maxv[BL] - minv[BL])) > maxdif) { maxdif = dif; split = BL; }
  
  /* Sort along the chosen dimension */
  switch (split)
  { case RD:	qsort (box, boxlen, sizeof (*box), cmp_red); break;
    case GR:	qsort (box, boxlen, sizeof (*box), cmp_grn); break;
    case BL:	qsort (box, boxlen, sizeof (*box), cmp_blu); break;
    default:	fprintf (stderr, "panic in split_box, split %d\n", split);
		fflush (stderr); fflush (stdout); abort ();
  }
  
  /* 
   * Split at the median, but make sure there are at least numclr/2
   * different colors on each side of the split, to avoid wasting
   * colors.
   *
   * Note: need to keep in mind that when the box is large, dont split
   *       too close to one edge.
   */
   
  half = numclr / 2;
  top = box;		bot = box + (boxlen-1);
  topw = top->cnt;	botw = bot->cnt;
  
  /* Set top and bot to point to min/max feasible splits */
  while ((top-box)+1 < half)		{ top++; topw += top->cnt; }
  while ((boxlen-(bot-box)) < half)	{ bot--; botw += bot->cnt; }

  /* Move top and bottom towards each other 1/8 of remaining distance */
  c = (bot-top) / 8;
  for (i=0; i<c; i++)			{ top++; topw += top->cnt; }
  for (i=0; i<c; i++)			{ bot--; botw += bot->cnt; }

  /* Now search for median */
  while (top < bot)
  { if (topw < botw)			{ top++; topw += top->cnt; }
    else				{ bot--; botw += bot->cnt; }
  }

  /* Decide which half gets the midpoint */
  if (topw > botw)			/* Median wants to go with top */
  { sbox = (top-box) + 1;
    snum = numclr - half;
  }
  else					/* Median wants to go with bottom */
  { sbox = (top - box);
    snum = half;
  }
  
  /* Handle boundary conditions with number of colors vs box size */
  if (sbox == 0) sbox++;
  else if (sbox == boxlen) sbox--;

  while (snum > sbox) snum--;
  while (numclr-snum > boxlen-sbox) snum++;

# ifndef OPTIMIZE
  /* Check for boundary condition errors */
  if (snum <= 0 || snum >= numclr)
  { fprintf (stderr, "panic, using zero colors for box\n");
    fflush (stderr);
    abort ();
  }

  if (boxlen-sbox < numclr-snum)
  { fprintf (stderr, "panic, about to used %d boxes for %d colors\n",
	     boxlen-sbox, numclr-snum);
    fflush (stderr);
    abort ();
  }

  if (sbox < snum)
  { fprintf (stderr, "panic, about to used %d boxes for %d colors\n",
	     sbox, snum);
    fflush (stderr);
    abort ();
  }
# endif

  if (debug)
  { int count = numclr, depth = 8;
    while (count > 0) { depth--; count /= 2; }
    for (i=0; i<depth; i++) fprintf (stderr, "  ");
    
    fprintf (stderr, "box [%d..%d|%d] r(%d,%d,%d) g(%d,%d,%d) b(%d,%d,%d) =>",
	     0, boxlen-1, numclr,
	     minv[RD], maxv[RD], numv[RD],
	     minv[GR], maxv[GR], numv[GR],
	     minv[BL], maxv[BL], numv[BL]);
    fprintf (stderr, " %c [%d..%d|%d] [%d..%d|%d]\n",
	     "RGB"[split], 0, sbox-1, snum, sbox, boxlen-1, numclr-snum);
  }

  /* Now recurse and split each sub-box */
  split_box (box,      sbox,          clr,      snum,        cmap);
  split_box (box+sbox, boxlen - sbox, clr+snum, numclr-snum, cmap);
}

/****************************************************************
 * box_weight: Determine the total count of a box
 ****************************************************************/

box_weight (box, boxlen)
register PIXEL *box;
register int boxlen;
{ register int sum = 0, i;

  for (i=0; i<boxlen; i++)
  { sum += box[i].cnt; }
  
  return (sum);
}

/****************************************************************
 * box_avg_red: Determine the average red value [0..255] of a box
 ****************************************************************/

box_avg_red (box, boxlen)
register PIXEL *box;
register int boxlen;
{ register int sum = 0, n = 0, i;

  for (i=0; i<boxlen; i++)
  { sum += GETR8(box[i].color); n++; }
  
  return (sum / n);
}

/****************************************************************
 * box_avg_grn: Determine the average grn value [0..255] of a box
 ****************************************************************/

box_avg_grn (box, boxlen)
register PIXEL *box;
register int boxlen;
{ register int sum = 0, n = 0, i;

  for (i=0; i<boxlen; i++)
  { sum += GETG8(box[i].color); n++; }
  
  return (sum / n);
}

/****************************************************************
 * box_avg_blu: Determine the average blu value [0..255] of a box
 ****************************************************************/

box_avg_blu (box, boxlen)
register PIXEL *box;
register int boxlen;
{ register int sum = 0, n = 0, i;

  for (i=0; i<boxlen; i++)
  { sum += GETB8(box[i].color); n++; }
  
  return (sum / n);
}


/****************************************************************
 * sort by increasing red ( then green and blue )
 ****************************************************************/

cmp_red (a, b)
PIXEL *a, *b;
{ register r;

  if (r = GETR(a->color) - GETR(b->color))
  { return (r); }
  
  if (r = GETG(a->color) - GETG(b->color))
  { return (r); }

  if (r = GETB(a->color) - GETB(b->color))
  { return (r); }
  
  return (0);
}

/****************************************************************
 * sort by increasing green ( then blue and red )
 ****************************************************************/

cmp_grn (a, b)
PIXEL *a, *b;
{ register r;

  if (r = GETG(a->color) - GETG(b->color))
  { return (r); }

  if (r = GETB(a->color) - GETB(b->color))
  { return (r); }
  
  if (r = GETR(a->color) - GETR(b->color))
  { return (r); }
  
  return (0);
}

/****************************************************************
 * sort by increasing blue ( then red and green )
 ****************************************************************/

cmp_blu (a, b)
PIXEL *a, *b;
{ register r;

  if (r = GETB(a->color) - GETB(b->color))
  { return (r); }
  
  if (r = GETR(a->color) - GETR(b->color))
  { return (r); }
  
  if (r = GETG(a->color) - GETG(b->color))
  { return (r); }

  return (0);
}

/****************************************************************
 * clr_quantize: Do Floyd Steinberg quantizing on the image
 ****************************************************************/

clr_quantize (input, output, cmap, colors)
FBM *input, *output;
COLOR *cmap;
int colors;
{ int **cerr, **lerr, **terr;
  int width = input->hdr.cols, height = input->hdr.rows;
  int rowlen = input->hdr.rowlen, plnlen = input->hdr.plnlen;
  register int i, j;
  register int rd, gr, bl;
  int rderr, grerr, blerr, clr;
  unsigned char *rp, *gp, *bp, *obm;

  /*-------- Copy colormap into output bitmap --------*/
  for (i=0; i<colors; i++)
  { output->cm[i]               = cmap[i].rd;
    output->cm[i+colors]        = cmap[i].gr;
    output->cm[i+colors+colors] = cmap[i].bl;
  }

  /*
   * Precompute necessary nearest neighbor query info.  Note that we wait
   * until after copying the colormap, since init reorders it
   */

  init_nearest (cmap, colors);

  /*-------- Do halftoning --------*/
  cerr = (int **) malloc (3 * sizeof (int *));
  lerr = (int **) malloc (3 * sizeof (int *));
  cerr[RD] = (int *) malloc (width * sizeof (int));
  cerr[GR] = (int *) malloc (width * sizeof (int));
  cerr[BL] = (int *) malloc (width * sizeof (int));
  lerr[RD] = (int *) malloc (width * sizeof (int));
  lerr[GR] = (int *) malloc (width * sizeof (int));
  lerr[BL] = (int *) malloc (width * sizeof (int));
  
  /*-------- Just use the nearest color everywhere --------*/
  if (!dither)
  {
    for (j=0; j<height; j++)
    { rp = &(input->bm[j*rowlen]);
      gp = rp+plnlen;
      bp = gp+plnlen;
      obm = &(output->bm[j*rowlen]);

      for (i=0; i<width; i++)
      { rd = *rp++; gr = *gp++; bl = *bp++;

        obm[i] = cmap[nearest (rd, gr, bl, cmap, colors)].indx;
      }
    }
    
    return;
  }

  /*------ Just use the nearest color around the left, right, and top ------*/

  /* Top border */
  rp = input->bm; gp = rp+plnlen; bp = gp+plnlen;
  obm = output->bm;
  for (i=0; i<width; i++)
  { obm[i] = cmap[nearest (rp[i], gp[i], bp[i], cmap, colors)].indx; }

  /* Left border */
  rp = input->bm; gp = rp+plnlen; bp = gp+plnlen;
  obm = output->bm;
  for (j=0; j<height; j++)
  { obm[j*rowlen] = cmap[nearest (rp[j*rowlen], gp[j*rowlen],
			 bp[j*rowlen], cmap, colors)].indx; }

  /* Right border */
  rp = &(input->bm[width-1]); gp = rp + plnlen; bp = gp + plnlen;
  obm = &(output->bm[width-1]);
  for (j=0; j<height; j++)
  { obm[j*rowlen] = cmap[nearest (rp[j*rowlen], gp[j*rowlen],
				  bp[j*rowlen], cmap, colors)].indx; }

  /*-------- Clear error vectors --------*/
  for (i=0; i<width; i++)
  { cerr[RD][i] = cerr[GR][i] = cerr[BL][i] = 0;
    lerr[RD][i] = lerr[GR][i] = lerr[BL][i] = 0;
  }

  /*-------- Major loop for Floyd-Steinberg --------*/
  for (j=1; j<height; j++)
  { rp = &(input->bm[j*rowlen+1]);
    gp = rp+plnlen;
    bp = gp+plnlen;
    obm = &(output->bm[j*rowlen+1]);

    for (i=1; i<width-1; i++)
    { rd = *rp++; gr = *gp++; bl = *bp++;

      /* Sum up errors using Floyd-Steinberg weights */
      rderr= cerr[RD][i-1] + 7*lerr[RD][i-1] + 5*lerr[RD][i] + 3*lerr[RD][i+1];
      grerr= cerr[GR][i-1] + 7*lerr[GR][i-1] + 5*lerr[GR][i] + 3*lerr[GR][i+1];
      blerr= cerr[BL][i-1] + 7*lerr[BL][i-1] + 5*lerr[BL][i] + 3*lerr[BL][i+1];

      rderr >>= 4;	/* Divide by 16 */
      grerr >>= 4;	/* Divide by 16 */
      blerr >>= 4;	/* Divide by 16 */

      /* Choose nearest color to adjusted RGB value */
      rd += rderr; gr += grerr; bl += blerr;

      clr = nearest (rd, gr, bl, cmap, colors);
      *obm++ = cmap[clr].indx;

      /* Compute accumulated error for this pixel */      
      rd -= (int) cmap[clr].rd;
      gr -= (int) cmap[clr].gr;
      bl -= (int) cmap[clr].bl;
      
      /* Limit error (avoids color shadows) to 75% if over MAXERR */
      if (rd < -MAXERR || rd > MAXERR)  rd = (rd * 3) >> 2;
      if (gr < -MAXERR || gr > MAXERR)  gr = (gr * 3) >> 2;
      if (bl < -MAXERR || bl > MAXERR)  bl = (bl * 3) >> 2;

      /* Store errors in error vectors */
      cerr[RD][i] = rd;
      cerr[GR][i] = gr;
      cerr[BL][i] = bl;
    }
    
    /* Swap error vectors */
    terr = lerr; lerr = cerr; cerr = terr;
  }
}

/****************************************************************
 * nearest: Choose nearest color
 ****************************************************************/

short cache[CUBSIZ];

init_nearest (cmap, colors)
COLOR *cmap;
int colors;
{ register int i;

  /* Initialize the cache */
  for (i=0; i<CUBSIZ; i++) { cache[i] = -1; }
  
  /* Sort the colormap by decreasing red, then green, then blue */
  qsort (cmap, colors, sizeof (COLOR), cmp_cmap);

  if (debug || showcolor)
  { fprintf (stderr, "\nFinal colormap:\n\n");
    for (i=0; i<colors; i++)
    { fprintf (stderr, "%3d:  <%3d,%3d,%3d> [%d]\n", 
	       i, cmap[i].rd, cmap[i].gr, cmap[i].bl, cmap[i].indx);
    }
  }
}

/* Fast square macro, uses local variable to avoid mulitple eval of X */
# define SQR(X) (sqrtmp = (X), sqrtmp*sqrtmp)

/* Fast distance macro */
# define CDIST(CPTR,CR,CG,CB)		\
(sumtmp =  SQR (((int) ((CPTR)->rd)) - CR),		\
 sumtmp += SQR (((int) ((CPTR)->gr)) - CG),		\
 sumtmp += SQR (((int) ((CPTR)->bl)) - CB),		\
 sumtmp)

# define restrict(X) ((X) < 0 ? 0 : (X) > 255 ? 255 : (X))

nearest (rd, gr, bl, cmap, colors)
int rd, gr, bl;
COLOR *cmap;
int colors;
{ register int clr, sqrtmp, sumtmp;
  register COLOR *a, *b, *c, *best, *ctail;
  int cindx, bestd, dist;

  rd = restrict (rd);
  gr = restrict (gr);
  bl = restrict (bl);

  /* Find array index in cache */
  cindx = CLRINDEX8 (rd, gr, bl);

  /* Check cache for color value */  
  if ((clr = cache[cindx]) >= 0)
  { return (clr); }
  
  /*
   * Search through colormap for nearest neighbor:
   * 1) Find nearest red value by binary search
   * 2) Search up and down, keeping best point
   * 3) Terminate search when red difference is greater than best pt
   */
  
  /* Binary search for nearest red value */
  ctail = &cmap[colors];
  for (a=cmap, b= ctail-1;  a<b;  )  
  { c = a + (b-a)/2;	/* Find midpoint */

    if (debug && verbose)
    { fprintf (stderr, "Searching for %d, a=%d (%d), b=%d (%d), c=%d (%d)\n",
		rd, a-cmap, a->rd, b-cmap, b->rd, c-cmap, c->rd);
    }

    if (c->rd == rd)
    { break; }
    else if (c->rd < rd)
    { a = ++c; }
    else
    { b = c; }
  }

  /*
   * c now points to closest red value, search up and down for closer
   * Euclidean distance, and stop search when the red distance alone
   * exceeds the best found.
   */

  /* Set best and bestd to best red value */
  best = c;
  bestd = CDIST (c, rd, gr, bl);

  if (debug && verbose)
    fprintf (stderr, "Found c=%d (%d)  dist = %d\n", c-cmap, c->rd, bestd);

  /* a and b are search pointers up and down */
  a = c-1;
  b = c+1;

  while (bestd > 0 && (a >= cmap || b < ctail))
  {
    if (debug && verbose)
    { fprintf (stderr, "  search: bestd %d, best %d|%d, a %d, b %d\n",
		bestd, best-cmap, best->indx, a-cmap, b-cmap);
    }

    if (a >= cmap)
    { if ((dist = CDIST (a, rd, gr, bl)) < bestd)
      { best = a; bestd = dist; }
      
      if (SQR ((int) a->rd - rd) >= bestd) a = cmap-1;
      else				   a--;
    }

    if (b < ctail)
    { if ((dist = CDIST (b, rd, gr, bl)) < bestd)
      { best = b; bestd = dist; }
      
      if (SQR ((int) b->rd - rd) >= bestd) b = ctail;
      else				   b++;
    }
  }
  
  if (best < cmap || best >= ctail)
  { perror ("returning invalid color\n");
    abort ();
  }

  clr = (best - cmap);
  
  if (debug && verbose)
  { fprintf (stderr, "Nearest(%3d,%3d,%3d) => <%3d,%3d,%3d> [%d]\n",
		rd, gr, bl, best->rd, best->gr, best->bl, best->indx);
  }

  return ((cache[cindx] = clr));  
}

/****************************************************************
 * sort colormap by increasing red ( then green and blue )
 ****************************************************************/

cmp_cmap (a, b)
register COLOR *a, *b;
{ register int r;

  if (r = (a->rd - b->rd)) { return (r); }
  if (r = (a->gr - b->gr)) { return (r); }
  if (r = (a->bl - b->bl)) { return (r); }
  
  return (0);
}

/****************************************************************
 * sort colormap by increasing intensity (Use NTSC weights)
 ****************************************************************/

# define RW 299
# define GW 587
# define BW 114

cmp_int (a, b)
register COLOR *a, *b;
{ register int ai, bi;

  ai = RW * a->rd + GW * a->gr + BW * a->bl;
  bi = RW * b->rd + GW * b->gr + BW * b->bl;

  return (ai - bi);
}