clear all
close all

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

% Initial data

disp("")
disp("Loading data...")
tic;


%[V, F, h_p, h_r] = gingerbread_LD();
%
%n = size(V,1);
%f = size(F,1);

V = gingerbread_LD_contour();
n = size(V,1);

N=40;
res = [N,N];
data = zeros(res);
nbPixels = prod(res);

dim = [0,0,1,1];

minX = min(V(:,1));
maxX = max(V(:,1));
minY = min(V(:,2));
maxY = max(V(:,2));

%V = 0.5*(V + kron([0.5, 0.5], ones(n,1)));

V = (V - kron([minX, minY], ones(n,1))) ./ kron([maxX, maxY]-[minX, minY], ones(n,1));
V = V .* kron(res-1, ones(n,1));

%V = V + kron([1,1]./res, ones(n,1));

%pkg load image
%for i=1:f
%%  data = or(data, poly2mask([V(F(i,1),1), V(F(i,2),1), V(F(i,3),1)]*res(1), [V(F(i,1),2), V(F(i,2),2), V(F(i,3),2)]*res(2), res(1), res(2)));
%  data = or(data, poly2mask([V(F(i,1),1), V(F(i,2),1), V(F(i,3),1)], [V(F(i,1),2), V(F(i,2),2), V(F(i,3),2)], res(1), res(2)));
%end

data = poly2mask(V(:,1), V(:,2), res(1), res(2));

V = V + 0.5*ones(n,2);

%for i=1:res(2)
%  tx = (i-1)/(res(2)-1);
%%  tx = (i-0.5)/(res(1)-1);
%  for j=1:res(1)
%    ty = (j-1)/(res(1)-1);
%%    ty = (j-0.5)/(res(2)-1);
%    
%    p = [tx, ty];
%    
%%    disp([num2str((i-1)*res(2)+j),'/',num2str(nbPixels)])
%    
%    for k=1:f
%    
%      
%      i1 = F(k,1);
%      i2 = F(k,2);
%      i3 = F(k,3);
%      
%      w  = cross([V(i2,:) - V(i1,:),0], [V(i3,:) - V(i1,:),0])(3);
%      w1 = cross([V(i2,:) - p,0], [V(i3,:) - p,0])(3)/w;
%      w2 = cross([p - V(i1,:),0], [V(i3,:) - V(i1,:),0])(3)/w;
%      w3 = cross([V(i2,:) - V(i1,:),0], [p - V(i1,:),0])(3)/w;
%      
%      if  and(w1 > 0, w2>0, w3>0)
%        data(j,i) = 1;
%      end
%    end
%  end
%end




data_X = kron(1:res(1), ones(res(2),1));
data_X = data_X .* data;
data_Y = kron(1:res(2), ones(res(1),1))';
data_Y = data_Y .* data;



figure()
subplot(1,3,1)
imagesc(data)
hold on
%trimesh (F, V(:, 1), V(:, 2), 'k', "linewidth", 2)



data_X_diffused = data_X + diffuseOutside(data, data_X);
data_Y_diffused = data_Y + diffuseOutside(data, data_Y);

V_reconstruct = [data_X_diffused(sub2ind(res, round(V(:,2)), round(V(:,1)))), data_Y_diffused(sub2ind(res, round(V(:,2)), round(V(:,1))))];

plot([V_reconstruct(:, 1);V_reconstruct(1,1)], [V_reconstruct(:, 2);V_reconstruct(1,2)], 'w-', "linewidth", 2)
plot([V(:, 1);V(1,1)], [V(:, 2);V(1,2)], 'w:', "linewidth", 1)

axis image
axis xy
title('Original rasterized')

subplot(1,3,2)
imagesc(data_X)
%imagesc(data_X_diffused)
axis image
axis xy
title('Original data field: X displacement')

subplot(1,3,3)
imagesc(data_Y)
%imagesc(data_Y_diffused)
axis image
axis xy
title('Original data field: Y displacement')

t  = toc;

disp(["... loaded model with resolution: ", num2str(res(1)), "x", num2str(res(2)), " (done in ",num2str(t)," s)."])


%return


% Energy matrix computation

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

L = sparse(nbPixels, nbPixels);

for i=1:res(1)
  for j=1:res(2)
  
    if data(i, j) == 0;
      continue
    end
    
    if i>1
%    if and(i>1, i<res(1))
      if data(i-1, j) == 1
%      if and(data(i-1, j) == 1, data(i+1, j) == 1)
        L(sub2ind(res, i  , j), sub2ind(res, i  , j)) = L(sub2ind(res, i  , j), sub2ind(res, i  , j)) - 1;
        L(sub2ind(res, i  , j), sub2ind(res, i-1, j)) = L(sub2ind(res, i  , j), sub2ind(res, i-1, j)) + 1;
      end
    end
    
    if i<res(1)
%    if and(i>1, i<res(1))
      if data(i+1, j) == 1
%      if and(data(i-1, j) == 1, data(i+1, j) == 1)
        L(sub2ind(res, i  , j), sub2ind(res, i  , j)) = L(sub2ind(res, i  , j), sub2ind(res, i  , j)) - 1;
        L(sub2ind(res, i  , j), sub2ind(res, i+1, j)) = L(sub2ind(res, i  , j), sub2ind(res, i+1, j)) + 1;
      end
    end
    
    if j>1
%    if and(j>1, j<res(2))
      if data(i, j-1) == 1
%      if and(data(i, j-1) == 1, data(i, j+1) == 1)
        L(sub2ind(res, i  , j), sub2ind(res, i, j  )) = L(sub2ind(res, i  , j), sub2ind(res, i, j  )) - 1;
        L(sub2ind(res, i  , j), sub2ind(res, i, j-1)) = L(sub2ind(res, i  , j), sub2ind(res, i, j-1)) + 1;
      end
    end
    
    if j<res(2)
%    if and(j>1, j<res(2))
      if data(i, j+1) == 1
%      if and(data(i, j-1) == 1, data(i, j+1) == 1)
        L(sub2ind(res, i  , j), sub2ind(res, i, j  )) = L(sub2ind(res, i  , j), sub2ind(res, i, j  )) - 1;
        L(sub2ind(res, i  , j), sub2ind(res, i, j+1)) = L(sub2ind(res, i  , j), sub2ind(res, i, j+1)) + 1;
      end
    end
  end
end

%
%N = sparse(nbPixels, nbPixels);
%
%for i=1:res(1)
%  for j=1:res(2)
%  
%    if data(i, j) == 0;
%      continue
%    end
%    
%    if i>1
%      if data(i-1, j) == 0
%        if data(i+1, j) == 1
%          N(sub2ind(res, i  , j), sub2ind(res, i  , j)) = N(sub2ind(res, i  , j), sub2ind(res, i  , j)) + 1;
%          N(sub2ind(res, i  , j), sub2ind(res, i+1, j)) = N(sub2ind(res, i  , j), sub2ind(res, i+1, j)) - 1;
%        end
%      end
%    end
%    
%    if i<res(1)
%      if data(i+1, j) == 0
%        if data(i-1, j) == 1
%          N(sub2ind(res, i  , j), sub2ind(res, i  , j)) = N(sub2ind(res, i  , j), sub2ind(res, i  , j)) + 1;
%          N(sub2ind(res, i  , j), sub2ind(res, i-1, j)) = N(sub2ind(res, i  , j), sub2ind(res, i-1, j)) - 1;
%        end
%      end
%    end
%    
%    if j>1
%      if data(i, j-1) == 0
%        if data(i, j+1) == 1
%          N(sub2ind(res, i  , j), sub2ind(res, i, j  )) = N(sub2ind(res, i  , j), sub2ind(res, i, j  )) + 1;
%          N(sub2ind(res, i  , j), sub2ind(res, i, j+1)) = N(sub2ind(res, i  , j), sub2ind(res, i, j+1)) - 1;
%        end
%      end
%    end
%    
%    if j<res(2)
%      if data(i, j+1) == 0
%        if data(i, j-1) == 1
%          N(sub2ind(res, i  , j), sub2ind(res, i, j  )) = N(sub2ind(res, i  , j), sub2ind(res, i, j  )) + 1;
%          N(sub2ind(res, i  , j), sub2ind(res, i, j-1)) = N(sub2ind(res, i  , j), sub2ind(res, i, j-1)) - 1;
%        end
%      end
%    end
%  end
%end


K = L;
%K = L+N;
%K = L-N;

M = speye(nbPixels, nbPixels);
A = K' * (M\ K);

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




% Energy matrix tests

disp("")
disp("Performing precision tests...")

%O = ones(nbPixels, 1);
O = reshape(data, nbPixels, 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_X = A*reshape(data_X, nbPixels, 1);
V_result_2_X = V_result_X'*V_result_X;
tr_V_result_2_X = trace(V_result_2_X)

V_result_Y = A*reshape(data_Y, nbPixels, 1);
V_result_2_Y = V_result_Y'*V_result_Y;
tr_V_result_2_Y = trace(V_result_2_Y)

if(and([tr_V_result_2_X < 1e-5, tr_V_result_2_Y < 1e-5]))
  disp("Linear precision : CHECK.")
else
  disp("Linear precision : FAILURE.")
endif

%return



% Handle creation

disp("")
disp("Selecting Handles (see fig. 2)...")

%constrained_pixels = [50,50;60,40;40,40];
%m = 3;
%constrained_pixels = floor(kron(res, ones(m,1))./[2.0, 2.0; 1.7,2.5; 2.5, 2.5]);

constrained_pixels=[];

fig = figure();
while 1
  hold off
  imagesc(data)
  axis image xy
  
  hold on
  if size(constrained_pixels,1) > 0
    plot(constrained_pixels(:,1), constrained_pixels(:,2), "kx", "linewidth", 10, "markersize", 3)
  end
  
  title("Left-click for selecting a new handle, right-click when you're done.")
  
  [x, y, buttons] = ginput(1);
  
  if buttons != 1
    break
  end
  
  x = round(x);
  y = round(y);
  if and(x>0, y>0, x<res(1)+1, y<res(2)+1)
    if data(y, x) == 1
      constrained_pixels = [constrained_pixels;x, y];
    end
  end
end

close(fig)

m = size(constrained_pixels,1);

disp(["... ",num2str(m)," handles selected."])




% Weight computation

disp("")
disp("Computing weights...")
tic

J = speye(m);

S = sparse(m, nbPixels);
for i=1:m
%  S(i, sub2ind(res, constrained_pixels(i,1), constrained_pixels(i,2))) = 1;
  S(i, sub2ind(res, constrained_pixels(i,2), constrained_pixels(i,1))) = 1;
endfor

T = sparse(nbPixels-m, nbPixels);

k=1;
for i=1:res(1)
  for j=1:res(2)
    if not(any(all(kron([i,j], ones(m,1))==constrained_pixels,2)))
%      T(k, sub2ind(res, i, j)) = 1;
      T(k, sub2ind(res, j, i)) = 1;
      k = k+1;
      continue
    endif
  end
end

%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. 2)")

figure()

nbLine = ceil(sqrt(m));
nbCol = ceil(sqrt(m));
if (nbLine-1)*nbCol >= m
  nbLine = nbLine-1;
end

for i=1:m
  subplot(nbLine, nbCol, i)
  imagesc(reshape(W(:,i)', res(1), res(2)))
  hold on
  plot(constrained_pixels(i,1), constrained_pixels(i,2), 'mx', 'markersize', 10, "linewidth", 3)
  axis image xy
  colorbar
end

%return


% Rest pose reproduction test

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

%H_X = data_X(sub2ind(res, constrained_pixels(:,1), constrained_pixels(:,2)));
%H_Y = data_Y(sub2ind(res, constrained_pixels(:,1), constrained_pixels(:,2)));

H_X = data_X(sub2ind(res, constrained_pixels(:,2), constrained_pixels(:,1)));
H_Y = data_Y(sub2ind(res, constrained_pixels(:,2), constrained_pixels(:,1)));

data_reconstruct_X = W * H_X;
data_reconstruct_Y = W * H_Y;

data_reconstruct_X = reshape(data_reconstruct_X, res(1), res(2));
data_reconstruct_Y = reshape(data_reconstruct_Y, res(1), res(2));

data_diff_X = data_X - data_reconstruct_X;
data_diff_X_2 = data_diff_X'*data_diff_X;
tr_data_diff_X_2 = trace(data_diff_X_2)

data_diff_Y = data_Y - data_reconstruct_Y;
data_diff_Y_2 = data_diff_Y'*data_diff_Y;
tr_data_diff_Y_2 = trace(data_diff_Y_2)

if(and([tr_data_diff_X_2 < 1e-5, tr_data_diff_Y_2 < 1e-5]))
  disp("Rest pose reproduciton : CHECK.")
else
  disp("Rest pose reproduciton : FAILURE.")
endif




data_reconstruct_X_2 = data_reconstruct_X+diffuseOutside(data, data_reconstruct_X);
data_reconstruct_Y_2 = data_reconstruct_Y+diffuseOutside(data, data_reconstruct_Y);

V_reconstruct = [data_reconstruct_X_2(sub2ind(res, round(V(:,2)), round(V(:,1)))), data_reconstruct_Y_2(sub2ind(res, round(V(:,2)), round(V(:,1))))];

figure()
imagesc(((data_reconstruct_X - data_X).^2 + (data_reconstruct_Y - data_Y).^2).^ 0.5)
%imagesc(data_reconstruct_X)
%imagesc(data_reconstruct_Y)
hold on
for i=1:m
  plot(constrained_pixels(i,1), constrained_pixels(i,2), 'kx', 'markersize', 10, "linewidth", 3)
end

plot(V_reconstruct(:,1), V_reconstruct(:,2), 'w-', "linewidth", 4)

axis image
axis xy
%axis square
colorbar
title("Error on original shape")
xlabel("x")
ylabel("y")

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

%return


% Deformation test

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

deform_values = zeros(m,2);

fig = figure();



H_X = data_X(sub2ind(res, constrained_pixels(:,2), constrained_pixels(:,1)));
H_Y = data_Y(sub2ind(res, constrained_pixels(:,2), constrained_pixels(:,1)));

while isfigure(fig)
  
  data_reconstruct_X = W * H_X;
  data_reconstruct_Y = W * H_Y;

  data_reconstruct_X = reshape(data_reconstruct_X, res(1), res(2));
  data_reconstruct_Y = reshape(data_reconstruct_Y, res(1), res(2));
  
  data_reconstruct_X_2 = data_reconstruct_X+diffuseOutside(data, data_reconstruct_X);
  data_reconstruct_Y_2 = data_reconstruct_Y+diffuseOutside(data, data_reconstruct_Y);

  V_reconstruct = [data_reconstruct_X_2(sub2ind(res, round(V(:,2)), round(V(:,1)))), data_reconstruct_Y_2(sub2ind(res, round(V(:,2)), round(V(:,1))))];

  hold off
  plot([V_reconstruct(:, 1);V_reconstruct(1,1)], [V_reconstruct(:, 2);V_reconstruct(1,2)], 'k', "linewidth", 2)
  hold on
  plot(H_X, H_Y, 'bx', "linewidth", 3, "markersize", 10)
  
  axis([0,res(1),0, res(2)])
  axis square
  
  
  [x0, y0, buttons] = ginput(1);
  
  [~,i_min] = min(sum(([H_X,H_Y] - ([x0;y0]*ones(1,m))').^ 2,2));
  plot(H_X(i_min), H_Y(i_min), 'rx', "linewidth", 4, "markersize", 10)
  
  
  [x1, y1, buttons] = ginput(1);
  
  H_X(i_min) = x1;
  H_Y(i_min) = y1;
end


