/****************************************************************************************

This is a template image processing program to be used in E161. This program contains 
the basic input, processing and output parts. You can copy this program to your own 
directory and modify it according to your needs. In particular, you can replace the 
sample processing function called negative included in this program by any other processing 
functions write for the projects assigned during the semester. Also included in this 
program are two histogram functions (one for color images, one for black/white images). 
These are gifts for you to be used in the class.

   To compile this program, enter:
        gcc -lm  processing.c -o processing
   To run the program, enter:
        processing filename_of_input_image filename_of_output_image
   or simply enter
        processing
   and the program will prompt you to enter the filenames for both input and output images.

*********************************************************************************************/

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>

#include "array_utility.h" // utility functions for matrix operations
#include "svd.h"           // singular value decomposition

#define glevel 256
#define Pi 3.1415926536
#define Sqrt2 1.4142136
#define SWAP(a,b) tempr=(a); (a)=(b); (b)=tempr

// These are global variables used by all functions 

char *strstr();

char in_filename[50], out_filename[50];
int M_in,N_in, M_out,N_out,Max,color_in, color_out;         
float **temp2_in, **temp2_out, ***temp3_in, ***temp3_out;

vflip(float **img, int m, int n) // vertical flip of img
{
  int i,j,m2=m/2;
  float v;
  for (j=0; j<n; j++)
    for (i=0; i<m2; i++)
      { v=img[i][j]; img[i][j]=img[m-1-i][j]; img[m-1-i][j]=v; }
}

float **zerocrossing();
float nderiv();
float myMAX(), myMIN();

int lookup[glevel];

main(argc, argv)
int argc;
char **argv;
{
  int i,j,k,l,m,n;
  short w;
  FILE *in_file, *out_file;
  
  // Check number of command line paramenters 
  if (argc!=3)
    {
      fprintf(stderr,"\nUsage: processing in_filename out_filename \n\n");
      printf("\nInput image filename: ");  scanf("%s",&in_filename);
      printf("\nOutput image filename: ");  scanf("%s",&out_filename);
    }
  else  
    { strcpy(in_filename,argv[1]);  strcpy(out_filename,argv[2]); }

  color_in=get_magicnumber(in_filename); // determine if color or b/w

  if (color_in) {             // color input image
    temp3_in=read_color_image(in_filename, &M_in, &N_in); 
    M_out=M_in; N_out=N_in;
    printf("use component Red (0), Green (1), Blue (2) or all three (3) ");
    scanf("%d",&k);
    if (k<3) {                // use one of three components
      color_out=0;
      temp2_in=temp3_in[k];
      gray_histogram(temp2_in,M_in,N_in);   
      temp2_out=alloc2df(M_out,N_out);    
      if (k==0)
	printf("Processing R-component of input image of size (%d x %d)\n",M_in,N_in);    
      else if (k==1)
	printf("Processing G-component of input image of size (%d x %d)\n",M_in,N_in);    
      else if (k==2)
	printf("Processing B-component of input image of size (%d x %d)\n",M_in,N_in);    
    }
    else {                    // use all three components
      color_out=1;
      color_histogram(temp3_in,M_in,N_in);    
      temp3_out=alloc3df(3,M_out,N_out);   
      printf("Processing color image of size (%d x %d)\n",M_in,N_in);
    }
  }
  else {                      // gray scale input image
    color_out=0;
    temp2_in=read_bw_image(in_filename, &M_in, &N_in); 
    M_out=M_in; N_out=N_in;
    temp2_out=alloc2df(M_out,N_out);     
    gray_histogram(temp2_in,M_in,N_in);    
    printf("Processing black/white image of size (%d x %d)\n",M_in,N_in);    
  }
  // Assume output image is of same size as input image. If this is not the case, 
  // modify M_out and N_out in your processing functions according to your algorithm.

  printf("Choose one of the processing functions:\n");
  printf(" (0) resizing\n");
  printf(" (1) Linear stretchn\n");
  printf(" (2) Histogram equalization\n");
  printf(" (3) Histogram specification\n");
  printf(" (4) Negative\n");
  printf(" (5) Fourier filters\n");
  printf(" (6) Walsh_Hadamard filter\n");
  printf(" (7) Discrete cosin filter\n");
  printf(" (8) Convolution\n");
  printf(" (9) Edge detection\n");
  printf("(10) Median filter\n");
  printf("(11) piecewise linear mapping\n");
  printf("(12) Color image enhancement\n");
  printf("(13) Deblur\n");

  scanf("%d",&k);
  switch (k) {
  case  0:  resize();  break;
  case  1:  stretch();  break;
  case  2:  hist_equal();  break;
  case  3:  hist_specify();  break;
  case  4:  negative();  break;
  case  5:  DFT_filtering(); break;
  case  6:  WHT_filtering(); break;
  case  7:  DCT_filtering(); break;
  case  8:  convolution(); break;
  case  9:  edge_detect(); break;
  case 10:  median(); break;
  case 11:  linearmap(); break;
  case 12:  color_proc(); break;
  case 13:  deblur(); break;
  }

  if (color_out) {
    write_color_image(out_filename,temp3_out,M_out,N_out,1);    
    color_histogram(temp3_out,M_in,N_in); 
  }
  else {
    write_bw_image(out_filename,temp2_out,M_out,N_out,1);    
    gray_histogram(temp2_out,M_in,N_in); 
  }

}


color_histogram(float ***img, int m, int n)
{
  int i,j,k,l,I,J,K,L,scale=4, nrow=glevel;
  float max_hist_ds, max_hist_ac, u,v,wds,max;
  float ***hist, **hist_ds, **hist_ac;
  char *sp, *filename;

  printf("Calculating histogram of color image %s...\n",in_filename);

  hist_ds=alloc2df(3,glevel);
  hist_ac=alloc2df(3,glevel);
  hist=alloc3df(3,nrow,glevel);
  max_hist_ds=max_hist_ac=(nrow-scale-1)/2;
  v=1.0/m/n;
  max=0;
  for (k=0; k<3; k++) { 
    for (l=0; l<glevel; l++) hist_ds[k][l]=0;
    for (j=0; j<n; j++) 
      for (i=0; i<m; i++) 
	hist_ds[k][(int)img[k][i][j]]+=v;  // find density 
    for (l=0; l<glevel; l++) 
      if (max < hist_ds[k][l]) { max=hist_ds[k][l]; K=k; L=l; } // find max
  }
  //  printf("K=%d, L=%d, max=%f, wds=%f\n",K,L,max,wds); pause();
  for (k=0; k<3; k++) {
    hist_ac[k][0]=hist_ds[k][0];
    for (l=1; l<glevel; l++) 
      hist_ac[k][l]=hist_ac[k][l-1]+hist_ds[k][l]; // accumulative

    for (l=0; l<glevel; l++) {
      hist_ac[k][l]=max_hist_ac*hist_ac[k][l]; 
      hist_ds[k][l]=max_hist_ds*hist_ds[k][l]/max;
      //      printf("(%3d %.2f %.2f) ",l,hist_ds[k][l],hist_ac[k][l]);
      //      if (k==K && l==L) 
      //	printf("hist_ds[%d][%d]=%f, max=%f\n",k,l,hist_ds[k][l],max);
      if (!(l%32)) v=255; else v=0;
      for (i=0; i<scale; i++) hist[k][i][l]=hist[k][i][l+1]=v; 
      for (i=scale; i<scale+hist_ds[k][l]; i++) 
	hist[k][i][l]=255;
      for (i=scale+max_hist_ds; i<scale+max_hist_ds+hist_ac[k][l]; i++) 
	hist[k][i][l]=255;
    }
    vflip(hist[k],nrow,glevel);
  }
  filename=in_filename;
  sp=strstr(in_filename,".p");
  if (sp) strcpy(sp,".hist");
  else strncat(filename,".hist",5);
  printf("Histogram filename: %s\n",filename);
  //  sprintf(fname, "%s_hist",in_filename);
  strcpy(filename,"histogram");
  display_color_image(filename,hist,nrow,glevel);
  printf("show histogram image: %s\n",filename);

  dealloc2df(hist_ds,3);
  dealloc2df(hist_ac,3);
  dealloc3df(hist,3,nrow);
}


gray_histogram(float **img, int m, int n)
{
  int i,j,k,l,I,J,K,L,scale=4, nrow=glevel, max_hist_ds, max_hist_ac;
  float u,v,max, **hist, *hist_ds, *hist_ac;
  char *sp, *filename;

  printf("Calculating histogram of bw image %s...\n",in_filename);
  hist_ds=alloc1df(glevel); 
  hist_ac=alloc1df(glevel); 
  hist=alloc2df(nrow,glevel);
  max_hist_ds=max_hist_ac=(nrow-scale-1)/2;
  v=1.0/m/n;

  for (l=0; l<glevel; l++) hist_ds[l]=0;
  for (j=0; j<n; j++) 
    for (i=0; i<m; i++) 
      hist_ds[(int)img[i][j]]+=v;  // find density 
  max=0;
  for (l=0; l<glevel; l++) 
    if (max < hist_ds[l]) { max=hist_ds[l]; L=l; } // find max

  //  printf("K=%d, L=%d, max=%f\n",K,L,max); pause();

  hist_ac[0]=hist_ds[0];
  for (l=1; l<glevel; l++) 
    hist_ac[l]=hist_ac[l-1]+hist_ds[l]; // accumulative
  
  for (l=0; l<glevel; l++) {
    hist_ac[l]=max_hist_ac*hist_ac[l];
    hist_ds[l]=max_hist_ds*hist_ds[l]/max;
    //      printf("(%3d %.2f %.2f) ",l,hist_ds[l],hist_ac[l]);
    //      if (l==L) printf("hist_ds[%d][%d]=%f, max=%f\n",k,l,hist_ds[l],max);
    if (!(l%32)) v=255; else v=0;
    for (i=0; i<scale; i++) hist[i][l]=hist[i][l+1]=v; 
    for (i=scale; i<scale+hist_ds[l]; i++) 
      hist[i][l]=255;
    for (i=scale+max_hist_ds; i<scale+max_hist_ds+hist_ac[l]; i++) 
      hist[i][l]=255;
  }
  vflip(hist,nrow,glevel);

  filename=in_filename;
  sp=strstr(in_filename,".p");
  if (sp) strcpy(sp,".hist");
  else strncat(filename,".hist",5);
  printf("Histogram filename: %s\n",filename);
  //  sprintf(fname, "%s_hist",in_filename);
  strcpy(filename,"histogram");
  display_bw_image(filename,hist,nrow,glevel);
  printf("show histogram image: %s\n",filename);

  free(hist_ds); 
  free(hist_ac);
  dealloc2df(hist,nrow);
}



get_magicnumber(char *fname)
{
  char word[10];
  char magic[4];
  FILE *fp;

  if ( (fp = fopen(fname, "r")) == NULL) 
    { printf("Unable to open file: %s for reading, exiting.\n", fname);
      exit(0);
    }
  if (getword(fp,word)!=0) eof_err();
  fclose(fp);
  strcpy(magic,word);
  if      (strcmp(magic,"P5")==0) return 0;   // bw image
  else if (strcmp(magic,"P6")==0) return 1;   // color image
  else {
    printf("\nBad file type, use raw grayscale (pgm) or raw color (ppm) file.\n");
    exit(0);
  }
}



display_bw_image(filename,tmp,m,n)
float **tmp;
char filename[];
int m,n;
{ int i,j,k,l,s;
  float v, min, max;
  unsigned char w;
  FILE *fp;

  k=m; if (k<n) k=n;
  s=256/k;
  s=1;

  //  printf("Display black/white image %s of size (%d, %d).\n",filename,m,n);

  min=9e9; max=-min;
  for (i=0; i<m; i++)
    for (j=0; j<n; j++)
      { if (min>tmp[i][j]) min=tmp[i][j];  
	if (max<tmp[i][j]) max=tmp[i][j]; 
      }
  v=255.0/(max-min);

  //  printf("   min=%f, max=%f, v=%f, s=%d\n", min,max,v,s); 

  if ( (fp = fopen(filename, "w")) == NULL) 
    { fprintf(stderr, "Unable to open image %s for writing, exiting.\n",filename);
      exit(0);
    }
  fprintf(fp,"%s\n%d %d\n%d\n", "P5", s*n,s*m, 255);
  for (i=0; i<m; i++)
    for (l=0; l<s; l++)
      for (j=0; j<n; j++)
	for (k=0; k<s; k++)
	  putc(w=(tmp[i][j]-min)*v,fp);
  fclose(fp);      
}


display_color_image(filename,tmp,m,n)
float ***tmp;
char filename[];
int m,n;
{ int i,j,k,l,s;
  float v, min, max;
  unsigned char w;
  FILE *fp;

  k=m; if (k<n) k=n;
  s=256/k;
  s=1;

  printf("Display color image %s of size (%d, %d).\n",filename,m,n);

  min=9e9; max=-min;
  for (k=0; k<3; k++)
    for (i=0; i<m; i++)
      for (j=0; j<n; j++)
	{ if (min>tmp[k][i][j]) min=tmp[k][i][j];  
	  if (max<tmp[k][i][j]) max=tmp[k][i][j]; 
	}
  v=255.0/(max-min);

/*  printf("   min=%f, max=%f, v=%f, s=%d\n", min,max,v,s); */

  if ( (fp = fopen(filename, "w")) == NULL) 
    { fprintf(stderr, "Unable to open image %s for writing, exiting.\n",filename);
      exit(0);
    }
  fprintf(fp,"%s\n%d %d\n%d\n", "P6", s*n,s*m, 255);
  for (i=0; i<m; i++)
    for (l=0; l<s; l++)
      for (j=0; j<n; j++)
	for (k=0; k<s; k++)
	  { putc(w=(tmp[0][i][j]-min)*v,fp);
	    putc(w=(tmp[1][i][j]-min)*v,fp);
	    putc(w=(tmp[2][i][j]-min)*v,fp);
	  }
  fclose(fp);      
}




/************************************************************************************

  In the following, we list all the processing functions.

************************************************************************************/

negative()
{
  int i,j,k;
  float Max, w;

  Max=-999;
  if (color_out)
    for (i=0; i<M_out; i++)
      for (j=0; j<N_out; j++)
	for (k=0; k<3; k++)
	  {
	    w=temp3_out[k][i][j]=255.0-temp3_in[k][i][j];
	    if (Max<w) Max=w;
	  } 
  else 
    for (i=0; i<M_out; i++)
      for (j=0; j<N_out; j++)
	{
	  w=temp2_out[i][j]=255.0-temp2_in[i][j];
	  if (Max<w) Max=w;
	}
}


fdct2d(x,m,n,inverse)    /* size has to be power of 2 */
float **x;
int m,n,inverse;
{
  int i,j,k;
  float *y;

  k=n; if (m>n) k=m;
  y = (float *) malloc(k*sizeof(float));

  printf("   row transform...\n");
  for (i=0; i<m; i++)
    { for (j=0; j<n; j++) y[j]=x[i][j]; 
      fdct(y,n,inverse);
      for (j=0; j<n; j++) x[i][j]=y[j];  
    }
  printf("   column transform...\n");
  for (j=0; j<n; j++)
    { for (i=0; i<m; i++) y[i]=x[i][j];
      fdct(y,m,inverse);
      for (i=0; i<m; i++) x[i][j]=y[i];
    }
  free(y);
}



dct2d(x,m,n,inverse)    /* for arbitrary size DCT2D */
float **x;
int m,n,inverse;
{
  int i,j,k;
  float **t,**y, u0,v0,u,v,w,wm,wn;

  k=m; if (n>m) k=n;
  wm=Pi/2/m, wn=Pi/2/n;
  u0=sqrt(1.0/m); u=sqrt(2.0/m);   
  v0=sqrt(1.0/n); v=sqrt(2.0/n); 
  t=alloc2df(k,k); y=alloc2df(m,n);

/* preparing for column transform matrix */  
  for (i=0; i<m; i++)
    for (j=0; j<m; j++)
      if (inverse) t[i][j]=cos( (2*i+1)*j*wm );
      else t[i][j]=cos( (2*j+1)*i*wm );
/* matrix multiplication for column transform */
  for (j=0; j<n; j++)                 /* for all columns */
    for (i=0; i<m; i++)               /* for each element in the column */
      { y[i][j]=0;
	for (k=0; k<m; k++)
	  {
	    if (inverse)
	      if (k) w=u;  else w=u0; 
	    else w=1;
	    y[i][j]+=w*t[i][k]*x[k][j];
	  }
      }
/* preparing for row transform matrix */  
  for (i=0; i<n; i++)
    for (j=0; j<n; j++)
      if (inverse) t[i][j]=cos( (2*j+1)*i*wn );
      else t[i][j]=cos(  (2*i+1)*j*wn );
/* matrix multiplication for row transform */
  for (i=0; i<m; i++)                 /* for all rows */
    for (j=0; j<n; j++)               /* for each element in the row */
      { x[i][j]=0;
	for (k=0; k<n; k++)
	  { if (inverse)
	      if (k) w=v;  else w=v0; 
	    else w=1;
	    x[i][j]+=w*y[i][k]*t[k][j];
	  }
      }
  if (!inverse)
    { x[0][0]*=u0*v0;
      for (i=1; i<m; i++) x[i][0]*=u*v0;
      for (j=1; j<n; j++) x[0][j]*=u0*v;
      for (i=1; i<m; i++)
	for (j=1; j<n; j++)
	  x[i][j]*=u*v;
    }  
  
  dealloc2df(t,k);  dealloc2df(y,m);	

}


/* dct based on fft, fast */

fdct(x,n,inverse)
float *x;
int n,inverse;
{ int i,j,k;
  float *yr,*yi;
  float a;

  yr=(float *) malloc(n*sizeof(float));
  yi=(float *) malloc(n*sizeof(float));

  if (inverse)
    for (i=0; i<n; i++)
      { if (i) x[i]*=Sqrt2;  a=-Pi*i/n/2;
	yr[i]=x[i]*cos(a); yi[i]=-x[i]*sin(a);
      }
  else
    for (i=0; i<n/2; i++)
      { yr[i]=x[2*i]; yr[n-1-i]=x[2*i+1]; yi[i]=yi[i+n/2]=0.0; }
  
  fft(yr,yi,n,inverse);     /* forward fft  */

  if (inverse)
    for (i=0; i<n/2; i++)
      { x[2*i]=yr[i]; x[2*i+1]=yr[n-1-i]; }
   else 
    { for (i=0; i<n; i++)
	{ a=-Pi*i/n/2;
	  x[i]=(cos(a)*yr[i]-sin(a)*yi[i])*Sqrt2;
	}
      x[0]/=Sqrt2;
    }

  free(yr); free(yi);
}


/* dct by definition. slow */
dct(x,n,inverse)
float x[];
int n,inverse;
{ int mmax,i,j;
  float *y,u,u0,w,t;

  w=0.5*Pi/n;  
  u0=sqrt(1.0/n); u=sqrt(2.0/n); 
  y=(float *)malloc(n*sizeof(float));
  
  if (inverse)
    for (i=0; i<n; i++)
      { y[i]=u0*x[0];
	for (j=1; j<n; j++)
	  y[i]+=u*x[j]*cos((2*i+1)*j*w);
      }
  else
    for (i=0; i<n; i++)
      { y[i]=0;
	for (j=0; j<n; j++)
	  y[i]+=x[j]*cos((2*j+1)*i*w);
	if (i) y[i]*=u; else y[0]*=u0;
      }
  for (i=0; i<n; i++) x[i]=y[i];  
  free(y);
}


int howo(i,m)    /*  converts a sequency index i to Haramard index j  */
int i,m;
{ int j,k;
/*  j=i>>1;  i=i^j;  /* converting binary to Gray code */
  i=i^(i>>1);
  j=0;
  for (k=0; k<m; ++k)
    j=(j << 1) | (1 & (i >> k));    /* bit-reversal */
  return j;
}

wht(x,m,sequency,inverse)
float x[];
int m,inverse,sequency;
{ int mmax,step,i,j,k,j1,n,nu,N;
  float *y,t;

  N=pow(2,m);
  y=(float *)malloc(N*sizeof(float));

  for (i=0; i<m; i++) {         /* for log N stages */
    n=pow(2,m-1-i);             /* length of section */
    k=0;
    while (k<N-1) {             /* before reaching end */
      for (j=0; j<n; j++) {     /* for each section */
	j1=k+j;
	t=x[j1]+x[j1+n];
	x[j1+n]=x[j1]-x[j1+n];
	x[j1]=t;
      }
      k+=2*n;
    }
  }
  if (inverse)        /* only for inverse transform */
    for (i=0; i<N; i++) x[i]=x[i]/N;

  if (sequency) {     /* converting to sequency (Walsh) order */
/*
    printf("\n before reordering...\n");
    for (i=0; i<N; i++)  printf("%3.0f ",x[i]);
    printf("\n");
*/ 
    for (i=0; i<N; i++) { j=howo(i,m);  y[i]=x[j]; }
    for (i=0; i<N; i++) x[i]=y[i];
/*
    printf("\n after reordering...\n");
    for (i=0; i<N; i++)  printf("%3.0f ",x[i]);
    printf("\n");
*/
    free(y);
  }
}


rgb_hsi(R,G,B,H,S,I,k) // based on RGB Cube model
float R, G, B, *H, *S, *I;
int k;
{ float r, g, b, w, i, min=1.e-6;
  *I=(i=R+G+B)/3.0; 
  r=R/i; g=G/i; b=B/i;
  //  printf("r=%6.2f, g=%6.2f, b=%6.2f\n",r,g,b); 

  if (R==G && G==B) 
    { *S=0; *H=0; }
  else 
    { w=0.5*(R-G+R-B)/sqrt((R-G)*(R-G)+(R-B)*(G-B));
      if (w>1) w=1;
      if (w<-1) w=-1;
      *H=acos(w);
      if (*H < 0) { printf("H<0: %f\n",*H); pause(); }
      if (B > G) *H=2*Pi-*H;
      if (r <= g && r <= b) *S=1-3*r;
      if (g <= r && g <= b) *S=1-3*g;
      if (b <= r && b <= g) *S=1-3*b;
    }
}

hsi_rgb(H,S,I,R,G,B) // based on RGBCube model
float *R, *G, *B, H, S, I;
{ float r, g, b;

//  printf("I=%f, H=%f, S=%f\n\n", I, H*180/Pi, S);

  if (S>1) S=1;
  if (I>1) I=1;
  if (S==0)
     *R=*G=*B=I;
  else
    { 
      if ( (H >= 0) && (H <2*Pi/3) )
	{ b=(1-S)/3;
	  r=(1+S*cos(H)/cos(Pi/3-H))/3;
	  g=1-r-b;
	}
      else if ( (H>=2*Pi/3) && (H<4*Pi/3) )
	{ H=H-2*Pi/3;
	  r=(1-S)/3;
	  g=(1+S*cos(H)/cos(Pi/3-H))/3;
	  b=1-r-g;
	} 
      else if ( (H>=4*Pi/3) && (H<2*Pi) )
	{ H=H-4*Pi/3;
	  g=(1-S)/3;
	  b=(1+S*cos(H)/cos(Pi/3-H))/3;
	  r=1-b-g;
	}
      else
	{ printf("\nH out of range: %f\n",H*180/Pi); pause();}
      if (r<0 || g<0 || b<0) 
	{ printf("\n\nr,g,b: %f, %f, %f",r,g,b);
	  printf("  h,s,i: %f, %f, %f\n",H,S,I);
	  pause();
	}
      if (r < 0) r=0;   if (g < 0) g=0;   if (b < 0) b=0;
      *R=3*I*r; *G=3*I*g; *B=3*I*b;
      if (*R > 1) *R=1;   if (*G > 1) *G=1;   if (*B > 1) *B=1;
    }
  //  printf("R=%6.2f, G=%6.2f, B=%6.2f\n\n",*R,*G,*B);  
}


RGB_HSI(r,g,b,h,s,v) // based on hexcone model
float r,g,b,*h,*s,*v;
{
  float max,min,delta;
  max=myMAX(r,g,b);
  min=myMIN(r,g,b);
  *v=max;
  if (max==min || max==0) { *s=*h=0.0; return 1; }
  else *s=(max-min)/max;
  delta=max-min;
  if (delta==0.0) { printf("(%f, %f, %f), max=%f, min=%f, delta=%f\n",r,g,b,max,min,delta); pause(); }
  if (r==max)
    *h=(g-b)/delta;
  else if (g==max)
    *h=2.0+(b-r)/delta;
  else if (b==max)
    *h=4.0+(r-g)/delta;
  *h*=Pi/3;
  if (*h<0.0) *h+=2*Pi;
}

HSI_RGB(h,s,v,r,g,b) // based on hexcone model
float *r, *g, *b, h, s, v;
{
  float f,p,q,t;
  int i;

  if (s>1.0) s=1.0;
  if (v>1.0) v=1.0;
  if (s==0.0) 
    { *r=*g=*b=v; 
    //      if (h!=0.0) { printf("Error, return..."); return 0; }
    }
  if (h>=2*Pi) h=0.0;
  h=h*3/Pi;
  i=h;
  f=h-i;
  p=v*(1-s);
  q=v*(1-s*f);
  t=v*(1-s*(1-f));
  switch (i){
  case 0:
    *r=v; *g=t; *b=p; break;
  case 1:
    *r=q; *g=v; *b=p; break;
  case 2:
    *r=p; *g=v; *b=t; break;
  case 3:
    *r=p; *g=q; *b=v; break;
  case 4:
    *r=t; *g=p; *b=v; break;
  case 5:
    *r=v; *g=p; *b=q; 
  }
}

float myMAX(x,y,z)
     float x,y,z;
{
  if (x>=y && x>=z) return x;
  if (y>=x && y>=z) return y;
  if (z>=x && z>=y) return z;
}

float myMIN(x,y,z)
     float x,y,z;
{
  if (x<=y && x<=z) return x;
  if (y<=x && y<=z) return y;
  if (z<=x && z<=y) return z;
}


sort(bin,k)
short *bin; int k;
{
  int i,j;
  short w;
  //  printf("\nk=%d\n",k);
  //  for (i=0; i<k; i++) printf("%d ",bin[i]);
  for (i=0; i<k-1; i++)
    for (j=i+1; j<k; j++)
      if ( bin[i]>bin[j] )
	{ w=bin[i]; bin[i]=bin[j]; bin[j]=w; }

  //  printf("\n");
  //  for (i=0; i<k; i++) printf("%d ",bin[i]);
}

testfft1d()
{
  float xr[8], xi[8];
  int i,j,k,m=8;

  for (i=0; i<m; i++)
    { xr[i]=i; xi[i]=0.0; }
  for (i=0; i<m; i++)
    printf("%f, ",xr[i]);
  printf("\n");

  fft(xr,xi,m,0);

  for (i=0; i<m; i++)
    printf("%f, ",xr[i]);
  printf("\n");
  for (i=0; i<m; i++)
    printf("%f, ",xi[i]);
  printf("\n");

  fft(xr,xi,m,1);

  for (i=0; i<m; i++)
    printf("%f, ",xr[i]);
  printf("\n");
  for (i=0; i<m; i++)
    printf("%f, ",xi[i]);
  printf("\n");

}


fdft2d(xxr,xxi,m,n,inverse)
float **xxr,**xxi;
int m,n,inverse;
{
  int i,j,k;
  float *xr,*xi,u,v,w;
  
  k=m; if (n>m) k=n;
  xr = (float *) malloc(k*sizeof(float));
  xi = (float *) malloc(k*sizeof(float));

  printf("Forward 2DFFT\n   row transform...\n");
  for (i=0; i<m; i++)
    { for (j=0; j<n; j++)
	{ xr[j]=xxr[i][j];  xi[j]=0; }
      fft(xr,xi,n,inverse);
      for (j=0; j<n; j++)
	{ xxr[i][j]=xr[j]; xxi[i][j]=xi[j]; }
    }
  printf("   column transform...\n");
  for (j=0; j<n; j++)
    { for (i=0; i<m; i++)
	{ xr[i]=xxr[i][j]; xi[i]=xxi[i][j]; }
      fft(xr,xi,m,inverse);
      for (i=0; i<m; i++)
	{ xxr[i][j]=xr[i]; xxi[i][j]=xi[i]; }
    }
  free(xr);  free(xi);
}

ifdft2d(xxr,xxi,m,n,inverse)
float **xxr,**xxi;
int m,n,inverse;
{
  int i,j,k;
  float *xr,*xi,u,v,w;
  
  k=m; if (n>m) k=n;
  xr = (float *) malloc(k*sizeof(float));
  xi = (float *) malloc(k*sizeof(float));

  printf("   column transform...\n");
  for (j=0; j<n; j++)
    { for (i=0; i<m; i++)
	{ xr[i]=xxr[i][j]; xi[i]=xxi[i][j]; }
      fft(xr,xi,m,inverse);
      for (i=0; i<m; i++)
	{ xxr[i][j]=xr[i]; xxi[i][j]=xi[i]; }
    }
  printf("Forward 2DFFT\n   row transform...\n");
  for (i=0; i<m; i++)
    { for (j=0; j<n; j++)
	{ xr[j]=xxr[i][j];  xi[j]=0; }
      fft(xr,xi,n,inverse);
      for (j=0; j<n; j++)
	{ xxr[i][j]=xr[j]; xxi[i][j]=xi[j]; }
    }
  free(xr);  free(xi);
}


dft2d(xr,xi,m,n,inverse)    /* for arbitrary size DFT2D */ 
float **xr,**xi;
int m,n,inverse;
{
  int i,j,k;
  float **yr,**yi,**tr,**ti,af,u,v,w,wm,wn;

  k=m; if (n>m) k=n; 
  wm=Pi*2/m, wn=Pi*2/n;  w=1.0/sqrt((float)(m*n));
  tr=alloc2df(k,k); ti=alloc2df(k,k);    /* transfor matrices */
  yr=alloc2df(m,n); yi=alloc2df(m,n);    /* spectral matrics */

/* preparing for column transform matrix */  
  printf("Column transform...\n");
  for (i=0; i<m; i++)
    for (j=0; j<m; j++)
      { af=-i*j*wm;
	if (inverse) af=-af;
	tr[i][j]=w*cos(af); ti[i][j]=w*sin(af);
      }
/* matrix multiplication for column transform */
  for (j=0; j<n; j++)                 /* for all columns */
    for (i=0; i<m; i++)               /* for each element in the column */
      { yr[i][j]=yi[i][j]=0;
	for (k=0; k<m; k++)
	  { yr[i][j]+=tr[i][k]*xr[k][j]-ti[i][k]*xi[k][j];
	    yi[i][j]+=ti[i][k]*xr[k][j]+tr[i][k]*xi[k][j];
	  }
      }
/* preparing for row transform matrix */  
  printf("Row transform...\n");
  for (i=0; i<n; i++)
    for (j=0; j<n; j++)
      { af=-i*j*wn;
	if (inverse) af=-af;
	tr[i][j]=w*cos(af); ti[i][j]=w*sin(af);
      }
/* matrix multiplication for row transform */
  for (i=0; i<m; i++)               /* for each element in the column */
    for (j=0; j<n; j++)                 /* for all columns */
      { xr[i][j]=xi[i][j]=0;
	for (k=0; k<n; k++)
	  { xr[i][j]+=yr[i][k]*tr[k][j]-yi[i][k]*ti[k][j];
	    xi[i][j]+=yi[i][k]*tr[k][j]+yr[i][k]*ti[k][j];
	  }
      }
  dealloc2df(tr,k);  dealloc2df(ti,k); 
  dealloc2df(yr,m);  dealloc2df(yi,m);	
}


fft2d(xxr,xxi,m,n,inverse)   /* 2D Fourier transform */
float **xxr, **xxi;
int m,n,inverse;
{ float *xr, *xi;
  int i,j,k;

  k=m; if (n>m) k=n;
  xr = (float *) malloc(k*sizeof(float));
  xi = (float *) malloc(k*sizeof(float));

  printf("\nRow xform...\n");
  for (j=0; j<n; j++)
    { 
      for (i=0; i<m; i++)
	{ xr[i]=xxr[i][j]; xi[i]=xxi[i][j]; }
      fft(xr,xi,m,inverse);
      for (i=0; i<m; i++)
	{ xxr[i][j]=xr[i]; xxi[i][j]=xi[i]; }
    }
  printf("\nColumn xform...\n");
  for (i=0; i<m; i++)
    { 
      for (j=0; j<n; j++)
	{ xr[j]=xxr[i][j]; xi[j]=xxi[i][j]; }
      fft(xr,xi,n,inverse);
      for (j=0; j<n; j++)
	{ xxr[i][j]=xr[j]; xxi[i][j]=xi[j]; }
    }
  free(xr);  free(xi);
}


fft(xr,xi,N,inverse)
float xr[],xi[];
int N,inverse;
{ int mmax,step,i,j,k,j1,m,n;
  float arg,s,c,w,tempr,tempi;
/*
  if (inverse) printf("IFFT of size %d \n",N);
  else printf("FFT of size %d \n",N);
*/
  m=log((float)N)/log(2.0);

  for (i=0; i<N; ++i) {       /* bit reversal */
    j=0;
    for (k=0; k<m; ++k)
      j=(j << 1) | (1 & (i >> k));
    if (j < i) 
      { SWAP(xr[i],xr[j]);  SWAP(xi[i],xi[j]); }
  }
  for (i=0; i<m; i++) {         /* for log N stages */
    n=w=pow(2.0,(float)i);  w=Pi/n; if (inverse) w=-w;
    k=0;
    while (k<N-1) {             /* for N components */
      for (j=0; j<n; j++) {     /* for each section */
	arg=-j*w; c=cos(arg); s=sin(arg); 
	j1=k+j;
	tempr=xr[j1+n]*c-xi[j1+n]*s;
	tempi=xi[j1+n]*c+xr[j1+n]*s;
	xr[j1+n]=xr[j1]-tempr;
	xi[j1+n]=xi[j1]-tempi;
	xr[j1]=xr[j1]+tempr;
	xi[j1]=xi[j1]+tempi;
      }
      k+=2*n;
    }
  }
  arg=1.0/sqrt((float)N);
  for (i=0; i<N; i++) 
    { xr[i]*=arg; xi[i]*=arg; }
}


/**********************************************************************

Given n data points (not necessarily equally spaced) of a time function 
data(time), nderiv finds the dth order numerical derivative of the function
at the kth data point.

***********************************************************************/
float nderiv(data,time,k,d,n)
     float *data, *time;
     int k,d,n;
{
  float u,v,w,dy;
  int i,j,d2=d/2;

  if (k<d/2 || k>=n-d/2) 
    { printf("index %d outside range (%d -- %d)...",k,d/2,n-d/2); pause(); }

  u=1.0;
  for (i=1; i<=d2; i++) 
    if (i) u*=(time[k]-time[k-i])*(time[k]-time[k+i]);
  dy=0;
  for (i=-d2; i<=d2; i++) {
    v=1.0;
    for (j=-d2; j<=d2; j++) 
      if (i!=j) v=v*(time[k+i]-time[k+j]);
    if (i) dy+=data[k+i]*u/(time[k]-time[k+i])/v;
    else 
      for (j=-d2; j<=d2; j++) 
	if (j) dy+=data[k]/(time[k]-time[k+j]);
  }
  return dy;
}