/* drawMarker
 * ----------
 */
void drawMarker(char* raster, int x, int y) {
  int size = 3;
  int row, col;
  for (col = x-size; col <= x+size; col++) {
    for (row = y-size; row <= y+size; row++) {
      markTiff(raster, col, row, 200, 0, 0);
    }
  }
}

/* getCentroid
 * -----------
 */
void getCentroid(int index, float* x, float* y) {
  image_t* vfield;
  IplImage* mhi = NULL;
  int i,j,k;
  float* data;
  float intensity;
  int xx[SILHOUETTE_FRAME];
  int yy[SILHOUETTE_FRAME];
  int count[SILHOUETTE_FRAME];
  float thres[SILHOUETTE_FRAME];
  char silPath[1000];
  char raster[3*IMAGE_HEIGHT*IMAGE_WIDTH];
  int total;


  vfield = getMotionFlow(index, SILHOUETTE_FRAME, &mhi);
  free(vfield);
  data = (float*) mhi->imageData;

  for (k = 0; k < SILHOUETTE_FRAME; k++) {
    xx[k] = yy[k] = count[k] = 0;
    thres[k] = 1.0 * ((k + 1) / (float) SILHOUETTE_FRAME);
  }

  for (i = 0; i < IMAGE_HEIGHT; i++) {
    for (j = 0; j < IMAGE_WIDTH; j++) {
      intensity = data[i * IMAGE_WIDTH + j];
      if (intensity > 0.0) {
	for (k = 0; k < SILHOUETTE_FRAME; k++) {
	  if (intensity <= thres[k]) {
	    count[k] ++;
	    xx[k] += j;
	    yy[k] += i;
	    break;
	  }
	}
      }
    }
  }

  /* DEBUG
  PRINTF("\n");
  for (k = 0; k < SILHOUETTE_FRAME; k++) {
    PRINTF("%d: xx=%d, yy=%d, count=%d: ", k, xx[k], yy[k], count[k]);
    PRINTF("(%f,%f)\n", xx[k] / (float) count[k], yy[k] / (float) count[k]);
  }
  */

  // better centroid computation
  total = 0;
  *x = *y = 0.0;
  for (k = 0; k < SILHOUETTE_FRAME; k++) {
    total += k * count[k];
    *x += k * xx[k];          // weight the brighter dots more
    *y += k * yy[k];
  }
  *x = *x / total;
  *y = *y / total;
  PRINTF("(%3.2f,%3.2f)", *x, *y);

  // naive centroid computation
  /*
  *x = xx[SILHOUETTE_FRAME - 1] / count[SILHOUETTE_FRAME - 1];
  *y = yy[SILHOUETTE_FRAME - 1] / count[SILHOUETTE_FRAME - 1];
  PRINTF("(%3.2f,%3.2f)", *x, *y);
  */


  //DEBUG write out the received intensity map with the centroid marker
  if (index < 10) {
    sprintf(silPath,silPathTemplate1,index);
  } else {
    sprintf(silPath,silPathTemplate2,index);
  }
  IplToTiffFloatMono(data,raster);
  drawMarker(raster, round(*x), round(*y));
  writeTiff(silPath,raster,IMAGE_WIDTH,IMAGE_HEIGHT,COMPRESSION_PACKBITS);


  cvReleaseImage(&mhi);
}

/* getCentroidSequence
 * -------------------
 */
vector_t* getCentroidSequence(int max) {
  vector_t* sequence = (vector_t*) malloc(sizeof(vector_t) * max);
  int i;

  for (i = 1; i < max; i++) {
    PRINTF("computing centroid(%d): ", i);
    getCentroid(i, &(sequence[i].x), &(sequence[i].y));
    PRINTF("\n");
  }

  // ugly hack.. don't want to leave the first entry empty
  sequence[0].x = sequence[1].x;
  sequence[0].y = sequence[1].y;

  return sequence;
}
