/********************************************************************
 * Project: CS664 Project 1            Justin Miller 
 * File: cannyEdge.c              CANNYEDGE.C
 *
 * Purpose: Implements the canny edge detector with NMS and one level
 *   thresholding in a callable procedure cannyEdge().
 * This File: 
 *   -the canny edge detector procedures
 *   -general use procedures for the Quad data structure
 * 
 * See Also: cannyEdge.h
 * Copyright (c) 1995 Cornell University.  All Rights Reserved.
 ********************************************************************/


#include <stdio.h>
#include "math.h"
#include "image.h"
#include "misc.h"
#include "dbug.h"
#include "cannyEdge.h"

/* PROTOTYPES FOR INTERNAL PROCEDURES*/
static void CalcDerivGrad(Quad q, FloatImage inimage);
static void CalcNMSPeaks(Quad q, float threshold);


/* PROCEDURES FOR QUADS*/


/* Quad createQuad(int x, int y)
 *
 * Creates a new Quad, which is a pointer to a structure that 
 *   holds three float images for the dx and dy partial derivatives 
 *   and the squared gradient magnitude, and one binary image for the 
 *   NMS peaks.
 * Input: x and y size for the images.
 * Output: The Quad with memory allocated for the four images.
 */

Quad createQuad(int x, int y){
  Quad q;
  
  q = malloc(sizeof(_Quad));
  q->dxPartial = (FloatImage)imNew(IMAGE_FLOAT, x, y);
  q->dyPartial = (FloatImage)imNew(IMAGE_FLOAT, x, y);
  q->sgMagnitude = (FloatImage)imNew(IMAGE_FLOAT, x, y);
  q->peaks = (BinaryImage)imNew(IMAGE_BINARY, x, y);

  return(q);
}


/* void destroyQuad(Quad q)
 *
 * Destroys a Quad by freeing the memory associated with the 
 *   three float images and the _Quad structure.
 * Input: a Quad
 * Output: (none)
 */
void destroyQuad(Quad q){
  
  imFree(q->dxPartial);
  imFree(q->dyPartial);
  imFree(q->sgMagnitude);
  free(q);
}

/* void killQuad(Quad q)
 *
 * Kills a Quad by freeing the memory associated with the 
 *   three float images and the _Quad structure.
 * Input: a Quad
 * Output: (none)
 */
void killQuad(Quad q){
  
  imFree(q->peaks);
  imFree(q->dxPartial);
  imFree(q->dyPartial);
  imFree(q->sgMagnitude);
  free(q);
}



/* PROCEDURES FOR CANNY EDGE DETECTOR*/


/* Quad cannyEdge(FloatImage inimage, float threshold)
 *
 * Performs the Canny Edge Detector Operation on an input float image.
 * See Also: CalcDerivGrad() and CalcNMSPeaks()
 * Input: a float image and a threshold for the magnitude of the
 *   gradient.
 * Output: a quad containing the partial derivatives with respect to the 
 *   x and y coordinates, the squared gradient magnitude, and a binary 
 *   image denoting the edge pixels.
 * Debug Output: Size of the image, and the threshold.
*/
Quad cannyEdge(FloatImage inimage, float threshold){
  Quad q;

  DEBUG("CANNY_EDGE: Input Size %d x %d \t Threshold = %f\n",
	(int)imGetWidth(inimage), (int)imGetHeight(inimage), threshold);
  q = createQuad((int)imGetWidth(inimage), (int)imGetHeight(inimage));
  CalcDerivGrad(q, inimage);
  CalcNMSPeaks(q, threshold);
  return(q);
}
  

/* static void CalcDerivGrad(Quad q, FloatImage inimage)
 *                       INTERNAL
 * Calculates the partial derivatives with respect to x and y using
 *   convolution with the unit doublet.  Note that the output image is 
 *   shifted by half a pixel to the left.  The last row and last column 
 *   are defined to have zero derivative.  The squared gradient magnitude
 *   is also calcuated.
 *         dxPartial(x,y) = I(x+1,y) - I(x,y)   or 0 for x = Max X
 *         dyPartial(x,y) = I(x,y+1) - I(x,y)   or 0 for y = Max Y
 *         sgMagnitude(x,y) = dxPartial(x,y)^2 + dyPartial(x,y)^2
 *
 * Input: a valid (already allocated) Quad, and the floating point image.
 * Output: (none). Directly modifies the Quad.
 * Debug Output: (none).
*/
static void CalcDerivGrad(Quad q, FloatImage inimage){
  int maxX, maxY;
  int x, y;
  float dx, dy;

  maxX = (int)imGetWidth(inimage) - 1;
  maxY = (int)imGetHeight(inimage) - 1;

  for(y = 0; y <= maxY; y++){
    imRef(q->dxPartial, maxX, y) = 0;
    imRef(q->dyPartial, maxX, y) = 0;
  }
  for(x = 0; x <= maxX; x++){
    imRef(q->dxPartial, x, maxY) = 0;
    imRef(q->dyPartial, x, maxY) = 0;
  }
  
  for(y = 0; y < maxY; y++){
    for(x = 0; x < maxX; x++){
      dx = imRef(q->dxPartial, x, y) = imRef(inimage, x+1, y) - 
	imRef(inimage, x, y);
      dy = imRef(q->dyPartial, x, y) = imRef(inimage, x, y+1) - 
	imRef(inimage, x, y);
      imRef(q->sgMagnitude, x, y) = dx*dx + dy*dy;
    }
  }
  return;
}


/* static void CalcNMSPeaks(Quad q, float threshold)
 *                   INTERNAL
 * Finds the NMS peaks of the image.  It first checks to see if the 
 *   magnitude of the gradient is greater than the threshold
 *   (actually, it checks if squared gradient magnitude < threshold^2.
 *   If so, then the direction of the gradient is calculated. Let 
 *   point1 be the nearest point in the direction of the gradient, and 
 *   point2 be the nearest point in the opposite direction.  Then the
 *   point is considered an edge iff 
 *      (point>point1&&point>=point2) || (point>=point1&&point>point2
 *   An edge point is represented by a binary 1 in the peaks image of the 
 *   input quad.
 * Input: a valid Quad for which dxPartial, dyPartial, and sgMagnitude 
 *   have been calculated, and a threshold.
 * Ouput: (none).  Directly modifies the peaks filed of the Quad.
 */
static void CalcNMSPeaks(Quad q, float threshold){
  int maxX, maxY;
  int x,y;
  int xOffset, yOffset;
  float point, point1, point2;
  float angle, sqrThresh;


  maxX = (int)imGetWidth(q->peaks) - 1;
  maxY = (int)imGetHeight(q->peaks) - 1;
  sqrThresh = threshold*threshold;
  

  for(y=0; y<maxY; y++){
    for(x=0; x<maxX; x++){
      point = imRef(q->sgMagnitude, x,y);
      if(point < sqrThresh || point == 0.0)
	imRef(q->peaks, x, y) = 0;
      else {
	angle=atan2(imRef(q->dyPartial, x, y), 
		      imRef(q->dxPartial, x, y));
	if (angle < 0.0)
	  angle = M_PI + angle;
	if (.125*M_PI<=angle&&angle<.375*M_PI) 
	  xOffset=yOffset=1;
	else if (.375*M_PI<=angle&&angle<.625*M_PI) {
	  yOffset=1;
	  xOffset=0;
	}
	else if (.625*M_PI<=angle&&angle<.875*M_PI) {
	  yOffset=1;
	  xOffset=-1;
	}
	else {
	  yOffset=0;
	  xOffset=1;
	}
	if((x-xOffset >= 0) && (y-yOffset >= 0) &&
	   (x+xOffset < maxX) && (y+yOffset < maxY)){
	  point1=imRef(q->sgMagnitude, x+xOffset, y+yOffset);
	  point2=imRef(q->sgMagnitude, x-xOffset, y-yOffset);
	  if ((point>point1&&point>=point2)||(point>=point1&&point>point2))
	      imRef(q->peaks, x, y) = 1;
	  else 
	      imRef(q->peaks, x, y) = 0;
	}
	else 
	    imRef(q->peaks, x, y) = 0;
      }
    }
  }
}

void thresholdQuad(Quad q, int t){
  int maxX, maxY;
  float *mag;
  unsigned char *current,*stopat;
  
  maxX = imGetWidth(q->peaks);
  maxY = imGetHeight(q->peaks);

  /* relies on the fact that there is no gap between adjacent rows.*/
  mag = &(imRef(q->sgMagnitude,0,0));
  current = &(imRef(q->peaks,0,0));
  stopat = current + (maxX)*(maxY) - 1;
  while(current < stopat){
    if(*current){
      if((int)(*mag) < t)
	  *current = 0;
    }
    current++;
    mag++;
  }
  return;
}

BinaryImage thresholdQuadNew(Quad q, int t){
  BinaryImage rimage;
  int maxX, maxY;
  float *mag;
  unsigned char *current,*stopat,*new;

  maxX = imGetWidth(q->peaks);
  maxY = imGetHeight(q->peaks);
  rimage = (BinaryImage)imNew(IMAGE_BINARY,maxX,maxY);
  
  /* relies on the fact that there is no gap between adjacent rows.*/
  new = &(imRef(rimage,0,0));
  mag = &(imRef(q->sgMagnitude,0,0));
  current = &(imRef(q->peaks,0,0));
  stopat = current + (maxX)*(maxY)-1;
  while(current < stopat){
    if(*current){
      if((int)(*mag) >= t)
	*new = 1;
    }
    new++; current++; mag++;
  }
  return(rimage);
}


