clear all
close all

disp("")
disp("This program tests the computation of a deformation sub-space on a 3D tetrahedral mesh.")

% Initial data

disp("")
disp("Loading data...")
fflush(stdout);

%[V, F, handle_indices] = tetrahedron_3()
%[V, F, handle_indices] = octahedron();
%[V, F, handle_indices] = icosahedron();
[V, F, handle_indices] = armadillo();
%[V, F, handle_indices] = truc_LD();
%[V, F, handle_indices] = truc_HD();
E = edges(F);
Tri = faces(F);

n = size(V,1);
f = size(F,1);
k = size(E,1);
t = size(Tri,1);

%handle_indices = int32(rand(1,100)*n);

disp(["... loaded model with ", num2str(n), " vertices, ",num2str(k)," edges, ", num2str(t)," triangles and ", num2str(f), " tetrahedra."])


% Energy matrix computation

disp("")
disp("Computing energy matrix...")
fflush(stdout);
tic

%Adj = vertices_edges_adjacency_matrix(E);
%Lcr = edge_edge_laplacian_3D(V, E, F);
%Ncr = sparse(k,k);
%%Ncr = -edge_edge_normal_derivative_3D(V,E,F);
%Mcr = speye(k,k);
%%Mcr = edge_mass_matrix_3D(V, E, F);


Adj = vertices_faces_adjacency_matrix(Tri);
%Lcr = sparse(t,t);
Lcr = face_face_laplacian_3D(V, E, Tri, F);
%Ncr = sparse(t,t);
Ncr = face_face_normal_derivative_3D(V,E, Tri,F);
%Mcr = speye(t,t);
Mcr = face_mass_matrix_3D(V, Tri, F);

A = Adj'*(Lcr+Ncr)'*inverse(Mcr)*(Lcr+Ncr)*Adj;

t=toc;
disp(["...done in ",num2str(t),"s."])


% Energy matrix tests

disp("")
disp("Performing precision tests...")
fflush(stdout);

O = ones(n, 1);
O_result = A*O;
O_result_2 = O_result'*O_result
if(O_result_2 < 1e-5)
  disp("Constant precision : CHECK.")
else
  disp("Constant precision : FAILURE.")
endif

V_result = A*V;
V_result_2 = V_result'*V_result;
tr_V_result_2 = trace(V_result_2)
if(tr_V_result_2 < 1e-5)
  disp("Linear precision : CHECK.")
else
  disp("Linear precision : FAILURE.")
endif



% Weights computation

disp("")
disp("Computing weights...")
fflush(stdout);
tic

m = size(handle_indices,2);
H = V(handle_indices, :);

J = speye(m);

S = sparse(m, n);
for i=1:m
  S(i, handle_indices(i)) = 1;
endfor

T = sparse(n-m, n);

j=1;
for i=1:n
  if(size(find(handle_indices-i), 2) == m)
    T(j, i) = 1;
    j = j+1;
  endif
endfor

%W = S'*J-T'*inverse(T*A*T')*T*A*S'*J;
W = S'*J-T'*((T*A*T')\T)*A*S'*J;

t=toc;
disp(["...done in ",num2str(t),"s."])
fflush(stdout);


%% Weights visualization


[~,on_b] = on_boundary(F); % for each vertex of each tetra, 1 if the face opposite the vertex is on boundary, 0 else
[on_b_f,on_b_c] = find(on_b); % indices of tetrahedra on border, and subscript index of the vertex opposed the triangle on border for each tetrahedron

% vertices of faces on the border
Pi = mod(on_b_c+1,4)+1;
Qi = mod(on_b_c+2,4)+1;
Ri = mod(on_b_c+0,4)+1;

P = F(sub2ind(size(F), on_b_f, Pi));
Q = F(sub2ind(size(F), on_b_f, Qi));
R = F(sub2ind(size(F), on_b_f, Ri));



##{
disp("")
disp("Visualizing weights (see fig. 1)")
fflush(stdout);

figure()
for i=1:m
  subplot(round(sqrt(m)), ceil(sqrt(m)), i)
  trisurf([P,Q,R], V(:, 1), V(:, 2), V(:, 3), W(:,i), "facecolor", "interp", "edgecolor", "k")
%  trisurf([P,Q,R], V(:, 1), V(:, 2), V(:, 3), min(max(W(:,i), -1.0),2.0), "facecolor", "interp", "edgecolor", "k")
%  shading("interp")
  hold on
  plot3(H(i, 1), H(i, 2), H(i, 3), 'kx', "linewidth", 2, "markersize", 10)
  axis square
  axis equal
  colorbar("EastOutside")
  title (["w_{",num2str(i), "}"]);
  view(3)
%  view (0, -90);
%  xlabel('x');
%  ylabel('y');
%  zlabel('z');
endfor
#}


% Rest pose reproduction test

disp("")
disp("Performing rest post reproduction test...")
fflush(stdout);

V_reconstruct = W * H;
V_diff = V - V_reconstruct;
V_diff_2 = V_diff'*V_diff;
tr_V_diff_2 = trace(V_diff_2)

if(tr_V_diff_2 < 1e-5)
  disp("Rest pose reproduciton : CHECK.")
else
  disp("Rest pose reproduciton : FAILURE.")
endif


figure()
%{
subplot(1,2,1)
hold on
surf1 = trisurf([P,Q,R], V(:,1), V(:,2), V(:,3), sum(V_diff.^2,2).^(0.5));
colorbar("EastOutside")
plot3(H(:,1), H(:,2), H(:,3), 'go', "linewidth", 2, "markersize", 10)
axis square
axis equal
view(3)

subplot(1,2,2)
%}
hold on
surf2 = trisurf([P,Q,R], V_reconstruct(:,1), V_reconstruct(:,2), V_reconstruct(:,3), sum(V_diff.^2,2).^(0.5));
colorbar("EastOutside")
plot3(H(:,1), H(:,2), H(:,3), 'go', "linewidth", 2, "markersize", 10)
axis square
axis equal
view(3)
%add_shadow();
title("Rest pose reproduction test")

%linkprop([surf1, surf2], 'view')

disp("Displaying result (see fig. 2)")
fflush(stdout);



% Deformation test

disp("")
disp("Visualizing deformation (see fig. 3)")
fflush(stdout);

m_deform = m;
deform_indices = 1:m_deform;
deform_values = 2*(rand(m_deform, 3)-0.5*ones(m, 3));



H_deform = H;
for i=1:m_deform
  H_deform(deform_indices(i), :) = H(deform_indices(i), :) + deform_values(i, :);
endfor

V_deform = W * H_deform;

%export_tetra_mesh(V_deform, F, "output/test.vtk")


figure()
hold on

trisurf([P,Q,R], V_deform(:,1), V_deform(:,2), V_deform(:,3), sum((V-V_deform).^2,2).^(0.5))
plot3(H(:,1), H(:,2), H(:,3), 'go', "linewidth", 2, "markersize", 10)
plot3(H_deform(:,1), H_deform(:,2), H_deform(:,3), 'bo', "linewidth", 2, "markersize", 10)
plot3([H(:, 1)'; H_deform(:, 1)'], [H(:, 2)'; H_deform(:, 2)'], [H(:, 3)'; H_deform(:, 3)'], 'k', "linewidth", 2)
axis square
axis equal
view(3)
title("Deformation test")

