close all; clear all; clc;

%% Get a single EKG Beat

mitdb = load("data\final_data\mitdb_100_beats.mat");
load("data\results\20.mat");

beat_leads = [mitdb.beats{69,1}];
beat = beat_leads(:,1);

beat_filt = sgolayfilt(beat, 0, 5);

p = 14;

%% 
y = beat_filt(5:5:end,1);
y = beat_filt;
vect = padflip(y)' / 10;
figure
pcolor(vect);
colormap("turbo");
set(gca,'Position',[0 0 1 1]);
axis equal, axis off;
% set(gcf,'Position',[100 100 p*12 p*8]);
% set(gcf,'PaperPositionMode','auto');
print('-dsvg', '-loose', 'images/final_images/x_vector.svg');

%% Plot a single beat cleanly
figure;
scatter(linspace(1,300,300), beat_filt, 25, beat_filt, 'filled');
colormap("turbo");
hold on;
test_beat = zeros(300,1);
test_beat(results.tailored.pivot(1:15)) = beat_filt(results.tailored.pivot(1:15));
test_beat(test_beat == 0) = nan;
plot(test_beat, 'ro', 'LineWidth', 3);
% scatter(linspace(1,300,300), test_beat, 'ro', 'LineWidth', 2);
% set(gca,'Position',[0 0 1 1]);
axis tight, axis off;
print('-dsvg', '-loose', 'images/final_images/beat_plot_w_markers.svg');

%% Plot y

new_y = padflip(beat_filt(sort(results.tailored.pivot(1:15))))';
figure;
pcolor(new_y);
colormap("turbo");
set(gca,'Position',[0 0 1 1]);
axis equal, axis off;
print('-dsvg', '-loose', 'images/final_images/y_vector.svg');

%% Plot C
figure;
C = pcolor(~results.tailored.C);
colormap("gray");
set(C, 'EdgeColor', 'none');
axis equal, axis off;
set(gca,'Visible','off')
print('-dsvg', '-loose', 'images/final_images/c_matrix.svg');

%% 

f1 = figure;% (Visible="on");
Y = pcolor(T, H, A');   
set(Y, 'EdgeColor', 'none');
set(gca,'ColorScale','log');
hold on
cmp1 = colormap(jet);
cmp1 = [1 1 1;cmp1];
set(gca, 'Colormap', cmp1)
Z = pcolor(T, H, abs(B'));
set(Z, 'EdgeColor', 'none');
set(gca,'ColorScale','log');
   
cmp2 = colormap("parula");
cmp2 = [1 1 1;cmp2];
set(gca, 'Colormap', cmp2);
hold off

%% Example Plots
npts = 250;
rng default
x = 2*randn(npts,1);
y = 2*randn(npts,1);
v = sin(x) .* sin(y);
figure; scatter(x,y,36,v,'filled')
colorbar
xlim([-2*pi 2*pi])
ylim([-2*pi 2*pi])
[xg,yg] = meshgrid(linspace(-2*pi,2*pi,125));
F = scatteredInterpolant(x,y,v);
vg = F(xg,yg);
figure; pcolor(xg,yg,vg);
shading flat;

%% Extract the SVD


% beat_indices = randperm(length(mitdb.beats), 10);
% for i=1:length(beat_indices)
%     tmp = mitdb.beats{beat_indices(i),1};
%     tmp = sgolayfilt(tmp(:,1), 0, 5);
%     svd_beat(:,i) =  tmp;
% end

for i=1:length(mitdb.beats)
    tmp = mitdb.beats{i,1};
    tmp = sgolayfilt(tmp(:,1), 0, 5);
    svd_beat(:,i) =  tmp;
end

svd_beat(:, 1906) = [];

figure;
subplot(2,1,1), plt_ensemble(svd_beat);
[Psi, S, V] = svd(svd_beat, 'econ');
subplot(2,1,2), plot(Psi(:,1));

%% Plot the SVD Breakdown  


[m, n] = size(svd_beat);

sing = diag(S);
thresh = optimal_SVHT_coef(m/n,0)*median(sing);
r = length(sing(sing>=thresh));
R = [10 50 r 250];

semilogy(sing,'.','color',.7*[1 1 1]);
hold on
plot(sing(1:r),'b.');
plot(R, sing(R),'ro');
plot(R, sing(R),'r.');
set(gca,'yscale','log','xlim',[0 length(sing)+1]);
xlabel("r",'Interpreter','latex','fontsize',20);
ylabel("$\sigma_{r}$",'Interpreter','latex','fontsize',20);
grid on
% print('-dsvg', '-loose', 'images/final_images/svd_beats.svg');

%% Plot the SVD Eigen Beats

mode = 2;
mode_beat = Psi(:, R(mode));

my_fig = figure;

% (1) Plot the beat
beat_plt = subplot(2,1,1);
scatter(linspace(1,300,300), mode_beat, 25, mode_beat, 'filled'), colormap("turbo"), axis off;

% (2) Plot the vector
pc_plt = subplot(2,1,2);
to_color = padflip(mode_beat)';
pcolor(to_color), colormap("turbo"), axis off;
set(pc_plt, 'DataAspectRatio', [10 1 1]);       % Truncate the matrix to a vector
new_pos = pc_plt.Position;
new_pos(2) = 0.37;
set(pc_plt, 'Position', new_pos)

prnt_str = strcat('images/final_images/mode_beat_', num2str(mode), '.svg');
print('-dsvg', '-loose', prnt_str);


%% Custom C

M = 15;
N = M*3;

new_c = zeros(M,N);

new_c(randperm(M*N,M)) = 1;
pcolor(~new_c);
colormap("gray");
axis equal, axis off;
print('-dsvg', '-loose', 'images/final_images/custom_c.svg');

%% Custom Psi

psi_new = dctmtx(N);
pcolor(psi_new);
colormap("turbo");
axis equal, axis off;
print('-dsvg', '-loose', 'images/final_images/custom_psi.svg');

%% Custom S

s_new = zeros(N, 1);
s_new(randperm(N, M)) = psi_new(randperm(M*N, M));
s_new(s_new == 0) = nan;
pcolor(padflip(s_new));
cm = colormap("turbo");
axis equal, axis off;
% print('-dsvg', '-loose', 'images/final_images/custom_s.svg');

%% Custom Theta

theta_new = new_c*psi_new;
pcolor(theta_new);
colormap("turbo");
axis equal, axis off;
print('-dsvg', '-loose', 'images/final_images/custom_theta.svg');

%% Plotting the Tailored Stuff

% subplot(1,3,1), pcolor(~results.tailored.C), colormap("gray"), axis equal, axis off;
% subplot(1,3,2), pcolor(results.tailored.Psi), colormap("turbo"), axis equal, axis off;
% subplot(1,3,3), pcolor(results.tailored.C*results.tailored.Psi), colormap("turbo"), axis equal, axis off;

tay_c = results.tailored.C(1:7, 130:170);
% tay_c(2,31) = 0;
% tay_c(3,23) = 0;

nSamps = length(tay_c);
tay_psi = results.tailored.Psi(75:74+nSamps, 75:74+nSamps);
tay_psi = tay_psi(:, 1:8);

nComp = size(tay_c,1);

tay_y = beat_filt((150 - floor(nComp/2)):(150 + floor(nComp/2)));
tay_y = beat_filt(150-8:148);

tay_a = randn(7,1);

tay_theta = tay_c*tay_psi;

plt = pcolor(tay_theta), colormap("turbo"), axis equal, axis off, grid off;
print('-dsvg', '-loose', 'images/final_images/tailored_cs_Theta.svg');

% plt = pcolor(padflip(tay_a)), colormap("turbo"), axis equal, axis off, grid off;
% print('-dsvg', '-loose', 'images/final_images/tailored_cs_A.svg');

% plt = pcolor(padflip(tay_y)), colormap("turbo"), axis equal, axis off, grid off;
% print('-dsvg', '-loose', 'images/final_images/tailored_cs_Y.svg');

% plt = pcolor(tay_psi), colormap("turbo"), axis equal, axis off, grid off;
% print('-dsvg', '-loose', 'images/final_images/tailored_cs_Psi.svg');

% plt = pcolor(~tay_c), colormap("gray"), axis equal, axis off, grid off;
% % % set(plt, 'EdgeColor', 'none');
% print('-dsvg', '-loose', 'images/final_images/tailored_cs_C.svg');

%% QR Pivoting

% (1) Get the beats
for i=1:length(mitdb.beats)
    tmp = mitdb.beats{i,1};
    tmp = sgolayfilt(tmp(:,1), 0, 5);
    svd_beat(:,i) =  tmp;
end
svd_beat(:, 1906) = [];

% figure;
% subplot(2,1,1), plt_ensemble(svd_beat);
[Psi, S, V] = svd(svd_beat, 'econ');

%% QR Pivoting

numPivot = 12;


psi_r = Psi(:, 1:numPivot);

% psi_r(results.tailored.pivot(1:numPivot), :) = 1;
% 
% pcolor(psi_r), colormap("jet"), axis off, axis equal;


m = mesh(Psi(:, 1:numPivot));
% set(m, 'LineStyle', 'none');
% xlabel("x");
% ylabel("y");
% zlabel("z");

hold on;
x_p = zeros(300,1);
y_p = linspace(1, 300, 300);
z_p = beat_filt - mean(z_p);
z_p = (z_p/2) + 0.13;
plot3(x_p, y_p, z_p, 'b', 'LineWidth',3);



x_scat = zeros(numPivot, 1);
y_scat = results.tailored.pivot(1:numPivot);
z_scat = z_p(y_scat);
scat = scatter3(x_scat, y_scat, z_scat, 'ro');
set(scat, 'LineWidth', 3);

plotoverlap = false;


for i=1:numPivot

    y_piv = ones(numPivot,1) * results.tailored.pivot(i);
    if plotoverlap
        z_piv = ones(numPivot,1) * 0.4;
    else
        z_piv = ones(numPivot,1) * z_p(results.tailored.pivot(i));
    end 
    
    x_piv = linspace(-1,numPivot,numPivot);
    plot3(x_piv, y_piv, z_piv, 'k', 'LineWidth',3);

end

set(gca,'xtick',[],'ytick',[], 'ztick',[],'title',[],'ylabel',[])
% xlabel("Columns of $\Psi_{r}$",'Interpreter','latex');
% ylabel("Rows of $\Psi_{r}$",'Interpreter','latex');

legend("$\Psi_{r}$", "X", "Y", "C$\Psi_{r}$", 'Interpreter','latex');




%% Functions

function X = padflip(X)
    [nx,ny] = size(X);
    X = flipud(X);
    X = [X zeros(nx,1);
        zeros(1,ny+1)];
end