#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <tiff.h>
#include <unistd.h>
#include "mytypes.h"
#include "constants.h"
#include "motionflow.h"
#include "mytiff.h"
#include "ipl.h" 
#include "cv.h"
#include "cvtypes.h"
#include "highgui.h"
#include "tracker.h"

extern IplImage* iplCloneImage(const IplImage* img);
extern void iplRGB2HSV(IplImage* rgbImage, IplImage* hsvImage);

#ifdef LESLIE
static int computedMotionFlow = 0;
#else
  #ifdef TESTMFLOW
  static int computedMotionFlow = 0;
  #else
     static int computedMotionFlow = 1;
  #endif
#endif

#ifdef TESTMFLOW
char BMAPDIR[1000] = "../../movies/bmaps/vaibhav2/";
char TIFFDIR[1000] = "../../movies/tifs/vaibhav2/";
char TIFFBASE[1000] = "zvaibhav";
char BMAPBASE[1000] = "bmvaibhav";

#else

#define BMAPDIR "../../movies/bmaps/d2girl3/"
#define TIFFDIR "../../movies/tifs/d2girl3/"
#define TIFFBASE "zd2girl"
#define BMAPBASE "bmd2girl"

#endif


#define DIGIT2

#define H_WEIGHT .9
#define S_WEIGHT .9
#define V_WEIGHT .2

#define WEIGHT_THRESH .5
#define BG_WEIGHT 1 /*.3*/
#define SKIN_WEIGHT 1/*.6*/
#define LOCALITY_WEIGHT 0 /*.6 */

/* LK threshold, if motion is more than this,
 * then consider it bad LK analysis and use average of
 * neighboring (valid) motions
 */
#define LK_THRESH 5

#define LOCALITY 3

/* set this lower for more critical */
#define MAX_SKIN_DIST .5

/* set this higher for more critical */
#define MAX_BG_DIST 25

/* motion history constants */
#define MOHI_MAX 1000
#define MOHI_MIN 0
#define MOHI_AP_SIZE 3

static HSV skinMean;

void getSkinMean();
/* returns "difference" between pixel and skin mean */
float skinDiff(HSV pixel);
void removeNonSkin(IplImage* ipl, float* iWeights);
IplImage* getBackground(int start, int n);
void removeBackground(IplImage* img, IplImage* bg, float* iWeights);
/* neighbors is how many surrounding pixels to pick:
 * 1 = only incident neighbors,
 * 2 = 5x5 super pixel with i,j pixel at center
 */
void localityConstraint(IplImage* previousSil, int neighbors, float* iWeights);
void useContours(IplImage* img);
void filterToWeights(IplImage* ipl, float* imgWeights);
void HSV2HSVCool(HSV* p);
IplImage* getSilhouette(int i);
void putSilhouettte(IplImage* img, int i);
/* moves silhouette according to LK optical flow */
void moveSilhouette(IplImage* img, int frameNum);
CvPoint2D32f* findFeatures(IplImage* img, int* count);
void moveFeatures(IplImage* img, CvPoint2D32f* originalFeaturesLocations, CvPoint2D32f* newFeaturesLocations, int count);

void HSV2HSVCool(HSV* p) {
  HSV tmp;

  p->h = (float) p->h / 255.0 * 2*M_PI; 
  p->s = (float) p->s / 255.0;
  p->v = (float) p->v / 255.0;

  tmp.h = p->h;
  tmp.s = p->s;
  tmp.v = p->v;

  /* convert to space without theta discontinuity in hue */
  /*  p->h = tmp.s * cos(tmp.h);
  p->s = tmp.s * sin(tmp.h);
  p->v = tmp.v; */
}

/* clears an array with val */
void clearArrayF(float* raster, int width, int height, float val) {
  int i;

  for(i = 0;i < width*height; i++) {
    raster[i] = val;
  }
}

/* converts a num to a string and pads with zeros */
void numString(int n, char* num) {
#ifdef DIGIT3
    if(n < 10) {
      sprintf(num, "00%d", n);
    }
    else if(n < 100) {
      sprintf(num, "0%d", n);
    }
    else sprintf(num, "%d", n);
#endif

#ifdef DIGIT2
    if(n < 10) {
      sprintf(num, "0%d", n);
    }
    else
      sprintf(num, "%d", n);
#endif
}


/* setups up the silhouettes for each frame for computation
   needed by getMotionFlow */
int computeMotionFlow(int max) {
  int i;
  uint32 w, h;
  uint32* img;
  char filename[80];
  char num[5];
  CvSize size = {IMAGE_WIDTH, IMAGE_HEIGHT};
  IplImage* ipl;
  IplImage* iplGauss;
  float* imgWeights;
  IplImage* silhouette; 

  if(sbrk(50000000) == (void*) -1) {
    fprintf(stderr, "error, unable to allocate more memory for motion
    computation!\n");
    exit(1);
  }
  if(computedMotionFlow)
    return 1;
  
  imgWeights = (float*) malloc(sizeof(float)*IMAGE_WIDTH * IMAGE_HEIGHT);
  assert(imgWeights != NULL);

  ipl = cvCreateImage(size, IPL_DEPTH_8U, 3);
  iplGauss = cvCreateImage(size, IPL_DEPTH_8U, 3);
  IplImage* bg = getBackground(max-2, 2); 

  /* compute the mean skin value */
  getSkinMean();

  for(i = 1; i <= max; i++) {
    clearArrayF(imgWeights, IMAGE_WIDTH, IMAGE_HEIGHT, 1);
    numString(i, num);
    sprintf(filename, "%s%s%s.tif", TIFFDIR, TIFFBASE, num);
    img = readTiff(filename, &w, &h);
    assert(w == IMAGE_WIDTH);
    assert(h == IMAGE_HEIGHT);
    
    tiffToIpl(img, ipl->imageData);
    fprintf(stderr, "--- Processing frame %d (%s) ---\n", i, filename);

    /* various filters */
    iplFixedFilter(ipl, iplGauss,  (IplFilter) IPL_GAUSSIAN_5x5);
    iplFixedFilter(iplGauss, ipl,  (IplFilter) IPL_GAUSSIAN_5x5);
    iplFixedFilter(ipl, iplGauss,  (IplFilter) IPL_GAUSSIAN_5x5);

    /* background */
    fprintf(stderr,"removing background...");
    removeBackground(iplGauss, bg, imgWeights);  
    filterToWeights(iplGauss, imgWeights); 

    /* skin */
    fprintf(stderr,"removing nonskin...");
    removeNonSkin(iplGauss, imgWeights);
    filterToWeights(iplGauss, imgWeights); 

    
    /* load previous silhouette for locality constraint */
    /*    silhouette = getSilhouette(i);
    fprintf(stderr,"locality constraint...");
    localityConstraint(silhouette, LOCALITY, imgWeights); 
    filterToWeights(iplGauss, imgWeights); 
    cvReleaseImage(&silhouette);*/

    /* more filters */
    fprintf(stderr,"eroding and gaussian...");
    iplErode(iplGauss, ipl, 1);
    iplFixedFilter(ipl, iplGauss,  (IplFilter) IPL_GAUSSIAN_5x5); 


    fprintf(stderr, "applying tracker..."); 
    tracker(iplGauss); 


#ifdef TESTMFLOW /* tests some 'new' contour matching algorithms... */
/*     fprintf(stderr, "applying contour calc's...");  */
/*            useContours(iplGauss);  */

#endif

#ifdef LK
    /* moves silhouette according to LK optical flow */
    moveSilhouette(iplGauss, i);
#endif

    fprintf(stderr, "writing silhouette...");
    /* iplGauss contains the new motion-moved silhouette */
    putSilhouettte(iplGauss, i+1);

    fprintf(stderr, "done!\n");
   
#ifdef TESTMFLOW
/*      cvvNamedWindow("1", 0);   */
/*      cvvShowImage("1",iplGauss);   */
/*      while(1);   */
#endif
    free(img);
  }
  cvReleaseImage(&iplGauss); 
  cvReleaseImage(&ipl);
  free(imgWeights);
  computedMotionFlow = 1;
  return 1;
  
}

image_t* getMotionFlow(int start, int nFrames, IplImage** mhiReturn) {
  int i /* ,j */;
/*   int c;  */
  image_t* vfield;
  float sumX=0;
  float sumY=0;
  int good=0;
  assert(start > 0);
  IplImage* sil;
  IplImage* mhi;
  IplImage* mhiGauss;
  IplImage* mhiDx;
  IplImage* mhiDy;
  IplImage* tmpImage;

  assert(mhiReturn != NULL);
  assert(*mhiReturn == NULL);

/*   IplImage* mask; */
/*   IplImage* orientation; */
/*   int good=0, bad=0; */

  char num[5];
  CvSize size = {IMAGE_WIDTH, IMAGE_HEIGHT};
  char filename[80];
  int index;
  FILE* xOut, *yOut, *xDirOut, *yDirOut, *indout, *out;
  double /* orient, */ mag;

  if(!computedMotionFlow) {
    fprintf(stderr, "Sorry, you didn't computeMotionFlow first!\n");
    exit(0);
  }

  vfield = (image_t*) malloc(sizeof(image_t));
  mhi = cvCreateImage(size, IPL_DEPTH_32F, 1);
  *mhiReturn = mhi;
  mhiGauss = cvCreateImage(size, IPL_DEPTH_32F, 1);
  mhiDx = cvCreateImage(size, IPL_DEPTH_32F, 1);  
  mhiDy = cvCreateImage(size, IPL_DEPTH_32F, 1);
  sil = cvCreateImage(size, IPL_DEPTH_8U, 1);


#ifdef UNREAL 
  for(c = 0; c < 10; c++) { 

    for(i = 0; i < IMAGE_HEIGHT; i++) {
      for(j = 0;j < IMAGE_WIDTH; j++) {
	sil->imageData[(i)*IMAGE_WIDTH+j] = 0;
      }
      
    }

    for(i = 0; i < IMAGE_HEIGHT; i++) {
      for(j = 0;j < IMAGE_WIDTH; j++) {
	if(i > 238 && i < 248 && j > 355 && j < 365)
	  sil->imageData[(i+c*5)*IMAGE_WIDTH+j] = 255;
      }
      
    }
    cvUpdateMotionHistory(sil, mhi, (double) (c+1) / 10.0, 1);

  } 
#else
  for(i = 0; i < nFrames; i++) {
    index = start+i;

    numString(index, num);

    sprintf(filename, "%s%s%s.tif", BMAPDIR, BMAPBASE, num);

    tmpImage = getImage(filename);
    
    iplColorToGray(tmpImage, sil);
    cvUpdateMotionHistory(sil, mhi, (double) (i+1) / nFrames, 1);
  }
#endif


  out = fopen("orientations.txt", "w"); 
  indout = fopen("indices.txt", "w");
  xDirOut = fopen("xdir.txt", "w");
  yDirOut = fopen("ydir.txt", "w");
  xOut = fopen("xpos.txt", "w");
  yOut = fopen("ypos.txt", "w");
  

/*   iplFixedFilter(mhi, mhiGauss, IPL_GAUSSIAN_3x3); */

/* #ifdef USEFILTER */
  iplFixedFilter(mhi, mhiDx, IPL_PREWITT_3x3_V);
  iplFixedFilter(mhi, mhiDy, IPL_PREWITT_3x3_H);

#ifdef TESTMFLOW
/*     cvvNamedWindow("horiz", 0);    */
/*     cvvShowImage("horiz", mhiDx);    */

/*     cvvNamedWindow("vert", 0);    */
/*     cvvShowImage("vert", mhiDy);    */

#endif

  for(i =0 ; i < IMAGE_WIDTH*IMAGE_HEIGHT; i++) {
    mag = sqrt(((float*) mhiDx->imageData)[i]*((float*) mhiDx->imageData)[i]+
	       ((float*) mhiDy->imageData)[i]*((float*) mhiDy->imageData)[i]);
    if(mag > 0) { /* set this value to cut off non moving pts */
/*       orient = atan2(((float*) mhiDy->imageData)[i], ((float*) mhiDx->imageData)[i]); */
/*       good++; */
/*        fprintf(out, "%f ", orient);  */
/*        fprintf(indout, "%d ", i);  */
      
      (*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].x = ((float*) mhiDx->imageData)[i];
      (*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].y = ((float*) mhiDy->imageData)[i];


#ifdef TESTMFLOW
 {

	  if((*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].x != 0 ||
	     (*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].y != 0) {
	    good++;
	    sumX += (*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].x;
	    sumY += (*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].y;
	    fprintf(yOut, "%d ", (int) i/IMAGE_WIDTH);
	    fprintf(xOut, "%d ",  i%IMAGE_WIDTH);
	    fprintf(xDirOut, "%f ", (*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].x);
	    fprintf(yDirOut, "%f ", (*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].y);

	  }
 };
#endif

    }
    else {
      (*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].x = 0;
      (*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].y = 0;
    }
  }
  
/*   fprintf(stderr, "mean: (%f, %f)\n", sumX, sumY); */
/*   fprintf(stderr, "good: %d\n", good); */
/* #endif */

#ifdef USECVMOTIONGRADIENT 

  mask = cvCreateImage(size, IPL_DEPTH_8U, 1);
  orientation = cvCreateImage(size, IPL_DEPTH_32F, 1);
  cvCalcMotionGradient(mhi, mask, orientation, MOHI_MAX, MOHI_MIN, MOHI_AP_SIZE);

  for(i = 0; i < IMAGE_WIDTH*IMAGE_HEIGHT; i++) {
    if(mask->imageData[i] == 1) {
      if(((float*) orientation->imageData)[i] >= 0 &&
	 ((float*) orientation->imageData)[i] <= 360) {

/* 	fprintf(stderr, "%f ", (float) ((float*) */
/* 				     orientation->imageData)[i] * M_PI/180.0);  */
	fprintf(out, "%f ", (float) ((float*)
				     orientation->imageData)[i] * M_PI/180.0); 
	fprintf(indout, "%d ", i);
	orient = ((float*) orientation->imageData)[i] * M_PI/180.0;
	(*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].x = cos(orient);
	(*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].y = sin(orient);

	if((*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].x != 0 &&
	   (*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].y != 0) {
#ifdef TESTMFLOW
	  
	  if((*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].x != 0 &&
	     (*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].y != 0) {
	    if(i > 232 && i < 300 && j > 350 && j < 370)
	      fprintf(stderr, "[%d][%d].x = %f, .y = %f\n",
		      (int) i/IMAGE_WIDTH, i%IMAGE_WIDTH, 
		      (*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].x,
		      (*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].y);

	}

#endif


	}
      }
    }
    else {
/*       ((float*) mhi->imageData)[i] = 0; */
      (*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].x = 0;
      (*vfield)[(int) i/IMAGE_WIDTH][i%IMAGE_WIDTH].y = 0;


    }
  }
  cvReleaseImage(&mask);
  cvReleaseImage(&orientation);

#endif 

  fclose(out); 
  fclose(indout); 
  fclose(xOut); 
  fclose(yOut); 
  fclose(xDirOut); 
  fclose(yDirOut); 


#ifdef TESTMFLOW
  cvvNamedWindow("1", 0);    
  cvvShowImage("1", mhi);    
  while(1); 
#endif

  cvReleaseImage(&mhiGauss);
  cvReleaseImage(&mhiDx);
  cvReleaseImage(&mhiDy);
  cvReleaseImage(&sil);

  return vfield;
}

void getSkinMean() {
  uint32* bmap;
  uint32 w, h;
  uint32* image1;
  float hSum=0, sSum=0, vSum=0; 
  int nPixels=0;
  int i, j;
  IplImage* ipl;
  IplImage* iplHSV;
  IplImage* sil;
  CvSize size = {IMAGE_WIDTH, IMAGE_HEIGHT};
  char filename[80];
  char num[10];

  numString(1, num);

  sprintf(filename, "%s%s%s.tif", BMAPDIR, BMAPBASE, num);
  bmap = readTiff(filename, &w, &h);
  assert(w == IMAGE_WIDTH);
  assert(h == IMAGE_HEIGHT);

  sprintf(filename, "%s%s%s.tif", TIFFDIR, TIFFBASE, num);
  image1 = readTiff(filename, &w, &h);
  assert(w == IMAGE_WIDTH);
  assert(h == IMAGE_HEIGHT);
  
  ipl = cvCreateImage(size, IPL_DEPTH_8U, 3);
  sil = cvCreateImage(size, IPL_DEPTH_8U, 3);
  tiffToIpl(bmap, sil->imageData);
  
  for(i = 0; i < IMAGE_HEIGHT * IMAGE_WIDTH; i++) {
    if(((char*) bmap)[4*i] == 0) {
      ((char*) image1)[4*i] = 0;
      ((char*) image1)[4*i+1] = 0;
      ((char*) image1)[4*i+2] = 0;
    }
  }
  
  tiffToIpl(image1, ipl->imageData);
  
  iplHSV = iplCloneImage(ipl);
  iplRGB2HSV(ipl, iplHSV); 

  for(i = 0; i < IMAGE_HEIGHT; i++) {
    for(j = 0; j < IMAGE_WIDTH; j++) {
      if(iplHSV->imageData[3*i*IMAGE_WIDTH+3*j] != 0 ||
	 iplHSV->imageData[3*i*IMAGE_WIDTH+3*j+1] != 0 ||
	 iplHSV->imageData[3*i*IMAGE_WIDTH+3*j+2] != 0) {
	hSum = hSum + (uint8) iplHSV->imageData[3*i*IMAGE_WIDTH+3*j];
	sSum = sSum + (uint8) iplHSV->imageData[3*i*IMAGE_WIDTH+3*j+1];
	vSum = vSum + (uint8) iplHSV->imageData[3*i*IMAGE_WIDTH+3*j+2];	
	nPixels++;
      }
    }
  }
  skinMean.h = hSum / (float) nPixels;
  skinMean.s = sSum / (float) nPixels;
  skinMean.v = vSum / (float) nPixels;

  /* renormalize to what we're used to for HSV and put in "good" color space*/
  HSV2HSVCool(&skinMean);
  fprintf(stderr, "computed skinMean: (%f, %f, %f)\n", skinMean.h, skinMean.s, skinMean.v);


#ifdef TESTMFLOW
  /*  cvvNamedWindow("foobar!", 0);
  cvvShowImage("foobar!",ipl);
  while(1);*/
#endif
  cvReleaseImage(&ipl); 
  cvReleaseImage(&sil);
  _TIFFfree(bmap);
  _TIFFfree(image1);
}

void localityConstraint(IplImage* previousSil, int neighbors, float* iWeights) {
  int i, j, a, b;
  int isNeighbor = 0;
  int badPixel;
  CvSize size = {IMAGE_WIDTH, IMAGE_HEIGHT};
  IplImage* imgDisplay = cvCreateImage(size, IPL_DEPTH_8U, 3);
  char* displayWeights;
  
  displayWeights = (char*) malloc(sizeof(char)*IMAGE_WIDTH*IMAGE_HEIGHT*3);


  for(i = 0; i < IMAGE_HEIGHT; i++) { 
    for(j = 0; j < IMAGE_WIDTH; j++) { 
      for(a = -neighbors; a <= neighbors; a++) {
				for(b = -neighbors; b <= neighbors; b++) {
					if(i+a < 0 || i+a > IMAGE_HEIGHT ||
						 j+b < 0 || j+b > IMAGE_WIDTH) {
						badPixel = 1;
						goto checkNeighbor;
					}
					
					if(previousSil->imageData[(3*(i+a))*IMAGE_WIDTH+(3*(j+b))] != 0
						 ||
						 previousSil->imageData[(3*(i+a))*IMAGE_WIDTH+(3*(j+b))+1] !=
						 0 ||
						 previousSil->imageData[(3*(i+a))*IMAGE_WIDTH+(3*(j+b))+2] !=
						 0) {
						isNeighbor = 1;
						goto checkNeighbor;
					}
				}
      }
    checkNeighbor:
      if(isNeighbor) { /* near neighbor */
/* 	iWeights[i*IMAGE_WIDTH+j] += .6; */
	displayWeights[3*i*IMAGE_WIDTH+3*j] = 255;
	displayWeights[3*i*IMAGE_WIDTH+3*j+1] = 255;
	displayWeights[3*i*IMAGE_WIDTH+3*j+2] = 255;


      }
      else { /* not near neighbor */
	iWeights[i*IMAGE_WIDTH+j] -= LOCALITY_WEIGHT;
	displayWeights[3*i*IMAGE_WIDTH+3*j] = 0;
	displayWeights[3*i*IMAGE_WIDTH+3*j+1] = 0;
	displayWeights[3*i*IMAGE_WIDTH+3*j+2] = 0;


      }
      if(badPixel) {
	iWeights[i*IMAGE_WIDTH+j] = -1000;
      }
      isNeighbor = 0;
      badPixel = 0;
    }
  } 

  /*  imgDisplay->imageData = displayWeights;
  cvvNamedWindow("skin", 0);
  cvvShowImage("skin",imgDisplay);
  */
  cvReleaseImage(&imgDisplay);
} 


void filterToWeights(IplImage* ipl, float* iWeights) {
  int i, j;
  for(i = 0; i < IMAGE_HEIGHT; i++) {
    for(j = 0; j < IMAGE_WIDTH; j++) {
      if(iWeights[i*IMAGE_WIDTH+j] <= WEIGHT_THRESH) {
	ipl->imageData[3*i*IMAGE_WIDTH+3*j] = 0;
	ipl->imageData[3*i*IMAGE_WIDTH+3*j+1] = 0;
	ipl->imageData[3*i*IMAGE_WIDTH+3*j+2] = 0;
      }
/*       else { */
/* 	ipl->imageData[3*i*IMAGE_WIDTH+3*j] = 255; */
/* 	ipl->imageData[3*i*IMAGE_WIDTH+3*j+1] = 255; */
/* 	ipl->imageData[3*i*IMAGE_WIDTH+3*j+2] = 255; */
/*       } */
    }
  }
}

void removeNonSkin(IplImage* ipl, float* iWeights) {
  int i, j;
  HSV pixel;
  IplImage* iplHSV = iplCloneImage(ipl);
  float dist;
  CvSize size = {IMAGE_WIDTH, IMAGE_HEIGHT};
  IplImage* imgDisplay = cvCreateImage(size, IPL_DEPTH_8U, 3);
  char* displayWeights;


  displayWeights = (char*) malloc(sizeof(char)*IMAGE_WIDTH*IMAGE_HEIGHT*3);
 
  iplRGB2HSV(ipl, iplHSV);

    
  for(i = 0; i < IMAGE_HEIGHT; i++) {
    for(j = 0; j < IMAGE_WIDTH; j++) {
      pixel.h = iplHSV->imageData[3*i*IMAGE_WIDTH+3*j];
      pixel.s = iplHSV->imageData[3*i*IMAGE_WIDTH+3*j+1];
      pixel.v = iplHSV->imageData[3*i*IMAGE_WIDTH+3*j+2];

      HSV2HSVCool(&pixel);
      dist = skinDiff(pixel);

      /*	fprintf(stderr, "dist: %f\n", dist); */
      if(dist > MAX_SKIN_DIST) { /* background */
	/*	ipl->imageData[3*i*IMAGE_WIDTH+3*j] = 0;
	ipl->imageData[3*i*IMAGE_WIDTH+3*j+1] = 0;
	ipl->imageData[3*i*IMAGE_WIDTH+3*j+2] = 0; */
	iWeights[i*IMAGE_WIDTH+j] -= SKIN_WEIGHT;
	displayWeights[3*i*IMAGE_WIDTH+3*j] = 0;
	displayWeights[3*i*IMAGE_WIDTH+3*j+1] = 0;
	displayWeights[3*i*IMAGE_WIDTH+3*j+2] = 0;
      }
      else { /* skin */
	displayWeights[3*i*IMAGE_WIDTH+3*j] = 255;
	displayWeights[3*i*IMAGE_WIDTH+3*j+1] = 255;
	displayWeights[3*i*IMAGE_WIDTH+3*j+2] = 255;
      }
    }
  }
  /*
  imgDisplay->imageData = displayWeights;
  cvvNamedWindow("skin", 0);
  cvvShowImage("skin",imgDisplay);
  */
  cvReleaseImage(&imgDisplay);
}

/* returns "difference" between pixel and skin mean */
float skinDiff(HSV pixel) {
  float /*diffH, */diffS, diffV;
  float forward, backward; /* angle measures */
  float minAngle;

  
  forward = fabsf(skinMean.h - pixel.h);
  backward = 2*M_PI - skinMean.h + pixel.h;

  minAngle = forward < backward ? forward : backward;

  diffS = fabsf(skinMean.s -  pixel.s);
  diffV = fabsf(skinMean.v - pixel.v);
  if(minAngle*180/M_PI < 50) {
    if(diffS < .125)
	return 0;
  }
   
  return 10000;
  /*
  diffH = pixel.h - skinMean.h;
  diffS = pixel.s - skinMean.s;
  diffV = pixel.v - skinMean.v;

  return sqrt(H_WEIGHT*(diffH*diffH) +
	      S_WEIGHT*(diffS*diffS) +
	      V_WEIGHT*(diffV*diffV)); 
  */
}

void removeBackground(IplImage* img, IplImage* bg, float* iWeights) {
  int i, j;
  RGB pixel;
  RGB bgPixel;
  float rDiff, gDiff, bDiff, dist;
  char* displayWeights;
  CvSize size = {IMAGE_WIDTH, IMAGE_HEIGHT};
  IplImage* imgDisplay = cvCreateImage(size, IPL_DEPTH_8U, 3);

  displayWeights = (char*) malloc(sizeof(char)*IMAGE_WIDTH*IMAGE_HEIGHT*3);

  for(i = 0; i < IMAGE_HEIGHT; i++) {
    for(j = 0; j < IMAGE_WIDTH; j++) {
      pixel.r = img->imageData[3*i*IMAGE_WIDTH+3*j];
      pixel.g = img->imageData[3*i*IMAGE_WIDTH+3*j+1];
      pixel.b = img->imageData[3*i*IMAGE_WIDTH+3*j+2];

      bgPixel.r = bg->imageData[3*i*IMAGE_WIDTH+3*j];
      bgPixel.g = bg->imageData[3*i*IMAGE_WIDTH+3*j+1];
      bgPixel.b = bg->imageData[3*i*IMAGE_WIDTH+3*j+2];

      rDiff = pixel.r - bgPixel.r;
      gDiff = pixel.g - bgPixel.g;
      bDiff = pixel.b - bgPixel.b;

      dist = sqrt(rDiff*rDiff+
		  gDiff*gDiff+
		  bDiff*bDiff);
      if(dist < MAX_BG_DIST) { /* it's background */
	/*	img->imageData[3*i*IMAGE_WIDTH+3*j] = 0;
	img->imageData[3*i*IMAGE_WIDTH+3*j+1] = 0;
	img->imageData[3*i*IMAGE_WIDTH+3*j+2] = 0; */
	iWeights[i*IMAGE_WIDTH+j] -= BG_WEIGHT;
	displayWeights[3*i*IMAGE_WIDTH+3*j] = 0;
	displayWeights[3*i*IMAGE_WIDTH+3*j+1] = 0;
	displayWeights[3*i*IMAGE_WIDTH+3*j+2] = 0;
      }
      else { /* it's foreground */
	displayWeights[3*i*IMAGE_WIDTH+3*j] = 255;
	displayWeights[3*i*IMAGE_WIDTH+3*j+1] = 255;
	displayWeights[3*i*IMAGE_WIDTH+3*j+2] = 255;
      }
    }
  }
  /*
  imgDisplay->imageData = displayWeights;
  cvvNamedWindow("background", 0);
  cvvShowImage("background",imgDisplay);
  */
  cvReleaseImage(&imgDisplay);
}

IplImage* getSilhouette(int i) {
  char num[5];
  char filename[80];
  uint32* bmap;
  uint32 w,h;
  IplImage* img;
  CvSize size = {IMAGE_WIDTH, IMAGE_HEIGHT};

  assert(i > 0);

  numString(i, num);

  sprintf(filename, "%s%s%s.tif", BMAPDIR, BMAPBASE, num);
  bmap = readTiff(filename, &w, &h);
  assert(w == IMAGE_WIDTH);
  assert(h == IMAGE_HEIGHT);
  
  img = cvCreateImage(size, IPL_DEPTH_8U, 3);
  tiffToIpl(bmap, img->imageData);
  _TIFFfree(bmap);
  return img;
}

void putSilhouettte(IplImage* img, int i) {
  char num[50];
  char filename[80];
  char* tiffIm;

  tiffIm = (char*) malloc(sizeof(char) * IMAGE_WIDTH*IMAGE_HEIGHT*3);
  assert(tiffIm != NULL);
  assert(i > 0);

  numString(i, num);
  sprintf(filename, "%s%s%s.tif", BMAPDIR, BMAPBASE, num);

  IplToTiff(img->imageData, tiffIm);
  writeTiff(filename, tiffIm, IMAGE_WIDTH, IMAGE_HEIGHT,
	    COMPRESSION_PACKBITS);
  fprintf(stderr, "(%s)...", filename);
  free(tiffIm);
}

/* moves silhouette according to LK optical flow */
void moveSilhouette(IplImage* img, int frameNum) {
  IplImage* currentFrame;
  IplImage* nextFrame;
  IplImage* currentFrameGray;
  IplImage* nextFrameGray;
  IplImage* imgTmp; 
  CvSize size = {IMAGE_WIDTH, IMAGE_HEIGHT};
  char filename[80];
  char num[5];
  CvPoint2D32f* originalFeaturesLocations;
  CvPoint2D32f* newFeaturesLocations;
  int count;
  char* status;
  CvTermCriteria criteria;
  CvSize winSize;

  winSize.width = 2;
  winSize.height = 2;

  criteria.type = CV_TERMCRIT_ITER;
  criteria.maxIter = 20;
  criteria.epsilon = 0;
  
  if(frameNum < 10) {
    sprintf(num, "00%d", frameNum);
  }
  else if(frameNum < 100) {
    sprintf(num, "0%d", frameNum);
  }
  else sprintf(num, "%d", frameNum);

  sprintf(filename, "%s%s%s.tif", TIFFDIR, TIFFBASE, num);
  currentFrame = getImage(filename);

  numString(frameNum+1, num);

  sprintf(filename, "%s%s%s.tif", TIFFDIR, TIFFBASE, num);
  nextFrame = getImage(filename);

  currentFrameGray = cvCreateImage(size, IPL_DEPTH_8U, 1);
  nextFrameGray = cvCreateImage(size, IPL_DEPTH_8U, 1);

  originalFeaturesLocations = findFeatures(img, &count);
  newFeaturesLocations = (CvPoint2D32f*)  malloc(sizeof(CvPoint2D32f)*count);
  status = (char*)  malloc(sizeof(char)*count);

  /* convert to gray scale */
  iplColorToGray(currentFrame, currentFrameGray);
  iplColorToGray(nextFrame, nextFrameGray);

  fprintf(stderr, "optical flow...");
  /* output is 32f (in newFeaturesLocations); */
  cvCalcOpticalFlowPyrLK(currentFrameGray, nextFrameGray, NULL, NULL,
			 originalFeaturesLocations, newFeaturesLocations,
			 count, winSize, 1, status, NULL, criteria, 0);

  fprintf(stderr, "moving features...");
  /* move the feature pts in the silhouette img */
  moveFeatures(img, originalFeaturesLocations, newFeaturesLocations, count);
  imgTmp = cvCloneImage(img);
  iplDilate(img, imgTmp, 3);
  iplDilate(imgTmp, img, 1);
  
  /*
  iplErode(img, imgTmp, 1);
  iplDilate(imgTmp, img, 16);
  */

  free(originalFeaturesLocations);
  free(newFeaturesLocations);
  free(status);
  cvReleaseImage(&currentFrame);
  cvReleaseImage(&nextFrame);
  cvReleaseImage(&currentFrameGray);
  cvReleaseImage(&nextFrameGray);
}

/* finds features (non zero pixels) in img and returns their x,y locations
   in an array, sets count to num of features found */
CvPoint2D32f* findFeatures(IplImage* img, int* count) {
  int i, j;
  CvPoint2D32f* loc;
  int c;

  *count = 0;
  /* first count... */
  for(i = 0; i < IMAGE_HEIGHT; i++) {
    for(j = 0; j < IMAGE_WIDTH; j++) {
      if(img->imageData[3*i*IMAGE_WIDTH+3*j] != 0 ||
	 img->imageData[3*i*IMAGE_WIDTH+3*j+1] != 0 ||
	 img->imageData[3*i*IMAGE_WIDTH+3*j+2] != 0) {
	*count = *count + 1;
      }
    }
  }

  loc = (CvPoint2D32f*) malloc(sizeof(CvPoint2D32f)*(*count));

  c=0;
  /* then found locations */
  for(i = 0; i < IMAGE_HEIGHT; i++) {
    for(j = 0; j < IMAGE_WIDTH; j++) {
      if(img->imageData[3*i*IMAGE_WIDTH+3*j] != 0 ||
	 img->imageData[3*i*IMAGE_WIDTH+3*j+1] != 0 ||
	 img->imageData[3*i*IMAGE_WIDTH+3*j+2] != 0) {
	loc[c].x = j;
	loc[c].y = i;
	c++;
      }
    }
  }
  assert(c == *count);

  return loc;
}

/* moves features in originalFeaturesLocations (x, y) positions to
   newFeaturesLocations in img...rounds to nearest pixel */
void moveFeatures(IplImage* img, CvPoint2D32f* originalFeaturesLocations,
		  CvPoint2D32f* newFeaturesLocations, int count) {
  int i;
  float diffX, diffY;
  float dist;
  IplImage* imgCopy;
  imgCopy = cvCloneImage(img);
  cvCopyImage(img, imgCopy);
  for(i = 0; i < count; i++) {
/*     fprintf(stderr, "%f %f\n", newFeaturesLocations[i].x, newFeaturesLocations[i].y); */
    
    /* check how much the pixel has moved */
    diffX = newFeaturesLocations[i].x - originalFeaturesLocations[i].x;
    diffY = newFeaturesLocations[i].y - originalFeaturesLocations[i].y;
    dist = sqrt((diffX*diffX)+(diffY*diffY));
    if(dist > LK_THRESH) {
      continue;
      /*       computeAverage */
    }


    /* r */
    img->imageData[3*((int) rintf(newFeaturesLocations[i].y))*IMAGE_WIDTH+
		   3*((int) rintf(newFeaturesLocations[i].x))] = 255;
      /*      imgCopy->imageData[3*((int) rintf(originalFeaturesLocations[i].y))*IMAGE_WIDTH+ 
	      3*((int) rintf(originalFeaturesLocations[i].x))]; */
    /* g */
    img->imageData[3*((int) rintf(newFeaturesLocations[i].y))*IMAGE_WIDTH+
		   3*((int) rintf(newFeaturesLocations[i].x))+1] = 255;
    /*    imgCopy->imageData[3*((int) rintf(originalFeaturesLocations[i].y))*IMAGE_WIDTH+ 
	  3*((int) rintf(originalFeaturesLocations[i].x))+1]; */
    /* b */
    img->imageData[3*((int) rintf(newFeaturesLocations[i].y))*IMAGE_WIDTH+
		   3*((int) rintf(newFeaturesLocations[i].x))+2] = 255;
    /*       imgCopy->imageData[3*((int) rintf(originalFeaturesLocations[i].y))*IMAGE_WIDTH+ 
	     3*((int) rintf(originalFeaturesLocations[i].x))+2]; */
    /* remove old pixels */
    /* r */
    img->imageData[3*((int) rintf(originalFeaturesLocations[i].y))*IMAGE_WIDTH+
		   3*((int) rintf(originalFeaturesLocations[i].x))] = 0;
    /* g */
    img->imageData[3*((int) rintf(originalFeaturesLocations[i].y))*IMAGE_WIDTH+
		   3*((int) rintf(originalFeaturesLocations[i].x))+1] = 0;
    /* b */
    img->imageData[3*((int) rintf(originalFeaturesLocations[i].y))*IMAGE_WIDTH+
		   3*((int) rintf(originalFeaturesLocations[i].x))+2] = 0;

/*     fprintf(stderr, "%f rounds to %d\n", newFeaturesLocations[i].y, (int) rintf(newFeaturesLocations[i].y)); */
  }
  cvReleaseImage(&imgCopy);
}


IplImage* getBackground(int start, int n) {
  IplImage* bg;
  int i, c;
  char filename[80];
  float* weightedValues; 

  weightedValues = (float*) malloc(sizeof(float)*IMAGE_WIDTH*IMAGE_HEIGHT*3);
  clearArrayF(weightedValues, IMAGE_WIDTH, IMAGE_HEIGHT*3, 0);

  for(c = 0; c < n; c++) {
    sprintf(filename, "%s%s%d.tif", TIFFDIR, TIFFBASE, start+c); 
    bg = getImage(filename);
    for(i = 0; i < IMAGE_WIDTH*IMAGE_HEIGHT*3; i++) {
      weightedValues[i] += bg->imageData[i]/(float) n;
    }
  }

  for(i = 0; i < IMAGE_WIDTH*IMAGE_HEIGHT*3; i++) {
    bg->imageData[i] = (short) ((int) rint(weightedValues[i]));
  }
  free(weightedValues);
  return bg;
}


void useContours(IplImage* img) {
  int i, n = 0, j;
  CvSize size = {IMAGE_WIDTH, IMAGE_HEIGHT};
  CvMemStorage* mem = cvCreateMemStorage(0);
  IplImage* imgGray = cvCreateImage(size, IPL_DEPTH_8U, 1);
  CvPoint pt;
  int total = 0;
  int iTotal = 0;
  
  CvSeq* seq = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvContour),
			   sizeof(CvPoint), mem);
  CvSeq* mSeq;

  iplColorToGray(img, imgGray);
  n=cvFindContours(imgGray, mem, &seq, sizeof(CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
  
  /* do something here with contours */
  fprintf(stderr, "%d contours found in image\n", n);
  for(i = 0; i < IMAGE_WIDTH*IMAGE_HEIGHT*3; i++) {
/*     imgGray->imageData[i] = 0; */
    img->imageData[i] = 0;
  }

  mSeq = seq;
  for(i = 0; i < n; i++) {
    if(mSeq->total > total) {
      iTotal = i;
      total = mSeq->total;
    }
    mSeq = mSeq->h_next;
  }

  mSeq = seq;
  for(i = 0; i < n; i++) {
    for(j = 0; j < mSeq->total; j++) {
      cvSeqPop(mSeq, &pt);
      if(i == iTotal) {
/* 	imgGray->imageData[pt.y*IMAGE_WIDTH+pt.x] = 255; */
	img->imageData[3*pt.y*IMAGE_WIDTH+pt.x*3] = 255;
	img->imageData[3*pt.y*IMAGE_WIDTH+pt.x*3+1] = 255;
	img->imageData[3*pt.y*IMAGE_WIDTH+pt.x*3+2] = 255;
      }
    }
    mSeq = mSeq->h_next;
  }


  /*  
  cvvNamedWindow("cont", 0);  
  cvvShowImage("cont",imgGray);  
  */

  cvReleaseImage(&imgGray);
  cvReleaseMemStorage(&mem);
}


#ifdef TESTMFLOW
#ifndef LESLIE
int main(int argc, char* argv[]) {
  /* int i,j; */
  image_t* vfield;
  IplImage* mhi = NULL;


/*   vfield = getMotionFlow(1,10, &mhi);  */

#ifdef TESTMFLOW
  cvvInitSystem(argc, argv);
  if(argc >= 6) {
    strcpy(TIFFDIR, argv[2]);
    strcpy(BMAPDIR, argv[3]);
    strcpy(TIFFBASE, argv[4]);
    strcpy(BMAPBASE, argv[5]);

    computeMotionFlow(atoi(argv[1]));
  }
  else 
    computeMotionFlow(MAXFRAME);
/*   getMotionFlow(4, 10, &mhi); */
#else
  computeMotionFlow(MAXFRAME);
#endif
  
  return 0;
}
#endif
#endif
