
static char rcsid[] = "@(#)$Header: boxfilter.c";


#include "boxfilter.h"

double fabs(double x);

inline float square(float x)
{
  return x*x;
}

/*
  No longer needed -- defined in misc.h
  
  #define SWAP(a, b)  ({typeof (a) __tmp__; __tmp__ = (a); (a) = (b); (b) = __tmp__;})
*/



float BoxFilterApproxSigma;



FloatImage boxFilterFloat(FloatImage img, int a, int b, int c, int d)
{
  return boxFilterFloatUsingStorage(img, a, b, c, d, (FloatImage) NULL);
}


FloatImage boxFilterFloatUsingStorage(FloatImage img, int a, int b, int c, int d,
				      FloatImage out)
{
  FloatVector cs;
  register float sum;
  int width;
  float *cs_begin, *cs_end, *cs_scan;
  float *output;
  float *end, *scan, *begin, *addsub;
  
  
  if (img == (FloatImage) NULL) {
    return (FloatImage) NULL;
  }

  if (a < 0  ||  b < 0  ||  c < 0  ||  d < 0) {
    return (FloatImage) NULL;
  }

  if (out == (FloatImage) NULL) {
    out = (FloatImage) imNewOffset(IMAGE_FLOAT, imGetWidth(img), imGetHeight(img),
				   imGetXBase(img), imGetYBase(img));

    if (out == (FloatImage) NULL) {
      return (FloatImage) NULL;
    }
  }
  else if (imGetXBase(img)  != imGetXBase(out)  ||
	   imGetYBase(img)  != imGetYBase(out)  ||
	   imGetWidth(img)  != imGetWidth(out)  ||
	   imGetHeight(img) != imGetHeight(out)) {
    return (FloatImage) NULL;
  }

  cs = (FloatVector) vecNewOffset(VECTOR_FLOAT, imGetWidth(img), imGetXBase(img));

  if (cs == (FloatVector) NULL) {
    return (FloatImage) NULL;
  }

 
  width    = imGetWidth(img);		     /* set up some variables */
  cs_begin = vecGetStore(cs);
  cs_end   = cs_begin + width;
  begin    = imGetStore(img);
  
  vecInit(cs, 0.0);			     /* initialize the column sums */
  for (addsub = begin, end = begin + width*d; addsub < end; ) {
    for (cs_scan = cs_begin; cs_scan < cs_end; ) {
      *(cs_scan++) += *(addsub++);
    }
  }

  end    = begin + imGetStoreLen(img);
  scan   = begin;
  output = imGetStore(out);
  for ( ; scan < end; scan += width) {
    if ((addsub = scan - width*(c + 1)) >= begin) { /* subtract out old row */
      for (cs_scan = cs_begin; cs_scan < cs_end; ) {
	*(cs_scan++) -= *(addsub++);
      }      
    }

    if ((addsub = scan + width*d) < end) {   /* add in new row */
      for (cs_scan = cs_begin; cs_scan < cs_end; ) {
	*(cs_scan++) += *(addsub++);
      }      
    }

    sum = 0.0;				     /* initialize sum */
    for (cs_scan = cs_begin; cs_scan < cs_begin + b; ) {
      sum += *(cs_scan++);
    }
    
    for (cs_scan = cs_begin; cs_scan < cs_end; cs_scan++) {
      if (cs_scan - a - 1 >= cs_begin) {
	sum -= *(cs_scan - a - 1);
      }

      if (cs_scan + b < cs_end) {
	sum += *(cs_scan + b);
      }

      *(output++) = sum;
    }
  }

  vecFree(cs);

  return out;  
}


FloatImage boxFilterGray(GrayImage img, int a, int b, int c, int d)
{
  return boxFilterGrayUsingStorage(img, a, b, c, d, (FloatImage) NULL);
}


FloatImage boxFilterGrayUsingStorage(GrayImage img, int a, int b, int c, int d,
				     FloatImage out)
{
  FloatVector cs;
  register float sum;
  int width;
  float *cs_begin, *cs_end, *cs_scan;
  float *output;
  uchar *end, *scan, *begin, *addsub;
  
  
  if (img == (GrayImage) NULL) {
    return (FloatImage) NULL;
  }

  if (a < 0  ||  b < 0  ||  c < 0  ||  d < 0) {
    return (FloatImage) NULL;
  }

  if (out == (FloatImage) NULL) {
    out = (FloatImage) imNewOffset(IMAGE_FLOAT, imGetWidth(img), imGetHeight(img),
				   imGetXBase(img), imGetYBase(img));

    if (out == (FloatImage) NULL) {
      return (FloatImage) NULL;
    }
  }
  else if (imGetXBase(img)  != imGetXBase(out)  ||
	   imGetYBase(img)  != imGetYBase(out)  ||
	   imGetWidth(img)  != imGetWidth(out)  ||
	   imGetHeight(img) != imGetHeight(out)) {
    return (FloatImage) NULL;
  }

  cs = (FloatVector) vecNewOffset(VECTOR_FLOAT, imGetWidth(img), imGetXBase(img));

  if (cs == (FloatVector) NULL) {
    return (FloatImage) NULL;
  }

 
  width    = imGetWidth(img);		     /* set up some variables */
  cs_begin = vecGetStore(cs);
  cs_end   = cs_begin + width;
  begin    = imGetStore(img);
  
  vecInit(cs, 0.0);			     /* initialize the column sums */
  for (addsub = begin, end = begin + width*d; addsub < end; ) {
    for (cs_scan = cs_begin; cs_scan < cs_end; ) {
      *(cs_scan++) += (float) *(addsub++);
    }
  }

  end    = begin + imGetStoreLen(img);
  scan   = begin;
  output = imGetStore(out);
  for ( ; scan < end; scan += width) {
    if ((addsub = scan - width*(c + 1)) >= begin) { /* subtract out old row */
      for (cs_scan = cs_begin; cs_scan < cs_end; ) {
	*(cs_scan++) -= (float) *(addsub++);
      }      
    }

    if ((addsub = scan + width*d) < end) {   /* add in new row */
      for (cs_scan = cs_begin; cs_scan < cs_end; ) {
	*(cs_scan++) += (float) *(addsub++);
      }      
    }

    sum = 0.0;				     /* initialize sum */
    for (cs_scan = cs_begin; cs_scan < cs_begin + b; ) {
      sum += *(cs_scan++);
    }
    
    for (cs_scan = cs_begin; cs_scan < cs_end; cs_scan++) {
      if (cs_scan - a - 1 >= cs_begin) {
	sum -= *(cs_scan - a - 1);
      }

      if (cs_scan + b < cs_end) {
	sum += *(cs_scan + b);
      }

      *(output++) = sum;
    }
  }

  vecFree(cs);

  return out;  
}



#include "approximations.h"		     /* lookup table for sigma -> params */
					     /* defines: PARAMS params[]; int nparams; */
					     /* MXSIGMA, MXPASS, SCALE */


static inline PARAMS *lookup_params(float sigma)
{
  int index = (int) (sigma * SCALE + 0.5);
  static PARAMS p;
  

  if (index < 0  ||  index >= nparams) {
    return (PARAMS *) NULL;
  }

  p = params[index];
  return &p;
}



FloatImage boxFilterGraySigma(GrayImage img, float sigma)
{
  return boxFilterGraySigmaUsingStorage(img, sigma, NULL);
}


FloatImage boxFilterFloatSigma(FloatImage img, float sigma)
{
  return boxFilterFloatSigmaUsingStorage(img, sigma, NULL);
}


FloatImage boxFilterGraySigmaUsingStorage(GrayImage img, float sigma, FloatImage out)
{
  PARAMS *pp;
  FloatImage tmp = (FloatImage) NULL;
  FloatImage rslt = (FloatImage) NULL;
  FloatImage out_orig = out;
  int pass, passes;
  int a=0, b=0, c=0, d=0;
  float scale;
  int neven;

  
  if (img == (GrayImage) NULL  ||
      (out != (FloatImage) NULL  &&  (imGetXBase(img)  != imGetXBase(out)  ||
				      imGetYBase(img)  != imGetYBase(out)  ||
				      imGetWidth(img)  != imGetWidth(out)  ||
				      imGetHeight(img) != imGetHeight(out)))) {
    return (FloatImage) NULL;
  }

  pp = lookup_params(sigma);
  
  /* tolerate 15% deviation */
  if (pp == NULL  ||  fabs(sigma - pp->sigma)/sigma > 0.15) { 
    return (FloatImage) NULL;
  }
  
  BoxFilterApproxSigma = pp->sigma;	     /* save away actual sigma */
    
  scale = 1.0;
  for (passes = 0; passes < MXPASS  &&  pp->wlst[passes] > 1; passes++) {
    scale *= square(pp->wlst[passes]);
  }
  scale = 1.0 / scale;

  if ((passes & 1) == 0) {		     /* SWAPS only really needed when */
    SWAP(tmp, out);			     /* out != NULL on entry */
  }

  /* neven keeps # even width==height box filters performed so far. 
     each gives 1/2 pixel group delay in +-x,+-y => must be an
     even number.  we make individual group delays add to zero.  */

  for (pass = 0, neven = 0; pass < passes; pass++) {
    if (pp->wlst[pass] & 0x1) {              /* odd length => easy case */
      a = b = c = d = (pp->wlst[pass] - 1) >> 1;
    }
    else {                                   /* even length */
      switch (neven & 3) {
      case 0:                                /* down and right 1/2 pixel */
        a = c = pp->wlst[pass] >> 1;
        b = d = pp->wlst[pass] - a - 1;
        break;

      case 1:                                /* up and left 1/2 pixel */
        b = d = pp->wlst[pass] >> 1;
        a = c = pp->wlst[pass] - b - 1;
        break;

      case 2:                                /* down and left 1/2 pixel */
        b = c = pp->wlst[pass] >> 1;
        a = d = pp->wlst[pass] - b - 1;
        break;

      case 3:                                /* up and right 1/2 pixel */
        a = d = pp->wlst[pass] >> 1;
        b = c = pp->wlst[pass] - a - 1;
        break;
      }

      neven++;
    }

    if (pass == 0) {
      rslt = boxFilterGrayUsingStorage(img, a, b, c, d, out);
    }
    else {
      rslt = boxFilterFloatUsingStorage(tmp, a, b, c, d, out);
    }

    if (rslt == (FloatImage) NULL) {
      if (out_orig)
        imFree(out_orig);
      return (FloatImage) NULL;
    }      

    out = rslt;
    SWAP(out, tmp);
  }

  assert((neven & 0x1) == 0);                /* must be even # */

  SWAP(out, tmp);
  
  if (tmp)				     /* free tmp */
    imFree(tmp);

  { register float *scan, *end;		     /* rescale output */

    scan  = imGetStore(out);
    end   = scan + imGetStoreLen(out);

    while (scan < end) {
      *(scan++) *= scale;
    }
  }

  return out;
}


FloatImage boxFilterFloatSigmaUsingStorage(FloatImage img, float sigma, FloatImage out)
{
  PARAMS *pp;
  FloatImage tmp = (FloatImage) NULL;
  FloatImage rslt = (FloatImage) NULL;
  FloatImage out_orig = out;
  int pass, passes;
  int a=0, b=0, c=0, d=0;
  float scale;
  int neven;


  if (img == (FloatImage) NULL  ||
      (out != (FloatImage) NULL  &&  (imGetXBase(img)  != imGetXBase(out)  ||
				      imGetYBase(img)  != imGetYBase(out)  ||
				      imGetWidth(img)  != imGetWidth(out)  ||
				      imGetHeight(img) != imGetHeight(out)))) {
    return (FloatImage) NULL;
  }

  pp = lookup_params(sigma);

  /* tolerate 15% deviation */
  if (pp == NULL  ||  fabs(sigma - pp->sigma)/sigma > 0.15) {  
    return (FloatImage) NULL;
  }

  BoxFilterApproxSigma = pp->sigma;	     /* save away actual sigma */
    
  scale = 1.0;
  for (passes = 0; passes < MXPASS  &&  pp->wlst[passes] > 1; passes++) {
    scale *= square(pp->wlst[passes]);
  }
  scale = 1.0 / scale;

  if ((passes & 1) == 0) {		     /* SWAPS only really needed when */
    SWAP(tmp, out);			     /* out != NULL on entry */
  }

  /* neven keeps # even width==height box filters performed so far. 
     each gives 1/2 pixel group delay in +-x,+-y => must be an
     even number.  we make individual group delays add to zero.  */

  for (pass = 0, neven = 0; pass < passes; pass++) {
    if (pp->wlst[pass] & 0x1) {              /* odd length => easy case */
      a = b = c = d = (pp->wlst[pass] - 1) >> 1;
    }
    else {                                   /* even length */
      switch (neven & 3) {
      case 0:                                /* down and right 1/2 pixel */
        a = c = pp->wlst[pass] >> 1;
        b = d = pp->wlst[pass] - a - 1;
        break;

      case 1:                                /* up and left 1/2 pixel */
        b = d = pp->wlst[pass] >> 1;
        a = c = pp->wlst[pass] - b - 1;
        break;

      case 2:                                /* down and left 1/2 pixel */
        b = c = pp->wlst[pass] >> 1;
        a = d = pp->wlst[pass] - b - 1;
        break;

      case 3:                                /* up and right 1/2 pixel */
        a = d = pp->wlst[pass] >> 1;
        b = c = pp->wlst[pass] - a - 1;
        break;
      }

      neven++;
    }

    if (pass == 0) {
      rslt = boxFilterFloatUsingStorage(img, a, b, c, d, out);
    }
    else {
      rslt = boxFilterFloatUsingStorage(tmp, a, b, c, d, out);
    }

    if (rslt == (FloatImage) NULL) {
      if (out_orig)
        imFree(out_orig);
      return (FloatImage) NULL;
    }      

    out = rslt;
    SWAP(out, tmp);
  }

  assert((neven & 0x1) == 0);                /* must be even # */

  SWAP(out, tmp);
  
  if (tmp)				     /* free tmp */
    imFree(tmp);

  { register float *scan, *end;		     /* rescale output */

    scan  = imGetStore(out);
    end   = scan + imGetStoreLen(out);

    while (scan < end) {
      *(scan++) *= scale;
    }
  }

  return out;
}
