/* fabs
 * ----
 * Absolute value function for float.
 */
float fabs(float a) {
  if (a >= 0.0)
    return a;
  else
    return -a;
}


/* printVfield
 * -----------
 * Generate matlab code for displaying a sub rectangular region of
 * a vector field
 */
void printVfield(image_t* vfield, const char* path) {
  int i,j;
  FILE* fp;
  int x1,x2,y1,y2;

  // define the ROI (not too big for matlab to handle)
  x1 = 263;
  x2 = 351;
  y1 = 175;
  y2 = 252;
  
  fp = fopen(path,"w");

  // define U
  fprintf(fp,"U = [ ");
  for (i = y1; i < y2; i++) {
    for (j = x1; j < x2; j++) {  
      fprintf(fp,"%3.2f ", (*vfield)[i][j].x);
    }
    fprintf(fp,"; ...\n");
  }
  fprintf(fp,"];\n");

  // define V
  fprintf(fp,"V = [ ");
  for (i = y1; i < y2; i++) {
    for (j = x1; j < x2; j++) {  
      fprintf(fp,"%3.2f ", (*vfield)[i][j].y);
    }
    fprintf(fp,"; ...\n");
  }
  fprintf(fp,"];\n");

  // plot vector field
  fprintf(fp,"quiver(U,V);\n");
  fclose(fp);
}




/* removeBadVectors
 * ----------------
 * Get rid of unmeaningful vectors from the silhouette vector field.
 */
void removeBadVectors(image_t* vfield, IplImage* intensity) {
  int i,j;
  int a,b,c;
  float* data = (float*) intensity->imageData;

  // First: zero out all the edge points of the image
  for (i = 0; i < IMAGE_HEIGHT; i++) {
    (*vfield)[i][0].x = 0.0;
    (*vfield)[i][0].y = 0.0;
    (*vfield)[i][IMAGE_WIDTH-1].x = 0.0;
    (*vfield)[i][IMAGE_WIDTH-1].y = 0.0;
  }
  for (i = 0; i < IMAGE_WIDTH; i++) {
    (*vfield)[0][i].x = 0.0;
    (*vfield)[0][i].y = 0.0;
    (*vfield)[IMAGE_HEIGHT-1][i].x = 0.0;
    (*vfield)[IMAGE_HEIGHT-1][i].y = 0.0;
  }

  // Second: zero out vectors involving unmeaningful intensity points
  // I assume a 3x3 filter is used.
  for (i = 1; i < IMAGE_HEIGHT - 1; i++) {
    for (j = 1; j < IMAGE_WIDTH - 1; j++) {
      a = (i-1) * IMAGE_WIDTH + j;
      b =  i    * IMAGE_WIDTH + j;
      c = (i+1) * IMAGE_WIDTH + j;
      if ((data[a-1] == 0.0) ||
	  (data[a] == 0.0) ||
	  (data[a+1] == 0.0) ||
	  (data[b-1] == 0.0) ||
	  (data[b+1] == 0.0) ||
	  (data[c-1] == 0.0) ||
	  (data[c] == 0.0) ||
	  (data[c+1] == 0.0)) {
	(*vfield)[i][j].x = 0.0;
	(*vfield)[i][j].y = 0.0;
      }
    }
  }
}


/* getGlobalVector
 * ---------------
 * Compute the global vector (overall direction) of a silhouette vector
 * field.  It's essentially a normalized sum of valid vectors.
 */
void getGlobalVector(int index, float* x, float* y) {
  int i,j;
  int validpixels;
  float xflow, yflow;
  image_t* vfield;
  IplImage* mhi = NULL;
  char raster[3*IMAGE_HEIGHT*IMAGE_WIDTH];

  // compute vfield and zero out bad vectors first
  vfield = getMotionFlow(index, SILHOUETTE_FRAME, &mhi);
  removeBadVectors(vfield, mhi);
  //DEBUGIplToTiffFloatMono((float*) mhi->imageData, raster); writeTiff("../test/sammy_sil.tif",raster,IMAGE_WIDTH,IMAGE_HEIGHT,COMPRESSION_PACKBITS); assert(0);
  cvReleaseImage(&mhi);

  // generate matlab code
  //DEBUG printVfield(vfield, "../test/sammy_vfield.m"); assert(0);

  // sum up all the valid vectors
  validpixels = 0;
  *x = 0.0;
  *y = 0.0;
  for (i = 0; i < IMAGE_HEIGHT; i++) {
    for (j = 0; j < IMAGE_WIDTH; j++) {
      xflow = (*vfield)[i][j].x;
      yflow = (*vfield)[i][j].y;
      if ((fabs(xflow) > JUNK_VECTOR_THRESHOLD) ||
	  (fabs(yflow) > JUNK_VECTOR_THRESHOLD)) {
	validpixels ++;
	*x += xflow; 
	*y += yflow;
	///DEBUG PRINTF("valid#=%d: (%d,%d)=(%3.2f,%3.2f) acc(%3.2f,%3.2f)\n",validpixels,i,j,xflow,yflow,*x,*y);
      }
    }
  }

  // normalize by the number of valid pixels
  if (validpixels > 0) {
    PRINTF("actual(%3.2f,%3.2f)/%d -> ", *x, *y, validpixels);
    *x = *x / validpixels;
    *y = *y / validpixels;
    PRINTF("normalized(%3.2f,%3.2f)", *x, *y);
  } else {
    printf("no valid pixels in vfield?");
    assert(0);
  }

  free(vfield);
}




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

  sequence[0].x = 0.0;
  sequence[0].y = 0.0;

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

  return sequence;
}


/* getTrajectoryFromGlobalVectors
 * ------------------------------
 */
vector_t* getTrajectoryFromGlobalVectors(int max, vector_t* globalVectorSeq) {
  int i;
  vector_t *trajectory = (vector_t*) malloc(sizeof(vector_t) * max);

  trajectory[0].x = trajectory[0].y = 0.0;
  for (i = 1; i < max; i++) {
    trajectory[i].x = trajectory[i - 1].x + globalVectorSeq[i].x;
    trajectory[i].y = trajectory[i - 1].y + globalVectorSeq[i].y;
  }

  return trajectory;
}

