

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


#include "smooth.h"
#include <math.h>


static inline double square(double x)	     /* square x */
{
  return (x*x);
}

static inline double gaussian(double s, double x) /* return GAUSS(SIGMA, X) */
{
  if (s == 0.0)
    return (x == 0.0 ? 1.0 : 0.0);
  else
    return (exp(-0.5*square(x/s))/(s*sqrt(2.0*M_PI)));
}


void smoothNormalizeMask(FloatVector mask)    /* normalize it to integrate to 1 */
{
  int x, len;
  float sum;


  len = vecGetLength(mask);
  sum = fabs(vecRef(mask, 0));
  for (x = 1; x < len; x++)
    sum += 2.0*fabs(vecRef(mask, x));
  
  for (x = 0; x < len; x++)
    vecRef(mask, x) /= sum;
}


FloatVector smoothMakeGaussianMask(float sigma, float width)
{
  int x, len;
  FloatVector mask;


  if (width <= 0.0  ||  width >= 20.0)
    width = 4.0;
  if (sigma < 0.0)
    sigma = 1.0;

  len  = (int) ceil(sigma*width) + 1;
  mask = (FloatVector) vecNew(VECTOR_FLOAT, len);

  if (mask == (FloatVector) NULL)
    return mask;

  for (x = 0; x < len; x++) {
    vecRef(mask, x) = gaussian(sigma, (float) x);
  }

  smoothNormalizeMask(mask);
  
  return mask;
}


AnyImage smooth_Image_(AnyImage input, FloatVector mask)
{
  return smoothImageUsingStorage(input, mask, imNULL, imNULL);
}

#define SETUP(TAG)                                                                     \
  if (intermed == (AnyImage) NULL) {                                                   \
    if ((intermed = (AnyImage) imNewOffset(TAG, height, width, 0, 0)) == NULL) {       \
      goto error;                                                                      \
    }                                                                                  \
    else {                                                                             \
      made_int = TRUE;                                                                 \
    }                                                                                  \
  }                                                                                    \
  else if (height != imGetWidth(intermed)  ||  width != imGetHeight(intermed)) {       \
    goto error;                                                                        \
  }                                                                                    \
                                                                                       \
  if (output == (AnyImage) NULL) {                                                     \
    if ((output = (AnyImage) imNewOffset(TAG, width, height, imGetXBase(input),        \
					 imGetYBase(input))) == NULL) {                \
      goto error;                                                                      \
    }                                                                                  \
    else {                                                                             \
      made_out = TRUE;                                                                 \
    }                                                                                  \
  }                                                                                    \
  else if (width != imGetWidth(output)  ||  height != imGetHeight(output)) {           \
    goto error;                                                                        \
  }                                                                                    \
                                                                                       \
  if (imGetType(intermed) != TAG  ||  imGetType(output) != TAG) {                      \
    goto error;                                                                        \
  }


#define SCALAR_ADD(A, B)     ((A) + (B))
#define SCALAR_MULT(A, B)    ((A) * (B))

#define RGB_ADD(A, B)     ({ RGB _a = (A), _b = (B);                                   \
			     ((RGBFloat) {_a.r + _b.r, _a.g + _b.g, _a.b + _b.b});})
#define RGB_MULT(A, B)    ({ float _a = (A); RGB _b = (B);                             \
			     ((RGBFloat) {_a * _b.r, _a * _b.g, _a * _b.b});})
#define RGBF_MULT(A, B)   ({ float _a = (A); RGBFloat _b = (B);                        \
			     ((RGBFloat) {_a * _b.r, _a * _b.g, _a * _b.b});})
#define RGBF_ADD(A, B)    ({ RGBFloat _a = (A), _b = (B);                              \
			     ((RGBFloat) {_a.r + _b.r, _a.g + _b.g, _a.b + _b.b});})


/* actually do the 2D convolution as two separate 1D convolutions */
/* this code is VERY hairy, especially now that its polymorphic.  */
/* See wjr's for an example of what it's really doing             */

#define SMOOTH(INTYPE, INDATA, OUTTYPE, OUTDATA, IADD, IMULT, OADD, OMULT)             \
  {                                                                                    \
    register INDATA *p_p;                                                              \
    register OUTDATA *p_s, *p_t;                                                       \
    register float *g;                                                                 \
    register int   z;                                                                  \
    register OUTDATA tmp;                                                              \
                                                                                       \
    /* 'i' stands for initial, 'm' stands for maximum */                               \
    /* 'p' is for pict, 's' is second step array,  */                                  \
    /* 't' is third step array */                                                      \
                                                                                       \
    register OUTDATA *s, *s_i_x, *s_m_x, *s_m_y, *t, *t_i_x;                           \
    register INDATA *pict, *p_m_y, *p_m_x, *p_i_x;                                     \
                                                                                       \
                                                                                       \
    pict = imGetStore(((INTYPE) input));                                               \
    s    = imGetStore(((OUTTYPE) intermed));                                           \
    t    = imGetStore(((OUTTYPE) output));                                             \
                                                                                       \
    for (p_m_y = (p_p = pict) + width*height, s_i_x = s; p_p < p_m_y; ) {              \
      for (p_i_x = p_p, p_m_x = p_p + size - 1, p_s = s_i_x++;                         \
	   p_p <= p_m_x;                                                               \
	   p_p++, p_s += height) {                                                     \
	for (z = 1, g = mask, tmp = IMULT(*(g++), *p_p); z <= size; z++)               \
	  tmp = OADD(tmp, OMULT(*(g++), IADD(*(p_p + z), *(MAX(p_p - z, p_i_x)))));    \
                                                                                       \
	*p_s = tmp;                                                                    \
      }                                                                                \
                                                                                       \
      for (p_m_x = p_i_x + width - size - 1; p_p <= p_m_x; p_p++, p_s += height) {     \
	for (z = 1, g = mask, tmp = IMULT(*(g++), *p_p); z <= size; z++)               \
	  tmp = OADD(tmp, OMULT(*(g++), IADD(*(p_p + z), *(p_p - z))));                \
                                                                                       \
	*p_s = tmp;                                                                    \
      }                                                                                \
                                                                                       \
      for (p_m_x = p_i_x + width - 1; p_p <= p_m_x; p_p++, p_s += height) {            \
	for (z = 1, g = mask, tmp = IMULT(*(g++), *p_p); z <= size; z++)               \
	  tmp = OADD(tmp, OMULT(*(g++), IADD(*(MIN(p_p + z, p_m_x)), *(p_p - z))));    \
                                                                                       \
	*p_s = tmp;                                                                    \
      }                                                                                \
    }                                                                                  \
                                                                                       \
    for (s_m_y = (p_s = s) + width*height, t_i_x = t; p_s < s_m_y; ) {                 \
      for (s_i_x = p_s, s_m_x = p_s + size - 1, p_t = t_i_x++;                         \
	   p_s <= s_m_x;                                                               \
	   p_s++, p_t += width) {                                                      \
	for (z = 1, g = mask, tmp = OMULT(*(g++), *p_s); z <= size; z++)               \
	  tmp = OADD(tmp, OMULT(*(g++), OADD(*(p_s + z), *(MAX(p_s - z, s_i_x)))));    \
                                                                                       \
	*p_t = tmp;                                                                    \
      }                                                                                \
                                                                                       \
      for (s_m_x = s_i_x + height - size - 1; p_s <= s_m_x; p_s++, p_t += width) {     \
	for (z = 1, g = mask, tmp = OMULT(*(g++), *p_s); z <= size; z++)               \
	  tmp = OADD(tmp, OMULT(*(g++), OADD(*(p_s + z), *(p_s - z))));                \
                                                                                       \
	*p_t = tmp;                                                                    \
      }                                                                                \
                                                                                       \
      for (s_m_x = s_i_x + height - 1; p_s <= s_m_x; p_s++, p_t += width) {            \
	for (z = 1, g = mask, tmp = OMULT(*(g++), *p_s); z <= size; z++)               \
	  tmp = OADD(tmp, OMULT(*(g++), OADD(*(MIN(p_s + z, s_m_x)), *(p_s - z))));    \
                                                                                       \
	*p_t = tmp;                                                                    \
      }                                                                                \
    }                                                                                  \
  }


AnyImage smooth_ImageUsingStorage_(AnyImage input, FloatVector fmask,
				   AnyImage intermed, AnyImage output)
{
  register float *mask = vecGetStore(fmask);
  register int    size = vecGetLength(fmask) - 1;
  int width  = imGetWidth(input);
  int height = imGetHeight(input);
  boolean made_int = FALSE, made_out = FALSE;

  
  if (2*size >= width  ||  2*size >= height) /* otherwise the convolution is silly */
    return (AnyImage) NULL;

  switch (imGetType(input)) {
  case IMAGE_FLOAT: {
    SETUP(IMAGE_FLOAT);
    SMOOTH(FloatImage, float, FloatImage, float,
	   SCALAR_ADD, SCALAR_MULT, SCALAR_ADD, SCALAR_MULT);
    break;
  }

  case IMAGE_GRAY: {
    SETUP(IMAGE_FLOAT);
    SMOOTH(GrayImage, uchar, FloatImage, float,
	   SCALAR_ADD, SCALAR_MULT, SCALAR_ADD, SCALAR_MULT);
    break;
  }

  case IMAGE_RGB: {
    SETUP(IMAGE_RGBF);
    SMOOTH(RGBImage, RGB, RGBFloatImage, RGBFloat,
	   RGB_ADD, RGB_MULT, RGBF_ADD, RGBF_MULT);
    break;
  }

  default:
    goto error;
    break;
  }

  if (made_int)    imFree(intermed);
  return output;

 error:
  if (made_int)    imFree(intermed);
  if (made_out)    imFree(output);
  return (AnyImage) NULL;
}


#undef SETUP
#undef SMOOTH_SCALAR
#undef SMOOTH_GEN
#undef RGB_ADD
#undef RGBF_MULT
#undef RGBF_ADD
#undef SCALAR_ADD
#undef SCALAR_MULT

