% Lin1.m
%          23 Feb 2017

clear

nn = 7 ;
% creating a 3-D shape  X
[X1, X2, X3] = ellipsoid(1,2,3, 2,5,1.5, nn);
%  X1 = X1+sin(X2); X2 = atan(X2);  
 X1 = 2*sin(1*X1) ; % X2 = 2*tanh(X2) ;
[Xr1, k] = sort(X1(:)) ;
Xr2 = X2(:) ; Xr2 = Xr2(k) ;
Xr3 = X3(:) ; Xr3 = Xr3(k) ;
X = [Xr1, Xr2, Xr3]' ; % datapoints
X =  X(:, (nn+1):end-nn)  ;

[D, N1] = size(X) ;
 
Y = ...
[3.75 3.87 4.26 4.83 5.26 5.50 5.94 6.21  6.97  7.76  8.20 8.81 9.25 9.66 10.04 10.17 10.21 9.92 9.53 9.09 8.74 8.28 7.87 7.65 7.54 6.97 6.55 6.40 6.36 6.53 6.82 7.24 8.00 8.57 9.23 9.71 10.14 10.19 9.71 9.23 8.63 7.80 7.47 6.86 6.29 5.61 5.15 4.78 4.39 3.60
 6.90 7.22 7.71 8.41 8.64 8.88 9.53 9.88 10.21 10.25 10.14 9.81 9.51 9.30  8.57  8.08  7.52 7.05 6.98 7.03 6.98 7.12 7.26 7.57 8.15 8.41 8.32 7.89 7.38 6.96 6.52 6.17 5.86 5.56 5.42 5.21  4.90  4.44 4.02 3.97 3.90 3.88 3.95 4.06 4.29 4.44 4.97 5.83 6.00 6.35
] ;
 [M, N] = size(Y) ;

 dst = zeros(N) ;
 for n=1:N
    % distance between each 2D point and all 3D points
    dst(n,:) = sum([(X(1:2,:)-Y(:,n)); X(3,:)].^2) ; 
 end
 
% we try to pair the point from 3D to their counterparts in  2D  
%  we pair the points wrt the distance between them. 
% If a pair allocation is made, we look for the next best distance

srt = zeros(N,1) ; % a column vector containing the closest 3D points
a = 0;
while sum(srt == 0) > 0 
  a = a+1 ;
  kr = find(srt == 0) ; % kn is a column vector
  kc = setdiff(1:N, srt(srt>0)) ;
  [mn1, k1] = min(dst(kr,kc)) ; 
  for n = 1:length(kr) 
   k2 = find(k1 == n) ;
   if ~isempty(k2)
     [mn3, k3] = min(mn1(k2)) ; 
      srt(kr(n)) = kc(k2(k3)) ;  
   end
  end
end
 
X = X(:, srt) ; % the 3D points are matched to their 2D counterparts
 
figure(1), clf
surf(X1, X2, X3), axis equal, grid on, hold on
plot3(X(1,:), X(2,:), X(3,:), 'ko'), % axis equal, grid on, hold on
for n =1:N
    text(X(1,n), X(2,n), X(3,n), num2str(n, 2)) ;
end

plot(Y(1,:), Y(2,:)), plot(Y(1,:), Y(2,:), '*')
for n =1:N
    text(Y(1,n), Y(2,n), num2str(n, 2)) ;
end
for n =1:N
    plot3([X(1,n), Y(1,n)], [X(2,n),Y(2,n)], [X(3,n), 0]) ; 
end

hold off
title('A 3-D shape to be mapped onto a 2-D shape')
% print(1, '-dpdf', 'Lin1')

% Extended autocorrelation matrix eqn 25
wNs = ones(1,N) ;
Xh =  [X ; wNs] ;
Rh = Xh*Xh'/N ;
% % Extended cross-correlation matrix eqn 26
Ch = Y*Xh'/N ;
% The matrix of parameters
W = Ch/Rh ;

% Given X, W find Z that approximate Y
Z = W*Xh;  % eqn (8)
% Approximation Error per point mse
mse = sum(sum((Z-Y).^2))/N ;

figure(2)
plot(Y(1,:), Y(2,:), 'k*', ... % Y(1,:), Y(2,:), 'k-', ...
     Z(1,:), Z(2,:), 'bo'), 
 grid on
title('The desired shape Y and its approximation by Z')
legend('Y', 'Z')
text(3.5,10, num2str(mse, 'mse = %1.2f'))
% print(2, '-dpdf', 'Lin1YZ')

