/*
 *   Image Processing Library
 *   Thomas Braunl, Univ. Stuttgart, 1996
 *   Additional Code by Gerrit Heitsch
 *   Adapted by Michael Rudolph
 *   derived after Braunl et al. : Parallel Image Processing, Springer 2001
 *
 */

#define IMPLEMENT_IPL

#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "improc.h"
#include "FW/labImage.h"
// #include "contour.h"

#define white 255
#define black 0

int  width;
int  height;
BYTE markerDone;
BYTE markerContour;

//Moravec, Track, etc..
#include "feat_track/globals.c"

//Optical Flow
#include "OpticFlow/optical_flow_camus.h"
#include "OpticFlow/optical_flow_liu.h"

//static int CheckCircle(int length, IPLPoint * contour, IPLPoint * cog);
//static int IsEdge(Picture *p_image, int x, int y, int w, BYTE threshold);
//static int FollowContour(Picture *p_imageIn,
//                         int x,
//                         int y,
//                         BYTE threshold,
//                         IPLPoint * points,
//                         int maxLength);
//

void IPL_init(IPL_CONFIG * config)
{
  width  = config->imageWidth;
  height = config->imageHeight;
  // black  = config->colorBlack;  /* now fixed */
  // white  = config->colorWhite;  /* now fixed */
  markerDone = white + 1;
  markerContour = white;
}

//Optical Flow and motion Routines//////
#define SUBSAMPLE_JUST 0
#define FACTOR 2    //by which factor?


void IPL_Just_Optic_Flow_Camus(Picture *p_input,
			       float par1, float par2, float par3, 
			       float par4, float par5, float par6,
			       Picture *p_output) 
{ 
#if SUBSAMPLE_JUST
  Picture *p_bet;
  p_bet=subsample_pic(p_input, p_output, FACTOR, 1);
  OF_by_camus(p_bet,par1, par2, par3+0.15,
	      par4, par5, par6, p_output);
#else 
  if(p_input->format==pix_grey)
    OF_by_camus(p_input, par1, par2, par3+0.15,
		par4, par5, par6, p_output);
  else
    printf("Must be greyscale image!\n");
#endif
}

void IPL_Just_Optic_Flow_Liu(Picture *p_input,
			       float par1, float par2, float par3, 
			       float par4, float par5, float par6,
			       Picture *p_output) 
{ 
#if SUBSAMPLE_JUST 
  Picture *p_bet;
  p_bet=subsample_pic(p_input, p_output, FACTOR, 2);
  OF_by_liu(p_bet , par1, par2, par3+0.15,
	    par4, par5, par6, p_output);
#else
  if(p_input->format==pix_grey)
    OF_by_liu(p_input, par1, par2, par3+0.15,
	      par4, par5, par6, p_output);
  else
    printf("Must be greyscale image! \n");
#endif
}

void IPL_cont_ori(Picture *p_imageIn, Picture *p_imageOut,
		  int par1, int par2, int par3)
{
  if(p_imageIn->format==pix_grey)
    contour(p_imageIn,p_imageOut,par1,par2,par3); 
  else
    printf("Must be gray image!\n");
}



void IPL_col2gray(Picture *p_imageIn, Picture *p_imageOut)
{
  int i;
  BYTE *imageOut, *imageIn;
  
  if(p_imageIn->format==pix_rgb24)
    {
      imageIn=(BYTE *)p_imageIn->data;

      p_imageOut->datasize=p_imageIn->datasize / 3;
      p_imageOut->bytes_per_line=p_imageIn->bytes_per_line / 3; 
      p_imageOut->bytes_per_pixel=1;     
      p_imageOut->width=p_imageIn->width;
      p_imageOut->height=p_imageIn->height;
	
      if(p_imageOut->format!=pix_grey)
	{
	  p_imageOut->format=pix_grey;
	  
	  if(p_imageOut->data!=NULL)
	    p_imageOut->data=(BYTE *)realloc(p_imageOut->data, 
					     p_imageOut->datasize);
	  else
	    p_imageOut->data=(BYTE *)malloc(p_imageOut->datasize); 
	}
                  
      imageOut=(BYTE *)p_imageOut->data;
      
      for (i = 0; i < (height)*width; i++)
	imageOut[i] = (imageIn[3*i] + imageIn[3*i+1] + imageIn[3*i+2])/ 3 ; 
      
      p_imageOut->format=pix_grey;
    }
  else
    {
      printf("Must be RGB Picture!\n");
    }
}

void IPL_matchcol(Picture *p_imageIn, Picture *p_imageOut, 
		  BYTE r, BYTE g, BYTE b)
{   // square color distance
  int i, sqdiff;
  int width=p_imageIn->width, height=p_imageIn->height;
  BYTE *imageOut,*imageIn;
    
  imageIn=(BYTE *)p_imageIn->data;
  imageOut=(BYTE *)p_imageOut->data;
  
  width=p_imageIn->width;
  height=p_imageIn->height;
   
  if(p_imageIn->format==pix_grey)
    {    
      for (i = width; i < (height-1)*width; i++)
	{
	  sqdiff = abs(imageIn[3*i]   - r) * abs(imageIn[3*i]   - r)
	    + abs(imageIn[3*i+1] - g) * abs(imageIn[3*i+1] - g)
	+ abs(imageIn[3*i+2] - b) * abs(imageIn[3*i+2] - b);
	  
	  imageOut[i] = (BYTE) (15 - 15 * sqdiff / (3*255*255));
	} 
    }
  else
    printf("Must be grayscale image !\n");
}

void IPL_laplace(Picture *p_imageIn, Picture *p_imageOut)
{
    int i;
    int delta;
    int width, height;
    
    BYTE *imageOut,*imageIn;
    
    if(p_imageIn->format==pix_grey)
      {    
	imageIn=(BYTE *)p_imageIn->data;
	imageOut=(BYTE *)p_imageOut->data;
	
	width=p_imageIn->width;
	height=p_imageIn->height;
	
	for (i = width; i < (height-1)*width; i++)
	  {
	    delta  = abs(4 * imageIn[i] -
			 imageIn[i-1] -
			 imageIn[i+1] -
			 imageIn[i-width] -
			 imageIn[i+width]);
	    
	    if (delta > white)
	      imageOut[i] = white;
	    else
	      imageOut[i] = (BYTE)delta;
	  }
      }
    else
      printf("Must be grayscale image !\n");
}
    
void IPL_TestPalette(Picture *p_imageOut)
{ 
  int dist = 15;
  int i,j,pos;
  float facx,facy;
  int width, height;
  BYTE *imageOut;

  if(p_imageOut->format==pix_rgb24)
    {
      imageOut=(BYTE *)p_imageOut->data;
      
      width=p_imageOut->width;
      height=p_imageOut->height;
  
      //    fprintf ( stderr, "IPL_TestPalette, w=%d, h=%d\n", width, height );
      facy = 255.0 / (float) height;
      facx = 255.0 / (float) width;
      pos = 0;
      printf("r/g/b black/white\n");
      for (i = 0; i < height; i++)
	for (j = 0; j < width; j++)
	  {
	    if (i == dist)
	      { imageOut[pos++] = 255;  // red
	      imageOut[pos++] = 0;    // green
	      imageOut[pos++] = 0;    // blue
	      }
	    else
	      if (i == 2*dist)
		{ imageOut[pos++] = 0;    // red
		imageOut[pos++] = 255;  // green
		imageOut[pos++] = 0;    // blue
		}
	      else
		if (i == 3*dist)
		  { imageOut[pos++] = 0;    // red
		  imageOut[pos++] = 0;    // green
		  imageOut[pos++] = 255;  // blue
		  }
		else
		  if (i == 5*dist)
		    { imageOut[pos++] = 0;    // red
		    imageOut[pos++] = 0;    // green
		    imageOut[pos++] = 0;    // blue
		    }
		  else
		if (i == 6*dist)
		  { imageOut[pos++] = 255;  // red
		  imageOut[pos++] = 255;  // green
		  imageOut[pos++] = 255;  // blue
		  }
		else
		  { imageOut[pos++] = (int) (facx * j);  // red
		  imageOut[pos++] = (int) (facy * i);  // green
		  imageOut[pos++] = 0;  // blue
		  }
	  }
    }
  else
    printf("Must be color image\n");
}

void IPL_noise(Picture *p_imageIn, Picture *p_imageOut, double noise)
{
  int i,pos;
    int imageSize ;
    int numberOfPixels ;
    int width, height;

    BYTE *imageOut,*imageIn;
    
    imageIn=(BYTE *)p_imageIn->data;
    imageOut=(BYTE *)p_imageOut->data;
    
    width=p_imageIn->width;
    height=p_imageIn->height;
    
    imageSize = width * height; 
    numberOfPixels = (int)(imageSize * noise);
    
    if(p_imageIn->format==pix_grey)
      {
	memcpy(imageOut, imageIn, imageSize);
	
	for (i = 0; i < numberOfPixels; i+=2)
	  {
	    imageOut[rand() % imageSize] = white;
	    imageOut[rand() % imageSize] = black;
	  }
      }
    else if (p_imageIn->format==pix_rgb24)
      {
	memcpy(imageOut, imageIn, 3*imageSize);
	
	for (i = 0; i < numberOfPixels; i+=2)
	  {
	    pos = rand() % imageSize;
	    imageOut[3*pos  ] = ColWhite;
	    imageOut[3*pos+1] = ColWhite;
	    imageOut[3*pos+2] = ColWhite;
	    pos = rand() % imageSize;
	    imageOut[3*pos  ] = ColBlack;
	    imageOut[3*pos+1] = ColBlack;         
	    imageOut[3*pos+2] = ColBlack;
	  }
      }
    else
      fprintf(stderr,"Not implemented for this type!\n");
}

static Picture *p_buf;
int buffer=0;
int IPL_difference(Picture *p_imageIn, Picture *p_imageOut)
{
  int i;
  int delta;
  int width, height;
  BYTE *imageOut,*imageIn,*buf;
    
  imageIn=(BYTE *)p_imageIn->data;
  imageOut=(BYTE *)p_imageOut->data;
  
  width=p_imageIn->width;
  height=p_imageIn->height;
  
  if(buffer<1)
    {
      p_buf=copyImage(p_imageIn);
      buf=(BYTE *)p_buf->data; 
      buffer++;
	return 0;
    }
    else
      {
	buf=(BYTE *)p_buf->data; 
	if(p_imageIn->format==pix_grey)
	  {
	    for (i = 0; i < width * height; i++)
	      {
		delta = *(buf++) - *(imageIn++);
		if (delta < 0) 
		  delta = - delta;
		imageOut[i] = (BYTE)delta;
	      }
	    
	    freeImage(p_buf);
	    p_buf=copyImage(p_imageIn);
	  
	  }
	 else if (p_imageIn->format==pix_rgb24)
	   { 
	     buf=(BYTE *)p_buf->data; 
	     for (i = 0; i < 3*width * height; i++)
	       {
		 delta = buf[i] - imageIn[i];
		 if (delta < 0) 
		   delta = - delta;
		 imageOut[i] = (BYTE)delta;
	       }
	     
	     freeImage(p_buf);
	     p_buf=copyImage(p_imageIn);
	   
	   }
	else
	  fprintf(stderr,"Not implemented for this type!\n");
      }
    
    return 0;
    
}

void IPL_sobel(Picture *p_imageIn, Picture *p_imageOut)
{
  int i=0;
  int grad;
  int deltaX, deltaY;
  
  BYTE *tmp, *imageIn;
  
  imageIn=(BYTE *)p_imageIn->data;
  tmp=(BYTE *)p_imageOut->data;
  
  memset(tmp, 0, p_imageIn->width);
  
  if(p_imageIn->format==pix_grey)
    {
	  for (i = p_imageIn->width+1; 
	       i < (p_imageIn->height-2)*p_imageIn->width; i++)
	    {
	      deltaX = 2*imageIn[i+1] + imageIn[i-p_imageIn->width+1] + 
		imageIn[i+p_imageIn->width+1] -
		2*imageIn[i-1] - imageIn[i-p_imageIn->width-1] - 
		imageIn[i+p_imageIn->width-1];
	      
	      deltaY = imageIn[i-p_imageIn->width-1] + 2*imageIn[i-p_imageIn->width] + 
		imageIn[i-p_imageIn->width+1] -
		imageIn[i+p_imageIn->width-1] - 2*imageIn[i+p_imageIn->width] - 
		imageIn[i+p_imageIn->width+1];
	      
	      grad = (abs(deltaX) + abs(deltaY)) / 3;
	      
	      if (grad > white)
		grad = white;
	      
	      tmp[i] = (BYTE)grad;
	    }
    }
  else
    printf("Must be grayscale image !\n");
  
  memset(tmp + i, 0, p_imageIn->width);
}

void IPL_mean(Picture *p_imageIn, Picture *p_imageOut)
{
    int i,width,height,tmp;
    
    BYTE *imageOut,*imageIn;
    
    imageIn=(BYTE *)p_imageIn->data;
    imageOut=(BYTE *)p_imageOut->data;
    
    width=p_imageIn->width;
    height=p_imageIn->height;

    if(p_imageIn->format==pix_grey)
      { 
	memset(imageOut, 0, width);
        
	for (i =width+1; i<(height-1)*width; i++)
	  {
	    tmp         = (int) (imageIn[i-width-1] +
				 imageIn[i-width]   +
				 imageIn[i-width+1] +
				 imageIn[i-1]       +
				 imageIn[i]         +
				 imageIn[i+1]       +
				 imageIn[i+width-1] +
				 imageIn[i+width]   +
				 imageIn[i+width+1]      )  / 9;
	    
	    imageOut[i] = (BYTE) tmp;
	  }
	
	memset(imageOut + i, 0, width);
      }
    else if(p_imageIn->format==pix_rgb24)
      {
	memset(imageOut, 0, 3*width);

	for (i = 3*width+3; i < 3*(height-2)*width; i+=3)
	  {
	    imageOut[i] = (BYTE)(imageIn[i-3*(width-1)] +
				 imageIn[i-3*(width)]   +
				 imageIn[i-3*(width+1)] +
				 imageIn[i-3]       +
				 imageIn[i]         +
				 imageIn[i+3]       +
				 imageIn[i+3*(width-1)] +
				 imageIn[i+3*(width)]   +
				 imageIn[i+3*(width+1)]   )  / 9;

	    imageOut[i+1] = (BYTE)(imageIn[i-3*(width-1)+1] +
				   imageIn[i-3*(width)+1]   +
				   imageIn[i-3*(width+1)+1] +
				   imageIn[i-3+1]       +
				   imageIn[i+1]         +
				   imageIn[i+3+1]       +
				   imageIn[i+3*(width-1)+1] +
				   imageIn[i+3*(width)+1]   +
				   imageIn[i+3*(width+1)+1]   )  / 9;
	    
	    imageOut[i+2] = (BYTE)(imageIn[i-3*(width-1)+2] +
				   imageIn[i-3*(width)+2]   +
				   imageIn[i-3*(width+1)+2] +
				   imageIn[i-3+2]       +
				   imageIn[i+2]         +
				   imageIn[i+3+2]       +
				   imageIn[i+3*(width-1)+2] +
				   imageIn[i+3*(width)+2]   +
				   imageIn[i+3*(width+1)+2]   )  / 9;
	  }
	memset(imageOut + i, 0, 3*width);
      }
    else
      fprintf(stderr,"Not implemented for this type!\n");
}

void IPL_median(Picture *p_imageIn, Picture *p_imageOut)
{
    int i;
    int width, height;
    BYTE values[9];
    
    BYTE *imageOut,*imageIn;
   
 if(p_imageIn->format==pix_grey)
      {  
	imageIn=(BYTE *)p_imageIn->data;
	imageOut=(BYTE *)p_imageOut->data;
	
    width=p_imageIn->width;
    height=p_imageIn->height;
    
    memset(imageOut, 0, width);
    
    // loop over the image, starting at the second row, second column
    for (i = width+1; i < width * (height - 1); i++)
      {
        int x, y;
        int k = 0, l;
        int j;

        /*
         *    sort the 3x3 environment by simple
         *    insertion sort.
	 */

        for (y = -1; y <= 1; y++)
	{
            for (x = -1; x <= 1; x++)
	    {
                BYTE val = imageIn[i + y * width + x];

                for (j = 0; j < k; j++)
                    if (val < values[j])
		      break;

                for (l = k; l > j; l--)
                    values[l] = values[l-1];
                values[j] = val;
                k++;
            }

            imageOut[i] = values[4];
	}
      }
    
    memset(imageOut + i, 0, width);
      }
    else
  printf("Must be grayscale image !\n"); 
}

void IPL_min(Picture *p_imageIn, Picture *p_imageOut)
{
  int i;
  int val;
  int x, y;
  int width, height;
  static BYTE *imageOut,*imageIn;
   
  if(p_imageIn->format==pix_grey)
    {
      imageIn=(BYTE *)p_imageIn->data;
      imageOut=(BYTE *)p_imageOut->data;
      
      width=p_imageIn->width;
      height=p_imageIn->height;
      
      memset(imageOut, 0, width);
      
      for (i = width+1; i < (height-1)*width; i++)
	{
      val = white;
      
      for (y = -1; y <= 1; y++)
	  for (x = -1; x <= 1; x++)
	    if (imageIn[i + x + y * width] < val)
	      val = imageIn[i + x + y * width];
      
      imageOut[i] = (BYTE)val;
	}
      
      memset(imageOut + i, 0, width); 
    }
  else
    printf("Must be grayscale image !\n"); 
}

void IPL_max(Picture *p_imageIn, Picture *p_imageOut)
{
    int i;
    int val;
    int x, y;
    int width, height;
    BYTE *imageOut,*imageIn;
   
    if(p_imageIn->format==pix_grey)
      {
	imageIn=(BYTE *)p_imageIn->data;
	imageOut=(BYTE *)p_imageOut->data;
	
	width=p_imageIn->width;
	height=p_imageIn->height;
	
	memset(imageOut, 0, width);

	for (i = width+1; i < (height-1)*width; i++)
	  {
	    val = black;

	    for (y = -1; y <= 1; y++)
            for (x = -1; x <= 1; x++)
	      if (imageIn[i + x + y * width] > val)
		val = imageIn[i + x + y * width];
	    
        imageOut[i] = (BYTE)val;
	  }
	
	memset(imageOut + i, 0, width);
      }
    else
      printf("Must be grayscale image !\n"); 
}

void IPL_identity(Picture *p_imageIn, Picture *p_imageOut)
{
int width, height;
  BYTE *imageOut,*imageIn;
  
  imageIn=(BYTE *)p_imageIn->data;
  imageOut=(BYTE *)p_imageOut->data;
  
  width=p_imageIn->width;
  height=p_imageIn->height;
  
  if(p_imageIn->datasize==p_imageOut->datasize)    
    memcpy(imageOut, imageIn,p_imageOut->datasize );
  
}

void IPL_threshold(Picture *p_imageIn, Picture *p_imageOut, BYTE threshold)
{
  int i; 
  int width, height;
  BYTE *imageOut,*imageIn;
  
  if(p_imageIn->format==pix_grey)
    {
      imageIn=(BYTE *)p_imageIn->data;
      imageOut=(BYTE *)p_imageOut->data;
      
      width=p_imageIn->width;
      height=p_imageIn->height;
      
      for (i = 0; i < width * height; i++)
	{
	  if (imageIn[i] >= threshold)
	    imageOut[i] = white;
	  else
	    imageOut[i] = black;
	}
    }
  else
    printf("Must be grayscale image!\n");
}

void IPL_negation(Picture *p_imageIn, Picture *p_imageOut)
{
  int i;
  int width, height;
  BYTE *imageOut,*imageIn;
  
  imageIn=(BYTE *)p_imageIn->data;
  imageOut=(BYTE *)p_imageOut->data;
  
  width=p_imageIn->width;
  height=p_imageIn->height;
  
  if(p_imageIn->format==pix_grey)
    {
      for (i = 0; i < width * height; i++)
	imageOut[i] = white - imageIn[i];
    }
  else if(p_imageIn->format==pix_rgb24)
    {
      for (i = 0; i < 3*width * height; i++)
	imageOut[i] = ColWhite - imageIn[i];
    }
  else
     fprintf(stderr,"Not implemented for this type!\n");
   
}

void IPL_gray_stretch(Picture *p_imageIn, Picture *p_imageOut)
{
    int i;
    int width, height;
    
    BYTE maxVal = black;
    BYTE minVal = white;
    BYTE range;
    double factor=1.0;
    
    BYTE *imageOut,*imageIn;
    
    if(p_imageIn->format==pix_grey)
      {
	imageIn=(BYTE *)p_imageIn->data;
	imageOut=(BYTE *)p_imageOut->data;
	
	width=p_imageIn->width;
	height=p_imageIn->height;
	
	
	for (i = 0; i < width*height; i++)
	  {
	    if (imageIn[i] < minVal)
	      minVal = imageIn[i];
	    
	    if (imageIn[i] > maxVal)
	      maxVal = imageIn[i];   
	  }
	
	range = maxVal - minVal;
	if (range == 0)
	  range = 1;
	factor = (double)white / range;
	
	for (i = 0; i < width*height; i++)
	  imageOut[i] = (BYTE)(factor * (imageIn[i] - minVal));
      }
    else
      printf("Must be grayscale image!\n");
}


void IPL_gray_reduce(Picture *p_imageIn, Picture *p_imageOut, int numvals)
{
  int i;
  int width, height;
  BYTE maxVal = black;
  BYTE minVal = white;
  BYTE range;
  
  BYTE *imageOut,*imageIn;
  
  if(numvals < 2)
    numvals=2;
  
  if(p_imageIn->format==pix_grey)
    {
      imageIn=(BYTE *)p_imageIn->data;
      imageOut=(BYTE *)p_imageOut->data;
      
      width=p_imageIn->width;
      height=p_imageIn->height;
      
      for (i = 0; i < width*height; i++)
	{
	  if (imageIn[i] < minVal) minVal = imageIn[i];
	  if (imageIn[i] > maxVal) maxVal = imageIn[i];   
	}
      
      range = maxVal - minVal;
      
      if (range + 1 > numvals)
	{
	  for (i = 0; i < width*height; i++)
	    {
            int temp = ((numvals-1) * (imageIn[i] - minVal) / range);
            imageOut[i] = (BYTE)((range * temp / (numvals - 1)) + minVal);
	    }
	}
      else
	memcpy(imageOut, imageIn, width * height);
    }
  else
    printf("Must be grayscale image!\n");
}

void IPL_gen_histogram(Picture *p_imageIn, int * histogram)
{
  int i;
  int width, height;
    
    BYTE *imageIn;
    
    imageIn=(BYTE *)p_imageIn->data;
    
    width=p_imageIn->width;
    height=p_imageIn->height;
    
    memset(histogram, 0, (white - black + 1) * sizeof(int));
    for (i = 0; i < width*height; i++)
        histogram[imageIn[i]]++;
}

void IPL_equal_histogram(Picture *p_imageIn, Picture *p_imageOut, int *histogram)
{
    int i, j;
    int sum = 0;
    int width, height;
    BYTE *imageOut,*imageIn;

    imageIn=(BYTE *)p_imageIn->data;
    imageOut=(BYTE *)p_imageOut->data;
    
    width=p_imageIn->width;
    height=p_imageIn->height;
    
    memset(histogram, 0, white * sizeof(int));

    for (j = black; j <= white; j++)
    {
        for (i = 0; i < width*height; i++)
        {
            if (imageIn[i] == j)
                imageOut[i] = (white * sum / (white - black));  
        }

        sum++;
        histogram[j]++;
    }
}

void IPL_erosion (Picture *p_imageIn, Picture *p_imageOut, char * struc)
{
    int x, y;
    int offset;
    int width, height;
    
    BYTE *imageOut,*imageIn;
    
    imageIn=(BYTE *)p_imageIn->data;
    imageOut=(BYTE *)p_imageOut->data;
    
    width=p_imageIn->width;
    height=p_imageIn->height;

    if(p_imageIn->format==pix_grey)
      {
	for (y = 1; y < height - 1; y++)
	  {
	    for (x = 1; x < width - 1; x++)
	      {
		offset = x + y * width;
		
		if ((struc[0] == 1 && imageIn[offset-1 - width] == white) ||
		    (struc[1] == 1 && imageIn[offset   - width] == white) ||
		    (struc[2] == 1 && imageIn[offset+1 - width] == white) ||
		    (struc[3] == 1 && imageIn[offset-1] == white) ||
		    (struc[4] == 1 && imageIn[offset] == white) ||
		    (struc[5] == 1 && imageIn[offset+1] == white) ||
		    (struc[6] == 1 && imageIn[offset-1+width] == white) ||
		    (struc[7] == 1 && imageIn[offset+width] == white) ||
		    (struc[8] == 1 && imageIn[offset+1+width] == white))
		  imageOut[offset] = white;
		else
		  imageOut[offset] = black;
	      }
	  }
      }
    else
      printf("Must be greyscale image!\n");
}

void IPL_dilation(Picture *p_imageIn, Picture *p_imageOut, char *struc)
{
  int x, y;
  int offset;
  int width, height;
  BYTE *imageOut,*imageIn;
  
  imageIn=(BYTE *)p_imageIn->data;
  imageOut=(BYTE *)p_imageOut->data;
  
  width=p_imageIn->width;
  height=p_imageIn->height;
  
  if(p_imageIn->format==pix_grey)
    {  
      for (y = 1; y < height - 1; y++)
	{
        for (x = 1; x < width - 1; x++)
	{
	  offset = x + y * width;
	  
	  if ((struc[0] == 1 && imageIn[offset-1 - width] == black) ||
	      (struc[1] == 1 && imageIn[offset   - width] == black) ||
	      (struc[2] == 1 && imageIn[offset+1 - width] == black) ||
	      (struc[3] == 1 && imageIn[offset-1] == black) ||
	      (struc[4] == 1 && imageIn[offset] == black) ||
	      (struc[5] == 1 && imageIn[offset+1] == black) ||
	      (struc[6] == 1 && imageIn[offset-1+width] == black) ||
	      (struc[7] == 1 && imageIn[offset+width] == black) ||
	      (struc[8] == 1 && imageIn[offset+1+width] == black))
	    imageOut[offset] = black;
	  else
	    imageOut[offset] = white;
	}
	}  
    }
  else
    printf("Must be greyscale image!\n");
}

void IPL_open(Picture *p_imageIn, Picture *p_imageOut, char *struc)
{

  Picture *p_temp;
  
  p_temp=copyImage(p_imageIn);
  clearImage(p_temp);
  
  IPL_erosion(p_imageIn, p_temp, struc);
  IPL_dilation(p_temp, p_imageOut, struc);
  
  freeImage(p_temp);
}

void IPL_close(Picture *p_imageIn, Picture *p_imageOut, char *struc)
{
  Picture *p_temp;
  
  p_temp=copyImage(p_imageIn);
  clearImage(p_temp);
  
  IPL_dilation(p_imageIn, p_temp, struc);
  IPL_erosion(p_temp, p_imageOut, struc);
  
  freeImage(p_temp);
}

void IPL_Coverlay(Picture *p_imageIn1, Picture *p_imageIn2, 
		  Picture *p_imageOut, BYTE r, BYTE g, BYTE b)
  // copy imageIn1 to image Out i                                        
  // change all positions in which imageIn2 = black to RGB_val in output 
  // ImageIn1 -> COL, ImageIn2 -> GRAY, ImageOut -> COL                  
{
  int i;
  int width, height;
  BYTE *imageOut, *imageIn1, *imageIn2;
  
  imageIn1=(BYTE *)p_imageIn1->data;
  imageOut=(BYTE *)p_imageOut->data;
  imageIn2=(BYTE *)p_imageIn2->data;
  
  width=p_imageIn1->width;
  height=p_imageIn1->height;
  
  if(p_imageIn1->format==pix_rgb24)
    {
      memcpy(imageOut, imageIn1, 3*width * height);
      
      for (i = 0; i < width * height; i++)
	if (imageIn2[i] == black)
	  {  
	    imageOut[3*i] = r;
	    imageOut[3*i+1] = g;
	    imageOut[3*i+2] = b;
	  }
    }
  else
    {
      memcpy(imageOut, imageIn1, width * height);
      
      for (i = 0; i < width * height; i++)
	if (imageIn2[i] == black) imageOut[i] = r;   
    }
}


// 2x2 ordered dithering, uses only upper left pixel in 2x2 areas     
void IPL_dither(Picture *p_imageIn, Picture *p_imageOut)
{
  int width, height;
  int x, y, i;
  int thres = white / 5;
    
  BYTE *imageOut,*imageIn;
  
  imageIn=(BYTE *)p_imageIn->data;
  imageOut=(BYTE *)p_imageOut->data;
    
  width=p_imageIn->width;
  height=p_imageIn->height;
  
  if(p_imageIn->format==pix_grey)
    {   
      memcpy(imageOut, imageIn, width * height);
      
      for (y = 0; y < height-1; y+=2)
	{
	  for (x = 0; x < width-1; x+=2)
	    {
	      i = x + width * y;
	      
            if (imageIn[i] <   thres)
	      imageOut[i] = black;
            else
	      imageOut[i] = white;
	    
            if (imageIn[i] < 3*thres)
	      imageOut[i+1] = black;
            else
	      imageOut[i] = white;
	    
            if (imageIn[i] < 4*thres)
	      imageOut[i+width] = black;
            else
	      imageOut[i] = white;
	    
            if (imageIn[i] < 2*thres)
	      imageOut[i+width+1] = black;
            else
	      imageOut[i] = white;
	    }
    } 
}
  else
    printf("Must be greyscale image!\n");
}


void col_invert(BYTE *rgb)
{ // invert 3 consecutive RGB bytes
   *rgb     = 255 - *rgb;
   *(rgb+1) = (255 - *(rgb+1));
   *(rgb+2) = (255 - *(rgb+2));
}

void IPL_showxy(Picture *p_imageIn, Picture *p_imageOut, int x, int y)
{    
  int width, height;
  BYTE *imageOut,*imageIn;
  
  imageIn=(BYTE *)p_imageIn->data;
  imageOut=(BYTE *)p_imageOut->data;
  
  width=p_imageIn->width;
  height=p_imageIn->height;
  
  if(p_imageIn->format==pix_grey)
    {
      memcpy(imageOut, imageIn, width * height);
      
      printf("show (%d,%d) = %d\n", x, y, imageIn[x + y * width]);
      
      if (x > 1 && x < width - 1 && y > 1 && y < height - 1)
	{
	  imageOut[x + y * width    ] = white - imageIn[x + y * width    ];
	  imageOut[x + y * width + 1] = white - imageIn[x + y * width + 1];
	  imageOut[x + y * width + 2] = white - imageIn[x + y * width + 2];
	  imageOut[x + y * width - 1] = white - imageIn[x + y * width - 1];
	  imageOut[x + y * width - 2] = white - imageIn[x + y * width - 2];
	  imageOut[x + (y+1) * width] = white - imageIn[x + (y+1) * width];
	  imageOut[x + (y+2) * width] = white - imageIn[x + (y+2) * width];
	  imageOut[x + (y-1) * width] = white - imageIn[x + (y-1) * width];
	  imageOut[x + (y-2) * width] = white - imageIn[x + (y-2) * width];
	}
    }
  else if(p_imageIn->format==pix_rgb24)
    {   
      int pos, pos2;
      
      memcpy(imageOut, imageIn, 3 * width * height);
      
      pos = 3*(x + y * width);
      printf("show (%d,%d) = %d %d %d\n", x, y,
	     imageIn[pos], imageIn[pos+1], imageIn[pos+2]);
      
      if (x > 1 && x < width - 1 && y > 1 && y < height - 1)
	{
	  col_invert(&imageOut[pos]);
	  col_invert(&imageOut[pos+3]);
	  col_invert(&imageOut[pos-3]);
	  col_invert(&imageOut[pos+6]);
	  col_invert(&imageOut[pos-6]);
	  pos2 = pos + 3 * width;
	  col_invert(&imageOut[pos2]);
	  pos2 = pos + 6 * width;
	  col_invert(&imageOut[pos2]);
	  pos2 = pos - 3 * width;
	  col_invert(&imageOut[pos2]);
	  pos2 = pos - 6 * width;
     col_invert(&imageOut[pos2]);
	}
    }
  else
      fprintf(stderr,"Not implemented for this type!\n");
  
}


void IPL_fill (Picture *p_imageIn, 
	       Picture *p_imageOut, int x, int y, char * struc)
{
  Picture *p_grow;
  Picture *p_new_grow;
  Picture *p_not_img;
  int width, height;
  
  int loopCount = 0;
  
  BYTE *imageOut, *imageIn;
  
  p_grow=copyImage(p_imageIn);
  p_new_grow=copyImage(p_imageIn);
  p_not_img=copyImage(p_imageIn);
  
  imageIn=(BYTE *)p_imageIn->data;
  imageOut=(BYTE *)p_imageOut->data;
  
  width=p_imageIn->width;
  height=p_imageIn->height;
  
  IPL_negation(p_imageIn, p_not_img);
  
  memset(p_new_grow->data, white, width * height);
  p_new_grow->data[x + y * width] = black;
  do
    {
      memcpy(p_grow->data, p_new_grow->data, width * height);
      IPL_dilation(p_grow, p_new_grow, struc);
      IPL_or(p_new_grow, p_not_img, p_new_grow);
      
      loopCount++;
    }
  while (!IPL_equal(p_grow, p_new_grow));
  
  memcpy(imageOut, p_grow->data, width * height);
  
  freeImage(p_grow);
  freeImage(p_new_grow);
  freeImage(p_not_img);
  
  printf("fill loops = %d\n", loopCount);
}

void IPL_region_growing(Picture *p_imageIn, Picture *p_imageOut, int thresh)
{
  // thresh = 10..100
  int seed = 40;
  int change;
  int x, y, offset = 0, off2 = 1;
  int loopCount = 0;
  int width, height;
  BYTE reg[MAX_IMAGE_SIZE];
  
  BYTE *imageOut,*imageIn;
  
  imageIn=(BYTE *)p_imageIn->data;
  imageOut=(BYTE *)p_imageOut->data;
  
  width=p_imageIn->width;
  height=p_imageIn->height;
   
  if(p_imageIn->format==pix_grey)
    {  
      memset(reg, 0, width * height);
      
      for (y = 0; y < height; y++)
	for (x = 0; x < width; x++, offset++)
	  if ((x % seed == 1) && (y % seed == 1))
	    reg[offset] = off2++;
      do
	{
	  change = 0;
	  
	  for (y = 1; y < height-1; y++)
	    {
	      for (x = 1; x < width-1; x++)
		{
                offset = x + width * y;
		
                if (reg[offset] != 0)
		  {
                    if (abs(imageIn[offset] - imageIn[offset - 1]) <= thresh &&
			reg[offset - 1] < reg[offset])
		      change = reg[offset - 1] = reg[offset];
		    
		    if (abs(imageIn[offset] - imageIn[offset + 1]) <= thresh &&
			reg[offset + 1] < reg[offset])
		      change = reg[offset + 1] = reg[offset];
		    
		    if (abs(imageIn[offset] - imageIn[offset - width]) <= thresh &&
			reg[offset - width] < reg[offset])
		      change = reg[offset - width] = reg[offset];
		    
		    if (abs(imageIn[offset] - imageIn[offset + width]) <= thresh &&
			reg[offset + width] < reg[offset])
		      change = reg[offset + width] = reg[offset];
		  }
		}
	    }
	  
        loopCount++;
	}
      while (change);
      
      memcpy(imageOut, reg, width * height);
      
      printf("reg(s=%d,t=%d) l=%d\n", seed, thresh, loopCount);
    }
  else 
    printf("Must be greyscale image !\n");
  
}

//  int IPL_equal_nobound(BYTE *image1, BYTE *image2)
//  {
//      int x, y;

//      for (y = 1; y < height - 1; y++)
//          for (x = 1; x < width - 1; x++)
//              if (image1[x + y * width] != image2[x + y * width])
//                  return 0;

//      return 1;
//  }


int IPL_equal(Picture *p_imageIn, Picture *p_imageOut)
{
  int i;
  int width, height;
  BYTE *imageOut,*imageIn;
  
  imageIn=(BYTE *)p_imageIn->data;
  imageOut=(BYTE *)p_imageOut->data;
  
  width=p_imageIn->width;
  height=p_imageIn->height;
  
  for (i = 0; i < width * height; i++)
    if (imageIn[i] != imageOut[i])
      return 0;
  
  return 1;
}


void IPL_and(Picture *p_imageIn1, Picture *p_imageIn2, Picture *p_imageOut)
{ 
  int i;
  int width, height;
  BYTE *imageOut,*imageIn1,*imageIn2;
  
  imageIn1=(BYTE *)p_imageIn1->data;
  imageIn2=(BYTE *)p_imageIn2->data;
  imageOut=(BYTE *)p_imageOut->data;
  
  width=p_imageIn1->width;
  height=p_imageIn1->height;
  
  for (i = 0; i < width * height; i++)
    imageOut[i] = imageIn1[i] & imageIn2[i];
}

void IPL_or(Picture *p_imageIn1, Picture *p_imageIn2, Picture *p_imageOut)
{ 
  int i;
  int width, height;
  BYTE *imageOut,*imageIn1,*imageIn2;
  
  imageIn1=(BYTE *)p_imageIn1->data;
  imageIn2=(BYTE *)p_imageIn2->data;
  imageOut=(BYTE *)p_imageOut->data;
  
  width=p_imageIn1->width;
  height=p_imageIn1->height;
  
  for (i = 0; i < width * height; i++)
    imageOut[i] = imageIn1[i] | imageIn2[i];
}


void IPL_connected (Picture *p_imageIn, Picture *p_imageOut, 
		    int x, int y, char *struc)
{
  Picture *p_grow;
  Picture *p_new_grow;
  int width, height;
  BYTE *imageOut,*imageIn;
  int loopCount = 0;
  
  p_grow=copyImage(p_imageIn);
  p_new_grow=copyImage(p_imageIn);

  imageIn=(BYTE *)p_imageIn->data;
  imageOut=(BYTE *)p_imageOut->data;
  
  width=p_imageIn->width;
  height=p_imageIn->height;
    
  memset(p_new_grow->data, white, width * height);
  p_new_grow->data[x + y * width] = black;
  
  do
    {
      memcpy(p_grow->data, p_new_grow->data, width * height);
      
      IPL_dilation(p_grow, p_new_grow, struc);
      IPL_or(p_new_grow, p_imageIn, p_new_grow);
      
      loopCount++;
    }
  while (!IPL_equal(p_grow, p_new_grow));
  
  memcpy(p_imageOut->data, p_grow->data, width * height);
  
  printf("connected loops = %d\n", loopCount);
  
  freeImage(p_grow);freeImage(p_new_grow);

}

void IPL_boundary (Picture *p_imageIn, Picture *p_imageOut, char *struc)
{
  IPL_erosion(p_imageIn, p_imageOut, struc);
  IPL_negation(p_imageOut, p_imageOut);
  IPL_or(p_imageIn, p_imageOut, p_imageOut);
}

void IPL_skeleton (Picture *p_imageIn, Picture *p_imageOut, char * struc)
{
  
  Picture *p_help;
  Picture *p_times;
  int loopCount = 0;
  int width, height;
  BYTE *imageOut,*imageIn;
  
  imageIn=(BYTE *)p_imageIn->data;
  imageOut=(BYTE *)p_imageOut->data;
  
  p_help=copyImage(p_imageIn);
  p_times=copyImage(p_imageIn);
  
  width=p_imageIn->width;
  height=p_imageIn->height;
  
  if(p_imageIn->format==pix_grey)
    { 
  memset(p_imageOut->data, white, width * height);
  memcpy(p_times->data, p_imageIn->data, width * height);
  
  while(IPL_count_nobound(p_times->data, p_help->data, black) > 0)
    {
      IPL_open(p_times, p_help, struc);
      IPL_negation(p_help, p_help);
      IPL_or(p_help, p_times, p_help);
      IPL_and(p_help, p_imageOut, p_imageOut);
      
      memcpy(p_help->data, p_times->data, width * height);
      IPL_erosion(p_help, p_times, struc);
      
      loopCount++;
    }
  
  printf("skeleton loops = %d\n", loopCount);
  
  freeImage(p_times);freeImage(p_help);
    }
  else
    printf("Must be greyscale image !\n");
}


int IPL_count(BYTE *imageIn, BYTE *imageOut, BYTE val)
{
  int i;
  int count;
  
  memcpy(imageOut, imageIn, width * height); // identity
  
  for (i = 0, count = 0; i < height * width; i++)
    if (imageIn[i] == val) count++;
  
  return count;
}

int IPL_count_nobound(BYTE *imageIn, BYTE *imageOut, BYTE val)
{
  int x, y;
  int count = 0;
  
  memcpy(imageOut, imageIn, width * height); // identity
  
  for (y = 1; y < height - 1; y++)
    for (x = 1; x < width - 1; x++)
      if (imageIn[x + y * width] == val) count++;
  
  return count;
}


void IPL_corner(Picture *p_imageIn, Picture *p_imageOut)
{
  int width, height;
  int i;
  int b;
  BYTE *imageOut,*imageIn;
  
  imageIn=(BYTE *)p_imageIn->data;
  imageOut=(BYTE *)p_imageOut->data;
  
  width=p_imageIn->width;
  height=p_imageIn->height;
   
  if(p_imageIn->format==pix_grey)
    {  
  for (i = width; i < (height - 1) * width; i++)
    {
      b = abs(-imageIn[i-width-1] - imageIn[i-width] - imageIn[i-width+1] -
	      imageIn[i-1]       + 8 * imageIn[i]   - imageIn[i+1] -
	      imageIn[i+width-1] - imageIn[i+width] - imageIn[i+width+1])/9;
      
      if (b > white)
	imageOut[i] = white;
      else
	imageOut[i] = (BYTE)b;
    }
    }
  else
    printf("Must be greyscale image!\n");
}


//   //      contour search with chain code

//  static int contourX[] = { 1,  1,  0, -1, -1, -1, 0, 1 };
//  static int contourY[] = { 0, -1, -1, -1,  0,  1, 1, 1 };
//  static IPLPoint contour[MAX_CONTOUR];
//  static double dcoord[MAX_CONTOUR];


//  static int IsEdge(BYTE * image, int x, int y, int w, BYTE threshold)
//  {
//      int i;
//      int left = 0, right = 0;
//      BYTE * p;

//      if (x - w < 0 || x + w >= width)
//          return 0;

//      p = &image[y * width + x - w];

//      for (i = 0; i < w; i++)
//      {
//          left  += p[i];
//          right += p[i + w];
//      }

//      if (right - left > threshold * w)
//          return 1;
//      else
//          return 0;
//  }

//  int IPL_FindCircles(Picture *p_imageIn, 
//  		    Picture *p_imageOut, XYDistance * result)
//  {
//    int i;
//    int count;
//    IPLPoint points[256];
  
//    count = IPL_contour(imageIn, imageOut, &points[0], 8);
//    if (result == NULL)
//      return count;
  
//      for (i = 0; i < count; i++)
//        {
//  	result[i].x = (double)points[i].x / width;
//        result[i].y = (double)(height - points[i].y) / height;
//        }
    
//      return count;
//  }


//  int  IPL_contour(Picture *p_imageIn,
//                   Picture *p_imageOut,
//  		 IPLPoint * result,
//                   BYTE threshold)
//  {
//      int x, y;
//      int countCircles = 0;

//      if (imageOut != NULL)
//          memset(imageOut, 0, width * height);

//      for (y = 1; y < height - 1; y++)
//      {
//          for (x = 1; x < width - 2; x++)
//  	{
//              if (imageIn[x + y * width] != markerDone &&
//                  IsEdge(imageIn, x, y, 2, 10))
//  	    {
//                  unsigned length = FollowContour(imageIn,
//                                                  x,
//                                                  y,
//                                                  threshold,
//                                                  &contour[0],
//                                                  MAX_CONTOUR);


//                  if (length > 4 && length < 30)
//  		{
//                      IPLPoint p;

//                      if (CheckCircle(length, &contour[0], &p) == 0)
//  		    {
//                          if (result != NULL)
//                              result[countCircles] = p;

//                          if (imageOut != NULL)
//  			{
//                              imageOut[p.x + p.y * width] = markerContour;

//                              while (length-- > 0)
//                              {
//                                  imageOut[contour[length].x + 
//                                           contour[length].y * width] = markerContour;
//  			    }
//                          }

//                          countCircles++;
//                      }
//                  }
//  	    }
//  	}
//      }

//      printf("%d circles found\n", countCircles);

//      return countCircles;
//  }

//  static int FollowContour(Picture *p_imageIn,
//                           int x,
//                           int y,
//                           BYTE threshold,
//                           IPLPoint * points,
//                           int maxLength)
//  {
//      int done = 0;
//      int startX = x;
//      int startY = y;
//      int newX, newY;
//      unsigned int direction = 6;
//      unsigned int newDir;
//      unsigned int length = 0;

//      //    follow the contour as long as the 'done' flag is not set
    
//      while (!done)
//      {
//          int i;

//          imageIn[x + y * width] = markerDone;

//  	//    store the coordinates in the 'points' array if a valid
//  	//    pointer is supplied and 'maxLength' is not reached.
        

//          if (points != NULL && length < maxLength)
//          {
//              points[length].x = (short)x;
//              points[length].y = (short)y;
//  	}

//          length++;

//          for (i = 0; i < 8; i++)
//          {
//              newDir = (direction + i - 3) % 8;
//              newX = x + contourX[newDir];
//              newY = y + contourY[newDir];

//              if (newX >= 0 && newY >= 0 && newX < width && newY < height &&
//                  imageIn[newX + newY * width] > threshold)
//                      break;
//  	}

//          if (i >= 8)
//              done = 1;
//          else
//          {
//              x = newX;
//              y = newY;
//              direction = newDir;
//          }

//          if (x == startX && y == startY)
//              done = 1;
//      }

//      return length;
//  }

//  static int CheckCircle(int length, IPLPoint * contour, IPLPoint * cog)
//  {
//      int i;
//      double cogX = contour[0].x;
//      double cogY = contour[0].y;
//      double cogDelta = 0.0;
//      double cogVar = 0.0;

//      if (length <= 0)
//          return -1;

//      /*
//       *    Determine the center of gravity. Take the x/y ratio of 4/3
//       *    into account.
//       */

//      for (i = 1; i < length; i++)
//      {
//          cogX += contour[i].x;
//          cogY += contour[i].y;
//      }

//      cogY = 4.0 * cogY / (3.0 * length);
//      cogX /= length;

//      cog -> x = (short)cogX;
//      cog -> y = (short)(cogY * 0.75);

//      for (i = 0; i < length; i++)
//      {
//          double delta =  sqrt((contour[i].x - cogX) * (contour[i].x - cogX) +
//                               (4.0 * contour[i].y / 3.0 - cogY) *
//                               (4.0 * contour[i].y / 3.0 - cogY));
//          dcoord[i] = delta;
//          cogDelta += delta;
//      }

//      cogDelta /= length;

//      for (i = 0; i < length; i++)
//          cogVar += (cogDelta - dcoord[i]) * (cogDelta - dcoord[i]);
//      cogVar /= length;


//      if (cogVar < 0.45)
//          return 0;
//      else
//          return 1;
//  }


#ifndef MIN
#define MIN(a,b) (a<b?a:b)
#endif
#ifndef MAX
#define MAX(a,b) (a>b?a:b)
#endif
#define NO_HUE -1

/*convert an rgb image to hue in grayscale*/
void IPL_HSVValue(Picture *p_imageIn, Picture *p_imageOut)
{int width, height;
 double hue, sat, val;
 double h, s, v, r, g, b;
 int index = 0;
 int pos;
 int j, k;
 BYTE *imageOut,*imageIn; 
 
 if(p_imageIn->format==pix_rgb24)
   {
     p_imageOut->format=pix_grey;
     p_imageOut->datasize=p_imageOut->width*p_imageOut->height;
     p_imageOut->bytes_per_line= p_imageOut->width;
     
     if(p_imageOut->data!=NULL && p_imageOut->format!=pix_grey)
       p_imageOut->data=(BYTE *)realloc((unsigned char*)p_imageOut->data,
					p_imageOut->datasize);
     
     imageIn=(BYTE *)p_imageIn->data;
     imageOut=(BYTE *)p_imageOut->data;
     
     width=p_imageIn->width;
     height=p_imageIn->height;
     
     index = 0;
     for(j = 0; j < height; j++)
       {
	 for(k = 0; k < width; k++)
	   {
	     pos = 3*(k + j * width);
	     
	     r = (double)imageIn[pos]/255;
	     g = (double)imageIn[pos +1]/255;
	     b = (double)imageIn[pos + 2]/255;
	     
	     rgbToHsv(r, g, b, &h, &s, &v);
	     
	     hue = h * 255;
	     sat = s * 255;
	     val = v * 255;
	     
	     imageOut[index ++] = (BYTE)hue;
	   }
       }
   }
 else
   printf("Must be RGB image!\n");
 
}


// Convert rgb to hsv
void rgbToHsv(double r, double g, double b, double *h, double *s, double *v)
{
  double max = MAX (r, MAX (g,b)), min = MIN(r, MIN(g,b));
  double delta = max - min;

  *v = max;
  if(max != 0.0)
    *s = delta/max;
  
  else
    *s = 0.0;
  
  if (*s == 0.0)
    *h = NO_HUE;
  
  else {
    if (r == max)
      *h = (g-b) / delta;
    
    else if (g == max)
      *h = 2 + (b - r) / delta;
    
    else if (b == max)
      *h = 4 + (r-g) / delta;
    
    *h *= 60.0;
    if(*h < 0)
      *h += 360.0;
    
    *h /= 360.0;
  }
}

void
IPL_match_color_range(Picture *p_imageIn, Picture *p_imageOut,
		      float rf, float gf, float bf)
{
  int r,g,b,intensity,i;
  
  int width, height;
  BYTE *imageOut,*imageIn;
  
  imageIn=(BYTE *)p_imageIn->data;
  imageOut=(BYTE *)p_imageOut->data;
  
  width=p_imageIn->width;
  height=p_imageIn->height;
  if(p_imageIn->format==pix_rgb24)
    {
      for (i = width; i < (height-2)*width; i++)
	{
	  r = imageIn[3*i];
	  g = imageIn[3*i+1];
	  b = imageIn[3*i+2];
	  
	  intensity = r+g+b;
	  if (r >= rf * intensity &&
	  g <= gf * intensity &&
	      b >= bf * intensity)
	    {
	      imageOut[3*i] = white;
	      imageOut[3*i+1] = white;
	      imageOut[3*i+2] = white;
	}      
	  else
	    {
	      imageOut[3*i] = black;
	      imageOut[3*i+1] = black;
	      imageOut[3*i+2] = black;
	    }
	}  
    }
 else
   printf("Must be RGB image!\n");
}



/*
 * Moravec's Interest Operator
 *
 * TODO:
 *   - The size of the window over which the operator is applied
 *     could be controlled with a slider.
 *   - Similarly, the size of the non-maximal suppression
 *     window could also be controlled by slider.
 *   - Currently, feature points are drawn ON the image - might
 *     want to fix. 
 */

void IPL_moravec(Picture *p_imageIn, Picture *p_imageOut)
{


  int no_points=0;
  
  BYTE* imageIn = p_imageIn->data;
  BYTE* imageOut = p_imageOut->data;

  double *var = (double*)calloc( sizeof(double) * width * height, 1);
  
  IPLPoint  point[MAX_POINTS];
  
  if (p_imageIn->format != pix_grey) 
    {
      printf("Must be greyscale image!\n");
      return;
    }
  
  interest(imageIn, M, M, width-M-1, height-M-1, var);
  
  non_max_suppress(var, SW, SW, width-SW-1, height-SW-1,
		   point, &no_points);

  memcpy(imageOut, imageIn, width * height);
  
  draw_points(imageOut, point, no_points);

  free(var);

}
   
/*
 * Tracks features detected by Moravec's interest operator.
 *
 * TODO:
 *   - feature points, the grid, and the crosshair are
 *     drawn to the image - might want to fix.
 */
#include "feat_track/angles.c"
void IPL_Track(Picture *p_imageIn, Picture *p_imageOut)
{

  BYTE* imageIn  = p_imageIn->data;
  BYTE* imageOut = p_imageOut->data;
  active grid;
  int  ok;
  IPLPoint target;

  char lum[width*height], lum2[width*height];

  if (p_imageIn->format == pix_rgb24)  /* using colour */
    {
      memcpy(imageOut, imageIn, width * height * 3);

#ifdef HIST
      Hist_Eq_Col(imageIn, imageIn);
#endif
      
      IPL_lum(p_imageIn, lum);
  
    }

  else if (p_imageIn->format == pix_grey)
    {
      
      memcpy(imageOut, imageIn, width * height);
     
#ifdef HIST
      Hist_Eq(imageIn, lum);
#else
      memcpy(lum, imageIn, width * height);
#endif

    }

  else
    {
      fprintf(stderr, "Invalid image format. Must use rgb24 or 256 grey\n");
      return;
    }

  if (first == TRUE)
    { 
      
      grid.windows = malloc( sizeof(window)*(int)XX*(int)YY );
      
      generate_windows(&grid);

      Gaussian_Smooth(lum, lum2,  
		      grid.main.x1-M, grid.main.y1-M, 
		      grid.main.x2+M, grid.main.y2+M);
 
      Detect_Prominent_Features(imageIn, lum2, &grid);
       
#ifdef DISPLAY
      
      if (p_imageIn->format == pix_rgb24)
	draw_grid(grid.windows, imageOut, TRUE);
      else 
	draw_grid(grid.windows, imageOut, FALSE);

#endif

      first = can_track();

      free(grid.windows);      
    
    }

  else  /* first == FALSE */
    {

#ifdef MOTION
      copy_features();
#endif

      smooth_areas(lum, lum2);   

      ok = find_features(imageIn, lum2); /* features updated */

#ifdef MOTION
      
      if ( ok == TRUE )
	detect_motion();

#endif     

    }
  
  direct(&target);

#ifdef DISPLAY

  if (p_imageIn->format == pix_rgb24)  /* using colour */
    {
      draw_features(imageOut, TRUE);  
      draw_crosshair(imageOut, &target, TRUE);
    }

  else
    {
      draw_features(imageOut, FALSE);  
      draw_crosshair(imageOut, &target, FALSE); 
    }

#endif

}


/*
 * Histogram Equalization
 */

void IPL_HistEq(Picture *p_imageIn, Picture *p_imageOut)
{
  
  BYTE* imageIn = p_imageIn->data;
  BYTE* imageOut = p_imageOut->data;
   
  if (p_imageIn->format == pix_rgb24)  /* using colour */
    Hist_Eq_Col(imageIn, imageOut);
  
  else if (p_imageIn->format == pix_grey)
    Hist_Eq(imageIn, imageOut);
  
  else
    fprintf(stderr, 
	    "\nHistogram equalization not implemented for this image format\n");

}


/*
 * Gaussian Smoothing
 *
 * TODO:
 *   - it would be nicer for a slider to control the size of 
 *     the window.
 *   - it would be nicer for a slider to control the variance
 *     (amount of smoothing)
 */

void IPL_Gauss(Picture *p_imageIn, Picture *p_imageOut)
{
  
  BYTE* imageIn = p_imageIn->data;
  BYTE* imageOut = p_imageOut->data;
  
  if (p_imageIn->format != pix_grey) 
    {
      printf("Must be greyscale image!\n");
      return;
    }
  
  memcpy(imageOut, imageIn, width * height);

  Gaussian_Smooth(imageIn, imageOut, 
		  G, G, 
		  width-G-1, height-G-1);

}


/*
 * Calculates the luminance of a rgb image
 */

void IPL_lum(Picture *p_imageIn, BYTE* lum)
{
  
  BYTE* imageIn = p_imageIn->data;
 
  int i;
   
  if (p_imageIn->format != pix_rgb24)
    return;

  for (i=0; i<width*height; i++)
    lum[i] = (BYTE) (0.299*(double)imageIn[3*i]   +
		     0.587*(double)imageIn[3*i+1] +
		     0.114*(double)imageIn[3*i+2]);
		     
}

#include "feat_track/misc.c"
#include "feat_track/moravec.c"
#include "feat_track/histeq.c"
#include "feat_track/track.c"
#include "feat_track/gaussian.c"

