clear all
close all

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

% Initial data

disp("")
disp("Loading data...")

%[V, F, handle_indices] = square_2D();
%[V, F, handle_indices] = gingerbread_LD();
[V, F, handle_indices] = gingerbread_HD();
E = edges(F);

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

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


% Energy matrix computation

disp("")
disp("Computing energy matrix...")
tic

Adj = vertices_edges_adjacency_matrix(E);
Lcr = edge_edge_laplacian(V, E, F);
Ncr = 0.*edge_edge_normal_derivative(V,E,F);
Mcr = edge_mass_matrix(V, E, 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...")

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...")
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."])


% Weights visualization

disp("")
disp("Visualizing weights (see fig. 1)")

figure()
for i=1:m
  subplot(ceil(sqrt(m)), ceil(sqrt(m)), i)
  trisurf (F, V(:,1), V(:,2), W(:,i), "facecolor", "interp", "edgecolor", "k")
%  shading("interp")
  hold on
  plot3(H(i, 1), H(i, 2), 1.0, 'kx', "linewidth", 2, "markersize", 10)
  axis square
  axis equal
  colorbar("EastOutside")
  title (["w_{",num2str(i), "}"]);
  view (0, 90);
endfor

%return


% Rest pose reproduction test

disp("")
disp("Performing rest post reproduction test...")

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
trisurf(F, V(:,1), V(:,2), sum(V_diff.^2,2).^(0.5), "facecolor", "interp", "edgecolor", "k", "linewidth", 1.0);
colorbar("EastOutside")
plot(H(:,1), H(:,2), 'go', "linewidth", 2, "markersize", 10)
axis square
axis equal
title("Error on original shape")

subplot(1,2,2)
hold on
trisurf(F, V_reconstruct(:,1), V_reconstruct(:,2), sum(V_diff.^2,2).^(0.5), "facecolor", "interp", "edgecolor", "k", "linewidth", 1.0);
colorbar("EastOutside")
plot(H(:,1), H(:,2), 'go', "linewidth", 2, "markersize", 10)
axis square
axis equal
title("Error on reconstructed shape")

disp("Displaying result (see fig. 2)")




% Deformation test

disp("")
disp("Visualizing deformation (see fig. 3)")

%deform_indices = [1, 2];
%deform_values = [  
%  -0.26501,   0.37173;
%  0.50687,   0.12233 
%]*0.5;
%m_deform = size(deform_indices, 2);

m_deform = m;

%{
deform_indices = 1:m_deform;
deform_values = 0.5*(rand(m_deform, 2)-[0.5, 0.5]);

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;

figure()
hold on
trimesh (F, V(:, 1), V(:, 2), 'g', "linewidth", 2)
trimesh (F, V_deform(:, 1), V_deform(:, 2), "b", "linewidth", 2)
plot(H(:,1), H(:,2), 'go', "linewidth", 2, "markersize", 10)
plot(H_deform(:,1), H_deform(:,2), 'bo', "linewidth", 2, "markersize", 10)
plot([H(:, 1)'; H_deform(:, 1)'], [H(:, 2)'; H_deform(:, 2)'], 'k', "linewidth", 2)
axis square
axis equal
legend("original", "deformed")

disp("")
%}



fig = figure();
V_deform = V;
H_deform = H;
while isfigure(fig)
  hold off
  trimesh (F, V_deform(:, 1), V_deform(:, 2), 'g', "linewidth", 2)
  hold on
  plot(H_deform(:,1), H_deform(:,2), 'bo', "linewidth", 2, "markersize", 10)
  axis square
  axis equal
  axis([-1,1,-1,1])
  
  [x, y, buttons] = ginput(2);
  
  [~,i_min]=min(sum((H_deform - ([x(1);y(1)]*ones(1,m))').^ 2,2));
  H_deform(i_min, :) = [x(2), y(2)];
  V_deform = W * H_deform;
endwhile


