clear
clc
X = double(imread('x1.bmp'));
Y = double(imread('y4.bmp'));
%Z = imread('x2.bmp');
X = [X; ones(1,size(X, 2))];

%AA = [1 2 3;4 5 6;7 8 9];
%A = MoorePenrose(X, Y);

[hx, wx] = size(X);
[hy, wy] = size(Y);
X_pinv = [];
best = 1e9;
beps = 1e9;
for d=1:-0.01:0%1:-0.01:0
    x_pinv = inv((X' * X + d * eye(wx, wy))) * X';

    a = Y * x_pinv + eye(hy, hx) * (eye(hx, hx) - X * x_pinv)';
    eps = max(max(a * X - Y));
    if eps < best
    X_pinv = x_pinv;
    best = eps;
    beps = d;
    end
end

A = Y * X_pinv;

res = A * X;
%X = [1 , -1 , 0 ; -1 , 2 , 1 ; 2 , -3 , -1 ; 0 , -1 , 1];
[h,w] = size(X);
x = X(1,:);
x_pinv = x'/(x*x');
eps = 1e-6;
for i=2:h
    a = X(i,:);
    I = eye(w, w);
    XPX = x_pinv * x;
    Z = I - XPX;
%   fprintf('a*Z*a is equal to %6.2f  %d.\n',a*Z*a',i);
    CK = a * Z * a';
    if a * Z * a' > eps
        d = a * Z * a';
        FI1 = Z*(a')*a*x_pinv;
        FI1 = FI1 / d;
        FI = x_pinv - FI1;
        SE = Z*a';
        SE = SE / d;
        x_pinv = [FI, SE];
        %    fprintf('fi\n');
    else
        R = x_pinv * (x_pinv');
        d = 1 + a * R * a';
        x_pinv = [x_pinv - (R*(a')*a*x_pinv) / d, R*a' / d];
    end;
    x = [x; a];
end;

%z = X(1,:);
%zz = z'/(z*z');
%
 A_G = Y * x_pinv;%Greville(X, Y);
 res_G = A_G * X;
%
 subplot(2, 2, 1); imshow(uint8(X)); title('X');
 subplot(2, 2, 2); imshow(uint8(Y)); title('Y');
%
 subplot(2, 2, 3); imshow(uint8(res)); title('Метод Мура-Пенроуза');
 subplot(2, 2, 4); imshow(uint8(res_G)); title('Метод Гревіля');