#include <stdio.h>
#include <stdlib.h>
#include <math.h>
/*  #include <alloc.h>
    #include <conio.h>
*/    
#include <string.h>

#define EXTREMO 0
#define EXTRAPOLADO 1

#define NULL 0
#define OK 1
#define TRUE 1
#define FALSE 0
#define ERR -1
#define NIL -1

FILE *entrada, *saida;
float erro_pixel, foco,
      fator_raio_max, cte_dist_retas;

int nptos_ext; /* numero de ptos extremo + extrapolados */


float escalp(float *a, float *b),
	  crossp(float *a, float *b, float *o),
	  normalize(float *a),
	  dist(float *p1, float *p2),
	  dist_retas(float *verts1, float *verts2, float *vertr1, float *vertr2,
				 float * pto_reta_s, float *pto_reta_r);


void leitura(void), uniao_seg(void), verifica(void), verific_real(void);


/*unsigned char ins_vert(struct vertice **ini_lista, struct vertice **fim_lista);*/

/* --------------------- estruturas de tipos ------------- */
/* Estrutura de pontos casados no espaco */
struct vertice{
	float    x,            /* coordenadas dos vertices */
		 y,
		 z;

	unsigned char   tipo;

	struct vertice *prev,
		   *next;
		 };

struct vertice *ini_vert = NULL,  /* inicio e fim da lista  */
			   *fim_vert = NULL;    /* de vertices */

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

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

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

}



float escalp(a, b)  /* produto escalar de dois vetores  a.b
									Retorna o produto   */
float *a, *b;
{
   float prod;

   prod = a[0]*b[0] + a[1]*b[1] + a[2]*b[2];

   return(prod);
}

float crossp(a, b, o) /* produto vetorial o = a x b  de vetores R3
				 Retorna modulo do vetor resultante */
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; */

	return(d);
}

float normalize(a) /* calcula o vetor unitario em R3
									e retorna o modulo do vetor  */
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);
}


/* dist_retas() - calcula a distancia minima entre duas retas.
							 retorna a distancia calculada */
float dist_retas(verts1, verts2, vertr1, vertr2, pto_reta_s, pto_reta_r)
float *verts1, *verts2, *vertr1, *vertr2;
float *pto_reta_r, *pto_reta_s; /* ptos retornados em r e s onde a distancia
                                   e minima entre as retas */  
{
  float  r[3],      /* vetor da reta r (vertr2 - vertr1) */
			s[3],     /* vetor da reta s */
			vetor_rxs[3];  /* produto vetorial de v x w */
  float vet_rs[3],   /* vetor incluindo um ponto de r e s */
	vet_dist[3];
  float mod_rxs, tr, ts, dist_reta;

  r[0] = vertr2[0]-vertr1[0];
  r[1] = vertr2[1]-vertr1[1];
  r[2] = vertr2[2]-vertr1[2];

  s[0] = verts2[0]-verts1[0];
  s[1] = verts2[1]-verts1[1];
  s[2] = verts2[2]-verts1[2];

  vet_rs[0] = verts1[0]-vertr2[0];
  vet_rs[1] = verts1[1]-vertr2[1];
  vet_rs[2] = verts1[2]-vertr2[2];

/*  printf("vet_rs = (%.2f , %.2f , %.2f ) \n",vet_rs[0],vet_rs[1],vet_rs[2]);
*/

  mod_rxs = crossp(r, s, vetor_rxs);

/*  printf("vetor_rxs = (%.2f , %.2f , %.2f ) \n",vetor_rxs[0],vetor_rxs[1],vetor_rxs[2]);
  printf("mod = %f\n",mod_rxs);
*/

  if(mod_rxs ==0)
	{
	  fprintf(stderr,"\n    Os dois segmentos sao paralelos.");
	  return(-1.0);
	}

  dist_reta = fabs(escalp(vetor_rxs, vet_rs)) / mod_rxs;

  vet_dist[0] = dist_reta* vetor_rxs[0]/mod_rxs;
  vet_dist[1] = dist_reta* vetor_rxs[1]/mod_rxs;
  vet_dist[2] = dist_reta* vetor_rxs[2]/mod_rxs;

  tr = (s[1]*(vet_dist[0]+vertr1[0]-verts1[0]) - s[0]*(vet_dist[1]+vertr1[1]-verts1[1]))/
	 (r[1]*s[0] - r[0]*s[1]);

  ts = (vet_dist[0] + vertr1[0] + r[0]*tr - verts1[0])/s[0];

  pto_reta_r[0] = r[0]*tr + vertr1[0];
  pto_reta_r[1] = r[1]*tr + vertr1[1];
  pto_reta_r[2] = r[2]*tr + vertr1[2];

  pto_reta_s[0] = s[0]*ts + verts1[0];
  pto_reta_s[1] = s[1]*ts + verts1[1];
  pto_reta_s[2] = s[2]*ts + verts1[2];


  return(dist_reta);
}

void leitura() /* leitura dos extremos dos segmentos */
{
  int nptos;  /* numero de extremos lidos */
  int i;
  char s[255];
  int cod;


  nptos = 0;

  while((fgets(s, 254, entrada)) != NULL)
	nptos++;

  nptos_ext = nptos;

  fprintf(stderr,"nptos = %d",nptos);

  rewind(entrada);

  for(i=0; i<nptos; i++)
	{
	  cod = ins_vert(&ini_vert, &fim_vert);
	  if(cod ==ERR)
		{
		  fprintf(stderr,"ERRO: Nao conseguiu inserir vertice\n");
		  exit(-1);
        }

	  fscanf(entrada,"%f %f %f\n",&fim_vert->x,&fim_vert->y,&fim_vert->z);
	  fim_vert->tipo = EXTREMO;
	}
}


void uniao_seg()
{

  float dist_ret, dist_pto_s1, dist_pto_s2, dist_pto_r1, dist_pto_r2,
        mod_s, mod_r;
  float vet_r1[3], vet_r2[3], vet_s1[3], vet_s2[3], pto_r[3], pto_s[3];
  unsigned char flag_s = FALSE,
                flag_r = FALSE;

  struct vertice *ptr1, *ptr2;

  ptr1 = ini_vert;

  while(ptr1 && (ptr1->tipo!= EXTRAPOLADO))
    {
      vet_s1[0] = ptr1->x;   /* coloca o vertice1 em vetor */
      vet_s1[1] = ptr1->y;
      vet_s1[2] = ptr1->z;
      
      ptr1 = ptr1->next;
      
      vet_s2[0] = ptr1->x;   /* coloca o vertice2 em vetor */
      vet_s2[1] = ptr1->y;
      vet_s2[2] = ptr1->z;
      
      mod_s = dist(vet_s1, vet_s2);
      
      ptr2 = ptr1->next;
      while(ptr2 && (ptr2->tipo != EXTRAPOLADO))
	{
	  vet_r1[0] = ptr2->x;   /* coloca o vertice3 em vetor */
	  vet_r1[1] = ptr2->y;
	  vet_r1[2] = ptr2->z;
	  
	  ptr2 = ptr2->next;
	  
	  vet_r2[0] = ptr2->x;   /* coloca o vertice4 em vetor */
	  vet_r2[1] = ptr2->y;
	  vet_r2[2] = ptr2->z;
	  
	  dist_ret = dist_retas(vet_s1, vet_s2, vet_r1, vet_r2, pto_s, pto_r);
	  /*	  printf("pto_s = %f, %f, %f\n",pto_s[0],pto_s[1],pto_s[2]);
		  printf("pto_r = %f, %f, %f\n",pto_r[0],pto_r[1],pto_r[2]);
	  */
/*---	  getch();
	  printf("\nd_ret = %.3f ",dist_ret);
  --*/	  
	  if(dist_ret > cte_dist_retas)
	    {    /* analisa proximo segmento  */
	      ptr2 = ptr2->next;
	      continue;
	    }
	  
	  if(dist_ret == -1.0)  /* retas paralelas  */
	    {    /* analisa proximo segmento  */
	      ptr2 = ptr2->next;
	      continue;
	    }
/*  -- calcula distancia entre extremo e extrapolado e compara com maximo -*/
	  flag_s = FALSE;
	  flag_r = FALSE;
	  
	  dist_pto_s1 = dist(vet_s1, pto_s);
	  dist_pto_s2 = dist(vet_s2, pto_s);
/* --
     printf("  dt_s1 = %8.3f  dt_s2 = %8.3f  mod_s =%8.3f",dist_pto_s1,
 			 dist_pto_s2,mod_s);
   --*/			 
	
	  if((dist_pto_s1 > (mod_s/2.0))&&(dist_pto_s2 > (mod_s/2.0)))
	    flag_s = TRUE;
	  
	  mod_r = dist(vet_r1, vet_r2);
	  
	  dist_pto_r1 = dist(vet_r1, pto_r);
	  dist_pto_r2 = dist(vet_r2, pto_r);

/* --
    printf("\n                dt_r1 = %8.3f  dt_r2 = %8.3f  mod_r =%8.3f",dist_pto_r1, dist_pto_r2,mod_r);
---*/
	  if((dist_pto_r1 > (mod_r/2.0))&&(dist_pto_r2 > (mod_r/2.0)))
	    flag_r = TRUE;
	  
	  if(flag_r||flag_s )
	    {   /* analisa proximo segmento  */
	      ptr2 = ptr2->next;
	      continue;
	    }

/*  ---- cria  novo vertice (extrapolado) -----*/

	  ins_vert(&ini_vert, &fim_vert);

	  fim_vert->x = (pto_s[0] + pto_r[0])/2.0;
	  fim_vert->y = (pto_s[1] + pto_r[1])/2.0;
	  fim_vert->z = (pto_s[2] + pto_r[2])/2.0;
	  
	  fim_vert->tipo = EXTRAPOLADO;
	  
	  nptos_ext++; 
	  
	  ptr2 = ptr2->next;
	}
      ptr1 = ptr1->next;
    }
}





void verific_real()  /* verifica os ptos distantes de um certo valor e os
	       transforma em extremos de arestas de um grafo */
{
 int i, j, x, y;
 int nptos_comp_extremo, nptos_comp_extrap;
 float d, raio_max,
       vertx_extrap, verty_extrap, vertz_extrap, k,
       vertx_extremo, verty_extremo, vertz_extremo;
 int *mat;

 float pto1[3], pto2[3]; /* vetores auxiliares p/ calculo da distancia */
 struct vertice **vetor_ptr_vert;  /* vetor utilizado no processamento do grafo */
 
 vetor_ptr_vert = (struct vertice **)malloc(nptos_ext * sizeof(struct vertice*));
 mat = (int *)malloc(nptos_ext * sizeof(int));
 
 
/* -- fprintf(stderr,"   EXTRAPOLADO  "); --*/

 raio_max = fator_raio_max*erro_pixel;
 
 for(i=0; i<nptos_ext; i++)
   mat[i] = NIL;

 vetor_ptr_vert[0] = ini_vert;
 for(i=1; i<nptos_ext; i++)
   {
     vetor_ptr_vert[i] = vetor_ptr_vert[i-1]->next;
   }

 for(i=0; i<nptos_ext; i++)
   {
     pto1[0] = vetor_ptr_vert[i]->x;
     pto1[1] = vetor_ptr_vert[i]->y;
     pto1[2] = vetor_ptr_vert[i]->z;
     
     for(j=i; j<nptos_ext; j++)
       {
	 pto2[0] = vetor_ptr_vert[j]->x;
	 pto2[1] = vetor_ptr_vert[j]->y;
	 pto2[2] = vetor_ptr_vert[j]->z;
	 
	 d = dist(pto1, pto2);
	 
	 if(d<raio_max)
	   {
	     x = i;
	     y = j;
	     
	     while(mat[x] != NIL)
	       x = mat[x];
	     
	     while(mat[y] != NIL)
	       y = mat[y];
	     
	     if(x != y)
	       mat[x] = y;
	   }
       }
   }
 
 
 /* Impressao  */
 fprintf(stderr,"\nComponentes\n");
 for(i=0; i<nptos_ext; i++)
   {
     nptos_comp_extremo = 0;
     nptos_comp_extrap = 0;
     k = 0.0;
     vertx_extrap = 0.0;
     verty_extrap = 0.0;
     vertz_extrap = 0.0;
     if( mat[i] == NIL)
       {
	 for(j=0; j<nptos_ext; j++)
	   {
	     
	     x = j;
	     while(mat[x] != NIL)
	       x = mat[x];
	     if(x == i && vetor_ptr_vert[j]->tipo == EXTRAPOLADO)
	       {
		 k = k + 1.0;
		 vertx_extrap += vetor_ptr_vert[j]->x;
		 verty_extrap += vetor_ptr_vert[j]->y;
		 vertz_extrap += vetor_ptr_vert[j]->z;
		 
		 nptos_comp_extrap++;
		 fprintf(stderr,"[%d] ",j);
	       }
	     if(x == i && vetor_ptr_vert[j]->tipo == EXTREMO)
	       {
		 nptos_comp_extremo++;
		 vertx_extremo = vetor_ptr_vert[j]->x;
		 verty_extremo = vetor_ptr_vert[j]->y;
		 vertz_extremo = vetor_ptr_vert[j]->z;
	       } 
	   }
	 
	 if(nptos_comp_extrap != 0)
	   {
	     vertx_extrap /= k;
	     verty_extrap /= k;
	     vertz_extrap /= k;
	     /*  fprintf(stderr,"%f %f %f\n",vertx_extrap,verty_extrap,
		 vertz_extrap);*/
	     fprintf(saida,"%f %f %f\n",vertx_extrap,verty_extrap,
		     vertz_extrap);
	   }
	 else
	   {
	     if(nptos_comp_extremo > 1)
	       continue;
	     else
	       fprintf(saida,"%f %f %f\n",vertx_extremo,verty_extremo,vertz_extremo);
	     /*   fprintf(stderr,"EXTREMO: %f %f %f\n",vertx_extremo,verty_extremo,vertz_extremo);*/
	     
	   }
	 
       }
   }
}



main(argc, argv)
int argc;
char *argv[];
{
  char parametros[50], saida_aux[50],saida_log[50], saida_vert[50], aux[100];
  char  entrada_iso_segs[50];
  
  float fator_dist_reta;

  double start, stop; /* para medida de tempo de execucao */
  FILE *arq_parametros;
  struct vertice *ptr;
/*  char cwd[100];  para imprimir diretorio corrente */

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

  strcpy(saida_aux, argv[1]);
  strcat(saida_aux, "-");
  strcat(saida_aux, argv[2]);
  
  strcpy(entrada_iso_segs, saida_aux);
  strcat(entrada_iso_segs, "_iso.segs");
  
/*DOS  strcpy(entrada_iso_segs, "\\usr\\marcelus\\tese\\progs\\iso.seg");*/

  strcpy(saida_vert, saida_aux);
  strcat(saida_vert, "_iso.vert");
  saida = fopen(saida_vert, "w");

/*  fprintf (stderr, "O diretorio corrente e %s\n", getcwd(cwd, 100));*/
	  
  if((entrada = fopen(entrada_iso_segs, "r"))== NULL)
	{
    /*      perror("BLABLABLA");*/
	  fprintf(stderr,"O arquivo de entrada [ %s ] nao pode ser aberto.\n",
			  entrada_iso_segs);
          exit(-1);
	}

/*  file_log = fopen(saida_log, "w");*/
  arq_parametros = fopen(parametros, "r");
  
  fgets(aux, 98, arq_parametros); /* leitura do cabecalho   */
  fscanf(arq_parametros,"erro_pixel = %f\n",&erro_pixel);
  fgets(aux, 98, arq_parametros); /* leitura da linha n_reta = */
  fscanf(arq_parametros,"foco = %f\n",&foco); 
  
  fprinff(stderr,"\nOBS: fator p/ unir vertices deve ser >= dist entre retas\n");
  fprintf(stderr,"\nfator_raio_max(p/unir vertices(2-7)) = "); /* en torno de 2 ou 7 */
  scanf("%f",&fator_raio_max);
  fprintf(stdout,"\n fator_raio_maximo = %f",fator_raio_max);
  
  fprintf(stderr,"\nfator_dist_reta(dist entre retas (4))= ");  /* usado =4.0 */
  scanf("%f",&fator_dist_reta);
  fprintf(stdout,"\n fator_dist_reta = %f\n",fator_dist_reta);
  
  
  cte_dist_retas = fator_dist_reta*erro_pixel;
  

  fprintf(stderr,"\n\n     Uniao em vertices.\n");

  fprintf(stderr,"\n\n       Dados.\n");
  fprintf(stderr,"\n Arquivo de vertices de entrada = %s",entrada_iso_segs);
  fprintf(stderr,"\n Arquivo saida dos vertices unidos = %s",saida_vert);
  fprintf(stderr,"\n erro posicional erro_pixel = %f",erro_pixel);

  
  leitura();
  
  uniao_seg();
  
  verific_real();

/*  printf("\n  Impressao dos vertices ext+ extrap \n");

	ptr = ini_vert;
	while(ptr)
	  {
	printf("%f %f %f",ptr->x,ptr->y,ptr->z);
	printf("    tipo = %d \n\n",ptr->tipo);
	ptr = ptr->next;
	  }
*/
return(0);
}


/* antigo -- nao utilizado atualmente */
void verifica()  /* verifica os ptos distantes de um certo valor e os
	       transforma em extremos de arestas de um grafo */
{
 int i, j, x, y;
 int kte;
 float d, raio_max, vertx, verty, vertz, k;
 int *mat;

 float pto1[3], pto2[3]; /* vetores auxiliares p/ calculo da distancia */
 struct vertice **vetor_ptr_vert;  /* vetor utilizado no processamento do grafo
                                     vetor de apontadores para a estrutura */
 
 vetor_ptr_vert = (struct vertice **)malloc(nptos_ext * sizeof(struct vertice*));
 mat = (int *)malloc(nptos_ext * sizeof(int));
 
 
 /* raio_max = 7 * erro_pixel;*/
 /* raio_max = 2*erro_pixel; */
 printf("kte = ");
 scanf("%d",&kte);
 raio_max = kte*erro_pixel;
 
 for(i=0; i<nptos_ext; i++)
   mat[i] = NIL;

 vetor_ptr_vert[0] = ini_vert;
 for(i=1; i<nptos_ext; i++)
   {
     vetor_ptr_vert[i] = vetor_ptr_vert[i-1]->next;
   }

 for(i=0; i<nptos_ext; i++)
   {
     pto1[0] = vetor_ptr_vert[i]->x;
     pto1[1] = vetor_ptr_vert[i]->y;
     pto1[2] = vetor_ptr_vert[i]->z;
     
     for(j=i; j<nptos_ext; j++)
       {
	 pto2[0] = vetor_ptr_vert[j]->x;
	 pto2[1] = vetor_ptr_vert[j]->y;
	 pto2[2] = vetor_ptr_vert[j]->z;
	 
	 d = dist(pto1, pto2);
	 
	 if(d<raio_max)
	   {
	     x = i;
	     y = j;
	     
	     while(mat[x] != NIL)
	       x = mat[x];
	     
	     while(mat[y] != NIL)
	       y = mat[y];
	     
	     if(x != y)
	       mat[x] = y;
	   }
       }
   }
 
 
 /* Impressao  */
 fprintf(stderr,"\n Componentes\n");
 for(i=0; i<nptos_ext; i++)
   {
     k = 0.0;
     vertx = 0.0;
     verty = 0.0;
     vertz = 0.0;
     if( mat[i] == NIL)
       {
	 for(j=0; j<nptos_ext; j++)
	   {
	     x = j;
	     while(mat[x] != NIL)
	       x = mat[x];
	     if(x == i)
	       {
		 k = k + 1.0;
		 vertx += vetor_ptr_vert[j]->x;
		 verty += vetor_ptr_vert[j]->y;
		 vertz += vetor_ptr_vert[j]->z;
		 fprintf(stderr,"[%d] ",j);
	       }
	   }
	 vertx /= k;
	 verty /= k;
	 vertz /= k;
/*	 fprintf(saida,"%f %f %f\n",vertx,verty,vertz);*/
	 printf("%f %f %f\n",vertx,verty,vertz);
       }
   }
}