

#include "gradient.h"


double sqrt(double x);


int gradientFull(FloatImage img, FloatImage *dx, FloatImage *dy, FloatImage *mag)
{
  register float *imgp, *dxp, *dyp, *magp, *img_xendp;
  register float x, y;
  register long width;
  register float a, b, c, d;
  register float *img_endp;
  register long height;
  int made_dx = 0, made_dy = 0, made_mag = 0;

  
  if (img == (FloatImage) NULL    ||  dx == (FloatImage *) NULL  ||
      dy  == (FloatImage *) NULL  ||  mag == (FloatImage *) NULL)
    return -1;

  width  = imGetWidth(img);
  height = imGetHeight(img);

  if (*dx == (FloatImage) NULL) {
    if ((*dx = imNew(IMAGE_FLOAT, width, height)) == (FloatImage) NULL)
      goto gradient_error;

    made_dx = 1;
  }
  else if (imGetWidth(*dx) != width  ||  imGetHeight(*dx) != height)
    goto gradient_error;

  if (*dy == (FloatImage) NULL) {
    if ((*dy = imNew(IMAGE_FLOAT, width, height)) == (FloatImage) NULL)
      goto gradient_error;

    made_dy = 1;
  }
  else if (imGetWidth(*dy) != width  ||  imGetHeight(*dy) != height)
    goto gradient_error;

  if (*mag == (FloatImage) NULL) {
    if ((*mag = imNew(IMAGE_FLOAT, width, height)) == (FloatImage) NULL)
      goto gradient_error;

    made_mag = 1;
  }
  else if (imGetWidth(*mag) != width  ||  imGetHeight(*mag) != height)
    goto gradient_error;

  imgp = imGetStore(img);
  dxp  = imGetStore(*dx);
  dyp  = imGetStore(*dy);
  magp = imGetStore(*mag);
  
  for (img_endp = imgp + width*(height-1) - 1; imgp < img_endp; ) {
    a = *imgp;
    c = *(imgp+width);
    
    for (img_xendp = imgp + width - 1; imgp < img_xendp; ) { /* this really works! */
      imgp++;

      b = *(imgp);
      d = *(imgp+width);

      a = d - a;
      c = b - c;
      x = a + c;
      y = a - c;

      a = b;
      c = d;
      
      *(dxp++)  = x;
      *(dyp++)  = y;
      *(magp++) = x*x + y*y;
    }

    imgp++;				     /* last column gets 0.0 */
    *(dxp++) = *(dyp++) = *(magp++) = 0.0;
  }

  for (img_endp += width; imgp <= img_endp; imgp++) /* last row gets 0.0 */
    *(dxp++) = *(dyp++) = *(magp++) = 0.0;

  return 0;
  
 gradient_error:
  if (made_dx)  imFree(*dx);
  if (made_dy)  imFree(*dy);
  if (made_mag) imFree(*mag);
  return -1;
}


int gradientX(FloatImage img, FloatImage *dx)
{
  register float *imgp, *dxp, *img_xendp;
  register long width;
  register float a, b, c, d;
  register float *img_endp;
  register long height;
  int made_dx = 0;

  
  if (img == (FloatImage) NULL  ||  dx == (FloatImage *) NULL)
    return -1;

  width  = imGetWidth(img);
  height = imGetHeight(img);

  if (*dx == (FloatImage) NULL) {
    if ((*dx = imNew(IMAGE_FLOAT, width, height)) == (FloatImage) NULL)
      goto gradient_error;

    made_dx = 1;
  }
  else if (imGetWidth(*dx) != width  ||  imGetHeight(*dx) != height)
    goto gradient_error;

  imgp = imGetStore(img);
  dxp  = imGetStore(*dx);
  
  for (img_endp = imgp + width*(height-1) - 1; imgp < img_endp; ) {
    a = *imgp;
    c = *(imgp+width);
    
    for (img_xendp = imgp + width - 1; imgp < img_xendp; ) {
      imgp++;

      b = *(imgp);
      d = *(imgp+width);
      
      *(dxp++) = d - a + b - c;
      
      a = b;
      c = d;
    }

    imgp++;				     /* last column gets 0.0 */
    *(dxp++) = 0.0;
  }

  for (img_endp += width; imgp <= img_endp; imgp++) /* last row gets 0.0 */
    *(dxp++) = 0.0;

  return 0;
  
 gradient_error:
  if (made_dx)  imFree(*dx);
  return -1;
}


int gradientY(FloatImage img, FloatImage *dy)
{
  register float *imgp, *dyp, *img_xendp;
  register long width;
  register float a, b;
  register float *img_endp;
  register long height;
  int made_dy = 0;

  
  if (img == (FloatImage) NULL  ||  dy == (FloatImage *) NULL)
    return -1;

  width  = imGetWidth(img);
  height = imGetHeight(img);

  if (*dy == (FloatImage) NULL) {
    if ((*dy = imNew(IMAGE_FLOAT, width, height)) == (FloatImage) NULL)
      goto gradient_error;

    made_dy = 1;
  }
  else if (imGetWidth(*dy) != width  ||  imGetHeight(*dy) != height)
    goto gradient_error;

  imgp = imGetStore(img);
  dyp  = imGetStore(*dy);
  
  for (img_endp = imgp + width*(height-1) - 1; imgp < img_endp; ) {
    a = *(imgp+width) - *imgp;
    
    for (img_xendp = imgp + width - 1; imgp < img_xendp; ) {
      imgp++;

      b = *(imgp+width) - *(imgp);

      *(dyp++) = a + b;

      a = b;
    }

    imgp++;				     /* last column gets 0.0 */
    *(dyp++) = 0.0;
  }

  for (img_endp += width; imgp <= img_endp; imgp++) /* last row gets 0.0 */
    *(dyp++) = 0.0;

  return 0;
  
 gradient_error:
  if (made_dy)  imFree(*dy);
  return -1;
}


int gradientXandY(FloatImage img, FloatImage *dx, FloatImage *dy)
{
  register float *imgp, *dxp, *dyp, *img_xendp;
  register float x, y;
  register long width;
  register float a, b, c, d;
  register float *img_endp;
  register long height;
  int made_dx = 0, made_dy = 0;

  
  if (img == (FloatImage) NULL    ||
      dx == (FloatImage *) NULL   ||  dy  == (FloatImage *) NULL)
    return -1;

  width  = imGetWidth(img);
  height = imGetHeight(img);

  if (*dx == (FloatImage) NULL) {
    if ((*dx = imNew(IMAGE_FLOAT, width, height)) == (FloatImage) NULL)
      goto gradient_error;

    made_dx = 1;
  }
  else if (imGetWidth(*dx) != width  ||  imGetHeight(*dx) != height)
    goto gradient_error;

  if (*dy == (FloatImage) NULL) {
    if ((*dy = imNew(IMAGE_FLOAT, width, height)) == (FloatImage) NULL)
      goto gradient_error;

    made_dy = 1;
  }
  else if (imGetWidth(*dy) != width  ||  imGetHeight(*dy) != height)
    goto gradient_error;

  imgp = imGetStore(img);
  dxp  = imGetStore(*dx);
  dyp  = imGetStore(*dy);
  
  for (img_endp = imgp + width*(height-1) - 1; imgp < img_endp; ) {
    a = *imgp;
    c = *(imgp+width);
    
    for (img_xendp = imgp + width - 1; imgp < img_xendp; ) {
      imgp++;

      b = *(imgp);
      d = *(imgp+width);

      a = d - a;
      c = b - c;
      x = a + c;
      y = a - c;

      a = b;
      c = d;
      
      *(dxp++)  = x;
      *(dyp++)  = y;
    }

    imgp++;				     /* last column gets 0.0 */
    *(dxp++) = *(dyp++) = 0.0;
  }

  for (img_endp += width; imgp <= img_endp; imgp++) /* last row gets 0.0 */
    *(dxp++) = *(dyp++) = 0.0;

  return 0;
  
 gradient_error:
  if (made_dx)  imFree(*dx);
  if (made_dy)  imFree(*dy);
  return -1;
}


int gradientMagSqrd(FloatImage img, FloatImage *mag)
{
  register float *imgp, *magp, *img_xendp;
  register float x, y;
  register long width;
  register float a, b, c, d;
  register float *img_endp;
  register long height;
  int made_mag = 0;

  
  if (img == (FloatImage) NULL  ||  mag == (FloatImage *) NULL)
    return -1;

  width  = imGetWidth(img);
  height = imGetHeight(img);

  if (*mag == (FloatImage) NULL) {
    if ((*mag = imNew(IMAGE_FLOAT, width, height)) == (FloatImage) NULL)
      goto gradient_error;

    made_mag = 1;
  }
  else if (imGetWidth(*mag) != width  ||  imGetHeight(*mag) != height)
    goto gradient_error;

  imgp = imGetStore(img);
  magp = imGetStore(*mag);
  
  for (img_endp = imgp + width*(height-1) - 1; imgp < img_endp; ) {
    a = *imgp;
    c = *(imgp+width);
    
    for (img_xendp = imgp + width - 1; imgp < img_xendp; ) {
      imgp++;

      b = *(imgp);
      d = *(imgp+width);

      a = d - a;
      c = b - c;
      x = a + c;
      y = a - c;

      a = b;
      c = d;
      
      *(magp++) = x*x + y*y;
    }

    imgp++;				     /* last column gets 0.0 */
    *(magp++) = 0.0;
  }

  for (img_endp += width; imgp <= img_endp; imgp++) /* last row gets 0.0 */
    *(magp++) = 0.0;

  return 0;
  
 gradient_error:
  if (made_mag) imFree(*mag);
  return -1;
}

int gradientMag(FloatImage img, FloatImage *mag)
{
  register float *imgp, *magp, *img_xendp;
  register float x, y;
  register long width;
  register float a, b, c, d;
  register float *img_endp;
  register long height;
  int made_mag = 0;

  
  if (img == (FloatImage) NULL  ||  mag == (FloatImage *) NULL)
    return -1;

  width  = imGetWidth(img);
  height = imGetHeight(img);

  if (*mag == (FloatImage) NULL) {
    if ((*mag = imNew(IMAGE_FLOAT, width, height)) == (FloatImage) NULL)
      goto gradient_error;

    made_mag = 1;
  }
  else if (imGetWidth(*mag) != width  ||  imGetHeight(*mag) != height)
    goto gradient_error;

  imgp = imGetStore(img);
  magp = imGetStore(*mag);
  
  for (img_endp = imgp + width*(height-1) - 1; imgp < img_endp; ) {
    a = *imgp;
    c = *(imgp+width);
    
    for (img_xendp = imgp + width - 1; imgp < img_xendp; ) {
      imgp++;

      b = *(imgp);
      d = *(imgp+width);

      a = d - a;
      c = b - c;
      x = a + c;
      y = a - c;

      a = b;
      c = d;
      
      *(magp++) = sqrt(x*x + y*y);
    }

    imgp++;				     /* last column gets 0.0 */
    *(magp++) = 0.0;
  }

  for (img_endp += width; imgp <= img_endp; imgp++) /* last row gets 0.0 */
    *(magp++) = 0.0;

  return 0;
  
 gradient_error:
  if (made_mag) imFree(*mag);
  return -1;
}
