/* este programa vai pedindo pontos no espaco e para cada pto, apartir
   de 2 ptos, ele calcula o baricentro ... (metrica dos candidatos) */
   
#include <stdio.h>
#include <math.h>

#define N_PTOS 200  /* num de ptos maximo da nuvem */
#define DIM 3  /* ptos com DIM dimensoes o que implica em matrizes
		  de nptos por DIM */

float erro_pixel; /* erro devido resolucao */

/* baricentro - calcula o baricentro dos ptos e os deslocam
                em relacao ao baricentro.  */
void baricentro(m_3d, centro, nptos)
float **m_3d;  /* matriz(n x 3) de entrada e saida dos pontos; n=nptos */
int nptos;     /* num. de linhas da matriz de entrada */
float *centro; /* retorna um vetor[3] dos ptos do centro */
{
 int i, j;

 for(j=0; j<DIM; j++)
   {
     centro[j] = 0.0;
   }
 
 for(i=0; i<nptos; i++)
   {
     for(j=0; j<DIM; j++)
       {
	 centro[j] += (m_3d[i][j] / nptos);
       }
   }

 for(i=0; i<nptos; i++)
   {
     for(j=0; j<DIM; j++)
       {
	 m_3d[i][j] = m_3d[i][j] - centro[j];
       }
   }
} 


/* mult_t - faz a multiplicacao da transposta por sua matriz(n x 3) */
void mult_t(m_3d, m_3x3, nptos)
float **m_3d;   /* matriz de entrada (n x 3) */
float **m_3x3;  /* retorna matriz (3 x 3) saida da multiplicacao */
int nptos;      /* num. de linhas da matriz de entrada */
{
 int i, j, k;

 for(i=0; i<DIM; i++)
   for(j=0; j<DIM; j++)
     m_3x3[i][j] = 0.0;

 for(k=0; k<DIM; k++)
   {
     for(j=0; j<DIM; j++)
       {
	 for(i=0; i<nptos; i++)
	   {
	     m_3x3[k][j] += (m_3d[i][k] * m_3d[i][j]);
  	   }
       }
   }
}


#define NMAX 3  /* para o caso de matrizes 3x3 */
/* auto_jacob - calcula os autovetores e autovalores de uma matriz
                simetrica pelo metodo Jacobiano */
int auto_jacob(a,n,d,v,nrot)
float **a;  /* matriz simetrica de entrada */
int n;      /* tamanho da matriz de entrada */
float *d;   /* retorna vetor com os n autovalores de a */
float **v;  /* retorna matriz que possui nas colunas os autovetores
	       normalizados de a */
int *nrot;  /* retorna o numero de rotacoes de Jacobi requeridas */
{
 int j, iq, ip, i;
 float tresh, theta, tau, t, sm, s, h, g, c;
 float b[NMAX], z[NMAX];

 for(ip=0; ip<n; ip++)
   {
     for(iq=0; iq<n; iq++)
       {
	 v[ip][iq] = 0.0;
       }
     v[ip][ip] = 1.0;
   }
 
 for(ip=0; ip<n; ip++)
   {
     b[ip] = a[ip][ip];
     d[ip] = b[ip];
     z[ip] = 0.0;
   }

 *nrot = 0;

 for(i=1; i<=50; i++)
   {
     sm = 0.0;
     for(ip =0; ip<(n-1); ip++)
       {
	 for(iq=ip+1; iq<n; iq++)
	   {
	     sm += fabs(a[ip][iq]);
	   }
       }

     if(sm == 0.0) return(0);
     if(i < 4)
       tresh = 0.2*sm/(n*n);
     else
       tresh = 0.0;

     for(ip = 0; ip<(n-1); ip++)
       {
	 for(iq = ip+1; iq<n; iq++)
	   {
	     g = 100.0*fabs(a[ip][iq]);
	     if((i>4)&&((fabs(d[ip])+g) == fabs(d[ip]))&&
		       ((fabs(d[iq]+g) == fabs(d[iq]))))
		   a[ip][iq] = 0.0;
	      else
		{
		  if(fabs(a[ip][iq])>tresh)
		    {
		      h = d[iq] - d[ip];
		      if((fabs(h)+g) == fabs(h))
			t = a[ip][iq]/h;
		      else
			{
			  theta = 0.5*h/a[ip][iq];
			  t = 1.0/(fabs(theta)+sqrt(1.0+theta*theta));
			  if(theta < 0.0)
			    t = -t;
		        }

		      c = 1.0/sqrt(1.0+t*t);
		      s = t*c;
		      tau = s/(1.0+c);
		      h = t*a[ip][iq];
		      z[ip] -= h;
		      z[iq] += h;
		      d[ip] -= h;
		      d[iq] += h;
		      a[ip][iq] = 0.0;

		      for(j=0; j<=(ip-1); j++)
			{
			  g = a[j][ip];
			  h = a[j][iq];
			  a[j][ip] = g-s*(h+g*tau);
			  a[j][iq] = h+s*(g-h*tau);
		        }

		      for(j=ip+1; j<=(iq-1); j++)
			{
			  g = a[ip][j];
			  h = a[j][iq];
			  a[ip][j] = g-s*(h+g*tau);
			  a[j][iq] = h+s*(g-h*tau);
		        }


		      for(j=iq+1; j<n; j++)
			{
			  g = a[ip][j];
			  h = a[iq][j];
			  a[ip][j] = g-s*(h+g*tau);
			  a[iq][j] = h+s*(g-h*tau);
		        }

		      for(j=0; j<n; j++)
			{
			  g = v[j][ip];
			  h = v[j][iq];
			  v[j][ip] = g-s*(h+g*tau);
			  v[j][iq] = h+s*(g-h*tau);
		        }

		      (*nrot)++;

		    }
	        }
	    
	   }
       }
   }

 for(ip=0; ip<n; ip++)
   {
     b[ip] += z[ip];
     d[ip] = b[ip];
     z[ip] = 0.0;
   }

 fputs(" 50 interacoes nao devem ocorrer \n",stderr);
 return(0);
}
    

/* vet_sort - ordena os auto_vetores na ordem descendente com
              relacao aos respectivos autovalores. */
void vet_sort(d, v, n)
float *d;   /* vetor de entrada dos n autovalores da matriz, e saida
	       dos autovalores ordenados*/
float **v;  /* matriz de entrada que possui nas colunas os autovetores
	       normalizados de a, e saida ordenada dos autovetores */
int n;      /* tamanho da matriz de entrada. */
{
 int k, i, j;
 float p;

 for(i=0; i<(n-1); i++)
   {
     k = i;
     p = d[i];

     for(j=i+1; j<n; j++)
       {
	 if(d[j]>=p)
	   {
	     k = j;
	     p = d[j];
	   }
       }

     if(k!=i)
       {
	 d[k] = d[i];
	 d[i] = p;
	 for(j=0; j<n; j++)
	   {
	     p = v[j][i];
	     v[j][i] = v[j][k];
	     v[j][k] = p;
	   }
       }
   }
}
	     

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

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

 dist = sqrt(dist);

 return(dist);
}
/* funcao para calculo da condicao do hiperboloide */
void cond_hiper(int j, int i, float **mpt, float *baric, float **autovetor,float *autovalor, float erro_pixel, float *condicao, float *dist, float *comp_d0)
{
    float x0, x1, x2, d0, d1, d2;

    x0= ((mpt[i][0] - baric[0])*autovetor[0][0] +
	 (mpt[i][1] - baric[1])*autovetor[1][0] +
	 (mpt[i][2] - baric[2])*autovetor[2][0]);
    d0= (autovalor[0]/(j-1) + erro_pixel * erro_pixel);
    x1= ((mpt[i][0] - baric[0])*autovetor[0][1] +
	 (mpt[i][1] - baric[1])*autovetor[1][1] +
	 (mpt[i][2] - baric[2])*autovetor[2][1]);
    d1= (autovalor[1]/(j-1) + erro_pixel * erro_pixel);
    x2= ((mpt[i][0] - baric[0])*autovetor[0][2] +
	 (mpt[i][1] - baric[1])*autovetor[1][2] +
	 (mpt[i][2] - baric[2])*autovetor[2][2]);
    d2= (autovalor[2]/(j-1) + erro_pixel * erro_pixel);

    *condicao = 1.0 + x0*x0/d0 - x1*x1/d1 - x2*x2/d2;
    *dist = (x0*x0+erro_pixel*erro_pixel)/d0 + (x1*x1+erro_pixel*erro_pixel)/d1 + (x2*x2+erro_pixel*erro_pixel)/d2;
    *comp_d0 = d0;
}



main()
{
 float *m3x3[3], *autovetor[3], **ma2, **mpt;
 float autovalor[3], baric[3];
 float var_para, /* variancia paralela a direcao de maior autovalor */
       var_per,  /* variancia perpendicular */
       val_reta; /* valor da reta  */
 float condicao, dist, comp_d0, cte_max;
 int i, j, nrot; 
 int ii, jj, k;
 
 mpt = (float **)malloc(N_PTOS * sizeof(float *));
 for(i=0; i<N_PTOS; i++)
   mpt[i] = (float *)malloc(3 * sizeof(float));
 
 ma2 = (float **)malloc(N_PTOS * sizeof(float *));
 for(i=0; i<N_PTOS; i++)
   ma2[i] = (float *)malloc(3 * sizeof(float));
 
 

 m3x3[0] = (float *)malloc(3*sizeof(float));
 m3x3[1] = (float *)malloc(3*sizeof(float));
 m3x3[2] = (float *)malloc(3*sizeof(float));
 
 autovetor[0] = (float *)malloc(3*sizeof(float));
 autovetor[1] = (float *)malloc(3*sizeof(float));
 autovetor[2] = (float *)malloc(3*sizeof(float));
 
 printf("Entre com o erro-pixel = ");
 scanf("%f",&erro_pixel);
 printf("\nEntre com o primeiro pto x y z\n");
 scanf("%f %f %f",&mpt[0][0],&mpt[0][1],&mpt[0][2]);
 printf("Entre com o proximo pto x y z\n");
 scanf("%f %f %f",&mpt[1][0],&mpt[1][1],&mpt[1][2]);
 
 j = 2; 
 
 for(i=0; i<j; i++)
   for(k=0; k<3; k++)
     ma2[i][k] = mpt[i][k];
 
 baricentro(ma2, baric, j);
 mult_t(ma2, m3x3, j);
 auto_jacob(m3x3, 3, autovalor, autovetor, &nrot);
 vet_sort(autovalor, autovetor, 3);
 
 var_para = autovalor[0]/(j-1) + erro_pixel*erro_pixel;
 var_per = (autovalor[1] + autovalor[2])/(j-1) + 2*erro_pixel*erro_pixel;

 val_reta = 2 * var_para / var_per;
 
 printf("var_para = %f  var_per = %f  qualid = %f\n",var_para,var_per,val_reta);
 
 printf("baric = %f  %f  %f\n",baric[0],baric[1],baric[2]);
 for(ii = 0; ii<3; ii++)
   {
     printf("autovalor[%d] = %f \n",ii,autovalor[ii]);
     printf("auto_vetor [%d] = ",ii);
     for(k=0; k<3; k++)
       {
	 printf(" %f  ",autovetor[k][ii]);
       }
     
     printf("\n\n");
   }
 
 
 for(jj = 2; jj<N_PTOS; jj++)
   {
     printf("\n    ----------------- \n");
     
     for(i=0; i<j; i++)
       for(k=0; k<3; k++)
	 ma2[i][k] = mpt[i][k];
     
     baricentro(ma2, baric, j);
     mult_t(ma2, m3x3, j);
     auto_jacob(m3x3, 3, autovalor, autovetor, &nrot);
     vet_sort(autovalor, autovetor, 3);
     
/*     printf("Entre com o [%d] pto x y z\n",(j+1));
     scanf("%f %f %f",&mpt[jj][0],&mpt[jj][1],&mpt[jj][2]);
*/
     mpt[jj][0] = 0.0;
     mpt[jj][1] = 0.0;
     mpt[jj][2] = mpt[jj-1][2] + 1.0;
     
     printf("pto [%d] : %.2f %.2f %.2f\n",(j+1),mpt[jj][0],mpt[jj][1],mpt[jj][2]);
     
    cond_hiper(j, jj, mpt, baric, autovetor, autovalor, erro_pixel, &condicao, &dist, &comp_d0);
     
     var_para = autovalor[0]/(j-1) + erro_pixel*erro_pixel;
     var_per = (autovalor[1] + autovalor[2])/(j-1) + 2.0*erro_pixel*erro_pixel;
     
     val_reta = 2.0 * var_para / var_per;
     
     dist = sqrt(dist);
     comp_d0 = sqrt(comp_d0);
     
     cte_max = (dist - 1.0)*comp_d0;
     
     printf("var_para  = %.2f   var_per = %.2f   qualid = %.2f   sqrt(d0) = %.2f\n",var_para,var_per,val_reta,comp_d0);
     
     printf("baric = %f  %f  %f\n",baric[0],baric[1],baric[2]);
     for(ii = 0; ii<3; ii++)
       {
	 printf("autovalor[%d] = %f \n",ii,autovalor[ii]);
	 printf("auto_vetor [%d] = ",ii);
	 for(k=0; k<3; k++)
	   {
	     printf(" %f  ",autovetor[k][ii]);
	   }
	 
	 printf("\n\n");
       }
     
     printf(" dist = %.2f   condicao = %.2f  cte_max = %.2f\n",dist,condicao,cte_max);
     j++;
     
   }
 
}