Matrices as operators

%% m05_matrix_basics.m  Module 05: the matrix workbench (Demo 05.2) in numbers
% Purpose : print every operation of the Module 05 workbench on the same small
%           matrices as the page, so each number on the page can be checked here:
%           A*x two ways, A*B vs B*A, .' vs ' on IQ data, inv/det of 2x2 and 3x3,
%           A\b (square and tall), rank of a rank-deficient matrix, projectors,
%           the DFT matrix F4, convolution as a Toeplitz matrix, kron for
%           per-element operators on stacked channel data, and a tiny sparse DAS matrix.
% Change  : edit any matrix below; e.g. make row 3 of A = row 1 + row 2 and rerun.
% Expect  : A*x = [4 3 -2]; det(A) = 24; A*B = [2 1 3; 1 5 -5; 4 5 -1] ~= B*A;
%           z.'*z = 0 but z'*z = 2; A\b = [1 -1 0.5]; tall fit slope -0.98 dB/MHz;
%           rank(M) = 2 with null vector [1 1 -1]; F4'*F4 = 4*I; T*x = conv(h,x);
%           DAS: nnz(B) = 10, B*d = [2 0.75]; hilb(10): A\b residual ~1e-16 vs
%           inv(A)*b ~4e-5 at equal forward error ~5e-4 (backslash is backward stable).
% Runtime : < 1 s.  Base MATLAB only; runs in MATLAB R2019b+ and GNU Octave 8.
rng(1);
pm = @(name, M) fprintf(['%s =\n' repmat([repmat('%9.4g', 1, size(M,2)) '\n'], 1, size(M,1))], name, M.');   % print a real matrix row by row

%% 1. A*x: row picture (one dot product per output) and column picture (mix of columns)
A = [ 2 1 0;
     -1 3 2;
      0 1 4];                  % 3x3 real, not symmetric
x = [1; 2; -1];                % 3x1 column vector
for i = 1:3
    fprintf('row %d: [%g %g %g] . [%g %g %g] = %g\n', i, A(i,:), x, A(i,:)*x);
end
y = zeros(3,1);
for j = 1:3
    y = y + x(j)*A(:,j);       % add x_j times column j
    fprintf('after column %d: [%g %g %g]\n', j, y);
end
fprintf('A*x = [%g %g %g]\n\n', A*x);

%% 2. A*B versus B*A: the order matters
B = [1 0 2;
     0 1 -1;
     1 1 0];
pm('A*B', A*B);  pm('B*A', B*A);  pm('A*B - B*A', A*B - B*A);
fprintf('(A*B)(2,3) = %g*%g + %g*%g + %g*%g = %g\n\n', A(2,1), B(1,3), A(2,2), B(2,3), A(2,3), B(3,3), A(2,:)*B(:,3));

%% 3. Transpose .' versus conjugate transpose ' on IQ data
Z = [1+2i, 0.5-1i, -1i;
     2,   -1+1i,   0.3+0.4i];  % K = 2 snapshots (rows) x N = 3 elements (columns), complex IQ
disp('Z.'' (transpose, no conjugate) ='); disp(Z.');
disp('Z''  (conjugate transpose)    ='); disp(Z');
z = [1; 1i];
fprintf('z = [1; 1i]:  z.''*z = %g   z''*z = %g\n', z.'*z, z'*z);
R = Z'*Z;                      % 3x3 sample covariance across elements (times K): Hermitian PSD
disp('R = Z''*Z ='); disp(R);
fprintf('diag(R) = [%.2f %.2f %.2f] (element energies, real)\n', real(diag(R)));
ev = sort(real(eig(R)), 'descend');
fprintf('eig(R) = [%.4f %.4f %.4f], rank(R) = %d (K = 2 snapshots)\n\n', ev, rank(R));

%% 4. Inverse and determinant, 2x2 and 3x3
A2 = [4 7; 2 6];
fprintf('det(A2) = %g, inv(A2) = (1/det)[d -b; -c a]:\n', det(A2)); disp(inv(A2));
fprintf('det(A) = %g\n', det(A));
pm('24*inv(A) (the adjugate)', round(24*inv(A)));
pm('inv(A)', inv(A));
fprintf('max|A*inv(A) - I| = %.1e\n\n', max(max(abs(A*inv(A) - eye(3)))));

%% 5. Solve A x = b: square (LU) and tall (least squares by QR)
b = [1; -3; 1];
fprintf('A\\b = [%g %g %g]\n', A\b);
f  = [2; 4; 6; 8];                     % MHz
yd = [-1.0; -3.2; -4.8; -7.0];         % log-spectral ratio, dB
Af = [ones(4,1) f];                    % 4x2: intercept and slope columns
cf = Af \ yd;                          % least squares (QR inside)
r  = yd - Af*cf;
fprintf('tall 4x2 fit: c = Af\\y = [%.4f %.4f]  (intercept dB, slope dB/MHz)\n', cf);
fprintf('residual r = [%.3f %.3f %.3f %.3f], Af''*r = [%.1e %.1e]\n\n', r, Af'*r);

%% 6. Rank of a rank-deficient matrix
M = [1 2 3;
     2 0 2;
     0 1 1;
     1 1 2];                    % column 3 = column 1 + column 2
s = svd(M);
fprintf('svd(M) = [%.4f %.4f %.2e], rank(M) = %d\n', s, rank(M));
nv = [1; 1; -1];
fprintf('M*[1;1;-1] = [%g %g %g %g] (null vector)\n\n', M*nv);

%% 7. Projectors: P^2 = P
N = 4; one = ones(N,1);
P = eye(N) - one*one'/N;        % removes the slow-time mean (simplest wall filter)
pm('P = I - 11''/4', P);
xs = [3.0; 3.2; 2.9; 3.1];      % slow-time samples at one pixel: tissue (mean) + small flow
fprintf('P*x = [%.2f %.2f %.2f %.2f] (mean %.2f removed)\n', P*xs, mean(xs));
fprintf('max|P^2 - P| = %.1e, eig(P) = [%s], trace = %g\n', max(max(abs(P*P - P))), num2str(sort(eig(P)).', '%.0f '), trace(P));
H = Af * ((Af'*Af) \ Af');      % hat matrix of the line fit in section 5 (fine here: kappa small)
pm('H = Af*(Af''*Af)^-1*Af''', H);
fprintf('H*y = [%.3f %.3f %.3f %.3f] = Af*c, max|H^2 - H| = %.1e, trace(H) = %g\n\n', H*yd, max(max(abs(H*H - H))), trace(H));

%% 8. DFT matrix F4: F'*F = 4 I
n = (0:3);
F = exp(-2i*pi*n.'*n/4);        % F(k+1,n+1) = exp(-j 2 pi k n / 4): entries 1, -j, -1, j
disp('F4 ='); disp(round(F));   % round removes 1e-16 residue in printing
disp('F4''*F4 ='); disp(round(real(F'*F)*1e12)/1e12);
xf = [1; 2; 0; -1];
Fx = F*xf; Fx(abs(Fx) < 1e-12) = 0; Fx = round(Fx*1e12)/1e12;   % clean 1e-16 rounding residue for printing
fprintf('F*x    = %s\nfft(x) = %s\n', mat2str(Fx.', 4), mat2str(fft(xf).', 4));
fprintf('||F*x||^2 = %g = 4*||x||^2 = %g\n\n', norm(F*xf)^2, 4*norm(xf)^2);

%% 9. Convolution as a Toeplitz matrix
h  = [2; 1; -1];                % 3-tap pulse
xr = [1; 0; 0; 2; 0];           % reflectivity: two scatterers
T  = toeplitz([h; zeros(4,1)], [h(1) zeros(1,4)]);   % (5+3-1) x 5 = 7x5
pm('T = toeplitz([h; 0 0 0 0], [h(1) 0 0 0 0])', T);
fprintf('T*x       = [%s]\nconv(h,x) = [%s]\n\n', num2str((T*xr).', '%g '), num2str(conv(h, xr).', '%g '));

%% 10. kron: a per-element operator on stacked channel data d = X(:)
X  = [1 2; 3 4; 5 6];           % 3 time samples x 2 elements
d  = X(:);                      % stacked element by element
Dp = diag([1, -1i]);            % phase screen: element 2 delayed by a quarter period (phase -pi/2)
K  = kron(Dp, eye(3));          % 6x6, block diagonal
dab = K*d;
fprintf('kron(D, I3)*d = %s\n', mat2str(dab.', 3));
fprintf('equals vec(X*D): %d; correction kron(D'', I3) restores d: max err %.1e\n\n', ...
        isequal(dab, reshape(X*Dp, [], 1)), max(abs(kron(Dp', eye(3))*dab - d)));

%% 11. A tiny DAS operator: 2 pixels x (4 samples x 3 elements), linear interpolation
Nt = 4; Ne = 3;
tau = [2.5 2.0 2.5;             % pixel 1: delay per element, in samples (1-based, illustrative)
       3.25 3.0 3.25];          % pixel 2 (deeper)
rr = []; cc = []; ww = [];
for p = 1:2
    for e = 1:Ne
        n0 = floor(tau(p,e)); fr = tau(p,e) - n0;
        rr = [rr p p]; cc = [cc (e-1)*Nt+n0, (e-1)*Nt+n0+1]; ww = [ww 1-fr fr];
    end
end
keep = ww > 0;                  % drop zero weights (integer delays)
Bd = sparse(rr(keep), cc(keep), ww(keep), 2, Nt*Ne);
pm('full(B)', full(Bd));
fprintf('nnz(B) = %d of %d entries\n', nnz(Bd), numel(Bd));
dd = full(Bd(1,:)).';           % channel data of a unit scatterer at pixel 1 (H = B'' model)
pm('X = reshape(d, 4, 3)', reshape(dd, Nt, Ne));
fprintf('img = B*d = [%g %g]\n', full(Bd*dd));
pm('B*B'' (PSF matrix: column 1 = image of pixel 1)', full(Bd*Bd'));

%% 12. Why backslash and not inv: an ill-conditioned example
Hh = hilb(10); xt = ones(10,1); bh = Hh*xt;
xb = Hh \ bh; xi = inv(Hh)*bh;
fprintf('hilb(10): cond = %.1e\n', cond(Hh));
fprintf('  A\\b     : residual %.1e, error %.1e\n', norm(Hh*xb - bh)/norm(bh), norm(xb - xt)/norm(xt));
fprintf('  inv(A)*b: residual %.1e, error %.1e\n', norm(Hh*xi - bh)/norm(bh), norm(xi - xt)/norm(xt));

%% 13. Figure: structure of the operators
figure;
mats = {A, abs(R), real(F), T, abs(K), full(Bd)};
ttl  = {'A (general 3x3)', '|Z^H Z| (rank 2)', 'Re F_4', 'Toeplitz T (7x5)', '|kron(D, I_3)|', 'DAS B (2 x 12)'};
for k = 1:6
    subplot(2,3,k); imagesc(mats{k}); colorbar; title(ttl{k}); xlabel('column j'); ylabel('row i');
    if k < 6, axis image; end
end
colormap(gray);

%% m05_convolution_matrix.m  Module 05: convolution is a Toeplitz matrix, the DFT diagonalizes circulants
% Purpose : build the Toeplitz convolution matrix T of a Gaussian-modulated pulse h,
%           verify T*x == conv(h,x); embed it in a circulant C, show that the DFT
%           matrix diagonalizes C with fft(first column) on the diagonal, and show
%           the circular wrap-around error when the circulant is too small.
% Change  : f0, frac_bw, fs (pulse); N (signal length); Nc (circulant size).
% Expect  : square 6x6 window: T6*x6 = first 6 samples of conv, last 3 lost;
%           max|T*x - conv(h,x)| = 0; max off-diagonal |F C F'/Nc| ~ 1e-15;
%           with Nc = N the circular result differs from linear conv in the first L-1 samples;
%           direct vs FFT convolution of a 2048-sample line cross over near L ~ 100 taps.
% Runtime : ~1 s.  Base MATLAB only; runs in MATLAB R2019b+ and GNU Octave 8.
rng(1);

%% 1. A square 6x6 convolution window (h has 4 taps): the full result needs 6+4-1 = 9 rows
h4 = [-0.23; -0.62; 0.62; 0.23];         % sampled Gaussian-modulated sine, zero mean
x6 = [1; 0; 0; 0.5; 0; -1];               % sparse reflectivity, 6 samples
T6 = zeros(6,6);
for j = 1:6                               % column j = h shifted down by j-1 (truncated at 6 rows)
    rows = j:min(6, j+3);
    T6(rows, j) = h4(1:numel(rows));
end
disp('T6 (lower-triangular Toeplitz, T(i,j) = h(i-j+1)) ='); disp(T6);
yfull = conv(h4, x6);                     % full linear convolution: 6 + 4 - 1 = 9 samples
fprintf('T6*x6         = [%s]\n', num2str((T6*x6).', '%7.3f'));
fprintf('conv(h,x)(1:6)= [%s]\n', num2str(yfull(1:6).', '%7.3f'));
fprintf('tail conv(7:9)= [%s]  (lost by the square 6x6 window)\n', num2str(yfull(7:9).', '%7.3f'));

%% 2. A realistic pulse: f0 = 5 MHz, 60 percent -6 dB bandwidth, fs = 40 MHz
f0 = 5e6; frac_bw = 0.6; fs = 40e6;       % Hz, fraction, Hz
sig_f = frac_bw*f0/2 / sqrt(2*log(2));    % std of the Gaussian amplitude spectrum (Hz)
sig_t = 1/(2*pi*sig_f);                   % std of the Gaussian envelope (s)
t = (-ceil(3*sig_t*fs):ceil(3*sig_t*fs)).' / fs;   % +-3 sigma_t
h = exp(-t.^2/(2*sig_t^2)) .* cos(2*pi*f0*t);     % RF pulse
L = numel(h);
fprintf('pulse: sigma_t = %.4f us, L = %d taps at %.0f MHz\n', sig_t*1e6, L, fs/1e6);

%% 3. Full Toeplitz convolution matrix, (N+L-1) x N
N = 64;                                   % signal length (samples)
x = zeros(N,1); x([10 25 26 47]) = [1 -0.6 0.8 0.5];   % four scatterers
T = zeros(N+L-1, N);
for j = 1:N
    T(j:j+L-1, j) = h;                    % every column is the pulse, shifted by one sample
end
fprintf('T is %d x %d, %d nonzeros (%.1f %% dense), Toeplitz: T(i,j) depends only on i-j\n', ...
        size(T,1), size(T,2), nnz(T), 100*nnz(T)/numel(T));
fprintf('max|T*x - conv(h,x)| = %.1e\n', max(abs(T*x - conv(h,x))));

%% 4. Circulant embedding: zero-pad to Nc >= N+L-1 and linear = circular convolution
Nc = N + L - 1;
cfirst = [h; zeros(Nc-L,1)];              % first column of the circulant
C = zeros(Nc);
for j = 1:Nc
    C(:,j) = circshift(cfirst, j-1);      % each column = previous column rotated down by one
end
xp = [x; zeros(Nc-N,1)];
yc = real(ifft(fft(cfirst) .* fft(xp)));  % FFT convolution
fprintf('Nc = %d: max|C*xp - conv| = %.1e, max|ifft(fft.*fft) - conv| = %.1e\n', ...
        Nc, max(abs(C*xp - conv(h,x))), max(abs(yc - conv(h,x))));

%% 5. The DFT matrix diagonalizes the circulant
F = fft(eye(Nc));                         % DFT matrix: F(k+1,n+1) = exp(-2i*pi*k*n/Nc)
fprintf('F''*F = Nc*I: max|F''*F/Nc - I| = %.1e\n', max(max(abs(F'*F/Nc - eye(Nc)))));
Dg = F * C * F' / Nc;                     % should be diag(fft(cfirst))
offd = Dg - diag(diag(Dg));
fprintf('max off-diagonal |F C F''/Nc| = %.1e, max|diag - fft(c)| = %.1e\n', ...
        max(abs(offd(:))), max(abs(diag(Dg) - fft(cfirst))));
fprintf('eigenvalues of C = fft(c): |lambda| max %.3f at %.2f MHz\n', ...
        max(abs(fft(cfirst))), (find(abs(fft(cfirst)) == max(abs(fft(cfirst))), 1) - 1)*fs/Nc/1e6);

%% 6. Too small a circulant: circular wrap-around (time aliasing)
Ns = N;                                   % no zero padding
cs = [h; zeros(Ns-L,1)];
ycirc = real(ifft(fft(cs) .* fft(x)));
ylin = conv(h, x);
err = abs(ycirc - ylin(1:Ns));
fprintf('Nc = N = %d: max wrap error = %.3f in samples 1..%d, %.1e afterwards\n', ...
        Ns, max(err(1:L-1)), L-1, max(err(L:end)));

%% 7. Cost of the two routes for one RF line
Nrf = 2048; Lf = [16 64 256];
for k = 1:3
    Nfft = 2^nextpow2(Nrf + Lf(k) - 1);
    direct = Nrf * Lf(k);                                 % real multiply-adds
    viafft = 2 * 2*Nfft*log2(Nfft) + 4*Nfft;              % 2 real-equivalent FFTs (~2 N log2 N real mults each) + products
    fprintf('L = %3d: direct %7d MACs, FFT route ~%7d mults (Nfft = %d)\n', Lf(k), direct, round(viafft), Nfft);
end

%% 8. Figures
figure;
subplot(2,2,1); imagesc(T); colorbar; title('Toeplitz T (N+L-1 x N)'); xlabel('input sample j'); ylabel('output sample i');
subplot(2,2,2); imagesc(C); axis image; colorbar; title('circulant C (wraps at the corner)'); xlabel('column'); ylabel('row');
subplot(2,2,3); imagesc(20*log10(abs(Dg)/max(abs(Dg(:))) + 1e-12)); axis image; colorbar;
caxis([-300 0]); title('|F C F^H / N_c| (dB): diagonal'); xlabel('column k'); ylabel('row k');
subplot(2,2,4); plot(0:numel(ylin)-1, ylin, 'k', 0:Ns-1, ycirc, 'r--');
xlabel('sample'); ylabel('amplitude'); title('linear (black) vs circular N_c = N (red)');
colormap(gray);

%% m05_das_as_matrix.m  Module 05: delay-and-sum as a sparse matrix, and its adjoint
% Purpose : build the DAS operator B (pixels x channel samples) for a 32-element
%           linear array and a 0-degree plane-wave transmit, with linear-interpolation
%           weights (2 nonzeros per element per pixel); apply it to simulated
%           point-target channel data; show that B' (the adjoint) maps an image back to
%           channel data along hyperbolas; check <B d, g> = <d, B' g>; spy(B) and density.
% Change  : Ne, pitch, the pixel grid (dxp), apod ('rect' or 'hann'), the targets.
% Expect  : B is 6161 x 43520 with 64 nonzeros per row (0.147 percent dense);
%           dot-test mismatch ~1e-15; lateral -6 dB width at z = 18 mm close to
%           1.21*lambda*z/D = 0.70 mm (rect); pulse-filtered B' column vs true echo
%           correlation ~0.99 (B is H^H up to the pulse matched filter).
% Runtime : ~3 s.  Base MATLAB only; runs in MATLAB R2019b+ and GNU Octave 8.
rng(1);

%% 1. System and grid
c = 1540;  f0 = 5e6;  fs = 40e6;  frac_bw = 0.6;    % m/s, Hz, Hz, -6 dB fractional bandwidth
lambda = c/f0;                                      % 0.308 mm
Ne = 32;   pitch = 0.3e-3;                          % elements, m
xe = ((0:Ne-1) - (Ne-1)/2) * pitch;                 % element x positions (m), aperture D = 9.6 mm
D = Ne*pitch;
apod = 'rect';
if strcmp(apod, 'hann'), we = sin(pi*((0:Ne-1) + 0.5)/Ne).^2; else, we = ones(1, Ne); end
dxp = 0.1e-3;                                       % pixel size (m), about lambda/3
xg = -3e-3:dxp:3e-3;  zg = 15e-3:dxp:25e-3;         % 61 x 101 pixels
[Xp, Zp] = meshgrid(xg, zg);  Np = numel(Xp);
Nt = 1360;                                          % samples per channel (34 us at 40 MHz)
t = (0:Nt-1).'/fs;
fprintf('lambda = %.3f mm, D = %.1f mm, pixels = %d x %d = %d, channel samples = %d x %d = %d\n', ...
        lambda*1e3, D*1e3, numel(zg), numel(xg), Np, Nt, Ne, Nt*Ne);

%% 2. Build the sparse DAS matrix B: row p = pixel p, column (e-1)*Nt + n = sample n of element e
Ip = zeros(2*Ne*Np, 1); Jc = Ip; Vw = Ip; k = 0;
for e = 1:Ne
    tau = (Zp(:) + sqrt((Xp(:) - xe(e)).^2 + Zp(:).^2)) / c;   % two-way delay, 0-degree plane wave (s)
    u = tau*fs;                                                 % fractional sample index (0-based)
    n0 = floor(u);  fr = u - n0;                                % neighbours n0, n0+1 and weights
    ok = n0 >= 0 & n0 + 1 <= Nt - 1;
    idx = find(ok);  m = numel(idx);
    Ip(k+1:k+m) = idx;        Jc(k+1:k+m) = (e-1)*Nt + n0(idx) + 1;  Vw(k+1:k+m) = we(e)*(1 - fr(idx));  k = k + m;
    Ip(k+1:k+m) = idx;        Jc(k+1:k+m) = (e-1)*Nt + n0(idx) + 2;  Vw(k+1:k+m) = we(e)*fr(idx);        k = k + m;
end
B = sparse(Ip(1:k), Jc(1:k), Vw(1:k), Np, Nt*Ne);
fprintf('B: %d x %d, nnz = %d (%.0f per row), density = %.3f %%\n', size(B,1), size(B,2), nnz(B), nnz(B)/Np, 100*nnz(B)/numel(B));
fprintf('memory: sparse ~%.1f MB vs dense %.1f GB (double)\n', nnz(B)*16/1e6, numel(B)*8/1e9);

%% 3. Simulate analytic channel data for three point targets (single scattering)
sig_f = frac_bw*f0/2/sqrt(2*log(2));  sig_t = 1/(2*pi*sig_f);    % Gaussian pulse, s
g = @(tt) exp(-tt.^2/(2*sig_t^2)) .* exp(2i*pi*f0*tt);          % analytic pulse
tg = [0 18; -1.5 20; 1.5 23]*1e-3;                              % targets [x z] (m)
d = zeros(Nt, Ne);
for s = 1:size(tg,1)
    for e = 1:Ne
        tau = (tg(s,2) + sqrt((tg(s,1) - xe(e))^2 + tg(s,2)^2))/c;
        d(:,e) = d(:,e) + g(t - tau);
    end
end
d = d + 0.01*(randn(Nt,Ne) + 1i*randn(Nt,Ne))/sqrt(2);         % weak white noise

%% 4. Beamform: one sparse matrix-vector product
img = B * d(:);                                 % complex image, Np x 1
I = reshape(abs(img), numel(zg), numel(xg));
IdB = 20*log10(I/max(I(:)));
% lateral -6 dB width at the first target
[~, iz] = min(abs(zg - 18e-3));  prof = I(iz,:)/max(I(iz,:));
[~, im] = max(prof);  l = im; while prof(l) > 0.5, l = l - 1; end;  r = im; while prof(r) > 0.5, r = r + 1; end
xl = xg(l) + (0.5 - prof(l))/(prof(l+1) - prof(l))*dxp;  xr = xg(r-1) + (0.5 - prof(r-1))/(prof(r) - prof(r-1))*dxp;
fprintf('lateral -6 dB width at z = 18 mm: %.2f mm (theory 1.21 lambda z / D = %.2f mm)\n', (xr - xl)*1e3, 1.21*lambda*18e-3/D*1e3);

%% 5. The adjoint B' maps an image back to channel data (back-projection)
dback = reshape(B' * img, Nt, Ne);              % every pixel re-emits its value along its own hyperbola
% dot test: <B d, g> must equal <d, B' g> for any d, g
dr = randn(Nt*Ne,1) + 1i*randn(Nt*Ne,1);  gr = randn(Np,1) + 1i*randn(Np,1);
lhs = gr' * (B*dr);  rhs = (B'*gr)' * dr;
fprintf('dot test |<Bd,g> - <d,B''g>| / |<Bd,g>| = %.1e\n', abs(lhs - rhs)/abs(lhs));

%% 6. B is H^H up to the pulse matched filter: compare B' e_j (pulse-filtered) with H e_j
[~, j] = min(abs(Xp(:) - 0) + abs(Zp(:) - 20e-3));            % pixel at (0, 20 mm)
aj = reshape(full(B(j,:)).', Nt, Ne);           % B' e_j: two-tap kernels on the hyperbola of pixel j
tp = (-ceil(4*sig_t*fs):ceil(4*sig_t*fs)).'/fs; gp = g(tp);
ajf = zeros(Nt, Ne);  hj = zeros(Nt, Ne);
for e = 1:Ne
    full_c = conv(aj(:,e), gp);  ajf(:,e) = full_c((1:Nt) + (numel(gp)-1)/2);   % 'same'-centred convolution
    tau = (Zp(j) + sqrt((Xp(j) - xe(e))^2 + Zp(j)^2))/c;  hj(:,e) = g(t - tau);  % H e_j: true echo of a point at pixel j
end
rho = abs(ajf(:)'*hj(:)) / (norm(ajf(:))*norm(hj(:)));
fprintf('|corr(pulse * B''e_j, H e_j)| = %.4f\n', rho);

%% 7. Figures
figure;
subplot(2,2,1); imagesc(1:Ne, t*1e6, real(d)); colorbar; xlabel('element'); ylabel('time (\mus)'); title('channel data d (real part)'); ylim([22 32]);
subplot(2,2,2); imagesc(xg*1e3, zg*1e3, IdB); caxis([-40 0]); axis image; colorbar;
xlabel('x (mm)'); ylabel('z (mm)'); title('image |B d| (dB)');
ix0 = find(abs(xg) < dxp/2);  rows0 = (ix0-1)*numel(zg) + (1:numel(zg));   % the 101 pixels of the x = 0 image column
subplot(2,2,3); spy(B(rows0, 1:4*Nt)); xlabel('column (e-1) N_t + n, elements 1-4'); ylabel('pixel (x = 0, z = 15..25 mm)');
title('spy(B): delay curves, 2 taps per element');
subplot(2,2,4); imagesc(1:Ne, t*1e6, abs(dback)); colorbar; xlabel('element'); ylabel('time (\mus)'); title('|B^H B d|: back-projection'); ylim([22 32]);
colormap(gray);

%% m05_least_squares_conditioning.m  Module 05: least squares, conditioning, noise amplification
% Purpose : fit an attenuation slope (dB vs MHz) with A\b, pinv and QR; show why
%           centering and scaling the columns matters; show that the normal equations
%           square kappa; tabulate kappa vs column angle; Monte Carlo the noise
%           amplification along the weak singular direction; Tikhonov teaser.
% Change  : sig_dB (spectral noise), the outlier, phi_deg (column angle), lambda grid.
% Expect  : slope ~ -1 dB/MHz, SE = 0.0666 dB/MHz, outlier shift +0.203 dB/MHz;
%           kappa = cot(phi/2): 57.3 at 2 deg, 11.43 at 10 deg; Monte Carlo std along
%           v2 within ~3 percent of sigma/sigma_2; order-7 raw-f fit: relative error ~1e-7
%           with QR, ~1e-2 with the normal equations; Tikhonov at lambda = sigma_2^2
%           halves the v2 noise (0.08 -> 0.04).
% Runtime : ~2 s.  Base MATLAB only; runs in MATLAB R2019b+ and GNU Octave 8.
rng(1);

%% 1. Spectral-difference attenuation model
% log-ratio of power spectra between two depths, dB:  y(f) = y0 - 2*alpha0*dz*f
alpha0 = 0.5;            % dB/cm/MHz (one-way, amplitude)
dz = 1.0;                % cm between the two windows
f = linspace(2, 8, 25).';                % MHz, 25 frequencies
ytrue = 2 - 2*alpha0*dz*f;               % slope -1 dB/MHz (round trip: factor 2)
sig_dB = 0.6;                            % noise std per frequency, dB
y = ytrue + sig_dB*randn(size(f));

A = [ones(size(f)) f];                   % columns: intercept, slope
c_bs   = A \ y;                          % QR-based least squares (what backslash does for tall A)
c_pinv = pinv(A) * y;                    % SVD-based pseudo-inverse
[Q, R] = qr(A, 0);                       % economy QR: A = Q*R, Q'*Q = I
c_qr   = R \ (Q' * y);
c_ne   = (A'*A) \ (A'*y);                % normal equations (fine here, kappa small)
fprintf('slope: \\ %.4f | pinv %.4f | QR %.4f | normal eq %.4f dB/MHz (truth -1)\n', ...
        c_bs(2), c_pinv(2), c_qr(2), c_ne(2));
fprintf('alpha0 estimate = %.3f dB/cm/MHz (truth %.2f)\n', -c_bs(2)/(2*dz), alpha0);
r = y - A*c_bs;
fprintf('residual orthogonal to the columns: max|A''*r| = %.1e\n', max(abs(A'*r)));
SE = sig_dB / sqrt(sum((f - mean(f)).^2));
fprintf('slope standard error sigma/sqrt(sum (f-fbar)^2) = %.4f dB/MHz\n', SE);
Cx = sig_dB^2 * inv(A'*A);               % covariance of the estimate (inv is fine for a 2x2 readout)
fprintf('same from sqrt(sigma^2 (A''A)^-1)(2,2) = %.4f\n', sqrt(Cx(2,2)));

%% 2. One outlier (+6 dB at 7.75 MHz)
yo = y; [~, io] = min(abs(f - 7.75)); yo(io) = yo(io) + 6;
co = A \ yo;
fprintf('outlier at %.2f MHz moves the slope by %+.3f dB/MHz (= %.1f SE); theory 6*(f_i - fbar)/Sxx = %+.3f\n', ...
        f(io), co(2) - c_bs(2), (co(2) - c_bs(2))/SE, 6*(f(io) - mean(f))/sum((f - mean(f)).^2));

%% 3. Column scaling: raw f versus centered t = (f-5)/3
At = [ones(size(f)) (f - 5)/3];
fprintf('cond([1 f]) = %.2f, cond([1 (f-5)/3]) = %.2f\n', cond(A), cond(At));
for p = [3 5 7]
    Araw = zeros(numel(f), p+1); Acen = Araw;
    for k = 0:p, Araw(:,k+1) = f.^k; Acen(:,k+1) = ((f - 5)/3).^k; end
    fprintf('order %d: cond raw = %.1e, centered = %.1f\n', p, cond(Araw), cond(Acen));
end

%% 4. Normal equations square kappa: order-7 polynomial in raw f, noise-free data
p = 7; Araw = zeros(numel(f), p+1);
for k = 0:p, Araw(:,k+1) = f.^k; end
ct = ones(p+1, 1); bt = Araw * ct;       % exact data, known answer
c1 = Araw \ bt;                          % QR
c2 = (Araw'*Araw) \ (Araw'*bt);          % normal equations
fprintf('order 7 raw: cond(A) = %.1e, cond(A''A) = %.1e (= cond(A)^2 = %.1e)\n', cond(Araw), cond(Araw'*Araw), cond(Araw)^2);
fprintf('  relative error: QR %.1e, normal equations %.1e\n', norm(c1 - ct)/norm(ct), norm(c2 - ct)/norm(ct));
fprintf('  digits lost ~ log10(kappa): QR %.1f, normal eq %.1f\n', log10(cond(Araw)), log10(cond(Araw)^2));

%% 5. kappa versus the angle between two unit columns
phis = [90 30 10 5 2];
fprintf('phi(deg)  kappa   cot(phi/2)\n');
for ph = phis
    Ap = [1 cosd(ph); 0 sind(ph)];
    fprintf('%6g  %7.2f  %7.2f\n', ph, cond(Ap), cotd(ph/2));
end

%% 6. Monte Carlo: noise is amplified only along the weak direction v2
phi_deg = 10; sig = 0.01; M = 4000;
Ap = [1 cosd(phi_deg); 0 sind(phi_deg)];
[U, S, V] = svd(Ap); s = diag(S);
xt = [1; 1]; bp = Ap*xt;
Xhat = Ap \ (bp + sig*randn(2, M));     % M solves at once
E = Xhat - xt;
std_v = std(V.' * E, 0, 2);              % std of the error along v1 and v2
fprintf('phi = %g deg: sigma = [%.4f %.4f], std along v1 = %.4f (theory %.4f), v2 = %.4f (theory %.4f)\n', ...
        phi_deg, s, std_v(1), sig/s(1), std_v(2), sig/s(2));

%% 7. Tikhonov teaser: (A'A + lambda I)^-1 A'b
% The bias is lambda/(sigma_i^2 + lambda) times the component of the truth along v_i.
% x = (1,1) lies exactly along v1 (equal unit columns), so shrinking v2 costs it no bias;
% x = (1,0) has a v2 component, so the same lambda buys less.
lams = [0 1e-4 1e-3 s(2)^2 1e-1 1];
truths = [1 1; 1 0].';
rmse = zeros(2, numel(lams));
for q = 1:2
    xq = truths(:,q); bq = Ap*xq; Nq = sig*randn(2, M);
    fprintf('truth x = (%g, %g): v2''x = %+.3f\n', xq, V(:,2).'*xq);
    fprintf('  lambda     f2      std_v2   bias    RMSE\n');
    for k = 1:numel(lams)
        lam = lams(k);
        Mreg = (Ap'*Ap + lam*eye(2)) \ Ap';
        Er = Mreg * (bq + Nq) - xq;
        f2 = s(2)^2/(s(2)^2 + lam);
        rmse(q,k) = sqrt(mean(sum(Er.^2, 1)));
        fprintf('  %8.1e  %5.3f  %7.4f  %6.4f  %6.4f\n', lam, f2, std(V(:,2).'*Er), norm(mean(Er, 2)), rmse(q,k));
    end
end

%% 8. Figures
figure;
subplot(2,2,1);
plot(f, y, 'ko', f, A*c_bs, 'b-', f, ytrue, 'k--', f(io), yo(io), 'rs', f, A*co, 'r-');
xlabel('frequency (MHz)'); ylabel('log-spectral ratio (dB)'); title('LS fit; red: with outlier');
subplot(2,2,2);
plot(E(1,:), E(2,:), '.', 'markersize', 2); axis equal;
hold on; tt = linspace(0, 2*pi, 200);
el = V * diag(2*sig./s) * [cos(tt); sin(tt)]; plot(el(1,:), el(2,:), 'r-', 'linewidth', 1.5); hold off;
xlabel('error in x_1'); ylabel('error in x_2'); title(sprintf('error cloud, phi = %g deg', phi_deg));
subplot(2,2,[3 4]);
semilogx(max(lams, 1e-5), rmse(1,:), 'o-', max(lams, 1e-5), rmse(2,:), 's-');
xlabel('\lambda (0 plotted at 1e-5)'); ylabel('RMSE of x'); legend('x = (1,1)', 'x = (1,0)');
title('Tikhonov RMSE');