//MAC 5749 	Análise de Formas e Classificação
//EP4		Thin-plate splines para campos vetoriais
//Aluno 	Robson Ribeiro Correia

//Primeiro, a criação dos pontos de controle
X=[-2:0.25:2]';
Y=[-2:0.25:2]';
Z=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,4,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,5,0,0,0,2,0,0,0,3,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,4,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
   0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0];

[xx yy zz]=genfac3d(X,Y,Z);

xbasc();

plot3d(X,Y,Z,35,25,"Xk@Yk@Z",[2,2,4]);


//Agora, o cálculo de (A), (B) e (D), as matrizes T, S e H

//Procura os pontos de controle na matriz Z
n=1;			//n: quantidade de pontos de controle
for xi=1:4:17,
   for yi=1:4:17,
      printf("Z(%d,%d) = %d",xi, yi, Z(xi,yi));
      
      //Se o ponto for diferente de 0...
      if ~(Z(xi,yi) == 0) then
         //Translação dos pontos da matriz Z para o plano cartesiano
         Xi=((xi-1)/4)-2;
         Yi=((yi-1)/4)-2;

         //Armazena os pontos na matriz S
         S(n,1)=1;
         S(n,2)=Xi;
         S(n,3)=Yi;

         printf("S[%d %d %d]\n", S(n,1), S(n,2), S(n,3));

	 //Armazena a "altura" do ponto na matriz H
	 H(n)=Z(xi,yi);

	 printf("H[%d]\n", H(n));

         n=n+1;
      end,
   end;
end;

//Corrige o valor de n
n=n-1;

//Completa H com 0s
for i=n+1:n+3,
   H(i)=0;
end;

//Calcula a matriz T
for i=1:n,
   for j=1:n,
      
      printf("T(%d,%d)\n", i, j);
      
      if i == j then
         T(i,j)=0;
      else
         //Obtém os dois pontos de controle
	 Xi=S(i,2);
	 Yi=S(i,3);

	 Xj=S(j,2);
	 Yj=S(j,3);

	 //Calcula G(Ro)
	 g=G(Ro(Xi,Yi,Xj,Yj));
	 
	 //Armazena os valores em T
	 //if ~(g == 0) then
	    T(i,j)=g;
	    T(j,i)=g;
	 //end,

         printf("==> Ro(%d,%d,%d,%d) = %2.4f\n", Xi,Yi,Xj,Yj, Ro(Xi,Yi,Xj,Yj));
         printf("==> G(Ro) = %2.4f\n", T(i,j));
      end,
   end;
end;

//Imprime a matriz T, para conferência
printf("Matriz T\n");
for i=1:n,
   for j=1:n,
      printf("%2.4f", T(i,j));
   end;
   printf("\n");
end;

//Constrói a matriz M (C)
for i=1:n,
   //Copia a matriz T
   for j=1:n,
      M(i,j)=T(i,j);
   end;

   //Copia a matriz S
   for j=n+1:n+3,
      M(i,j)=S(i,j-n);
   end;
end;

for i=n+1:n+3,
   //Copia a transposta de S
   for j=1:n,
      M(i,j)=S(j,i-n);
   end;
   //Preenche com 0s
   for j=n+1:n+3,
      M(i,j)=0;
   end;
end;

//Imprime a matriz M, para conferência
for i=1:n+3,
   for j=1:n+3,
      printf("M(%d,%d) = %2.4f", i, j, M(i,j));
   end;
end;

//Calcula C (E)
C=inv(M)*H;

//Imprime C, para conferência
for i=1:n+3,
   printf("C[%d] = %2.4f\n", i, C(i));
end;

//Calcula a energia de dobramento
for i=1:n,
   W(i)=C(i);
end;

//Imprime W, para conferência
for i=1:n,
   printf("W[%d] = %2.4f\n", i, W(i));
end;

E=W.'*T*W;

printf("E= %2.4f\n", E);

//Interpolação da Thin-plate spline encontrada
for xi=1:17,
   for yi=1:17,
      //Translação para o plano cartesiano
      Xi=((xi-1)/4)-2;
      Yi=((yi-1)/4)-2;

      //Somatória dos coeficientes de w
      w=0;
      for i=1:n,
         w=w+C(i)*G(Ro(Xi,Yi,S(i,2),S(i,3)));
      end;
      
      ZZ(xi,yi)= C(n+1) + C(n+2)*Xi + C(n+3)*Yi + w;

      printf("ZZ(%d,%d) = %2.4f", xi, yi, ZZ(xi,yi));
   end;
end;

xbasc();

plot3d(X,Y ,ZZ, 35, 25, "Xk@Yk@Z", [2,2,4]);