/**
 * @file vehicle-speed.c
 * @author Diogo Luvizon <diogo@luvizon.com>
 * @date 29/01/2014
 */

#define _DLEVEL VDSM_DLEVEL

#include <stdlib.h>
#include <string.h>
#include <limits.h>
#include <unistd.h>
#include <math.h>
#include <locale.h>

#include <vehicle-speed.h>
#include <dsm-common.h>
#include <index.h>
#include <font.h>
#include <video-defs.h>
#include <common/vehicle-speed-lsq.h>

#include <klt/klt.h>
#include <klt/pyramid.h>
#include <sift/key.h>
#include <svm/svm.h>
#include <speed/speed.h>
#include <debug.h>
#include <gsl/gsl_fit.h>
#include <dsm_image.h>
#include <dsm_segment.h>

const bool use_detection = true;

static char *old_locale;

volatile int _dbg_tab;

/*------------------------ Error reporting ----------------------------*/
/* This function prints an error message and exits.  It takes a variable
   number of arguments that function just like those in printf.
*/
void FatalError(const char *fmt, ...)
{
  va_list args;

  va_start(args, fmt);
  fprintf(stderr, "Error: ");
  vfprintf(stderr, fmt, args);
  fprintf(stderr,"\n");
  va_end(args);
  exit(1);
}

static void dsm_paint_plates(uint8_t *img, int w, int h, vector<plate> &plates,
			     uint8_t cb, uint8_t cr)
{
  int xmin;
  int xmax;
  int ymin;
  int ymax;
  int k;

  for (k = 0; k < plates.size(); k++) {
    xmin = plates.at(k).x;
    xmax = xmin + plates.at(k).w;
    ymin = plates.at(k).y;
    ymax = ymin + plates.at(k).h;

    if (ymin < 1) {
      ymin = 1;
    }
    if (ymax > h - 2) {
      ymax = h - 2;
    }
    if (xmin < 1) {
      xmin = 1;
    }
    if (xmax > w - 2) {
      xmax = w - 2;
    }

    paint_chrom_rect(img, w, h, xmin, ymin, xmax, ymax, cb, cr);
  }

}

static void vs_unnecessary_remove_features(vector<KLT_FeatureList> &features)
{
  vector<KLT_FeatureList>::iterator it;
  KLT_FeatureList element;
  int i, j, k;

  //print_tab(DBG_CALL, "Removing features...\n");

  //printf("                 features.size() = %lu\n", features.size());

  /* for all features list, do ... */
  for (it = features.begin(); it != features.end();) {
    int in = 0;

    element = *it;
    for (j = 0; (j < element->nFeatures); j++) {
      if (element->feature[j]->val >= 0) {
	in++;
      }
    }

    if (element->falsepositive || (in == 0)) {
      KLTFreeFeatureList(element);
      it = features.erase(it);
    } else {
      it++;
    }
  }
  //print_tab(DBG_EXIT, "Removing features...\n");
}

extern "C" {


  Image crop_image(const unsigned char *src, int nrows, int ncols,
		   int xmin, int ymin, int xmax, int ymax)
  {
    if (xmin < 0) { xmin = 0; }
    if (ymin < 0) { ymin = 0; }
    if (xmax >= ncols) { xmax = ncols - 1; }
    if (ymax >= nrows) { ymax = nrows - 1; }

    Image dst = CreateImage (ymax - ymin + 1, xmax - xmin + 1, PERM_POOL);

    for (int r = 0; r < dst->rows; r++) {
      for (int c = 0; c < dst->cols; c++) {
	int p = ncols * (r + ymin) + (c + xmin);
	//dst->pixels[r][c] = ((float)src[p]) / 255.0;
	dst->pixels[r][c] = src[p];
      }
    }
    return dst;
  } 

  /* Draw a white line from (r1,c1) to (r2,c2) on the image.  Both points
     must lie within the image.
  */
  void DrawLine(Image image, int r1, int c1, int r2, int c2)
  {
    int i, dr, dc, temp;

    if (r1 == r2 && c1 == c2)  /* Line of zero length. */
      return;

    /* Is line more horizontal than vertical? */
    if (ABS(r2 - r1) < ABS(c2 - c1)) {

      /* Put points in increasing order by column. */
      if (c1 > c2) {
        temp = r1; r1 = r2; r2 = temp;
        temp = c1; c1 = c2; c2 = temp;
      }
      dr = r2 - r1;
      dc = c2 - c1;
      for (i = c1; i <= c2; i++)
        image->pixels[r1 + (i - c1) * dr / dc][i] = 1.0;

    } else {

      if (r1 > r2) {
        temp = r1; r1 = r2; r2 = temp;
        temp = c1; c1 = c2; c2 = temp;
      }
      dr = r2 - r1;
      dc = c2 - c1;
      for (i = r1; i <= r2; i++)
        image->pixels[i][c1 + (i - r1) * dc / dr] = 1.0;
    }
  }

  /* Return a new image that contains the two images with im1 above im2.
   */
  Image CombineImagesVertically(Image im1, Image im2)
  {
    int rows, cols, r, c;
    Image result;

    rows = im1->rows + im2->rows;
    cols = FUNCMAX(im1->cols, im2->cols);
    result = CreateImage(rows, cols, PERM_POOL);

    /* Set all pixels to 0,5, so that blank regions are grey. */
    for (r = 0; r < rows; r++)
      for (c = 0; c < cols; c++)
        result->pixels[r][c] = 0.5;

    /* Copy images into result. */
    for (r = 0; r < im1->rows; r++)
      for (c = 0; c < im1->cols; c++)
        result->pixels[r][c] = im1->pixels[r][c];
    for (r = 0; r < im2->rows; r++)
      for (c = 0; c < im2->cols; c++)
        result->pixels[r + im1->rows][c] = im2->pixels[r][c];

    return result;
  }

  /* Return squared distance between two keypoint descriptors.
   */
  int DistSquared (Keypoint k1, Keypoint k2)
  {
    int i, dif, distsq = 0;
    unsigned char *pk1, *pk2;

    pk1 = k1->ivec;
    pk2 = k2->ivec;

    for (i = 0; i < 128; i++) {
      dif = (int) *pk1++ - (int) *pk2++;
      distsq += dif * dif;
    }
    return distsq;
  }

  /* This searches through the keypoints in klist for the two closest
     matches to key.  If the closest is less than 0.6 times distance to
     second closest, then return the closest match.  Otherwise, return
     NULL.
  */
  Keypoint CheckForMatch(Keypoint key, Keypoint klist)
  {
    int dsq, distsq1 = 100000000, distsq2 = 100000000;
    Keypoint k, minkey = NULL;

    /* Find the two closest matches, and put their squared distances in
       distsq1 and distsq2.
    */
    for (k = klist; k != NULL; k = k->next) {
      dsq = DistSquared(key, k);

      if (dsq < distsq1) {
        distsq2 = distsq1;
        distsq1 = dsq;
        minkey = k;
      } else if (dsq < distsq2) {
        distsq2 = dsq;
      }
    }

    /* Check whether closest distance is less than 0.6 of second. */
    //if (10 * 10 * distsq1 < 8 * 8 * distsq2)
    if (10 * 10 * distsq1 < 6 * 6 * distsq2)
      return minkey;
    else return NULL;
  }

  static void WritePGM(FILE *fp, Image image)
  {
    int r, c, val;

    fprintf(fp, "P5\n%d %d\n255\n", image->cols, image->rows);

    for (r = 0; r < image->rows; r++)
      for (c = 0; c < image->cols; c++) {
	val = (int) (255.0 * image->pixels[r][c]);
	fputc(FUNCMAX(0, FUNCMIN(255, val)), fp);
      }
  }

  static void WritePGMFile(char *filename, Image image)
  {
    FILE *file;

    /* The "b" option is for binary input, which is needed if this is
       compiled under Windows.  It has no effect in Linux.
    */
    file = fopen(filename, "wb");
    if (! file)
      FatalError("Could not open file: %s", filename);

    WritePGM(file, image);
    fclose(file);
  }

/* Given a pair of images and their keypoints, pick the first keypoint
  from one image and find its closest match in the second set of
  keypoints. Then write the result to a file.
   */
  extern "C" void FindMatches(Image im1, Keypoint keys1, Image im2,
			      Keypoint keys2, KLT_FeatureList fl,
			      int shift_x1, int shift_y1, int shift_x2, int shift_y2, int iframe, const char *output_path)
  {
    Keypoint k;
    Keypoint match;
    Image result;
    int count = 0;
#define DEBUG_SIFT 1

    /* Create a new image that joins the two images vertically. */
    if (DEBUG_SIFT) {
      result = CombineImagesVertically(im1, im2);
    }

    /* Match the keys in list keys1 to their best matches in keys2. */
    fl->dx = 0.0;
    fl->dy = 0.0;

    /*
      printf("Procurando matchings na area: xmin = %d, ymin = %d, xmax = %d,"
      "ymax = %d\n", fl->xmin, fl->ymin, fl->xmax, fl->ymax);
    */

        
    /*TESTE PARA REMOCAO DE OUTLIER*/
    int outliers = 0;
    static const int SDEV = 2;
    static const double CDEV = 0.5;
    do {
      outliers = 0;
      int nfeatures = 0;
      double sum_x = 0.0, sum_y = 0.0;
      for (k= keys1; k != NULL; k = k->next) {
	match = CheckForMatch(k, keys2);
	if ((match != NULL) && (k->valid)) {
	  double d_x = ((fl->xmin - shift_x2) + match->col) - ((fl->xmin - shift_x1) + k->col);
	  double d_y = ((fl->ymin - shift_y2) + match->row) - ((fl->ymin - shift_y1) + k->row);
	  sum_x += d_x;
	  sum_y += d_y;
	  nfeatures++;
	} 
      }
      if (nfeatures > 0) {
	double mu_x = (sum_x / (double)nfeatures);
	double mu_y = (sum_y / (double)nfeatures);
	//printf("Mu - %f %f\n", mu_x, mu_y);
	sum_x = 0.0; sum_y = 0.0;
	for (k= keys1; k != NULL; k = k->next) {
	  match = CheckForMatch(k, keys2);
	  if ((match != NULL) && (k->valid)) {
	    double d_x = ((fl->xmin - shift_x2) + match->col) - ((fl->xmin - shift_x1) + k->col);
	    double d_y = ((fl->ymin - shift_y2) + match->row) - ((fl->ymin - shift_y1) + k->row);
	    sum_x += (d_x - mu_x)*(d_x - mu_x);
	    sum_y += (d_y - mu_y)*(d_y - mu_y);
	  } 
	}
	double dev_x = sqrt(sum_x / (double) (nfeatures - 1));
	double dev_y = sqrt(sum_y / (double) (nfeatures - 1));
	//printf("Dev - %f %f\n", dev_x, dev_y);
	for (k= keys1; k != NULL; k = k->next) {
	  match = CheckForMatch(k, keys2);
	  if ((match != NULL) && (k->valid)) {
	    double d_x = ((fl->xmin - shift_x2) + match->col) - ((fl->xmin - shift_x1) + k->col);
	    double d_y = ((fl->ymin - shift_y2) + match->row) - ((fl->ymin - shift_y1) + k->row);
	    if ( (fabs(d_x - mu_x) > SDEV * dev_x && (dev_x > CDEV)) || ((fabs(d_y - mu_y) > SDEV * dev_y) && (dev_y > CDEV)) ) {
	      k->valid = 0;
	      outliers++;
	    }
	  } 
	}
      }
    } while (outliers > 0); 

    for (k= keys1; k != NULL; k = k->next) {
      match = CheckForMatch(k, keys2);

      // Draw a line on the image from keys1 to match.  Note that we
      // must add row count of first image to row position in second
      // so that line ends at correct location in second image.
      //
      if ((match != NULL) && (k->valid)) {
	count++;
	if (DEBUG_SIFT) {
	  DrawLine(result, (int) k->row, (int) k->col,(int) (match->row + im1->rows),(int) match->col);
	}
	//fl->dy += (match->row - (180 + k->row));
	//fl->dx += (match->col - (180 + k->col));
	//fl-dy += (match->row - k->row);
	fl->dy += ((fl->ymin - shift_y2) + match->row) -
	  ((fl->ymin - shift_y1) + k->row);
	fl->dx += ((fl->xmin - shift_x2) + match->col) -
	  ((fl->xmin - shift_x1) + k->col);
      }
    }

    if (count != 0) {
      fl->dx /= count;
      fl->dy /= count;
    } else {
      fl->dx = 0.0;
      fl->dy = 0.0;
    }

    /*if (true) {
      char filename[256];
      sprintf(filename, "%s/snapshot/matches_%05d.pgm", output_path, iframe);
      FILE *debug = fopen(filename, "w");
      WritePGM(debug, result);
      fclose(debug);
      DisallocMatrix(result->pixels, result->rows, result->cols, PERM_POOL);
      free(result);
      }*/
  }


  void update_displacement (KLT_FeatureList fl) {

    int j;

    double mean_velx = 0.0;
    double mean_vely = 0.0;

    int nFeat = 0;

    fl->xmin = fl->ymin = +9999999;
    fl->xmax = fl->ymax = -9999999;

    for (j = 0; j < fl->nFeatures; j++) {

      if (fl->feature[j]->val == KLT_TRACKED) {

	mean_velx += fl->feature[j]->x - fl->feature[j]->prev_x;
	mean_vely += fl->feature[j]->y - fl->feature[j]->prev_y;

	if (fl->feature[j]->x < fl->xmin) {
	  fl->xmin = fl->feature[j]->x;
	}
	if (fl->feature[j]->x > fl->xmax) {
	  fl->xmax = fl->feature[j]->x;
	}
	if (fl->feature[j]->y < fl->ymin) {
	  fl->ymin = fl->feature[j]->y;
	}
	if (fl->feature[j]->y > fl->ymax) {
	  fl->ymax = fl->feature[j]->y;
	}

	nFeat++;
      }
    }
    fl->dx = (mean_velx/nFeat);
    fl->dy = (mean_vely/nFeat)*0.9;
  }

  static void draw_circle_nv12(uint8_t *chrom, int w, int h, int x, int y,
			       int cb, int cr)
  {
    print_pixel_nv12(chrom, w, h, x, y, cb, cr);
    print_pixel_nv12(chrom, w, h, x + 2, y, cb, cr);
    print_pixel_nv12(chrom, w, h, x - 2, y, cb, cr);
    print_pixel_nv12(chrom, w, h, x, y + 2, cb, cr);
    print_pixel_nv12(chrom, w, h, x, y - 2, cb, cr);
  }

  static void draw_line(uint8_t *chrom, int y1, int x1, int y2, int x2,
			int w, int h, int cb, int cr)
  {
    int temp;
    int dr;
    int dc;
    int i;

    /* line of zero length */
    if (y1 == y2 && x1 == x2) {
      return;
    }
    /* is line more horizontal than vertical? */
    if (ABS(y2 - y1) < ABS(x2 - x1)) {

      /* put points in increasing order by column */
      if (x1 > x2) {
	temp = y1;
	y1 = y2;
	y2 = temp;
	temp = x1;
	x1 = x2;
	x2 = temp;
      }
      dr = y2 - y1;
      dc = x2 - x1;
      for (i = x1; i <= x2; i++) {
	int y = y1 + (i - x1) * dr / dc;
	int x = i;

	print_pixel_nv12(chrom, w, h, x, y, cb, cr);
      }
    } else {
      if (y1 > y2) {
	temp = y1;
	y1 = y2;
	y2 = temp;
	temp = x1;
	x1 = x2;
	x2 = temp;
      }
      dr = y2 - y1;
      dc = x2 - x1;
      for (i = y1; i < y2; i++) {
	int y = i;
	int x = x1 + (i - y1) * dc / dr;

	print_pixel_nv12(chrom, w, h, x, y, cb, cr);
      }
    }
  }



  static void draw_motion_vectors(uint8_t *image, int width, int height,
				  int frame_i, KLT_FeatureList features, int color)
  {
    uint8_t *chrom;
    int act_x, act_y, prv_x, prv_y;
    int MAX_INT = 99999;
    int min_x = MAX_INT, min_y = MAX_INT;

    chrom = image + width * height;
    for (int i = 0; i < features->nFeatures; i++)  {
      if (features->feature[i]->val == KLT_TRACKED) {
	act_x = (int) features->feature[i]->x;
	act_y = (int) features->feature[i]->y;
	if (act_x < min_x) {
	  min_x = act_x;
	}
	if (act_y < min_y) {
	  min_y = act_y;
	}
	draw_circle_nv12(chrom, width, height, act_x, act_y,
			 color, 255 - color);

	for (int k = 0; k < (SMPSIZE - 1); k++) {
	  act_x = (int) features->feature[i]->
	    prv_x[(frame_i - k) % SMPSIZE];
	  act_y = (int) features->feature[i]->
	    prv_y[(frame_i - k) % SMPSIZE];
	  prv_x = (int) features->feature[i]->
	    prv_x[(frame_i - k - 1) % SMPSIZE];
	  prv_y = (int) features->feature[i]->
	    prv_y[(frame_i - k - 1) % SMPSIZE];
	  if ((act_x != NOT_FILLED) &&
	      (act_y != NOT_FILLED) &&
	      (prv_x != NOT_FILLED) &&
	      (prv_y != NOT_FILLED) ) {
	    draw_line(chrom, prv_y, prv_x, act_y,
		      act_x, width, height,
		      color, 255 - 42 * k);
	  }
	}
      }
    }
  }

  /* Stores the center row of {img} into {R[0..nx-1]},
    where {nx} is the width of {img}, 
    converted to doubles in {0..255}.*/
  void extract_center_row_as_vector(dsm_image_t *img, double *R)
    { 
      int nx = img->nx;
      int ny = img->ny;
      assert((ny % 2) == 1);
      int ym = ny/2;
      for (int x = 0; x < nx; x++) {
	R[x] = (double)img->p[ym*nx + x];
      }
    }
 
  /*Given a frame {img}, extracts a band {img_band} centered on row 
    {y0}.  Then creates a copy {img_red} of {img_band}, reduced by 
    scale factors {xreduc} and {yreduc}, using sampling windows of 
    size {nxredw} by {nyredw}. Then computes a blurred version 
    {img_red_blur} of {img_red}. */
void extract_rythm_row 
  ( dsm_image_t *img,
    int y0,
    int xreduc,
    int yreduc,
    int nxredw, 
    int nyredw,
    dsm_image_t *img_band,
    dsm_image_t *img_red,
    dsm_image_t *img_red_blur
    ) 
  {
    dsm_image_extract_band (img, y0, img_band);
    dsm_image_shrink (img_band, xreduc, yreduc, nxredw, nyredw, img_red);
    dsm_image_blur(img_red, 4, 4, img_red_blur);
  }

#define MAX_FRAMES 40000

  /* Initializes the {vs} data structure for a video with 
   {nx} columns and {ny} rows of pixels per frame. 
   This function must be called at the first
   frame. */
  dsm_vs_data_t *vs_preload(dsm_common_data_t *cd, int nx, int ny) {

    printf("vou comecar tudo\n");
    dsm_vs_data_t *vs = (dsm_vs_data_t *)malloc(sizeof(dsm_vs_data_t));
    assert(vs != NULL);

    char fname[256];
    int nr_class;
    int ret;
    int i;
    int img_size;

    old_locale = strdup(setlocale(LC_ALL, NULL));
    setlocale(LC_ALL, "C");

    print_dbg("Vehicle Detection Speed is preloading...\n");
    vs->img_nx = nx;
    vs->img_ny = ny;
    vs->object_counter = 0;

    /*Parameters for visual rythm extraction: */
    vs->xreduc = 30;        /*Width reduction factor*/
    vs->yreduc = 4;        /*Height reduction factor*/
    
    vs->nxredw = 4 * vs->xreduc - 1; /*Horizontal subsampling filter window size*/
    vs->nyredw = 4 * vs->yreduc - 1; /*Vertical subsampling filter window size*/
    
    vs->band_ny = 7 * vs->yreduc;     /*Height of horizontal band extracted from image.*/
    assert(vs->band_ny >= vs->nyredw); /* The reuction filter window must fit inside the band image. */

    vs->yext0 = 1050;                    /*Index of the upstream frame row to extract.*/ 
    vs->yext1 = vs->yext0 - 20*vs->yreduc;  /*Index of the downstream frame row to extract.*/ 
    
    /*Parameters for background matching and estimation: */
    vs->lambda_bg = 0.18;
    vs->cleanup_bg = 5; 
        
    /*Parameters for background matching and estimation (OLD): */
    vs->sigma_bg = 10.0; 
    vs->tau_bg = 3.0; 
    vs->beta_bg = 0.95;
    vs->ynlines_bg = 15;

    /*Global settings: */
    cd->tp = 0;
    cd->fp = 0;
    cd->fn = 0;
    cd->positives = 0;
 
    vs->beg = 0;
    vs->end = 0;

    /*Extracted visual rythm images: */
    cd->vr_nx = vs->img_nx/vs->xreduc; 
    cd->vr_ny = 0; /* Initially; incremented after each frame is processed. */

    cd->visual_rythm0       = dsm_image_alloc (cd->vr_nx, MAX_FRAMES, 0); /* Upstream. */
    cd->visual_rythm1       = dsm_image_alloc (cd->vr_nx, MAX_FRAMES, 0); /* Downstream. */
    /* These images are all derived from the downstream pixel row: */
    cd->visual_events       = dsm_image_alloc (cd->vr_nx, MAX_FRAMES, 0); 
    cd->visual_diff         = dsm_image_alloc (cd->vr_nx, MAX_FRAMES, 0);
    cd->visual_update       = dsm_image_alloc (cd->vr_nx, MAX_FRAMES, 0);
    cd->visual_weights      = dsm_image_alloc (cd->vr_nx, MAX_FRAMES, 0); 
    cd->visual_background   = dsm_image_alloc (cd->vr_nx, MAX_FRAMES, 0);
    cd->visual_segmentation = dsm_image_alloc (cd->vr_nx, MAX_FRAMES, 0);
    cd->visual_velx 	    = dsm_image_alloc (cd->vr_nx, MAX_FRAMES, 0);
    cd->visual_vely 	    = dsm_image_alloc (cd->vr_nx, MAX_FRAMES, 0);
        
    /* Estimated camera gain per frame: */
    cd->visual_gain         = (double *) malloc((MAX_FRAMES) * sizeof(double)); 

    vs->start_row = 0;

    /* Working vectors */
    vs->vect_X  = (double *)malloc(cd->vr_nx * sizeof(double));
    vs->vect_F  = (double *)malloc(cd->vr_nx * sizeof(double));

    vs->img_row0 = (double *)malloc(cd->vr_nx * sizeof(double));
    vs->img_row1 = (double *)malloc(cd->vr_nx * sizeof(double));
    vs->bg_row1  = (double *)malloc(cd->vr_nx * sizeof(double));
    vs->weight   = (double *)malloc(cd->vr_nx * sizeof(double));
    
    /* Size of extracted image bands (full width): */
    int nx_band = nx;
    int ny_band = vs->band_ny;

    /* Extracted image bands (reduced scale): */
    int nx_red = nx_band/vs->xreduc;
    int ny_red = ny_band/vs->yreduc;

    vs->img0_red         = dsm_image_alloc (nx_red, ny_red, 0); 
    vs->img1_red         = dsm_image_alloc (nx_red, ny_red, 0); 
    vs->background_red   = dsm_image_alloc (nx_red, ny_red, 0); 
    vs->weight_red       = dsm_image_alloc (nx_red, ny_red, 0); 

    vs->img0_red_blur         = dsm_image_alloc (nx_red, ny_red, 0); 
    vs->img1_red_blur         = dsm_image_alloc (nx_red, ny_red, 0); 
    vs->background_red_blur   = dsm_image_alloc (nx_red, ny_red, 0); 
    vs->weight_red_blur       = dsm_image_alloc (nx_red, ny_red, 0); 

    vs->temp_band = dsm_image_alloc (nx_band, ny_band, 0); 
    vs->temp_vr   = dsm_image_alloc (cd->vr_nx, MAX_FRAMES, 0); 

    snprintf(fname, sizeof(fname), "%s/../background.pgm", cd->output_path);
    int back_nrows, back_ncols;
    vs->background = dsm_image_alloc (nx, ny, 0); 
    { FILE *fp = fopen(fname, "r");
      assert(fp != NULL);
      dsm_image_read_pgm_ascii (fp, &(vs->background)); 
      assert((vs->background.nx == nx) && (vs->background.ny == ny));
      fclose (fp);
    }

    /*Extract the relevant band around downstream row from the background image: */
    extract_rythm_row 
      ( &(vs->background), vs->yext1,
	vs->xreduc, vs->yreduc, 
	vs->nxredw, vs->nyredw, 
	&(vs->temp_band), &(vs->background_red), &(vs->background_red_blur) );
    extract_center_row_as_vector(&(vs->background_red), vs->bg_row1);

    cd->timestamp = 0.0;
    vs->vt = gtruth_vehicles_new(cd->vehicles_xml);
    vs->tc = KLTCreateTrackingContext();
    vs->tc_gt = KLTCreateTrackingContext();
    vs->last = 0;
    vs->kltbufs.tmp = _KLTCreateFloatImage(nx, ny);
    vs->kltbufs.aux = _KLTCreateFloatImage(nx, ny);
    vs->should_save_frame = false;
    print_dbg("vs_preload done\n");
    return vs;
  }

#if (VDSM_DLEVEL > 1)
  /* {buff} is the raster of a colo image with {nx} cols and {ny} rows. */
  static void vs_write_features_to_image(uint8_t *buff, int nx, int ny,  KLT_FeatureList fl)
  {
    uint8_t *chrom; /* Start of chrominance channel? */
    int x, y;
    int mi;
    int f;

    chrom = buff + nx * ny;
    for (f = 0; f < fl->nFeatures; f++) {
      if (fl->feature[f]->val >= 0) {
	x = (int) fl->feature[f]->x;
	y = (int) fl->feature[f]->y;
#if (VDSM_DLEVEL > 2)
	mi = INDEX_INIT(nx, x, y);
	buff[mi] = 0;
	buff[mi + 1] = 0;
	buff[mi - 1] = 0;
	buff[mi + nx] = 0;
	buff[mi - nx] = 0;
	buff[mi + nx + 1] = 255;
	buff[mi + nx - 1] = 255;
	buff[mi - nx + 1] = 255;
	buff[mi - nx - 1] = 255;
	buff[mi + 2] = 255;
	buff[mi - 2] = 255;
	buff[mi + 2 * nx] = 255;
	buff[mi - 2 * nx] = 255;
#endif
	print_pixel_nv12(chrom, nx, ny, x, y, 255, 255);
	print_pixel_nv12(chrom, nx, ny, x + 2, y + 2, 255, 255);
	print_pixel_nv12(chrom, nx, ny, x + 2, y - 2, 255, 255);
	print_pixel_nv12(chrom, nx, ny, x - 2, y + 2, 255, 255);
	print_pixel_nv12(chrom, nx, ny, x - 2, y - 2, 255, 255);
      }
    }
  }
#else
#define vs_write_features_to_image(...)
#endif

  static void vs_print_start (uint8_t *chrom, int nx, int ny, int starty, int sum)
  {
    uint8_t cb;
    uint8_t cr;
    int x, y;
    int pix;

    for (y = starty - 1; y <= starty + 1; y++) {
      for (x = 0; x < nx; x += 2) {
	cb = 122 + sum;
	cr = 156 + sum; 
	print_pixel_nv12(chrom, nx, ny, x, y, cb, cr);
      }
    }
  }

  void vs_log_vehicle_speed(KLT_FeatureList fl, int iframe,
			    const char *out_path, const char *log_type, bool *save_frame)
  {
    char fname[256];
    FILE *file;
    double s;
    double r;
    int xav;
    int yav;
    int ret;
    float sp, er;
    int remain_feat;

    sp = fl->speed[iframe % SMPSIZE];
    if (fl->vehicle.radar) {
      er = sp - fl->vehicle.speed;
    } else {
      er = 0.0;
    }

    yav = (fl->ymin + fl->ymax) / 2;
    xav = (fl->xmin + fl->xmax) / 2;

    remain_feat = KLTCountRemainingFeatures(fl);
    if (0 == remain_feat) {
      /* Nothing to do if there isn't features tracked. */
      return;
    }

    snprintf(fname, sizeof(fname), "%s/%s/f%d/%04ld.txt",
	     out_path, log_type, fl->vehicle.lane, fl->object_id);
    ret = access(fname, F_OK);
    if (-1 == ret) {
      /* the file does not exist */
      file = fopen(fname, "w");
    } else {
      file = fopen(fname, "a");
    }
    if (NULL == file) {
      printf("Error: could not open file '%s'\n", fname);
      exit(1);
    }
    if (-1 == ret) {
      /* if the files was created now, put the header first */
      fprintf(file, "# Log file generated by " PACKAGE_STRING "\n\n");
      fprintf(file, "# Global parameters:\n");
      fprintf(file, "id : %lu\n", fl->object_id);
      fprintf(file, "lane : %d\n", fl->vehicle.lane);
      fprintf(file, "plate : %d\n", !!fl->vehicle.plate);
      fprintf(file, "radar : %d\n", !!fl->vehicle.radar);
      fprintf(file, "sema : %d\n", !!fl->vehicle.sema);
      fprintf(file, "moto : %d\n", !!fl->vehicle.moto);
      fprintf(file, "real speed : %.1f\n", fl->vehicle.speed);
      fprintf(file, "# ---------------------------------------\n\n");

      fprintf(file,   "frame\t"
	      "speed\t"
	      "err\t"
	      "x\t"
	      "y\t"
	      "dx\t"
	      "dy\t"
	      "ipm_x\t"
	      "ipm_y\t"
	      "nfeat\n");
      fprintf(file, "# ---------------------------------------------"
	      "--------------------------------\n");
    }
    fprintf(file, "%05d\t", iframe);
    fprintf(file, "%.1f\t", sp);
    fprintf(file, "%.1f\t", sp - fl->vehicle.speed);
    fprintf(file, "%d\t", xav);
    fprintf(file, "%d\t", yav);
    fprintf(file, "%.1f\t", fl->dx);
    fprintf(file, "%.1f\t", fl->dy);
    fprintf(file, "%.1f\t", fl->ipm_x);
    fprintf(file, "%.1f\t", fl->ipm_y);
    fprintf(file, "%d\n", remain_feat);
    fclose(file);

    if (fl->vehicle.radar && (fl->vehicle.speed > 10.0) &&
	(sp > 10.0) && (fabs(er) < 10.0)) {
      if ((er > 2.0) | (er < -3.0)) {
	*save_frame = true;
      }
    }
    if (!(*save_frame) && (remain_feat < 2)) {
      *save_frame = true;
    }
  }

  static void save_frame_window_to_pgm(dsm_image_t *img, int imgw, int nx, int ny,
				       const char *filename)
  {
    FILE *file;
    int pix_cnt;
    int mi;
    int x;
    int y;

    file = fopen(filename, "w");
    if (!file) {
      print_err("could not open file %s", filename);
      return;
    }

    pix_cnt = 0;
    mi = 0;

    fprintf(file, "P2\n");
    fprintf(file, "%d %d\n", nx, ny);
    fprintf(file, "255\n");

    for (y = 0; y < ny; y++) {
      for (x = 0; x < nx; x++) {
	fprintf(file, "%d ", (int) img->p[mi]);
	if (pix_cnt % 12 == 0) {
	  fprintf(file, "\n");
	}
	pix_cnt++;
	mi++;
      }
      mi += (imgw - nx);
    }

    fclose(file);
  }

  static vector<plate> read_plates_from_xml(struct vehicle_table *vt, int iframe)
  {
    vector<plate> plates;
    struct vehicle_mark *vehicle;
    plate p;

    if ((iframe >= vt->size) || (iframe < 0)) {
      /* Out of range for plates. */
      goto end;
    }
    if (!vt->vehicles[iframe]) {
      /* If there is not a vehicle in this position, return. */
      goto end;
    }
    vehicle = vt->vehicles[iframe];
    p.x = vehicle->x;
    p.y = vehicle->y;
    p.w = vehicle->w;
    p.h = vehicle->h;
    p.lane = vehicle->lane;
    p.quality = vehicle->quality;
    p.discard = vehicle->discard;
    if (vehicle->radar) {
      p.speed = vehicle->speed;
    } else {
      p.speed = 0.0;
    }
    p.from = vehicle;
    plates.push_back(p);

  end:
    return plates;
  }

  /* Draws on a color image {img} which has {nx} cols and {ny} rows. */
  static void draw_speed_values(uint8_t *img, int nx, int ny,
				int iframe, KLT_FeatureList fl, const char *out_path)
  {
    int x, y;

    x = (fl->xmin + fl->xmax) / 2 - 100;
    y = (fl->ymin + fl->ymax) / 2 - 50;
    if ((x < 10) || (x > nx - 100)) {
      return;
    }
    if ((y < 40) || (y > ny - 40)) {
      return;
    }
    if (fl->vehicle.radar) {
      draw_text(img, nx, x, y, 3, 0, 255, " %.1f / %.1f",
		fl->speed[iframe % SMPSIZE], fl->vehicle.speed);
    } else {
      draw_text(img, nx, x, y, 3, 0, 255, " %.1f",
		fl->speed[iframe % SMPSIZE]);
    }

    write_nv12_to_jpeg(img, nx, ny, "%s/snapshot/vel_%05d.jpg",
		       out_path, iframe);
  }

  static void decolorize(uint8_t *img, int nx, int ny)
  {
    uint8_t *chr;
    int size;

    chr = img + nx * ny;
    size = nx * ny / 2;
    while (size--) {
      *chr = (*chr + 128) / 2;
      chr++;
    }
  }

  void compute_features_from_plates(
				    vector<plate> &plates, 
				    vector<KLT_FeatureList> &features, 
				    dsm_vs_data_t *vs, 
				    dsm_common_data_t *cd) {

    uint8_t *roimg = grab_past_buffer(cd, 0); /* Pixels of current color image. */
    int nx = vs->img_nx;
    int ny = vs->img_ny;
    int k;

    if (plates.size() == 0) { return; }

    for (k = 0; k < plates.size(); k++) {
      KLT_FeatureList fl = KLTCreateFeatureList(20);
      KLTSelectGoodFeatures(vs->tc, roimg, nx, ny, fl, plates.at(k));
      fl->lane = plates.at(k).lane;
      fl->seen = false;
      fl->discovered = cd->iframe;
      fl->falsepositive = 0;
      fl->object_id = ++vs->object_counter;
      fl->quality = plates.at(k).quality;
      fl->discard = plates.at(k).discard;
      if (plates.at(k).from) {
	fl->vehicle = *plates.at(k).from;
      } else {
	/* Null vehicle pointer from. */
	fl->vehicle.iframe = cd->iframe;
	fl->vehicle.plate = true;
	fl->vehicle.sema = false;
	fl->vehicle.moto = false;
	fl->vehicle.radar = false;
	fl->vehicle.x = plates.at(k).x;
	fl->vehicle.y = plates.at(k).y;
	fl->vehicle.w = plates.at(k).w;
	fl->vehicle.h = plates.at(k).h;
	fl->vehicle.frame_start = cd->iframe;
	fl->vehicle.frame_end = cd->iframe;
	fl->vehicle.speed = 0.0;
      }
      features.push_back(fl);
    }
  }

  void process_feature_list(vector<KLT_FeatureList> &features,
			    KLT_TrackingContext tc, dsm_vs_data_t *vs,
			    dsm_common_data_t *cd, const char *savelog,
			    bool print, int color)
  {
    int nx = vs->img_nx;
    int ny = vs->img_ny;
    uint8_t *roimg = grab_past_buffer(cd, 0);
    uint8_t *prev = grab_past_buffer(cd, 1);
	
    int shift_x1;
    int shift_y1;
    int shift_x2;
    int shift_y2;
    int k;
    //printf("oh loco 0\n"); 

    if (0 == features.size()) { 
      //printf("dio mio 0\n"); 
      if (tc->pyramid_last != NULL) {
	_KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last);
	_KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last_gradx);
	_KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last_grady);
	tc->pyramid_last = NULL;
      }
      //printf("dio mio 1\n"); 
      return;
    }
    for (k = 0; k < features.size(); k++) {
      if (features.at(k)->discovered + 1 == cd->iframe) {
	shift_x1 = (features.at(k)->xmin - 20 > 0 ?
		    20 : features.at(k)->xmin);
	shift_y1 = (features.at(k)->ymin - 20 > 0 ?
		    20 : features.at(k)->ymin);
	shift_x2 = (features.at(k)->xmin - 200 > 0 ?
		    200 : features.at(k)->xmin);
	shift_y2 = (features.at(k)->ymin-200 > 0 ?
		    200 : features.at(k)->ymin);

	Image cimg1 = crop_image(prev, ny, nx,
				 features.at(k)->xmin - shift_x1,
				 features.at(k)->ymin - shift_y1,
				 features.at(k)->xmax + 20,
				 features.at(k)->ymax + 20);
	Keypoint key1;
	key1 = GetKeypoints(cimg1);

	Image cimg2 = crop_image(roimg, ny, nx,
				 features.at(k)->xmin - shift_x2,
				 features.at(k)->ymin - shift_y2,
				 features.at(k)->xmax + 200,
				 features.at(k)->ymax + 200);
	Keypoint key2;
	key2 = GetKeypoints(cimg2);

	FindMatches(cimg1, key1, cimg2, key2, features.at(k),
		    shift_x1, shift_y1, shift_x2, shift_y2, cd->iframe, cd->output_path);

	DisallocMatrix(cimg1->pixels, cimg1->rows,
		       cimg1->cols, PERM_POOL);
	free(cimg1);
	DisallocMatrix(cimg2->pixels, cimg2->rows,
		       cimg2->cols, PERM_POOL);
	free(cimg2);

	Keypoint aux;
	aux = key1;
	while (aux != NULL) {
	  Keypoint next = aux->next;
	  free(aux->ivec);
	  free(aux);
	  aux = next;
	}
	aux = key2;
	while (aux != NULL) {
	  Keypoint next = aux->next;
	  free(aux->ivec);
	  free(aux);
	  aux = next;
	}
      }
    }

    klt_update_pyramid(features, tc, &vs->kltbufs, roimg, nx, ny);
    for (k = 0; k < features.size(); k++) {
      if (features.at(k)->discovered < cd->iframe) {

	/*{
	  char filename[256];
	  sprintf(filename, "%s/snapshot/track_prev_%05d.pgm", cd->output_path, cd->iframe);
	  KLTWriteFeatureListToPPM (features.at(k), prev, nx, ny, filename);
	  }

	  {
	  char filename[256];
	  sprintf(filename, "%s/snapshot/track_chute_%05d.pgm", cd->output_path, cd->iframe);
	  KLTWriteFeatureListToPPMChute (features.at(k), roimg, nx, ny, filename, features.at(k)->dx, features.at(k)->dy);
	  }*/

	/* Kanade-Lucas-Tomasi Pyramidal Tracking: */
	KLTTrackFeatures (tc, nx, ny, features.at(k), cd->iframe, k);

	/*{
	  char filename[256];
	  sprintf(filename, "%s/snapshot/track_next_%05d.pgm", cd->output_path, cd->iframe);
	  KLTWriteFeatureListToPPM (features.at(k), roimg, nx, ny, filename);
	  }*/

	update_displacement(features.at(k));

	/* Speed estimation */
	compute_ipm_features(features.at(k), cd->iframe);

	outlier_removal(features.at(k), cd->iframe);

	speed_i speed = compute_velocity_vector(features.at(k),
						cd->iframe - 1, cd->iframe);

	features.at(k)->ipm_dx = speed.m_x -
	  features.at(k)->ipm_x;
	features.at(k)->ipm_dy = speed.m_y -
	  features.at(k)->ipm_y;
	features.at(k)->ipm_x = speed.m_x;
	features.at(k)->ipm_y = speed.m_y;

	features.at(k)->speed[cd->iframe % SMPSIZE] =
	  metric_calc_velocity_ipm(speed, (unsigned)
				   features.at(k)->vehicle.lane);
	features.at(k)->speed[cd->iframe % SMPSIZE] *= 3.6;

	if ((features.at(k)->speed[cd->iframe % SMPSIZE] < .25)
	    && (features.at(k)->discovered + 1 <
		cd->iframe)) {
	  features.at(k)->falsepositive = 1;
	}
      }
    }
    for (k = 0; k < features.size(); k++) {
      KLT_FeatureList kltf = features.at(k);

      if (print) {
	vs_write_features_to_image(roimg, nx, ny, kltf);
	draw_motion_vectors(roimg, nx, ny, cd->iframe,
			    kltf, color);
      }
      if (kltf->discovered + 1 < cd->iframe) {
	vs_log_vehicle_speed(kltf, cd->iframe, cd->output_path,
			     savelog, &vs->should_save_frame);
      }
      if (print && (VDSM_DLEVEL > 2)) {
	draw_speed_values(prev, nx, ny,
			  cd->iframe, kltf, cd->output_path);
      }
    }
  }

  void vs_end_of_stream_handler (
				 void *_data, 
				 dsm_common_data_t *cd, 
				 dsm_vs_data_t *vs) {

    print_warn("received EOS event! Exiting...\n");
    printf("True positives: %d, false positives: %d - Total: %d\n", cd->tp, cd->fp, cd->positives);
    double precision = (double)cd->tp/(double)(cd->tp + cd->fp);
    double recall    = (double)cd->tp/(double)(cd->tp + (cd->positives - cd->tp));
    double fmeasure  = (2 * precision * recall)/(precision + recall); 
    printf("Precision: %f, Recall: %f, F-measure: %f\n", precision, recall, fmeasure);
    int nx = cd->vr_nx; /* Width of visual rythm images. */
    int ny = cd->vr_ny; /* Height of visual rythm images. */

    assert(cd->visual_diff.nx == nx); /* Should check the others too... */
   
    write_uchar_to_pgm (cd->visual_diff.p, nx, ny, "%s/snapshot/visual_diff.pgm", cd->output_path);
    write_uchar_to_pgm (cd->visual_rythm0.p, nx, ny, "%s/snapshot/visual_rythm_up.pgm", cd->output_path);
    write_uchar_to_pgm (cd->visual_rythm1.p, nx, ny, "%s/snapshot/visual_rythm.pgm", cd->output_path);
    write_uchar_to_pgm (cd->visual_update.p, nx, ny, "%s/snapshot/visual_update.pgm", cd->output_path);
    write_uchar_to_pgm (cd->visual_weights.p, nx, ny, "%s/snapshot/visual_weights.pgm", cd->output_path);
    write_uchar_to_pgm (cd->visual_background.p, nx, ny, "%s/snapshot/visual_background.pgm", cd->output_path);
    write_uchar_to_pgm (cd->visual_segmentation.p, nx, ny, "%s/snapshot/visual_segmentation.pgm", cd->output_path);
    /* Configure the old locale back. */
    setlocale(LC_ALL, old_locale);
    free(old_locale);
    exit(0);
  }

  int verify_region (int rxmin, int rxmax, int rymin, int rymax, KLT_FeatureList &features) {
    bool in = false;
    int xmin = 99999, ymin = 99999, xmax = 0, ymax = 0;
    for (int j = 0; (j < features->nFeatures); j++) {
      if (features->feature[j]->val == KLT_TRACKED) {
	if (features->feature[j]->x < xmin) { xmin = features->feature[j]->x; }  
	if (features->feature[j]->y < ymin) { ymin = features->feature[j]->y; }  
	if (features->feature[j]->x > xmax) { xmax = features->feature[j]->x; }  
	if (features->feature[j]->y > ymax) { ymax = features->feature[j]->y; } 
	in = true;  
      }
    }
    if ( (xmin >= rxmin) && (ymin >= rymin) && (xmax <= rxmax) && (ymax <= rymax) && (in) && (features->discard == true) && (features->seen == false)) {
      features->seen = true;
      return 99;
    } 
    else if ( (xmin >= rxmin) && (ymin >= rymin) && (xmax <= rxmax) && (ymax <= rymax) && (in) && (features->seen == false)) {
      features->seen = true;
      return 1;
    } 
    else if ( (xmin >= rxmin) && (ymin >= rymin) && (xmax <= rxmax) && (ymax <= rymax) && (in) && (features->seen == true)) {
      return -1;  
    }
    return 0;
  }

// /* ----------------------------------------------------------------------
//     /* Region of interest is rows {yext-yrad .. yext+yrad} of each video frame. */
//     int yext; /* Reference row of frame (middle row of band to extract) */
//     int yrad; /* Half-width of band to extract. */
//     dsm_image_extract_band(img, st->yext, &(st->im));
// 
// ---------------------------------------------------------------------- */

void detect_and_process_vehicle_ends
    ( uint8_t *img, 
      dsm_common_data_t *cd, 
      dsm_vs_data_t *vs,
      vector<KLT_FeatureList> &features,
      vector<plate> plates_gt );

void write_visual_rythm_images(dsm_common_data_t *cd);
void write_reduced_images(dsm_common_data_t *cd, dsm_vs_data_t *vs);

  /* Copies the center row of {img} to row {yr} of the visual rythm image {rythm}. */
  void copy_center_row_to_rythm(dsm_image_t *img, dsm_image_t *rythm, int yr)
    {
      int nx = img->nx;
      int ny = img->ny;
      assert((ny % 2) == 1);
      int ym = ny/2; /* Central row of {img}. */
      assert(rythm->nx == nx);
      assert((yr >= 0) && (yr < rythm->ny));
      for (int x = 0; x < nx; x++) {
	rythm->p[yr*nx + x] = img->p[ym*nx + x];
      }
    }

 /* Computes a correlation coefficient between pixels {pix0[0..n-1]}
   and {pix1[0..n1]} weighted by weights {w[0..n-1]}.  */
double pixel_correl
  ( int n,
    uint8_t pix0[],
    uint8_t pix1[],
    double w[] )  
  { 
    /* Compute the averages of the two sequences: */
    double sumw0 = 0;
    double sumw1 = 0;
    double sumw = 0;
    for (int i = 0; i < n; i++) {
      double p0 = (double)pix0[i];
      double p1 = (double)pix1[i];
      sumw0 += w[i]*p0;
      sumw1 += w[i]*p1;
      sumw += w[i];
    }
    if (sumw <= 0) { return 0; }
    double avg0 = sumw0/sumw;
    double avg1 = sumw1/sumw;
    
    /* Compute variances and covariance: */
    double sumw00 = 0;
    double sumw11 = 0;
    double sumw01 = 0;
    for (int i = 0; i < n; i++) {
      double d0 = (double)pix0[i] - avg0;
      double d1 = (double)pix1[i] - avg1;
      sumw00 += d0*d0*w[i];
      sumw11 += d1*d1*w[i];
      sumw01 += d0*d1*w[i];
    }
    double den = sqrt(sumw00*sumw11);
    double cos01 = (den > 0 ? sumw01/den : 0);
    return cos01;
}

  /* Computes a correlation coefficient between pixels in row {yr} of
   {rythm1} centered at column {xr}, and the pixels in row {yr-1},
   displaced by {d} columns.  Specifically, compares columns
   {xr-mtest+1 .. xr+mtest-1} of row {yr} and columns {xr+d-mtest+1
   .. xr+d+mtest-1} of row {y-1}.  Assumes that the two images are
   defined up to row {yr} inclusive. */
double horizontal_rythm_correl
  ( dsm_image_t *rythm1,
    int xr,
    int yr, 
    int d,
    int mtest )
  {
    int nx = rythm1->nx;
    int nmax = 2*mtest + 1; /* Max number of pixels to consider. */
   
    /* Extract the pixels and assign weights: */
    uint8_t pix0[nmax];
    uint8_t pix1[nmax];
    double wpix[nmax];
    int npix = 0; /* Number of extracted pixels. */
    double wt = 1.0;
    for (int k = 0; k <= mtest-1; k++) {
      for (int dir = -1; dir <= +1; dir += 2) {
        if ((k > 0) || (dir == +1)) {
	  int xp0 = xr + dir*k;
	  int xp1 = xr + d + dir*k;
	  if ((xp0 >= 0) && (xp0 < nx) && (xp1 >= 0) && (xp1 < nx)) {
	    pix0[npix] = rythm1->p[yr * nx + xp0];
	    pix1[npix] = rythm1->p[(yr-1) * nx + xp1];
	    wpix[npix] = wt;
	    npix++;
	  }
	}
      }
      wt = 0.8*wt; /* !!! Acertar !!! */
    }
    double cos01 = pixel_correl(npix, pix0, pix1, wpix);
    return cos01;
  }

  /* Computes a correlation coefficient between pixels in column {xr}
   of {rythm0} and {rythm1}, displaced by {d} rows.  Specifically,
   compares rows {yr-d-mtest+1 .. yr-d} and {yr-mtest+1 .. yr} if {d}
   is positive or zero, or at rowns {yr-mtest+1 .. yr} and
   {yr+d-mtst+1 .. yr+d} if {d} is negative.  Assumes that the two
   images are defined up to row {yr} inclusive. */
double vertical_rythm_correl
  ( dsm_image_t *rythm0,
    dsm_image_t *rythm1,
    int xr,
    int yr, 
    int d,
    int mtest )
  {
    int nx = rythm0->nx;
    assert(nx == rythm1->nx);
    assert((xr >= 0) && (xr < nx));
   
    int nmax = mtest; /* Max number of pixels to consider. */
   
    /* Compute index of last row in each image: */
    int yend0 = (d > 0 ? yr - d : yr);
    int yend1 = (d > 0 ? yr : yr + d);

    /* Extract the pixels and assign weights: */
    uint8_t pix0[nmax];
    uint8_t pix1[nmax];
    double wpix[nmax];
    int npix = 0; /* Number of extracted pixels. */
    double wt = 1.0;
    for (int k = 0; k < mtest; k++) {
       int yp0 = yend0 - k;
       int yp1 = yend1 - k;
       if ((yp0 >= 0) && (yp1 >= 0)) {
	 pix0[npix] = rythm0->p[yp0 * nx + xr];
	 pix1[npix] = rythm1->p[yp1 * nx + xr];
         wpix[npix] = wt;
	 npix++;
	 wt = 0.8*wt; /* !!! Acertar !!! */
       }
    }
    double cos01 = pixel_correl(npix, pix0, pix1, wpix);
    //fprintf(stderr, "    row %d col %d  d = %d  npix = %d cos01 = %.5f\n", yr, xr, d, npix, cos01);
    return cos01;
  }

/* Estimates the velocity vectors at each pixel of row 
  {vr_ny} of the downstream visual rythm image
  {rythm1} by comparing it to the upstream 
  rythm {rythm0}.  Assumes that both rythm images are 
  defined up to row {vr_ny} inclusive. Stores the
  result in row {vr_ny} of {velx} and {vely},
  encoded as unsigned bytes with offset = 128. */
void estimate_velocities
  ( dsm_image_t *rythm0,
    dsm_image_t *rythm1,
    int vr_ny,
    dsm_image_t *velx, dsm_image_t *vely,
    dsm_vs_data_t *vs 
  )
  {
    int vr_nx = rythm0->nx;
    assert(rythm1->nx == vr_nx);
    assert(velx->nx == vr_nx);
    assert(vely->nx == vr_nx);

    assert((vr_ny >= 0) && (vr_ny < rythm0->ny));
    assert(rythm1->ny == rythm0->ny);
    assert(velx->ny == rythm0->ny);
    assert(vely->ny == rythm0->ny);

    int mtest = 30; /* Number of frames to compare. */
    int dmax = 10;  /* Max delay (frames) between matches. */
     
    for (int x = 0; x < vr_nx; x++) { 
        int dbest;
        double cbest;
	double wd; /* Bias weight favoring small displacements. */

        /* Estimate the vertical velocity at pixel {x}
          by looking for a corrleation between past 
          frames of {rythm0} and {rythm1} displaced vertically
          by varying amounts between {-dmax} and {+dmax}. */
        cbest = -HUGE; /* Best correlation found. */
        dbest = INT_MAX; /* Delay of best correlation. */
	wd = 1.0;
        for (int d = 0; d <= dmax; d++) {
          for (int dir = -1; dir <= +1; dir+= 2) {
	    if ((d > 0) || (dir == +1)) {
	      double c = wd*vertical_rythm_correl(rythm0, rythm1, x, vr_ny, dir*d, mtest);
              if (c > cbest) { cbest = c; dbest = dir*d; }
	    }
	  }
	  wd = 0.90*wd;
        }
        fprintf(stderr, "  row %d col %d vely: dbest = %d cbest = %.3f", vr_ny, x, dbest, cbest);

        /* The vertical velocity in pixels per frame: */
	double dy = ((double)(vs->yext0 - vs->yext1));
	double vy = (dbest == 0 ? 0 : dy/dbest);
	double vymax = dy;
        vely->p[vr_ny*vr_nx + x] = dsm_image_quantize(vy/vymax, -1.0, +1.0);
    
        /* Estimate the horizontal velocity at pixel {x}
          by looking for a correlation between the 
          last two rows of {rythm1}
          frames of {rythm0} and {rythm1} displaced
          horizontally by varying amounts between {-dmax} and {+dmax}. */

        cbest = -HUGE; /* Best correlation found. */
        dbest = INT_MAX; /* Delay of best correlation. */
	wd = 1.0;
        for (int d = 0; d <= dmax; d++) {
	  for (int dir = -1; dir <= +1; dir+= 2) {
	    if ((d > 0) || (dir == +1)) {
	      double c = wd*horizontal_rythm_correl(rythm1, x, vr_ny, dir*d, mtest);
	      if (c > cbest) { cbest = c; dbest = dir*d; }
	    }
	  }
	  wd = 0.90*wd;
        }
        fprintf(stderr, "  velx: dbest = %d cbest = %.3f\n", dbest, cbest);
    
        /* The horizontal velocity in pixels per frame: */
	double fac = (double)vs->xreduc;
	double vx = fac * ((double)dbest);
        double vxmax = fac*dmax;
        velx->p[vr_ny*vr_nx + x] = dsm_image_quantize(vx/vxmax, -1.0, +1.0);
       }
  }

  /* Process a color image {img} assumed to have {vs->img_nx}
     columns and {vs->img_ny} rows. */
  void visual_rythm_new 
    ( uint8_t *img, 
      dsm_vs_data_t *vs, 
      dsm_common_data_t *cd,
      vector<KLT_FeatureList> &features,
      vector<plate> plates_gt
      )
  {

    int nx = vs->img_nx;
    int ny = vs->img_ny;

    int vr_nx = cd->vr_nx;
    int vr_ny = cd->vr_ny;

    int nx_band = vs->img_nx;
    int ny_band = vs->band_ny;
    assert(nx_band == nx);

    int nx_red = nx_band / vs->xreduc;
    int ny_red = ny_band / vs->yreduc;

    assert((ny_red % 2) == 1);

    int ynbefore = ny_band / 2;
    int y0_red = ynbefore / vs->yreduc; /*visual rythm row in the reduced band image*/
    int median_radius = 2;

    /* Make a descriptor for the grey channel of {img}: */
    dsm_image_t img_gray = (dsm_image_t){ .nx = nx, .ny = ny, .p = img };


    /* Extract the upstream visual rythm row (we need only the {Y0} vector): */
    extract_rythm_row 
      ( &(img_gray), vs->yext0,
	vs->xreduc, vs->yreduc, 
	vs->nxredw, vs->nyredw, 
	&(vs->temp_band), &(vs->img0_red), &(vs->img0_red_blur) );
    extract_center_row_as_vector(&(vs->img0_red), vs->img_row0);
    copy_center_row_to_rythm (&(vs->img0_red), &(cd->visual_rythm0), cd->vr_ny);

    /* Extract the downstream visual rythm row (images and {Y1} vector): */
    extract_rythm_row 
      ( &(img_gray), vs->yext1,
	vs->xreduc, vs->yreduc, 
	vs->nxredw, vs->nyredw, 
	&(vs->temp_band), &(vs->img1_red), &(vs->img1_red_blur) );
    extract_center_row_as_vector(&(vs->img1_red), vs->img_row1);
    copy_center_row_to_rythm(&(vs->img1_red), &(cd->visual_rythm1), cd->vr_ny);

    /* Compute background indicator image for upstream row: */
    dsm_segment_identify_background 
      ( &(vs->img1_red), &(vs->img1_red_blur), 
        &(vs->background_red), &(vs->background_red_blur), 
        vs->lambda_bg, 
        &(vs->weight_red) );
    dsm_image_median_filter (&(vs->weight_red), &(vs->weight_red_blur), median_radius);
    extract_center_row_as_vector(&(vs->weight_red), vs->weight);
    copy_center_row_to_rythm(&(vs->weight_red), &(cd->visual_weights), cd->vr_ny);
  
    /*Update background images if the line is totally background: */
    double w = dsm_image_min (&(vs->weight_red_blur)) / 255.0;
    if (w > 0.03) {
      for (int y = 0; y < ny_red; y++) {
	for (int x = 0; x < nx_red; x++) {
	  double p = (double)vs->img1_red.p[y * nx_red + x];
	  double b = (double)vs->background_red.p[y * nx_red + x];  
	  double p_blur = (double)vs->img1_red_blur.p[y * nx_red + x];
	  double b_blur = (double)vs->background_red_blur.p[y * nx_red + x];  
	  double b_est = (p + 1) * (b_blur + 1)/(p_blur + 1) - 1;
	  double b_new = w * b_est + (1 - w) * b;
	  vs->background_red.p[y * nx_red + x] = dsm_image_quantize (b_new, 0.0, 255.0); 
	}
      }
      dsm_image_blur (&(vs->background_red), 4, 4, &(vs->background_red_blur)); 
    }
    copy_center_row_to_rythm(&(vs->background_red), &(cd->visual_background), cd->vr_ny);
    
    /* Compute time derivative and binary classification visual rythm rows: */
    int ixvr = vr_ny * vr_nx; /* Index of first pixel of current VR row. */
    for (int x = 0; x < vr_nx; x++) {
      double p = (double)vs->img1_red.p[y0_red * vr_nx + x];
      double b = (double)vs->background_red.p[y0_red * vr_nx + x];
      double w = (double)vs->weight_red_blur.p[y0_red * vr_nx + x]/255.0;
      int fore = (w < 0.3);  /* TRUE if pixel seems foreground. */
      if (vr_ny > 0) {
        int ixvr_prev = ixvr - vr_nx; /* Index of first pixel of previous VR row. */
        double p_prev = cd->visual_rythm1.p[ixvr_prev + x];
	cd->visual_update.p[ixvr + x] = (int)fabs(p - p_prev);
	if (cd->visual_update.p[ixvr + x] > 20) { fore = 1; }
      }
      if (fore) {
	cd->visual_update.p[ixvr + x] = 255;
	cd->visual_segmentation.p[ixvr + x] = FOREGROUND;
      }
      else {
	cd->visual_update.p[ixvr + x] = 0;
	cd->visual_segmentation.p[ixvr + x] = BACKGROUND;
      }
    }

    /* Estimate velocity vectors on the downstream row: */
    estimate_velocities
      ( &(cd->visual_rythm0), &(cd->visual_rythm1), 
       cd->vr_ny, 
       &(cd->visual_velx), &(cd->visual_vely),
       vs );

    /* Detect end of vehicles and process them: */
    detect_and_process_vehicle_ends(img, cd, vs, features, plates_gt);
   
    if (false) { 
      write_reduced_images(cd, vs);
    }


    if ((vr_ny % 200) == 0) {
      write_visual_rythm_images(cd);
    }
    
    cd->vr_ny++;
  }

void write_visual_rythm_images(dsm_common_data_t *cd)
  {
    int vr_nx = cd->vr_nx;
    int vr_ny = cd->vr_ny;

    write_uchar_to_pgm (cd->visual_rythm1.p, vr_nx, vr_ny, "%s/snapshot/visual_rythm.pgm", cd->output_path);
    write_uchar_to_pgm (cd->visual_rythm0.p, vr_nx, vr_ny, "%s/snapshot/visual_rythm_up.pgm", cd->output_path);
    write_uchar_to_pgm (cd->visual_update.p, vr_nx, vr_ny, "%s/snapshot/visual_update.pgm", cd->output_path);
    write_uchar_to_pgm (cd->visual_events.p, vr_nx, vr_ny, "%s/snapshot/visual_events.pgm", cd->output_path);
    write_uchar_to_pgm (cd->visual_weights.p, vr_nx, vr_ny, "%s/snapshot/visual_weights.pgm", cd->output_path);
    write_uchar_to_pgm (cd->visual_background.p, vr_nx, vr_ny, "%s/snapshot/visual_background.pgm", cd->output_path);
    write_uchar_to_pgm (cd->visual_segmentation.p, vr_nx, vr_ny, "%s/snapshot/visual_segmentation.pgm", cd->output_path);
    write_uchar_to_pgm (cd->visual_velx.p, vr_nx, vr_ny, "%s/snapshot/visual_velx.pgm", cd->output_path);
    write_uchar_to_pgm (cd->visual_vely.p, vr_nx, vr_ny, "%s/snapshot/visual_vely.pgm", cd->output_path);
  }


void write_reduced_images(dsm_common_data_t *cd, dsm_vs_data_t *vs)
  { 
     int nx_red = vs->img1_red_blur.nx;
     int ny_red = vs->img1_red_blur.ny;
    
     char filename[512];
   
      snprintf(filename, sizeof(filename), "%s/snapshot/frame_red_blur_%05ld.pgm", cd->output_path, cd->iframe);
      write_uchar_to_pgm (vs->img1_red_blur.p, nx_red, ny_red, filename);

      snprintf(filename, sizeof(filename), "%s/snapshot/frame_up_red_blur_%05ld.pgm", cd->output_path, cd->iframe);
      write_uchar_to_pgm (vs->img0_red_blur.p, nx_red, ny_red, filename);

      snprintf(filename, sizeof(filename), "%s/snapshot/frame_red_%05ld.pgm", cd->output_path, cd->iframe);
      write_uchar_to_pgm (vs->img1_red.p, nx_red, ny_red, filename);
     
      snprintf(filename, sizeof(filename), "%s/snapshot/frame_up_red_%05ld.pgm", cd->output_path, cd->iframe);
      write_uchar_to_pgm (vs->img0_red.p, nx_red, ny_red, filename);
     
      snprintf(filename, sizeof(filename), "%s/snapshot/frame_weight_red_%05ld.pgm", cd->output_path, cd->iframe);
      write_uchar_to_pgm (vs->weight_red.p, nx_red, ny_red, filename);
   
      snprintf(filename, sizeof(filename), "%s/snapshot/frame_weight_red_blur_%05ld.pgm", cd->output_path, cd->iframe);
      write_uchar_to_pgm (vs->weight_red_blur.p, nx_red, ny_red, filename);
     
      snprintf(filename, sizeof(filename), "%s/snapshot/frame_background_red_blur_%05ld.pgm", cd->output_path, cd->iframe);
      write_uchar_to_pgm (vs->background_red_blur.p, nx_red, ny_red, filename);

      snprintf(filename, sizeof(filename), "%s/snapshot/frame_background_red_%05ld.pgm", cd->output_path, cd->iframe);
      write_uchar_to_pgm (vs->background_red.p, nx_red, ny_red, filename);
    }

  /* Assumes that {cd->vr_ny} has not been incremented yet. */
  void detect_and_process_vehicle_ends
    ( uint8_t *img, 
      dsm_common_data_t *cd, 
      dsm_vs_data_t *vs,
      vector<KLT_FeatureList> &features,
      vector<plate> plates_gt ) 
  {
 
    int nx = vs->img_nx;
    int ny = vs->img_ny;

    int vr_nx = cd->vr_nx;
    int vr_ny = cd->vr_ny;
    
    int nx_band = vs->img_nx;
    int ny_band = vs->band_ny;
    assert(nx_band == nx);

    int nx_red = nx_band / vs->xreduc;
    int ny_red = ny_band / vs->yreduc;

    int ixvr = vr_ny * vr_nx; /* Index of first pixel of current VR row. */

    int y0 = vs->yext1; /* Downstream detection row. */
    
    double hmin = 8, wmin = 16, dilate_radius = 2;

    assert((vs->temp_vr.nx == vr_nx) && (vs->temp_vr.ny >= vr_ny));
    Vector cc = find_connected_components_inverse 
      ( cd->visual_segmentation.p, vr_ny, vr_nx,
	hmin, wmin, 
	&(vs->start_row), dilate_radius, 
	cd->output_path, 
	cd->iframe, 
	vs->temp_vr.p
      );
    
    for (int i = 0; i < vs->R.size(); i++) {
      region *ra = vs->R.at(i);
      for (int j = 0; j < vector_count(&cc); j++) {
	region *rb = (region *)vector_get(&cc, j);
	if ( (intersection_area(ra, rb)/region_area(ra) > 0.9) && (intersection_area(ra, rb)/region_area(rb) > 0.9) && (!ra->finish) ) {
	  rb->detected = ra->detected;
	  rb->plates += ra->plates;
	} 
      }
    }

    for (int x = 0; x < vr_nx; x++) {
      cd->visual_events.p[ixvr + x] = cd->visual_segmentation.p[ixvr + x];
    }

    for (int i = 0; i < vector_count(&cc); i++) {

      region *r = (region *)vector_get(&cc, i);

      /*{
	int xmin = (int)(r->box[0][0] * vs->xreduc);
	int xmax = (int)(r->box[0][1] * vs->xreduc);
	int ymin = y0 - 300;
	int ymax = y0;
	char filename[256];
	snprintf(filename, sizeof(filename), "%s/snapshot/%05d_%02d_regiao.pgm", cd->output_path, cd->iframe, i);
	save_frame_window_to_pgm (img + xmin + (ymin * nx), nx, xmax - xmin, ymax - ymin, filename); 
	}*/

      double *rad = radii (r);
      //if (r->angle < 0.03) {
      /*Normalize angle to the range [-pi/2-+pi/2]*/
      double ang = r->angle_rad;
      if (ang > +M_PI/2) { ang -= M_PI; }
      if (ang < -M_PI/2) { ang += M_PI; }
      //printf("Angulo: %f\n", ang); 
      if (ang < 0) {
	r->detected = true;
 
	/*{
	  int xmin = (int)(r->box[0][0] * vs->xreduc);
	  int xmax = (int)(r->box[0][1] * vs->xreduc);
	  int ymin = y0 - 300;
	  int ymax = y0;
	  char filename[256];
	  snprintf(filename, sizeof(filename), "%s/snapshot/%05d_%02d_detectado.pgm", cd->output_path, cd->iframe, i);
	  save_frame_window_to_pgm (img + xmin + (ymin * nx), nx, xmax - xmin, ymax - ymin, filename); 
	  }*/

	//int xmin = (int)(r->box[0][0] * vs->xreduc) - 100;
	int xmin = (int)(r->box[0][0] * vs->xreduc);
	int xmax = (int)(r->box[0][1] * vs->xreduc);
	int ymin = y0 - 700;//(int)(r->box[1][0] * vs->yreduc);
	int ymax = y0 - 100;//(int)(r->box[1][1] * vs->yreduc);
	if (xmin > 800) {
	  xmin -= 200;
	  xmax -= 50;
	}
	/*else if (xmin < 400) {
	  xmin += 200;
	  xmax += 200;
	  }*/

	int sizef = features.size();

	for (int kf = 0; kf < sizef; kf++) {

	  int outcome = verify_region (xmin, xmax, ymin, ymax, features.at(kf));
             
	  if (outcome == 1) {
	    //write_uchar_to_pgm (out, w, cd->vr_ny, "%s/snapshot/tp_cleaned_%05d.pgm", cd->output_path, cd->iframe);
	    //char filename[256];
	    //snprintf(filename, sizeof(filename), "%s/snapshot/%05d_%02d_tp.pgm", cd->output_path, cd->iframe, i);
	    //save_frame_window_to_pgm (img + xmin + (ymin * nx), nx, xmax - xmin, ymax - ymin, filename); 
	    cd->tp++; 
	    r->plates++;
	  }
	  else if (outcome == 99) { 
	    r->discard = true;
	  }
	} 
	paint_chrom_rect (img, nx, ny, xmin, ymin, xmax, ymax, 255, 0);
	cd->visual_events.p[ixvr + 5] = KEEP;
      }

      if (r->finish) {
        if (r->detected && r->plates > 0 && (r->discard == false)) {
	  for (int xx = r->box[0][0]; xx <= r->box[0][1]; xx++) {
            cd->visual_segmentation.p[ixvr + xx] = DETECTED;
	  }
	  /*{
	    int xmin = (int)(r->box[0][0] * vs->xreduc);
	    int xmax = (int)(r->box[0][1] * vs->xreduc);
	    int ymin = y0 - 300;
	    int ymax = y0;
	    char filename[256];
	    snprintf(filename, sizeof(filename), "%s/snapshot/%05d_%02d_finished.pgm", cd->output_path, cd->iframe, i);
	    save_frame_window_to_pgm (img + xmin + (ymin * nx), nx, xmax - xmin, ymax - ymin, filename); 
	    }*/
        }

        if ((r->detected) && (r->plates == 0) && (r->discard == false)) {

	  for (int xx = r->box[0][0]; xx <= r->box[0][1]; xx++) {
	    cd->visual_segmentation.p[ixvr + xx] = 60;
	  }
	  /*{
	    int xmin = (int)(r->box[0][0] * vs->xreduc);
	    int xmax = (int)(r->box[0][1] * vs->xreduc);
	    int ymin = y0 - 300;
	    int ymax = y0;
	    char filename[256];
	    snprintf(filename, sizeof(filename), "%s/snapshot/%05d_%02d_fp.pgm", cd->output_path, cd->iframe, i);
	    save_frame_window_to_pgm (img + xmin + (ymin * nx), nx, xmax - xmin, ymax - ymin, filename); 
	    }*/
	  printf("False positives: %d - \n", cd->fp);

	  cd->fp++;
	} 
      }   
    }

    for (int kk = 0; kk < plates_gt.size(); kk++) {
      if (!(plates_gt.at(kk).discard)) {
	cd->positives++;
      }
    }

    vs->R.clear();

    for (int i = 0; i < vector_count(&(cc)); i++) {
      region *ra = (region *)vector_get(&(cc), i);
      vs->R.push_back(ra);
    }
  }
							
  /*
   * Main callback function.
   * @param buff Pointer to current image buffer.
   * @param vs Pointer to the structure returned by vs_preload function.
   */
  void vs_chain_callback(dsm_common_data_t *cd, void *_data)
  {
    static vector<struct sub_slope> slopes;
    vector<plate> plates_gt;
    struct sub_data *sub;

    /* Get structs. */
    dsm_vs_data_t *vs = (dsm_vs_data_t *) _data;
    int nx = vs->img_nx;
    int ny = vs->img_ny;

    /* Get buffers. */
    uint8_t *roimg = grab_past_buffer(cd, 0); /* Current color frame. */
    uint8_t *chrom = roimg + (nx * ny); /* Chroma channels of {roimg}. */
    decolorize (roimg, nx, ny);
        
    //if ((cd->iframe % 200) == 0) {
    //   printf("Frame: %d\n", cd->iframe);
    //print_timestamp("frame " cterm(TB_WHT, "%d\n"), cd->iframe);
    //}

    plates_gt = read_plates_from_xml(vs->vt, cd->iframe);

    vs_print_start (chrom, nx, ny, vs->yext0, 32);
    vs_print_start (chrom, nx, ny, vs->yext1, 32);

    compute_features_from_plates(plates_gt, vs->features_gt, vs, cd);

    dsm_paint_plates(roimg, nx, ny, plates_gt, 0, 255);

    visual_rythm_new (roimg, vs, cd, vs->features_gt, plates_gt);
    
	
    vs_unnecessary_remove_features(vs->features_gt);

    process_feature_list (vs->features_gt, vs->tc_gt, vs, cd, "groundtruth", true, 0);
    cd->timestamp += VIDEO_FRAME_PERIOD;
  }
}