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, h_p, h_r] = 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 = 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

handle_indices_r = cell2mat(h_r);
handle_indices = [h_p, handle_indices_r];

m_p = length(h_p);
m_r = length(handle_indices_r);
m = m_p + m_r;

r = length(h_r);
h = m_p+r*3;


I_m_p = speye(m_p);

cols = zeros(m_r,3);
ci = 1;
for i=1:r
  cols(ci:(ci+length(h_r{i})-1), :) = 3*(i-1);
  ci = ci+length(h_r{i});
end
cols = cols + kron(1:3,ones(m_r, 1));
J_2 = sparse([(1:m_r)', (1:m_r)', (1:m_r)'], cols, [V(handle_indices_r, :), ones(m_r, 1)], m_r, r*3);

J = [I_m_p, sparse(m_p, r*3); sparse(m_r, m_p), J_2];


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 for scalar weights, fig. 2 for vectorial weights)")


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


component = {'x', 'y', 'w'};
figure()
for i=1:r
  for j=1:3
    subplot(3, r, (j-1)*r + i)
    trisurf (F, V(:,1), V(:,2), W(:,m_p+(i-1)*3+j), "facecolor", "interp", "edgecolor", "k")
    hold on
%    trimesh (F(1:4,:), V(:,1), V(:,2), "color", "r")
    plot3(V(h_r{i},1), V(h_r{i},2), W(h_r{i}, m_p+(i-1)*3+j), 'ko', "linewidth", 2, "markersize", 2)
    axis square
    axis equal
    colorbar("EastOutside")
    title (["w_{",num2str(i), ",",component{j},"}"]);
    view (0, 90);
  end
end

%figure()
%for i=1:r
%  for j=1:2
%    subplot(3, r, (j-1)*r + i)
%    trisurf (F, V(:,1), V(:,2), T'*T*W(:,m_p+(i-1)*3+j) ./ W(:,m_p+(i-1)*3+3), "facecolor", "interp", "edgecolor", "k")
%    hold on
%%    trimesh (F(1:4,:), V(:,1), V(:,2), "color", "r")
%    plot3(V(h_r{i},1), V(h_r{i},2), W(h_r{i}, m_p+(i-1)*3+j), 'ko', "linewidth", 2, "markersize", 2)
%    axis square
%    axis equal
%    colorbar("EastOutside")
%    title (["w_{",num2str(i), ",",component{j},"} / w_{",num2str(i), ",",component{3},"}"]);
%    view (0, 90);
%  end
%end

%return


% Rest pose reproduction test

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

C_1 = V(h_p, :);
id = [eye(2);zeros(1,2)];
C_2 = kron(ones(r,1), id);
H = [C_1;C_2];


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(V(h_p, 1), V(h_p,2), 'go', "linewidth", 2, "markersize", 10)
plot(V(handle_indices_r, 1), V(handle_indices_r,2), 'ro', "linewidth", 2, "markersize", 2)
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(V(h_p, 1), V(h_p,2), 'go', "linewidth", 2, "markersize", 10)
plot(V(handle_indices_r, 1), V(handle_indices_r,2), 'ro', "linewidth", 2, "markersize", 2)
axis square
axis equal
title("Error on reconstructed shape")

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

%return



% Deformation test

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


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(V_deform(h_p, 1), V_deform(h_p,2), 'ro', "linewidth", 2, "markersize", 10)
  plot(V_deform(handle_indices_r, 1), V_deform(handle_indices_r,2), 'ro', "linewidth", 2, "markersize", 2)
  axis square
  axis equal
  axis([-1,1,-1,1])
  
  title("Click on a handle to select it.\n ")
  [x0, y0, ~] = ginput(1);
  
  if x0 < -1 && y0 < -1
    disp("Exiting interactive session.")
    break
  end
  
  [~,i_min]=min(sum((V_deform([h_p,handle_indices_r],:) - kron([x0,y0],ones(m,1))).^ 2,2));
  if i_min <= m_p
    plot(V_deform(h_p(i_min), 1), V_deform(h_p(i_min),2), 'bo', "linewidth", 3, "markersize", 10)
    title(["You selected point handle #", num2str(i_min), ", click on its new location.\n"])
    [x1, y1, ~] = ginput(1);
    H_deform(i_min, :) = [x1, y1];
  else
    i_r = 0;
    i_in_hr = handle_indices_r(i_min - m_p);
    for j=1:r
      if prod(h_r{j}-i_in_hr) == 0
        i_r = j;
        break
      end
    end
    
    plot(V_deform(h_r{i_r}, 1), V_deform(h_r{i_r},2), 'bo', "linewidth", 3, "markersize", 2)
    title(["You selected region handle #", num2str(i_r), ", left-click on its new location,\nmiddle-click on its center of scaling, or right-click on its center of rotation."])
    [x1, y1, b] = ginput(1);
    
    current_rot = H_deform(m_p + (i_r-1)*3 +(1:2),:)';
    current_pos = H_deform(m_p + (i_r-1)*3 +3,:)';
    
    if b==1
      rot = current_rot;
%      trans = [x1;y1] - V_deform(i_in_hr,:)' + current_pos;
      trans = [x1;y1] - [x0;y0] + current_pos;
      
      transform = [rot,trans];
      H_deform(m_p + (i_r-1)*3 + (1:3),:) = transform';
    elseif b==2
      plot(x1, y1, 'k+', "linewidth", 3, "markersize", 10)
      title(["Select the scaling factor (2 clicks).\n"])
      [x2, y2, ~] = ginput(2);
      
      d1 = [x2(1);y2(1)] - [x1;y1];
      d1 = norm(d1);
      d2 = [x2(2);y2(2)] - [x1;y1];
      d2 = norm(d2);
      
      factor = d2/d1;
      
      pos = factor *(current_pos - [x1;y1]) + [x1;y1];
      
      transform = [factor*current_rot, pos];
      H_deform(m_p + (i_r-1)*3 + (1:3),:) = transform';
      
    else
      plot(x1, y1, 'k+', "linewidth", 3, "markersize", 10)
      title(["Select the rotation angle (2 clicks).\n"])
      [x2, y2, ~] = ginput(2);
      
      d1 = [x2(1);y2(1)] - [x1;y1];
      d1 = d1 / norm(d1);
      d2 = [x2(2);y2(2)] - [x1;y1];
      d2 = d2 / norm(d2);
      
      cos_a = d1'*d2;
      sin_a = cross([d1;0], [d2;0]);
      sin_a = sin_a(3);
      rot = [cos_a, -sin_a;sin_a,cos_a];
      
      pos = rot *(current_pos - [x1;y1]) + [x1;y1];
      
      transform = [rot * current_rot, pos];
      H_deform(m_p + (i_r-1)*3 + (1:3),:) = transform';
    end
  end
  V_deform = W * H_deform;
endwhile


