/*
  st-match - reads intensity stesp from two images, finds matching edgepoints
  
  usage: st-match [ OPTIONS... ] LEFTSTEPS.ist RIGHTSTEPS.ist > MATCHES.mtc
  
  Format of MATCES.mtc is:
  
*/

#include <sys/time.h>
#include <sys/resource.h>
#include <stdio.h>
#include <string.h>

/*  --------------------- constants ------ */

/* INTERNAL PROTOTYPES */

int main(int argc, char* argv[]);
static void parse_options(
  int *argc, char **argv, 
  int *wxP, int *wyP, char **imgnameP
);
static FILE *open_rd(char *name);
static void sort_grays(gray *v, int n);

/* ROUTINES */
 
int main(int argc, char **argv)
  { 
    char *l_name, *r_name;
    FILE *l_file, *r_file;
    int cols, rows;
    gray maxval;
    int l_format, r_format;
    
    parse_options(&argc, argv, ..., &l_name, &r_name);
    l_file = open_rd(l_name);
    r_file = open_rd(r_name);
    { int c, r, m, f;
      pgm_readpgminit(l_file, &c, &r, &m, &l_format);
      cols = c; rows = r; maxval = m;
      pgm_readpgminit(r_file, &c, &r, &m, &r_format);
      if ((cols != c) | (rows != r) || (maxval != m))
        pm_error("left and right images must have the same size and depth\n");
    }

    writematch(stdout, cols, rows, maxval, 0);

  char parametros[50], saida[50], saida_degraus_l[50], saida_degraus_r[50],
       saida_casa_2d[50], saida_casa_3d[50], saida_casa_iso[50];
  char aux[100];
  double start, stop; /* para medida de tempo de execucao */
  FILE *arq_parametros;
  
  float zmax, zmin;
 
  strcpy(right_image, argv[1]);
  strcat(right_image, "_r.pgm");
  strcpy(left_image, argv[1]);
  strcat(left_image, "_l.pgm");
  
  strcpy(parametros, argv[2]);
  strcat(parametros, ".parms");
 
  /* ENTRADA DE DADOS  */
  if((arq_parametros = fopen(parametros, "r")) == NULL)
     {
       fprintf(stderr,"\n ---- Arquivo de parametros nao pode ser aberto -EXIT\n");
       exit(1);
     }
  fgets(aux, 98, arq_parametros);  /* leitura do cabecalho   */
  fgets(aux, 98, arq_parametros);  /* leitura de duas linhas do arquivo de   */
  fgets(aux, 98, arq_parametros);  /* entrada que e usada apenas em tese-seg */
  fscanf(arq_parametros,"foco = %f\n",&foco);
  fscanf(arq_parametros,"baseline = %f\n",&baseline);
  fscanf(arq_parametros,"delta = %d\n",&delta);
  fscanf(arq_parametros,"larg_min = %d\n",&larg_min);
  fscanf(arq_parametros,"maxaresta = %d\n",&maxaresta);
  fscanf(arq_parametros,"alfa = %f\n",&alfa);
  fscanf(arq_parametros,"beta = %f\n",&beta);
  fscanf(arq_parametros,"disparity = %d\n",&disparity);
  fscanf(arq_parametros,"nmax_match = %f\n",&nmax_match);
  
  fprintf(stderr,"     TESTE  cod. ( %s )\n",saida); 
 
  /* ---  ---*/
  fprintf(stderr,"zmin = ");
  scanf("%f",&zmin);
  fprintf(stderr,"\nzmax = ");
  scanf("%f",&zmax);
 
  disparity_inf = foco*baseline/zmax;
  disparity_sup = foco*baseline/zmin; 
  
  
  fprintf(stderr,"\ndisp_inf = %f\n",disparity_inf);  
  fprintf(stderr,"disp_sup = %f\n",disparity_sup);    
  
   

  
  fprintf(stderr,"\n nmin_match = ");
  scanf("%f",&nmin_match);  
  
  /* preparacao dos nomes dos arquivos de saidas */
  strcpy(saida, argv[1]);
  strcat(saida, "-");
  strcat(saida, argv[2]);
  
  strcpy(saida_degraus_l, saida);
  strcpy(saida_degraus_r, saida);
  strcpy(saida_casa_2d, saida);
  strcpy(saida_casa_3d, saida);
  strcpy(saida_casa_iso, saida);
  strcat(saida_degraus_l, "_l.degraus");
  strcat(saida_degraus_r, "_r.degraus");
  strcat(saida_casa_2d, "_2d.casa");
  strcat(saida_casa_3d, "_3d.casa");
  strcat(saida_casa_iso, "_iso.casa");
  
  nmatch = maxaresta * maxaresta;
  if(nmatch>N_MATCH)
    {
      fprintf(stdout," ****---- nmatch > NMATCH -----******\n");
      fprintf(stdout," Programa abortado em match\n");
      exit(1);
    }

   
  inicia_lista();

  abre_arq();

  start = now();
  carreira();
  stop = now();
  fprintf(stdout,"ISTEP - Total time (ms) =  %.0f\n",(stop -start)/1000.0);
  
  fclose(l_file);
  fclose(r_file);
  
  start = now();
  match();
  stop = now();
  fprintf(stdout,"MATCH - Total time (ms) =  %.0f\n",(stop -start)/1000.0);

  fprintf(stdout,"\n\n\n      -----------------***********------------\n\n\n");
  


 /* saida para arquivos de degraus e casamento */
 { 
     FILE *degraus_l,        /* arquivo da degraus da esquerda */
          *degraus_r,        /* arquivo da degraus da direita  */
	  *casa_2d,    /* arquivo de casamentos reais em 2d */
	  *casa_3d;    /* arquivo de casamentos reais em 3d */
     FILE *casa_iso; /* aponta para arquivo  de saida com pontos
			      em 3d isometricos   */
     struct IStep *ptr;
     struct MPair *point;
     int i, x, nptos;
     float const_cord_x;
     float const_cord_y;
     float x3d, y3d, z3d;  /* coordenadas em 3d do range-map */
     int x2d, y2d;

     const_cord_x = ((float)sizex-1.0)/2.0;
     const_cord_y = ((float)sizey-1.0)/2.0;
     degraus_l = fopen(saida_degraus_l, "w");
     if(degraus_l == NULL)
       {
	 fputs(" Erro ao criar arquivo IStep_l. \n",stdout);
	 fputs(" ********* Programa abortado ******\n",stdout);
	 exit(1);
       }
       
     degraus_r = fopen(saida_degraus_r, "w");
     if(degraus_r == NULL)
       {
	 fputs(" Erro ao criar arquivo IStep_r. \n",stdout);
	 fputs(" ********* Programa abortado ******\n",stdout);
	 exit(1);
       }
       
     casa_2d = fopen(saida_casa_2d, "w");
     if(casa_2d == NULL)
       {
	 fputs(" Erro ao criar arquivo casa-2d. \n",stdout);
	 fputs(" ********* Programa abortado ******\n",stdout);
	 exit(1);
       }     

     casa_3d = fopen(saida_casa_3d, "w");
     if(casa_3d == NULL)
       {
	 fputs(" Erro ao criar arquivo casa-3d. \n",stdout);
	 fputs(" ********* Programa abortado ******\n",stdout);
	 exit(1);
       }     
       
     casa_iso = fopen(saida_casa_iso, "w");
     if(casa_iso == NULL)
       {
	 fputs(" Erro ao criar arquivo casa-iso. \n",stdout);  
	 fputs(" ********* Programa abortado ******\n",stdout);
	 exit(1);
       }

     for(i=0; i<sizey; i++)
       {
	ptr = lista_left[i][0];
	while( ptr )
	  {
	   x = (int)(ptr->x_mid + 0.5);
	   fprintf(degraus_l,"%d %d\n",x,i);
	   ptr = ptr->next;
	  }

	ptr = lista_right[i][0];
	while( ptr )
	  {
	   x = (int)(ptr->x_mid + 0.5);
	   fprintf(degraus_r,"%d %d\n",x,i);
	   ptr = ptr->next;
	  }
       }
     nptos = 0;
     point = start_MPair;
     while( point )
       {
	 nptos++;
	 point = point->next;
       }
       
     /* escrita em arquivo de saida em coordenadas isotropicas */
     fprintf(casa_iso,"%d\n",nptos);
     point = start_MPair;     
     while( point )
       {
	 x3d = point->x;
	 y3d = point->y;
	 z3d = point->z;
	 fprintf(casa_iso,"%f %f %f\n",x3d,y3d,z3d);
	 point = point->next; 
       }

     
/* --- escrita em arquivo de saida em coordenadas reais 3D ---*/
     fprintf(casa_3d,"nptos = %d\n",nptos);
     fprintf(casa_3d,"foco = %f\n",foco);
     point = start_MPair;     
     while( point )
       {
	 x3d = baseline * (point->x - const_cord_x)/(2.0*point->z);
	 y3d = baseline * (point->y*1.414214 - const_cord_y)/(2.0*point->z);
	 z3d = foco*baseline/(2.0*point->z);
	 fprintf(casa_3d,"%f %f %f\n",x3d,y3d,z3d);
	 point = point->next; 
       }
      
       
/* escrita do casamento em coordenadas reais projetadas em 2D*/
 
/*     fprintf(casa_2d,"%d\n",nptos);*/
     point = start_MPair;     
     while( point )
       {
	 x3d = baseline * (point->x - const_cord_x)/(2.0*point->z);
	 y3d = baseline * (point->y*1.414214 - const_cord_y)/(2.0*point->z);
	 z3d = foco*baseline/(2.0*point->z);
	 
	 x2d =(int) (foco * x3d/z3d + const_cord_x);
	 y2d =(int) (foco * y3d/z3d + const_cord_y);
	 fprintf(casa_2d,"%5d %5d\n",x2d,y2d);
	 point = point->next; 
       }
       
       
       
  /* imprimir parametros lidos do stdout */

     fprintf(stdout,"     TESTE  cod. ( %s )\n",saida);
     fprintf(stdout,"delta = %d\n",delta);
     fprintf(stdout,"larg_min = %d\n",larg_min);
     fprintf(stdout,"maxaresta = %d\n",maxaresta);
     fprintf(stdout,"alfa = %f\n",alfa);
     fprintf(stdout,"beta = %f\n",beta);
     fprintf(stdout,"foco = %f\n",foco);
     fprintf(stdout,"baseline = %f\n",baseline);
     fprintf(stdout,"disparity = %d\n",disparity); 
     fprintf(stdout,"nmax_match = %f\n",nmax_match);
     fprintf(stdout,"\nNptos casados = %d\n",nptos);
       
   }

}

/* --------------------- variaveis globais -------------*/ 
struct MPair *start_MPair = NULL,  /* inicio e fim da lista  */
             *end_MPair = NULL;    /* de profundidade */

unsigned char line_left[SCREENWIDTH],  /* buffers para leitura de */
              line_right[SCREENWIDTH]; /* linhas das imagens */

/* apontam para o inicio e fim da lista de IStep de cada linha das imagens */
struct IStep *lista_left[SCREENHEIGHT][2],
              *lista_right[SCREENHEIGHT][2];




char left_image[50], right_image[50]; /* nome das imagens de
                                         entrada */

int sizex;  /* largura da imagem em pixels */
int sizey;  /*  altura da imagem em pixels */

int larg_min, maxaresta, disparity, delta;
float foco, baseline, alfa, beta;
float disparity_inf, disparity_sup;
int  nmatch;
float nmax_match, nmin_match;


/* --------------------------- funcao para medida de tempo ------------ */
/* now() - retorna uma medida da hora atual em micro segundos */
double now(void) 
  { struct rusage ru;
    
    getrusage(RUSAGE_SELF, &ru);
    return(((double)ru.ru_utime.tv_sec)*1000000.0 + ((double)ru.ru_utime.tv_usec));
  }


/* ----------------------------- filtro mediana --------------------------- */
/* ordena um vetor v de tamanho tamf */
void ordena_vetor(v,tamf)
int *v;
int tamf;
{
int i, j, a;

for(i = 0; i<tamf; i++)
    for(j = (i+1); j<tamf; j++)
       {
	a = v[i];
        if(v[j] < a)
	  {
	   v[i] = v[j];
	   v[j] = a;
	  }
       }
}


void filtro_mediana(unsigned char *line_image)
{
int tamf; /* tamf=tamanho do filtro. Deve ser impar */
int f[20];
int o[20];
int meio;
int ix, iy, j;
unsigned char aux_line[SCREENWIDTH];

for(j=0; j<20; j++) f[j] = 0;

tamf = 2*larg_min - 1;
meio = (tamf - 1)/2;

   for(ix = 0; ix < sizex; ix++)
     {
      for(j = 0; j < tamf; j++)
	{
	 if((ix + j - meio)<0) continue;
	 if((ix + j - meio)>=sizex) continue;
	 f[j] = line_image[ix+j-meio];
        }
      for(j = 0; j < tamf; j++) o[j] = f[j];

      ordena_vetor(o,tamf);
      aux_line[ix] = o[meio];
/*      line_image[ix] = o[meio];*/

     }
   for(ix = 0; ix < sizex; ix++)
     line_image[ix] = aux_line[ix];
}



/* ----------------------------- fun_arq ---------------------------- */

/* abre_arq() - Abre arquivos das imagens e le cabecalho de 4 linhas */
void abre_arq(void)
{
  char carac;
  char s[512];
  
  l_file = fopen(left_image, "r");
  r_file = fopen(right_image, "r");
   
  /* leitura e verificacao de cabecalho p/imagem da direita */
  fgets(s, 510, r_file);
  if(strcmp(s, "P5\n") != 0)
    {
      fprintf(stdout," Imagem da direita com formato diferente de PGM \n");
	exit(1);
    }
  
  carac = getc(r_file);
  while(carac == '#')
    {
      fgets(s, 510,r_file);
      carac = getc(r_file);
    }
    
  ungetc(carac, r_file);
  
  fscanf(r_file,"%d %d\n",&sizex,&sizey);
  fprintf(stdout,"imagem direita: sizex = %d sizey = %d\n",sizex,sizey);
  
  fgets(s, 510, r_file); /* leitura da ultima linha = niveis de cinza */
  
  
  /* leitura e verificacao de cabecalho p/imagem da esquerda */
  fgets(s, 510, l_file);
  if(strcmp(s, "P5\n") != 0)
    {
      fprintf(stdout," Imagem da esquerda com formato diferente de PGM \n");
      exit(1);
    }
  
  carac = getc(l_file);
  while(carac == '#')
    {
      fgets(s, 510,l_file);
      carac = getc(l_file);
    }
    
  ungetc(carac, l_file);
  
/*  
  carac = getc(l_file);
  if(carac == '#')
    fgets(s, 510,l_file);
  else
    ungetc(carac, l_file);
*/

  fscanf(l_file,"%d %d\n",&sizex,&sizey);
  fprintf(stdout,"imagem esquerda: sizex = %d sizey = %d\n\n",sizex,sizey);
  
  fgets(s, 510, l_file); /* leitura da ultima linha = niveis de cinza */
	
}



/* Inicializa vetor que aponta para listas de degraus */
void inicia_lista(void)
{
int i;

for(i=0 ; i< sizey; i++)
  {
      lista_left[i][0] = NULL;
      lista_left[i][1] = NULL;
      lista_right[i][0] = NULL;
      lista_right[i][1] = NULL;
  }
}



/* Le linha dos arquivos, filtra-as com filtro da mediana e 
   as escrevem no buffer */
void le_linha(void)
{
    fread(line_left, sizeof(unsigned char), sizex, l_file);
    filtro_mediana(line_left);
    fread(line_right, sizeof(unsigned char), sizex, r_file);
    filtro_mediana(line_right);
}


/* --------------------------- carreiras ---------------------------- */

/* ins_blk - insere um novo bloco na lista de blocos de ISteps */
unsigned char ins_blk(ini_lista, fim_lista)
struct IStep **ini_lista, **fim_lista;
{
   struct IStep *ptr, *elemento;

   ptr = (struct IStep *)malloc(sizeof (struct IStep));

   if ( ptr == NULL )
      return ( ERR );

   if ( *ini_lista == NULL )
	{
		*ini_lista = ptr;
		*fim_lista = ptr;
		ptr->next = NULL;
		ptr->prev = NULL;
	}
    else
	{
	   elemento = *fim_lista;
	   elemento->next = ptr;
	   ptr->prev = elemento;
	   ptr->next = NULL;

	   *fim_lista = ptr;
	}

   return ( OK );

}


/* valor_IStep()
   calcula um val inerente a IStep  */
int val_IStep(x_beg, x_end, pix_beg, pix_end)
int x_beg, x_end;
int pix_beg, pix_end;
{
 int val;

 val = abs(pix_end - pix_beg) * (W_ISTEP - (x_end - x_beg));

 return(val);
}

/* Calcula o centro nominal da IStep: */
float centro_IStep(int r, int s, unsigned char line[])
{
  float pos, pa, pb;
  /*
    int i = r + 1;
    float avg = (((float) line[r]) + ((float)line[s]))/2.0;
    if (line[r] < line[s])
      {  while (line[i] <= avg) i++; }
    else
      {  while (line[i] >= avg) i++; }
    pa = line[i-1];
    pb = line[i];
    pos = ((float) (i-1)) + (avg - pa)/(pb - pa);
  */
  pos = ((float)(r+s))/2.0;
  return (pos);
}
  


/* carreira()
   procura ISteps pelo metodo de carreiras  */
void carreira()
{
 int pix_fim1, pix_ini2, x_fim1, x_ini2, ix, iy, p1, p2;
 int larg,      /* largura da carreira corrente */
     num_carr,  /* numero =1 ou 2(para duas carr consecutivas) */
     val;     /* val inerente da IStep   */
 unsigned char flag; /* flag para teste de contagem de carreira */

 int num_IStep;    /* Contador do num. de ISteps em cada linha   */

 struct IStep *ptr, *point, *min_point;

 for(iy=0; iy<sizey; iy++)
   {
    le_linha();

/* codigo para analise da imagem da esquerda */

    num_IStep = 0;
    larg = 1;
    num_carr = 0;
    flag =1;

    for(ix=1; ix<sizex; ix++)
      {
	  p1 = line_left[ix-1];
	  p2 = line_left[ix];
	  
	  if(abs(p2-p1) <= 2*delta)
	    {
		larg++;
	    }
	  else
	    {
		if(num_carr == 1)
		  {
		      pix_ini2 = p2;
		      x_ini2 = ix;
		      larg = 1;
		      
		      if(flag ==0)
			{
			    pix_fim1 = p1;
			    x_fim1 = ix-1;
			}
		  }
		
		
		if(num_carr == 0) larg = 1;
		flag =1;
	    }
	  
	  
	  if(larg >= larg_min)
	    {
		if(flag ==1) num_carr++;
		flag = 0;
		
		if(num_carr == 2) /* existe IStep.inserir na lista */
		  {
		      val = val_IStep(x_fim1, x_ini2, pix_fim1, pix_ini2);
		      
		      if(num_IStep >= maxaresta)
			{
			    min_point = lista_left[iy][0];
			    point = min_point->next;
			    while(point)
			      {
				  if(point->val < min_point->val)
				    min_point = point;
				  point = point->next;
			      }
			    
			    if(min_point->val < val)
			      {
				  ptr = min_point;
				  ptr->x_beg = x_fim1;
				  ptr->x_end = x_ini2;
				  ptr->x_mid = centro_IStep(ptr->x_beg, ptr->x_end, line_left);
				  ptr->pix_beg = pix_fim1;
				  ptr->pix_end = pix_ini2;
				  ptr->val = val;
			      }
			}
		      
		      else
			{
			    ins_blk(&lista_left[iy][0], &lista_left[iy][1]);
			    num_IStep++;
			    
			    ptr = lista_left[iy][1];
			    
			    ptr->x_beg = x_fim1;
			    ptr->x_end = x_ini2;
			    ptr->x_mid = centro_IStep(ptr->x_beg, ptr->x_end, line_left);
			    ptr->pix_beg = pix_fim1;
			    ptr->pix_end = pix_ini2;
			    ptr->val = val;
			}
		      
		      num_carr = 1;
		  }
	    }
      }
/* fim codigo de analise da imagem da esquerda */


/*  codigo para analise da imagen da direita */

    num_IStep = 0;
    larg = 1;
    num_carr = 0;
    flag =1;

    for(ix=1; ix<sizex; ix++)
      {
       p1 = line_right[ix-1];
       p2 = line_right[ix];

       if(abs(p2-p1) <= 2*delta)
	 {
	  larg++;
	 }


       else
	 {
	  if(num_carr == 1)
	       {
		pix_ini2 = p2;
		x_ini2 = ix;
		larg = 1;

		if(flag ==0)
		  {
		   pix_fim1 = p1;
		   x_fim1 = ix-1;
		  }
		}


	  if(num_carr == 0) larg = 1;
	  flag =1;
	 }


       if(larg >= larg_min)
	    {
	     if(flag ==1) num_carr++;
	     flag = 0;

	     if(num_carr == 2) /* existe IStep.inserir na lista */
	       {
		val = val_IStep(x_fim1, x_ini2, pix_fim1, pix_ini2);

		if(num_IStep >= maxaresta)
		  {
		   min_point = lista_right[iy][0];
		   point = min_point->next;
		   while(point)
		     {
		      if(point->val < min_point->val)
			 min_point = point;
		      point = point->next;
		     }

		   if(min_point->val < val)
		     {
		      ptr = min_point;
		      ptr->x_beg = x_fim1;
		      ptr->x_end = x_ini2;
		      ptr->x_mid = centro_IStep(ptr->x_beg, ptr->x_end, line_right);
		      ptr->pix_beg = pix_fim1;
		      ptr->pix_end = pix_ini2;
		      ptr->val = val;
		     }
		  }

		else
		  {
		   ins_blk(&lista_right[iy][0], &lista_right[iy][1]);
		   num_IStep++;

		   ptr = lista_right[iy][1];

		   ptr->x_beg = x_fim1;
		   ptr->x_end = x_ini2;
		   ptr->x_mid = centro_IStep(ptr->x_beg, ptr->x_end, line_right);
		   ptr->pix_beg = pix_fim1;
		   ptr->pix_end = pix_ini2;
		   ptr->val = val;
		  }

		num_carr = 1;
	       }
	    }
      }
/* fim codigo de analise da imagem da direita */


  }
}


/* --------------------------- match ---------------------------- */

/* ins_MPair - insere um novo bloco na lista de blocos de profundidade */
unsigned char ins_MPair(ini_lista, fim_lista)
struct MPair **ini_lista, **fim_lista;
{
   struct MPair *ptr, *elemento;

   ptr = (struct MPair *)malloc(sizeof (struct MPair));

   if ( ptr == NULL )
      return ( ERR );

   if ( *ini_lista == NULL )
	{
		*ini_lista = ptr;
		*fim_lista = ptr;
		ptr->next = NULL;
		ptr->prev = NULL;
	}
    else
	{
	   elemento = *fim_lista;
	   elemento->next = ptr;
	   ptr->prev = elemento;
	   ptr->next = NULL;

	   *fim_lista = ptr;
	}

   return ( OK );

}

/* funcao para o calculo do max e minimo entre modulos de diferencas */
void min_max(pixl1, pixl2, pixr1, pixr2, min, max)
int pixl1,pixl2,
    pixr1,pixr2;  /* pixels das imagens esq e dir */
int *min, *max;            /* valores retornados */
{
int dif1, dif2;

dif1 = abs(pixl1 - pixr1);
dif2 = abs(pixl2 - pixr2);

if(dif1 >= dif2)
  {
      *min = dif2;
      *max = dif1;
  }
else
  {
      *min = dif1;
      *max = dif2;
  }

/*fputs("  passou por mim_max",stderr);*/
}

/* f_normaliza
   calcula x, y, z normalizados */
void f_normaliza(xl, xr, yl, yr, x, y, z)
float  xl, xr, yl, yr;
float *x, *y, *z;
{
    *x = (xl + xr)/2.0;

    *z = (xl - xr)/2.0;

    *y = yr/1.414214;   /* sqrt(2) */
}


/* f_MPair() ----!!!!!VERIFICAR CALCULOS DAS COORDENADAS
   funcao para calculo das coordenadas X, Y, Z de um pto
   a partir das coordenadas X,Y de imagens estereoscopicas */
void f_MPair(xl, xr, yl, yr, x, y, z)
float  xl, xr, yl, yr;
float *x, *y, *z;
{
  if((xl - xr) >= 0)
   {
    *x = (baseline/2)*(xl + xr - (sizex-1))/(xl - xr);

    *z = foco * baseline /(xl - xr);

    *y = (baseline/2)*(yr + yl)/(xl - xr);
   }

/*fputs("  passou por MPair",stderr);*/
}


/* escalonar()
   funcao para mapear ptos em diferentes escalas.
   Mapeia variacoes de x0 a x1 em variacoes de h0 a h1.  */
float escalonar(x, x0, x1, h0, h1)
float x, x0, x1, h0, h1;
{
 float h; /* h e' funcao de x */

 h = x*(h1 - h0)/(x1 - x0) + (h0*x1 - h1*x0)/(x1 -x0);

/* fputs("  passou por escalonar",stderr);*/

 return(h);
}

/* rotina para escolher entre nmin e nmax melhores dentre nin dados.
   os dados estao no vetor value em ordem crescente de valores.
   voce quer os nmax menores valores
   retorna indice do ultimo val nao incluido*/
int n_melhor(int nmin, int nmax, float value[])
{
  int nout;

  nout = nmin;
  if (nmin > 1)
  { float avggap = (value[nmin-1]-value[0])/((float)nmin);
    float lastgap = value[nmin-1]-value[nmin-2];
    float tiegap = (lastgap < avggap ? lastgap : avggap);
    while(
      (nout < nmax) &&
      ((value[nout]-value[nout-1]) <= tiegap)
    ) nout++;
  }
  return(nout);
}



/* funcao para ordenar em ordem crescente um vetor de valores e
   um respectivo vetor de inteiros */
void sort(int n, float value[], int elem[])
  { int p, q, r, mx;
    float cc, vmax;
    int ccelem;
        
    /* 1. Build heap with largest element at value[0] */
    for (p = 0; p<n; p++)
      { cc = value[p]; 
	ccelem = elem[p];
	r = p;
	while(r > 0)
	  { q = (r-1) / 2;
	    if (value[q] < cc) 
	      { value[r] = value[q]; 
		elem[r] = elem[q];
	      }
	    else break;
	    r = q;
	  }
	value[r] = cc; 
	elem[r] = ccelem;
      }

    /* 2. Remove elements from heap and insert at end */
    for (p = n-1; p > 0; p--)
      { 
	/* save value[p] */
	cc = value[p]; 
	ccelem = elem[p];
	/* Move largest heap element to povalue[p] */
	value[p] = value[0]; 
	elem[p] = elem[0];
	/* Insert cc in remaining heap, from root down  */
	q = 0;
	while(1)
	  {
	    value[q] = cc; 
	    elem[q] = ccelem;
	    r = 2*q+1;
	    /* Find largest among cc, value[LEFT(q)], value[RIGHT(q)]  */
	    mx = q; vmax = cc;
	    if ((r<p)&&(value[r]>vmax))
	      { mx = r; vmax = value[r]; }
	    r++;
	    if((r<p)&&(value[r]>vmax))
	      { mx = r; vmax = value[r]; }
	    /*  See who won  */
	    if (mx==q) break;
	    /*  Promote child and advance  */
	    value[q] = value[mx];  
	    elem[q] = elem[mx]; 
	    q = mx;
	  }
      }
  }


/* funcao de casamento de ptos correspondentes */
void match()
{
int iy, i, kmatch;
int min, max;  /* max e min entre pixels */
int num_match; /* numero de casamentos correntes por linha */

float x_left,      /* coordenadas das ISteps das imagens   */
      x_right,     /* em       */
      y_left;      /* pixels.  */
     /* y_right = y_left */

float x3d, y3d, z3d; /* coordenadas no espaco calculadas por MPair() */

float fator;   /* fator de casamento  */

float matriz_MPair[N_MATCH][4]; /* matriz com os dados do casamento
				  [][0] = xl
				  [][1] = xr
				  [][2] = y = yr = yl
				  [][3] = fator de casamento   */
float vetor_fator[N_MATCH]; /* vetor aux para ordenacao dos fatores */
int vetor_index[N_MATCH];   /* vetor indice dos fatores ordenados */
struct IStep *ptr_left, *ptr_right;

struct MPair *max_ptr, *ptr;
 

for(iy=0; iy<sizey; iy++)
  {
   ptr_left = lista_left[iy][0]; 
   num_match = 0;
   while(ptr_left)
     {
      ptr_right = lista_right[iy][0];
      while(ptr_right)
	{
	 y_left = (float) iy;

         x_left = ptr_left->x_mid;
         x_right = ptr_right->x_mid;
	 
/*	 ------- restricao da epipolar line */
	 if(x_left <= (x_right + disparity_inf))
	   {
	       ptr_right = ptr_right->next;
	       continue;
	   }
	   
	 if(x_left >= (x_right + disparity_sup))
	   {
	     ptr_right = ptr_right->next;
	     continue;
	   }
/*	---------------------  */
	
	 /* restricao da epipolar line utlizada
	 if(x_left <= x_right + disparity)
	   {
	       ptr_right = ptr_right->next;
	       continue;
	   }
	   
	 */
	 min_max(ptr_left->pix_beg, ptr_left->pix_end,
		 ptr_right->pix_beg, ptr_right->pix_end, &min, &max);

	 fator = alfa * min + beta * max;

	 matriz_MPair[num_match][0] = x_left;
	 matriz_MPair[num_match][1] = x_right;
	 matriz_MPair[num_match][2] = y_left;
	 matriz_MPair[num_match][3] = fator;
	 vetor_fator[num_match] = fator;
	 vetor_index[num_match] = num_match;
/* --	   
	 if(iy==210)
	   {
	       fprintf(stderr,"%d  %d  %f   ",
		 (int)x_left, (int)x_right,fator);
	   } 	 
	   
          if(iy==210)
	    {
	      fprintf(stderr,"%d %d %d %d  %d  %d\n",
		      ptr_left->pix_beg, ptr_left->pix_end,
		      ptr_right->pix_beg, ptr_right->pix_end,min,max);
	    } 
---*/
/*	 if(iy==240)
	   {
	       fprintf(stderr,"%d %d %d %d \n",
		 ptr_left->pix_beg, ptr_left->pix_end,
		 ptr_right->pix_beg, ptr_right->pix_end);
	       fprintf(stderr,"%d %d %.4f\n",min,max,fator);
	   } */
	   
	 num_match++;
	 ptr_right = ptr_right->next;
	 
        }
      ptr_left = ptr_left->next;
     }

   if(num_match == 0)
     continue;
    sort(num_match, vetor_fator, vetor_index);

   { int nleft = 0;
     int nright = 0;
     int nmaxlr, nmax, nmin;
     struct IStep *p;
     p = lista_right[iy][0]; while(p) { nright++; p = p->next; }
     p = lista_left[iy][0]; while(p) { nleft++; p = p->next; }
     nmaxlr = (nleft > nright ? nleft : nright);
     nmax = (int)(nmax_match * (float) nmaxlr) + 1;
     if(num_match < nmax) nmax = num_match;
     nmin = (int)(nmin_match * (float)nmaxlr) +1;
     if (nmin > nmax) nmin = nmax;
     kmatch = n_melhor(nmin, nmax, vetor_fator);
/* --     fprintf(stderr,
       "linha %d: nleft = %d nright = %d num_match = %d nmin = %d nmax = %d kmatch = %d\n",
       iy, nleft, nright, num_match, nmin, nmax, kmatch
     );
 ---- */     
   }
  
    for(i=0; i<kmatch; i++)
      {
	  f_normaliza(matriz_MPair[vetor_index[i]][0],matriz_MPair[vetor_index[i]][1],matriz_MPair[vetor_index[i]][2],matriz_MPair[vetor_index[i]][2],&x3d,&y3d,&z3d);

/*	  if(iy ==240)
	    {
		fprintf(stderr,"--- %.4f",matriz_MPair[vetor_index[i]][3]);
	    }*/

	  ins_MPair(&start_MPair, &end_MPair);
	  ptr = end_MPair;
	  ptr->fator = fator; /* --ESTA ERRADO ---*/
	  ptr->x = x3d;
	  ptr->y = y3d;
	  ptr->z = z3d;
      }
         
  }

/*fputs("  passou por match",stderr);*/
}