void superImposeMask(char* raster, mask_t* mask) {
  int row, col;
  for (row = 0; row < IMAGE_HEIGHT; row += 2) {
    for (col = 0; col < IMAGE_WIDTH; col += 2) {
      if (getMask(mask, col, row)) {
	markTiff(raster, col, row, 200, 0, 0);
      }
    }
  }
}

void drawMask(char* raster, mask_t* mask) {
  int row, col;
  for (row = 0; row < IMAGE_HEIGHT; row += 1) {
    for (col = 0; col < IMAGE_WIDTH; col += 1) {
      if (getMask(mask, col, row)) {
	markTiff(raster, col, row, 200, 0, 0);
      } else {
	markTiff(raster, col, row, 0, 0, 0);
      }
    }
  }
}

float computeErrorWithMask(mask_t* mask, float* intensity) {
  int i;
  float diff;
  float total = 0.0;
  for (i = 0; i < IMAGE_HEIGHT*IMAGE_WIDTH; i++) {
    diff = intensity[i] - (*mask)[i];
    total += diff * diff;
  }
  return total;
}

float computeErrorWithModel(model_t* model, float* intensity) {
  mask_t* tmpMask = generateModelMask(model);
  float ans = computeErrorWithMask(tmpMask, intensity);
  freeMask(tmpMask);
  return ans;
}

void updateModel(model_t* model, float* intensity) {
  int iter;
  mask_t* mask;
  float best, current;
  model_t* trial = allocModel(0,0,0,0,0);

  copyModel(model,trial);

  // compute the current error as the best error
  best = computeErrorWithModel(trial,intensity);

  for (iter = 0; iter < MODEL_ITERATIONS; iter++) {
    // try tweaking model's parameters (angles and coordinates)
    // tweak one at a time (not even locally optimal)
    // - trial contains the original model at the beginning of the iteration
    // - model always reflects the best match
    // - best always reflects the best score (model's score)
    // therefore, only one change per iteration.
    
    // (1)
    trial->ang1 -= MODEL_ANG_ALPHA;
    current = computeErrorWithModel(trial,intensity);
    if (current < best) { copyModel(trial,model); best = current; }
    trial->ang1 += MODEL_ANG_ALPHA;
    
    // (2)
    trial->ang1 += MODEL_ANG_ALPHA;
    current = computeErrorWithModel(trial,intensity);
    if (current < best) { copyModel(trial,model); best = current; }
    trial->ang1 -= MODEL_ANG_ALPHA;
    
    // (3)
    trial->ang2 -= MODEL_ANG_ALPHA;
    current = computeErrorWithModel(trial,intensity);
    if (current < best) { copyModel(trial,model); best = current; }
    trial->ang2 += MODEL_ANG_ALPHA;
  
    // (4)
    trial->ang2 += MODEL_ANG_ALPHA;
    current = computeErrorWithModel(trial,intensity);
    if (current < best) { copyModel(trial,model); best = current; }
    trial->ang2 -= MODEL_ANG_ALPHA;
  
    // (5)
    trial->ang3 -= MODEL_ANG_ALPHA;
    current = computeErrorWithModel(trial,intensity);
    if (current < best) { copyModel(trial,model); best = current; }
    trial->ang3 += MODEL_ANG_ALPHA;
  
    // (6)
    trial->ang3 += MODEL_ANG_ALPHA;
    current = computeErrorWithModel(trial,intensity);
    if (current < best) { copyModel(trial,model); best = current; }
    trial->ang3 -= MODEL_ANG_ALPHA;
  
    // (7)
    trial->x -= MODEL_TRA_ALPHA;
    current = computeErrorWithModel(trial,intensity);
    if (current < best) { copyModel(trial,model); best = current; }
    trial->x += MODEL_TRA_ALPHA;
  
    // (8)
    trial->x += MODEL_TRA_ALPHA;
    current = computeErrorWithModel(trial,intensity);
    if (current < best) { copyModel(trial,model); best = current; }
    trial->x -= MODEL_TRA_ALPHA;
  
    // (9)
    trial->y -= MODEL_TRA_ALPHA;
    current = computeErrorWithModel(trial,intensity);
    if (current < best) { copyModel(trial,model); best = current; }
    trial->y += MODEL_TRA_ALPHA;
  
    // (10)
    trial->y += MODEL_TRA_ALPHA;
    current = computeErrorWithModel(trial,intensity);
    if (current < best) { copyModel(trial,model); best = current; }
    trial->y -= MODEL_TRA_ALPHA;

    // commit to the best match in this iteration
    copyModel(model,trial);
  }
  
  freeModel(trial);
}

model_t* getCardboardSequence(int max) {
  model_t* seq = (model_t*) malloc(sizeof(model_t) * max);
  model_t* model = allocModel(MODEL_X, MODEL_Y, MODEL_A1, MODEL_A2, MODEL_A3);
  mask_t* mask;
  int i;
  IplImage* mhi;
  image_t* vfield;
  float* data;
  char silPath[1000];
  char raster[3*IMAGE_HEIGHT*IMAGE_WIDTH];
  float error;

  mask = generateModelMask(model);
  drawMask(raster, mask);
  freeMask(mask);
  writeTiff("../test/mask.tif",raster,IMAGE_WIDTH,IMAGE_HEIGHT,COMPRESSION_PACKBITS);
  assert(0);


  // fill first slot of seq
  copyModel(model, &(seq[0]));

  for (i = 1; i < max; i++) {
    PRINTF("computing model(%d): ", i);

    // get intensity map
    mhi = NULL;
    vfield = getMotionFlow(i, SILHOUETTE_FRAME, &mhi);
    free(vfield);
    data = (float*) mhi->imageData;

    // prepare model
    updateModel(model, data);
    copyModel(model, &(seq[i]));

    // generate model projection and compute difference
    mask = generateModelMask(model);
    error = computeErrorWithMask(mask, data);
    PRINTF("error(%4.3f)", error);

    // write out the composite image
    if (i < 10) {
      sprintf(silPath,silPathTemplate1,i);
    } else {
      sprintf(silPath,silPathTemplate2,i);
    }
    IplToTiffFloatMono(data,raster);
    superImposeMask(raster, mask);
    writeTiff(silPath,raster,IMAGE_WIDTH,IMAGE_HEIGHT,COMPRESSION_PACKBITS);
    cvReleaseImage(&mhi);

    // free mask
    freeMask(mask);


    PRINTF("\n");
  }

  freeModel(model);
  return seq;
}


void printModels(int max, model_t* modelSeq) {
  int i;
  for (i = 0; i < max; i++) {
    printf("%2d: ", i);
    printModel(&(modelSeq[i]));
  }
}
