/* Programa que faz o casamento do modelo com os dados obtidos.
   O arquivo de entrada dos vertices deve estar no formato
   x y z    para cada vertice.  */

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

#define NUM_CASA 3 /* num minimo de vertices casados (dados com modelo transladado) */
#define ERR -1
#define OK 1
#define FALSE 0
#define SIZEX 512
#define SIZEY 512
#define NUM_FACES 3 /* numero de faces incidente em cada vertice */
#define MAXSAIDA 30

float baseline, foco, const_cord_x, const_cord_y;
float dist_mod_max;
float **mv,  /* matriz de vertices do modelo */
      **mu_3d,  /* matriz de vertices dos dados em 3d */
      **mu_iso; /* matriz de vertices dos dados isotropicos */
float **mv_homog;  /* mat. para coord. homogeneas dos
			   vertices do modelo*/
float **mv_map_hom,  /* vertices do modelo mapeados pela 
	                          mat final em coord. homogeneas */
        **mv_map_iso, /* vertices mv_map_hom em 
	                    cord. cartezianas isotropicos */
        **mat_faces; /* matriz de coeficientes para as faces
                       primeiro indice = indice da face */
		       
float camara_l[3], camara_r[3];

int  **mat_ind_face, /* matriz das faces incidentes nos vertices.
                                      primeiro ind = vertice, segundo,terceiro = indice
			 da face  */
      **mat_lig;   /* matriz de ligacoes entre vertices */
FILE  *saida, *file_log;

int nvertice_dados, nvertice_mod, nligacoes;  /* num. de vertices obtidos dos dados
                                      e do modelo e num de ligacoes */
int num_saida;

struct saida_casa{
       int    ncasados,
              nvisiveis;
       float  valor,
              discrepancia,
	          mat_final[4][4];
       struct saida_casa *next,
                         *prev;
     };
     
     
struct saida_casa *ini_saida = NULL,  /* inicio e fim da lista  */
			   *fim_saida = NULL;    /* de saida_casas */     
  
		
/* --------------------------- 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));
  }
		

/* ins_vert - insere um novo bloco na lista de blocos saida_casas */
unsigned char ins_saida(ini_lista, fim_lista)
struct saida_casa **ini_lista, **fim_lista;
{
   struct saida_casa *ptr, *elemento;

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

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

   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 );

}



/* multiplicacao de vetor linha por matriz  em R4 */
void prod_row_mat(x, m, res)
float *x, m[4][4], *res; /* x[4], res[4] */
{
    float s;
    int j, k;

    for(j=0; j<4 ;j++)
      {
	  s = 0.0;
	  for(k=0; k<4 ;k++)
	    {
		s += x[k]*m[k][j];
	    }
	  res[j] = s;
      }
}

/* multiplicacao de matriz 4x4 por vetor coluna 4x1 */
void prod_mat_colum(a, b, o)  /* produto de matrizes o = ab
				       a(4 x 4),  b(4 x 1) */
float a[4][4], *b, *o;
{
    int i, j, k;
    float s;

    for(k=0; k<4; k++)
      for(i=0; i<4; i++)
	{
	    s = 0.0;
	    for(j=0; j<4; j++)
	      {
		  s += a[k][j]*b[j];
	      }
	    o[k] = s;
	}
}
	


/* determinante de uma matriz 3x3 */
float det3x3(m)
float m[3][3];
{
    float res, d0, d1, d2;

    d0 = m[1][1]*m[2][2] - m[1][2]*m[2][1];
    d1 = m[1][0]*m[2][2] - m[1][2]*m[2][0];
    d2 = m[1][0]*m[2][1] - m[1][1]*m[2][0];
    res = d0 * m[0][0] - d1 * m[0][1] + d2 * m[0][2];

    return(res);
}

/* cofator do elemento [ix][jx] de uma matriz 4x4 */
float cof(matin, ix, jx)
float matin[4][4];
int ix, jx;
{
  float t[3][3];
  float res, d;
  int ii, jj, i, j;

  ii = 0;
  for(i=0; i<4; i++)
    {
	if(i!=ix)
	  {
	      jj = 0;
	      for(j=0; j<4; j++)
		{
		    if(j!=jx)
		      {
			  t[ii][jj] = matin[i][j];
			  jj++;
		      }
		}
	      ii++;
	  }
    }

  d = det3x3(t);

  if(((ix+jx)%2)!=0)
    res = -d;
  else
    res = d;

  return(res);
}


/* determinante de uma matriz 4x4 */
float det4x4(m)
float m[4][4];
{
  float res;

  res = m[0][0] * cof( m, 0, 0 ) + m[0][1] * cof( m, 0, 1 ) + 
        m[0][2] * cof( m, 0, 2 ) + m[0][3] * cof( m, 0, 3 );

  return(res);
}
		  


/* inversa de uma matriz 4x4 : m * result = I * det(m)**2   */
void inversa(matin, matout)
float matin[4][4], matout[4][4];
{
  float d;
  int i, j;

  for(i=0; i<4; i++)
    for(j=0; j<4; j++)
      matout[i][j] = 0.0;
  
  d = det4x4(matin);
  if(d == 0.0)
    return;

  for(i=0; i<4; i++)
    for(j=0; j<4; j++)
      matout[i][j] = cof(matin, j, i)/d;

  return;
}




void mat_prod(a, b, o)  /* produto de matrizes o = ab
				       a(4 x 4),  b(4 x 4) */
float a[4][4], b[4][4], o[4][4];
{
    int i, j, k;
    float s;

    for(k=0; k<4; k++)
      for(i=0; i<4; i++)
	{
	    s = 0.0;
	    for(j=0; j<4; j++)
	      {
		  s += a[k][j]*b[j][i];
	      }
	    o[k][i] = s;
	}
}
	
	
 
     

void crossp(a, b, o) /* produto vetorial o = a x b  de vetores R3  */
float *a, *b, *o;
{
    float d;
    o[0] = a[1]*b[2] - a[2]*b[1];
    o[1] = a[2]*b[0] - a[0]*b[2];
    o[2] = a[0]*b[1] - a[1]*b[0];

    d = sqrt(o[0]*o[0] + o[1]*o[1] + o[2]*o[2]);
    o[0] /= d;
    o[1] /= d;
    o[2] /= d;
}

float normalize(a) /* calcula o modulo do vetor em R3  */
float *a;
{
    float d;

    d = sqrt(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);

    a[0] /= d;
    a[1] /= d;
    a[2] /= d;

    return(d);
}

/* dist() - calcula a distancia entre dois ptos no espaco R3.
            Retorna a distancia calculada.   */
float dist(p1, p2)
float *p1,  /* ponto 1 ; vetor[3] */
      *p2;  /* ponto 2 ; vetor[3] */
{
 int i;
 float dist = 0.0;  /* distancia entre p1 e p2  */

 for(i=0; i<3; i++)
   {
     dist += (p1[i] - p2[i]) * (p1[i] - p2[i]);
   }

 dist = sqrt(dist);

 return(dist);
}

/* para visualizar uma matriz */
void ver2(m, nlinhas, ncol)
int nlinhas, ncol;
float m[nlinhas][ncol];
{
  int i, j;
  fprintf(stderr,"\n");
  for(i = 0; i<nlinhas; i++)
    {
      for(j=0; j<ncol; j++)
	{
	  fprintf(file_log,"   %f",m[i][j]);
        }

      fprintf(file_log,"\n\n");
    }
}


/* para visualizar uma matriz- forma diferente de alocacao*/
void ver1(m, nlinhas, ncol)
int nlinhas, ncol;
float **m;
{
  int i, j;
  fprintf(stderr,"\n");
  for(i = 0; i<nlinhas; i++)
    {
      for(j=0; j<ncol; j++)
	{
	  fprintf(stderr,"   %f",m[i][j]);
        }

      fprintf(stderr,"\n\n");
    }
}

/* para visualizar uma matriz inteira - forma diferente de alocacao*/
void ver_int(m, nlinhas, ncol)
int nlinhas, ncol;
int **m;
{
  int i, j;
  fprintf(stderr,"\n");
  for(i = 0; i<nlinhas; i++)
    {
      for(j=0; j<ncol; j++)
	{
	  fprintf(stderr,"   %d",m[i][j]);
        }

      fprintf(stderr,"\n\n");
    }
}


/* funcao conversao de iso para 3d */
void conv_iso_3d(x_iso, y_iso, z_iso, x_3d, y_3d, z_3d)
float x_iso, y_iso, z_iso;
float *x_3d, *y_3d, *z_3d;
{
  *x_3d = baseline * (x_iso - const_cord_x)/(2.0*z_iso);
  *y_3d = baseline * (y_iso*1.414214 - const_cord_y)/(2.0*z_iso);
  *z_3d = foco*baseline/(2.0*z_iso);
}  

/* funcao conversao de 3d para iso */
void conv_3d_iso(x_3d, y_3d, z_3d, x_iso, y_iso, z_iso)
float *x_iso, *y_iso, *z_iso;
float x_3d, y_3d, z_3d;
{
  *z_iso = foco*baseline/(2.0*z_3d);
  *x_iso = 2.0*(*z_iso)*x_3d/baseline + const_cord_x;
  *y_iso = 1.414214*(*z_iso)*y_3d/baseline + 0.707107*const_cord_y;
}  
  

unsigned char visibilidade(camara_l, camara_r, matriz_map, ind_vert)
float *camara_l, *camara_r; 
float matriz_map[4][4];
int ind_vert;
{
  unsigned char teste_visib;
  int i, p;
  float cam1, cam2;
  float m_map_inv[4][4],
        face_map[4];
	
  teste_visib = FALSE;
  
  inversa(matriz_map, m_map_inv); /* calcula a inversa */
  
  for(p=0; p<NUM_FACES; p++)
    {
        /* mapea plano */
      prod_mat_colum(m_map_inv, mat_faces[mat_ind_face[ind_vert][p]], face_map);
   
  
      cam1 = 0.0;
      cam2 = 0.0;
  
      for(i=0; i<4; i++)
	{
	  cam1 += face_map[i]*camara_l[i];
	  cam2 += face_map[i]*camara_r[i];
	}

      if(cam1>0.0 && cam2>0.0)
	{
	  teste_visib = OK; /* vertice visivel */
	  break;
	}
    }
  
  return(teste_visib);
}
    
  

  
void calculos()
{
  float bar_v[3], bar_u[3];  /* baricentro do modelo e dados */
  float trans_mod[4][4], trans_dados[4][4],
        scal[4][4],  /* mat translacao e escala */
        aux1[4][4], aux2[4][4], aux1_inv[4][4], /* auxiliares */
        rot[4][4], mat_final[4][4]; /* rotacao e final */
  
  float var_v, var_u, alfa; /* variancia dos vert, dados e alfa */
  float aux4, aux5, aux3;
  float vet_aux1[3], vet_aux2[3];

  float d, dmin; /* para calculo de distancia */

  float quad_dist, discrepancia, valor;
  int ncasados, nvisiveis;
  
  unsigned char teste_visib;
  
  
  float normal_v1[3], normal_v2[3], normal_v3[3];/* normais dos vert.*/
  float normal_u1[3], normal_u2[3], normal_u3[3];/* normais dos dados*/
  
  int i, j, k,  /* indices utilizados p/ vertices do modelo */
      l, m, n;  /* indices utilizados p/ vertices dos dados */

  int t, h;

  struct saida_casa *ptr_saida, *ptr_aux;

  num_saida = 0;
    
  for(i=0; i<nvertice_mod; i++)
    for(j=i+1; j<nvertice_mod; j++)
      for(k=j+1; k<nvertice_mod; k++)
	{
	  for(l=0; l<nvertice_dados; l++)
	    for(m=0; m<nvertice_dados; m++)
	      for(n=0; n<nvertice_dados; n++)
		if((l!=m)&&(l!=n)&&(m!=n))
		  {
		    /* ----- calculo do baricentro --- */
		    bar_v[0] = (mv[i][0] + mv[j][0] + mv[k][0])/3.0;
		    bar_v[1] = (mv[i][1] + mv[j][1] + mv[k][1])/3.0;
		    bar_v[2] = (mv[i][2] + mv[j][2] + mv[k][2])/3.0;
		    
		    bar_u[0] = (mu_3d[l][0] + mu_3d[m][0] + mu_3d[n][0])/3.0;
		    bar_u[1] = (mu_3d[l][1] + mu_3d[m][1] + mu_3d[n][1])/3.0;
		    bar_u[2] = (mu_3d[l][2] + mu_3d[m][2] + mu_3d[n][2])/3.0;

		    
		    
		    /* ------ Matriz de translacao --- */
		    trans_mod[0][0] = 1.0;
		    trans_mod[0][1] = - bar_v[0];
		    trans_mod[0][2] = - bar_v[1];
		    trans_mod[0][3] = - bar_v[2];
		    
		    trans_mod[1][0] = 0.0;
		    trans_mod[1][1] = 1.0;
		    trans_mod[1][2] = 0.0;
		    trans_mod[1][3] = 0.0;
		    
		    trans_mod[2][0] = 0.0;
		    trans_mod[2][1] = 0.0;
		    trans_mod[2][2] = 1.0;
		    trans_mod[2][3] = 0.0;
		    
		    trans_mod[3][0] = 0.0;
		    trans_mod[3][1] = 0.0;
		    trans_mod[3][2] = 0.0;
		    trans_mod[3][3] = 1.0;

		    trans_dados[0][0] = 1.0;
		    trans_dados[0][1] = bar_u[0] ;
		    trans_dados[0][2] = bar_u[1] ;
		    trans_dados[0][3] = bar_u[2] ;
		    
		    trans_dados[1][0] = 0.0;
		    trans_dados[1][1] = 1.0;
		    trans_dados[1][2] = 0.0;
		    trans_dados[1][3] = 0.0;
		    
		    trans_dados[2][0] = 0.0;
		    trans_dados[2][1] = 0.0;
		    trans_dados[2][2] = 1.0;
		    trans_dados[2][3] = 0.0;
		    
		    trans_dados[3][0] = 0.0;
		    trans_dados[3][1] = 0.0;
		    trans_dados[3][2] = 0.0;
		    trans_dados[3][3] = 1.0;

		    
		    
		    
	       /* ---------- Variancia total ---- */
		    aux4 = dist(mv[i],bar_v);
		    aux4 *= aux4;
		    aux5 = dist(mv[j],bar_v);
		    aux5 *= aux5;
		    aux3 = dist(mv[k],bar_v);
		    aux3 *= aux3;
		    var_v = aux4 + aux5 + aux3;
		    
		    aux4 = dist(mu_3d[l],bar_u);
		    aux4 *= aux4;
		    aux5 = dist(mu_3d[m],bar_u);
		    aux5 *= aux5;
		    aux3 = dist(mu_3d[n],bar_u);
		    aux3 *= aux3;
		    var_u = aux4 + aux5 + aux3;
		    
		    alfa = sqrt(var_u/var_v);

	       /* ---------- Matriz de escala --- */
		    scal[0][0] = 1.0;
		    scal[0][1] = 0.0;
		    scal[0][2] = 0.0;
		    scal[0][3] = 0.0;
		    scal[1][0] = 0.0;
		    scal[1][1] = alfa;
		    scal[1][2] = 0.0;
		    scal[1][3] = 0.0;
		    scal[2][0] = 0.0;
		    scal[2][1] = 0.0;
		    scal[2][2] = alfa;
		    scal[2][3] = 0.0;
		    scal[3][0] = 0.0;
		    scal[3][1] = 0.0;
		    scal[3][2] = 0.0;
		    scal[3][3] = alfa;
		    


	        /*----- --- calculo das normais --- */
		    vet_aux1[0] = mv[j][0] - mv[i][0];
		    vet_aux1[1] = mv[j][1] - mv[i][1];
		    vet_aux1[2] = mv[j][2] - mv[i][2];
		    
		    vet_aux2[0] = mv[k][0] - mv[i][0];
		    vet_aux2[1] = mv[k][1] - mv[i][1];
		    vet_aux2[2] = mv[k][2] - mv[i][2];
		    
		    crossp(vet_aux1, vet_aux2, normal_v1);

		    vet_aux1[0] = mu_3d[m][0] - mu_3d[l][0];
		    vet_aux1[1] = mu_3d[m][1] - mu_3d[l][1];
		    vet_aux1[2] = mu_3d[m][2] - mu_3d[l][2];

		    vet_aux2[0] = mu_3d[n][0] - mu_3d[l][0];
		    vet_aux2[1] = mu_3d[n][1] - mu_3d[l][1];
		    vet_aux2[2] = mu_3d[n][2] - mu_3d[l][2];
		    
		    crossp(vet_aux1, vet_aux2, normal_u1);

		/* ------- calculo de vetor ortogonal a normal_1 */
		    normal_v2[0] = mv[i][0] - bar_v[0];
		    normal_v2[1] = mv[i][1] - bar_v[1];
		    normal_v2[2] = mv[i][2] - bar_v[2];
		    
		    normalize(normal_v2);
		    
		    normal_u2[0] = mu_3d[l][0] - bar_u[0];
		    normal_u2[1] = mu_3d[l][1] - bar_u[1];
		    normal_u2[2] = mu_3d[l][2] - bar_u[2];
		    
		    normalize(normal_u2);
		    
	     /* ------ calculo do terceiro vetor ortogonal */
		    crossp(normal_v1, normal_v2, normal_v3);
		    crossp(normal_u1, normal_u2, normal_u3);


		/*----- matriz de rotacao ----- */

		    aux1[0][0] = 1.0;
		    aux1[0][1] = 0.0;
		    aux1[0][2] = 0.0;
		    aux1[0][3] = 0.0;
		    aux1[1][0] = 0.0;
		    aux1[1][1] = normal_v1[0];
		    aux1[1][2] = normal_v1[1];
		    aux1[1][3] = normal_v1[2];
		    aux1[2][0] = 0.0;
		    aux1[2][1] = normal_v2[0];
		    aux1[2][2] = normal_v2[1];
		    aux1[2][3] = normal_v2[2];
		    aux1[3][0] = 0.0;
		    aux1[3][1] = normal_v3[0];
		    aux1[3][2] = normal_v3[1];
		    aux1[3][3] = normal_v3[2];
		    
		    aux2[0][0] = 1.0;
		    aux2[0][1] = 0.0;
		    aux2[0][2] = 0.0;
		    aux2[0][3] = 0.0;
		    aux2[1][0] = 0.0;
		    aux2[1][1] = normal_u1[0];
		    aux2[1][2] = normal_u1[1];
		    aux2[1][3] = normal_u1[2];
		    aux2[2][0] = 0.0;
		    aux2[2][1] = normal_u2[0];
		    aux2[2][2] = normal_u2[1];
		    aux2[2][3] = normal_u2[2];
		    aux2[3][0] = 0.0;
		    aux2[3][1] = normal_u3[0];
		    aux2[3][2] = normal_u3[1];
		    aux2[3][3] = normal_u3[2];
		    
		    inversa(aux1, aux1_inv); /* calcula matriz
						inversa de aux1 */
		    
		    mat_prod(aux1_inv, aux2, rot); /* rot */



	       /* --------- matriz  final---------  */
		      
		    mat_prod(trans_mod, scal, aux1); /* aux1 = trans_mod*scal*/
	            mat_prod(aux1, rot, aux2);  /* aux2 = trans_mod*scal*rot */
                    mat_prod(aux2, trans_dados, mat_final);  
		      /* mat_final = trans_mod*scal*rot*trans_dados    */


	      /* ------ QUALIDADE DA MATRIZ FINAL ----*/
	      /* ----- vertices do modelo mapeados em mv_map_hom[4] -----*/

		    for(h=0; h<nvertice_mod; h++)
		      prod_row_mat(mv_homog[h], mat_final, mv_map_hom[h]);

		    

		/*--- procura do mv_map_hom mais proximo de u ----*/
		
		    		
		    /* transforma vertices mapeados de 3d para isotropicos */
		/* --------------------------
		    for(h=0; h<nvertice_mod; h++)
		      conv_3d_iso(mv_map_hom[h][1], mv_map_hom[h][2], mv_map_hom[h][3], &mv_map_iso[h][0], &mv_map_iso[h][1], &mv_map_iso[h][2]);
		  ------------------ */

		    nvisiveis = 0;
		    ncasados = 0;
		    discrepancia =0.0;
		    for(h=0; h<nvertice_mod; h++)
		      {
			teste_visib = visibilidade(camara_l, camara_r, mat_final, h);
			
			if(!teste_visib)
			  continue;
			nvisiveis++;
			
			conv_3d_iso(mv_map_hom[h][1], mv_map_hom[h][2], mv_map_hom[h][3], &mv_map_iso[h][0], &mv_map_iso[h][1], &mv_map_iso[h][2]); 
			    
			dmin = dist(mu_iso[0], mv_map_iso[h]);
			
			for(t=1; t<nvertice_dados; t++)
			  {
			    d = dist(mu_iso[t], mv_map_iso[h]);
			    if(d < dmin)
			      dmin = d;
			  }
			
			quad_dist = dmin*dmin;
	/*		fprintf(stderr,"QUAD_DIST = %f\n",quad_dist); */
			if(quad_dist<dist_mod_max)
			  {
			    ncasados++;
			  }
			discrepancia += quad_dist;
		      }

		  /* --- imprimir resultado -----*/

		    if(ncasados>=NUM_CASA)
		      {
			valor = sqrt(discrepancia/(ncasados + 1));
			
/* ----------
                        fprintf(file_log," Ncasados = %d\n    Valor = %f\n",
				ncasados, valor);
			fprintf(file_log," Discrepancia = %f\n",discrepancia);
			fprintf(file_log," Vertices usados com modelo = [%d] [%d] [%d]\n",i,j,k);
			fprintf(file_log," Vertices usados com dados  = [%d] [%d] [%d]\n",l,m,n);
			
			  fprintf(file_log,"  Matriz trans\n");
			  ver2(trans,4,4);
			  fprintf(file_log,"  Matriz Scal\n");
			  ver2(scal,4,4);
			  fprintf(file_log,"  Matriz rot\n");
			  ver2(rot,4,4);
			  fprintf(file_log,"  Matriz final\n");
			  ver2(mat_final,4,4);
			  fprintf(file_log,"  Matriz mu_3d dos dados\n");
			  ver1(mu_3d,7,3);
			  fprintf(file_log,"  Matriz mv dos vertices\n");
			  ver1(mv,8,3);
			  
			  fprintf(file_log,"  Matriz mv_map_hom[8][4] vertices mapeados\n");
			  ver1(mv_map_hom,nvertice_mod,4);

------------- */

/* ---- insere na lista saida a matriz final -----*/

			if(num_saida >= MAXSAIDA)
			  {
			    ptr_saida = ini_saida;
			    ptr_aux = ptr_saida->next;
			    while(ptr_aux)
			      {
				if(ptr_aux->discrepancia > ptr_saida->discrepancia)
				  ptr_saida = ptr_aux;
				ptr_aux = ptr_aux->next;
			      }
			    
			    if(ptr_saida->discrepancia > discrepancia)
			      {
				ptr_saida->ncasados = ncasados;
				ptr_saida->nvisiveis = nvisiveis;
				ptr_saida->valor = valor;
				ptr_saida->discrepancia = discrepancia;
				
				for(h=0; h<4; h++)
				  for(t=0; t<4; t++)
				    ptr_saida->mat_final[h][t] = mat_final[h][t];
			      }
			  }
			
			else
			  {
			    ins_saida(&ini_saida, &fim_saida);
			    num_saida++;
			    
			    ptr_saida = fim_saida;
			    ptr_saida->ncasados = ncasados;
			    ptr_saida->nvisiveis = nvisiveis;
			    ptr_saida->valor = valor;
			    ptr_saida->discrepancia = discrepancia;
			    
			    for(h=0; h<4; h++)
			      for(t=0; t<4; t++)
				ptr_saida->mat_final[h][t] = mat_final[h][t];
			  }
			
			
			
		      }
		    
		  }
        }
}

/* 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;
	  }
      }
  }


void escreve_saida()
{
  int i, h;
  struct saida_casa *ptr;
  float discrepancia, valor;
  struct saida_casa *vetor_ptr[MAXSAIDA];
  int vet_indice[MAXSAIDA];
  float value[MAXSAIDA];
  
  i = 0;
  ptr = ini_saida;
  while(ptr)
    {
      vetor_ptr[i] = ptr;
      vet_indice[i] = i;
      value[i] = ptr->discrepancia;
      i++;
      ptr = ptr->next;
    }

  sort(num_saida, value, vet_indice); /* ordena em ordem crescente de 
                                        discrepancia */

  for(i=0; i<num_saida; i++)
    {
      for(h=0; h<nvertice_mod; h++)
	prod_row_mat(mv_homog[h], vetor_ptr[vet_indice[i]]->mat_final, mv_map_hom[h]);
      
      
      /* ESCRITA DOS PTOS MAPEADOS EM ARQUIVO
	 PARA PLOTAGEM EM CANVAS */
      
      fprintf(saida,"%3d  %11.4f %11.4f %11.4f   %3d  %3d  %9.4f  %9.4f\n",i,mv_map_hom[mat_lig[0][0]][1],mv_map_hom[mat_lig[0][0]][2],mv_map_hom[mat_lig[0][0]][3], vetor_ptr[vet_indice[i]]->ncasados, vetor_ptr[vet_indice[i]]->nvisiveis, vetor_ptr[vet_indice[i]]->discrepancia, vetor_ptr[vet_indice[i]]->valor);
      fprintf(saida,"%11.4f %11.4f %11.4f\n",mv_map_hom[mat_lig[0][1]][1],
	      mv_map_hom[mat_lig[0][1]] [2],mv_map_hom[mat_lig[0][1]][3]);
      
      for(h=1; h<nligacoes; h++)
        {
	  fprintf(saida,"%11.4f %11.4f %11.4f\n",mv_map_hom[mat_lig[h][0]][1],
		  mv_map_hom[mat_lig[h][0]][2],mv_map_hom[mat_lig[h][0]][3]);
	  fprintf(saida,"%11.4f %11.4f %11.4f\n",mv_map_hom[mat_lig[h][1]][1],
		  mv_map_hom[mat_lig[h][1]][2],mv_map_hom[mat_lig[h][1]][3]);
	}
      
      fprintf(saida,"%f %f %f\n",-1.0,-1.0,-1.0);
      
    }
  
}

void main(argc, argv)
int argc;
char *argv[];
{
  int i;
  char aux[255], carac;
  char parametros[50], saida_aux[50],saida_log[50], saida_mod[50],
       entrada_vert[50], modelo[50];
  
  double start, stop; /* para medida de tempo de execucao */
  int nfaces; /* numero de faces do objeto do modelo */
      
  float erro_pixel, fator_dist_mod_max; /* fator a ser multiplicado por erro_pixel */
  
     
  FILE *arq_parametros, *file_modelo, *entrada;
  
  if(argc!=4)
    {
      fprintf(stderr,"\n ----------SINTAXE INCORRETA\n");
      fprintf(stderr," sintaxe: casa-mod imagem parametro modelo(sem extensao)\n");
      fprintf(stderr,"   Programa abortado\n\n");
      exit(1);
    }

  strcpy(parametros, argv[2]);
  strcat(parametros, ".parms");
  
  strcpy(saida_aux, argv[1]);
  strcat(saida_aux, "-");
  strcat(saida_aux, argv[2]);

  strcpy(saida_log, saida_aux);
  strcat(saida_log, "_mod.log");  

  strcpy(saida_mod, saida_aux);
  strcat(saida_mod, "_3d.mod");  
  
  strcpy(entrada_vert, saida_aux);
  strcat(entrada_vert, "_iso.vert");
  
  strcpy(modelo, argv[3]);
  strcat(modelo, ".modelo");  

  entrada = fopen(entrada_vert, "r"); 
  arq_parametros = fopen(parametros, "r");
  file_modelo = fopen(modelo, "r");
  saida = fopen(saida_mod, "w");
  file_log = fopen(saida_log, "w");
  
  if(!entrada || !arq_parametros || !file_modelo)
    {
      fprintf(stderr," Arquivo de entrada nao pode ser aberto\n");
      exit(1);
    }

  
  fgets(aux, 254, arq_parametros);  /* leitura do cabecalho   */
  fscanf(arq_parametros,"erro_pixel = %f\n",&erro_pixel);
  
  fgets(aux, 254, arq_parametros); /* leitura de dados nao usados */

  fscanf(arq_parametros,"foco = %f\n",&foco);
  fscanf(arq_parametros,"baseline = %f\n",&baseline);
  
  fgets(aux, 254, arq_parametros); /* leitura de dados nao usados */
  fgets(aux, 254, arq_parametros);
  fgets(aux, 254, arq_parametros);
  fgets(aux, 254, arq_parametros);
  fgets(aux, 254, arq_parametros);
  fgets(aux, 254, arq_parametros);
  fgets(aux, 254, arq_parametros);
  
  fscanf(arq_parametros,"fator_dist_mod_max = %f\n",&fator_dist_mod_max);
  
  dist_mod_max = fator_dist_mod_max * erro_pixel;
  
  const_cord_x = (SIZEX-1.0)/2.0;
  const_cord_y = (SIZEY-1.0)/2.0;
  
  camara_l[0] = 1.0;
  camara_l[1] = -baseline/2.0;
  camara_l[2] = 0.0;
  camara_l[3] = 0.0;
  
  camara_r[0] = 1.0;
  camara_r[1] = baseline/2.0;
  camara_r[2] = 0.0;
  camara_r[3] = 0.0;
  
/* --
  printf("camaL= %f %f %f %f\n",camara_l[0],camara_l[1],camara_l[2],camara_l[3]);
  
  printf("camaR= %f %f %f %f\n",camara_r[0],camara_r[1],camara_r[2],camara_r[3]);  
  
--- */  
  
/* ---
  fprintf(file_log,"\n\n   Casamento do modelo com dados obtidos.\n");

  fprintf(file_log,"\n\n       Dados.\n");
  fprintf(file_log,"\n Arquivo de vertices de entrada = %s",entrada_vert);
    
  fprintf(file_log,"\n Arquivo saida(v-mod-mapeados)  = %s\n",saida_mod);

----*/

  nvertice_dados = 0;

  while((fgets(aux, 254, entrada)) != NULL)
    nvertice_dados++;

/* --  fprintf(stderr,"nvertice_dados = %d\n",nvertice_dados);
--- */
    

/*  --- Leitura dos dados sobre o modelo ------------- */

  carac = getc(file_modelo);
  while(carac == '#')
    {
      fgets(aux, 254,file_modelo);
	  carac = getc(file_modelo);
    }
  
  ungetc(carac, file_modelo);
  
  fscanf(file_modelo,"nvertices  = %d\n",&nvertice_mod);
  fscanf(file_modelo,"nfaces = %d\n",&nfaces);
  fscanf(file_modelo,"nligacoes = %d\n",&nligacoes);
  
/* ---  
  fprintf(stderr,"nvertice_mod = %d\n",nvertice_mod);
  fprintf(stderr,"nfaces = %d\n",nfaces);
  fprintf(stderr,"nligacoes = %d\n",nligacoes);  
---- */


  mv = (float **)malloc(nvertice_mod * sizeof(float *));
  for(i=0; i<nvertice_mod; i++)
    mv[i] = (float *)malloc(3 * sizeof(float));
    
  mat_ind_face = (int **)malloc(nvertice_mod * sizeof(int *));
  for(i=0; i<nvertice_mod; i++)
    mat_ind_face[i] = (int *)malloc(NUM_FACES * sizeof(int));
    
  mat_faces = (float **)malloc(nfaces * sizeof(float *));
  for(i=0; i<nfaces; i++)
    mat_faces[i] = (float *)malloc(4 * sizeof(float));

  mat_lig = (int **)malloc(nligacoes * sizeof(int *));
  for(i=0; i<nligacoes; i++)
    mat_lig[i] = (int *)malloc(2 * sizeof(int));


  mv_homog = (float **)malloc(nvertice_mod * sizeof(float *));
  for(i=0; i<nvertice_mod; i++)
    mv_homog[i] = (float *)malloc(4 * sizeof(float));

  mv_map_hom = (float **)malloc(nvertice_mod * sizeof(float *));
  for(i=0; i<nvertice_mod; i++)
    mv_map_hom[i] = (float *)malloc(4 * sizeof(float));

  mv_map_iso = (float **)malloc(nvertice_mod * sizeof(float *));
  for(i=0; i<nvertice_mod; i++)
    mv_map_iso[i] = (float *)malloc(3 * sizeof(float));


  mu_3d = (float **)malloc(nvertice_dados * sizeof(float *));
  for(i=0; i<nvertice_dados; i++)
    mu_3d[i] = (float *)malloc(3 * sizeof(float));


  mu_iso = (float **)malloc(nvertice_dados * sizeof(float *));
  for(i=0; i<nvertice_dados; i++)
	mu_iso[i] = (float *)malloc(3 * sizeof(float));

/* ---Leitura dos vertices dos dados ---*/

  rewind(entrada);

  for(i=0; i<nvertice_dados; i++)
    fscanf(entrada,"%f %f %f\n",&mu_iso[i][0],&mu_iso[i][1],&mu_iso[i][2]);

 /* transforma vertices isotropicos em 3d */
  for(i=0; i<nvertice_dados; i++)
    conv_iso_3d(mu_iso[i][0], mu_iso[i][1], mu_iso[i][2], &mu_3d[i][0], &mu_3d[i][1], &mu_3d[i][2]); 


/*  --- Leitura dos vertices do modelo e indices das faces --- */

  carac = getc(file_modelo);
  while(carac == '#')
    {
      fgets(aux, 254,file_modelo);
      carac = getc(file_modelo);
    }
  
  ungetc(carac, file_modelo);
  
  for(i=0; i<nvertice_mod; i++)
	fscanf(file_modelo,"%f %f %f %f %d %d %d\n",&mv_homog[i][0],&mv_homog[i][1],&mv_homog[i][2],&mv_homog[i][3],&mat_ind_face[i][0],&mat_ind_face[i][1],&mat_ind_face[i][2]);

/* vertices do modelo em coordenadas cartezianas :    */   

  for(i=0; i<nvertice_mod; i++)
    {
      mv[i][0] = mv_homog[i][1];
      mv[i][1] = mv_homog[i][2];
      mv[i][2] = mv_homog[i][3];
    }
    
  /* leitura dos coeficientes das faces */
  
  carac = getc(file_modelo);
  while(carac == '#')
    {
      fgets(aux, 254,file_modelo);
      carac = getc(file_modelo);
    }
  
  ungetc(carac, file_modelo);
  
  for(i=0; i<nfaces; i++)
    fscanf(file_modelo,"%f %f %f %f\n",&mat_faces[i][0],&mat_faces[i][1],&mat_faces[i][2],&mat_faces[i][3]);

/* leitura dos coeficientes das ligacoes */

  carac = getc(file_modelo);
  while(carac == '#')
    {
      fgets(aux, 254,file_modelo);
      carac = getc(file_modelo);
    }
  
  ungetc(carac, file_modelo);
  
  for(i=0; i<nligacoes; i++)
    fscanf(file_modelo,"%d %d",&mat_lig[i][0],&mat_lig[i][1]);

    ver1(mv_homog,nvertice_mod,4);
    ver_int(mat_lig,nligacoes,2);
    ver1(mat_faces,nfaces,4);
    ver_int(mat_ind_face,nvertice_mod,3);

    
/* ---   
    fprintf(file_log,"  Matriz mu_iso dos dados\n");
    ver1(mu_iso,nvertice_dados,3);

    fprintf(file_log,"  Matriz mu_3d dos dados\n");    
    ver1(mu_3d,nvertice_dados,3);
    
    fprintf(file_log,"  Matriz mv dos vertices\n");
    ver1(mv,nvertice_mod,3);
    
    fprintf(file_log,"  Matriz mv dos vertices homogeneos\n");
    ver1(mv_homog,nvertice_mod,4);
---- */ 

  
  fclose(arq_parametros);
  fclose(file_modelo);
  fclose(entrada);
  
  /* imprimir parametros lidos do stdout */
  fprintf(stderr,"\n\n     TESTE  cod. ( %s )\n",saida_aux); 
  fprintf(stdout,"     TESTE  cod. ( %s )\n",saida_aux);
  fprintf(stdout,"\n   Casamento do modelo com dados obtidos.\n");
  fprintf(stdout,"\n       Dados.\n");
  fprintf(stdout,"\n Arquivo de vertices de entrada = %s",entrada_vert);
  fprintf(stdout,"\n Arquivo saida(v-mod-mapeados)  = %s",saida_mod);
  fprintf(stdout,"\n Arquivo Modelo = %s",modelo);
  fprintf(stdout,"\n fator_dist_mod_max = %f",fator_dist_mod_max);
  fprintf(stdout,"\n dist_mod_max = %f",dist_mod_max);
  fprintf(stdout,"\n erro_pixel = %f\n",erro_pixel);
  
  start = now();
  calculos();
  stop = now();
  
  fprintf(stdout,"\n Numero de casamentos = %d\n",num_saida);  
  fprintf(stdout,"\n CASA-MOD: Total time (ms) =  %.0f\n",(stop -start)/1000.0);

  escreve_saida();
  
  fclose(saida);
  fclose(file_log);
}