% 冗長な連立一次方程式
%
% 冗長な連立一次方程式 Ax = b の一般解を求める．
% 行列 A の値は最終的に [U,B] となる（Uは上三角行列）
% 行の入れ替え row 列の入れ替え col
% Aorg(row,col) = L*[U,B] （Lは上三角行列）

A = [ 0,  0, -3, -1, -2;...
      2, -4,  5,  1, -2;...
      2, -4,  2, -5,  1];
b = [4; -2; 7];

Aorg = A;       % 行列 A の元の値
borg = b;       % ベクトル c の元の値
sz = size(A); m = sz(1); n = sz(2);
L = eye(m,m);

row = 1:m;
col = 1:n;
for k=1:m
    pv = find(A(k:m,k))+(k-1);
    if pv
        p = pv(1);
        A([p,k],:) = A([k,p],:);
        row([p,k]) = row([k,p]);
        b([p,k]) = b([k,p]);
    else
        pv = find(A(k,k:n))+(k-1);
        if ~pv
            disp "cannot find a pivot";
            exit;
        end
         p = pv(1);
         A(:,[p,k]) = A(:,[k,p]);
         col([p,k]) = col([k,p]);
    end
    for l=k+1:m
        s = A(l,k)/A(k,k);
        A(l,:) = A(l,:) - s*A(k,:);
        b(l) = b(l) - s*b(k);
        L(l,k) = s;
    end
end

U = A(1:m,1:m);
B = A(1:m,m+1:n);

fprintf("行列 A と [U,B]\n");
A
[U,B]

fprintf("行列 Aorg(row,col) と L*[U,B]\n");
L*[U,B]
Aorg(row,col)

ysp = zeros(n,1);
ysp(1:m) = U\b;
yarb = [-inv(U)*B; eye(n-m)];

inverseindex = index_inverse(col);
xsp = ysp(inverseindex);
xarb = yarb(inverseindex,:);

% x = xsp * xarb*alpha （alphaは任意の(n-m)次ベクトル）

xsp
xarb

fprintf("Aorg*xsp と borg\n");
Aorg*xsp
borg

fprintf("Aorg*xarb\n");
Aorg*xarb

fprintf("テスト\n");
for k=1:10
    x = xsp + xarb*rand(n-m,1);
    y = Ainit*x;
    fprintf("[%f %f %f %f %f] [%f %f %f]\n", x', y');
end
