clear
close all
addpath(genpath('Data'));
addpath(genpath('Functions'));
addpath(genpath('Objects'));

%% Paraméterek
fs = 48000;
N = 3;
rmic = 0.1;
micArray = spherical_design('File','firthenmike.dat',rmic);

%% Impulzusválasz betöltése
% Az impulzusválasz egy [Nsamples x 24] méretű mátrix
% Cseréld ki a saját fájlodra:
load('rir_right_trunc.mat');  % rir: [Nsamples x 24]
rir = h;
[Nsamples, numChannels] = size(rir);
t = (0:Nsamples-1)' / fs;

%% Beamforming szűrők előkészítése
blockSize = 4096;
Nf = blockSize;
[~, Hrad] = get_beamforming_filters( rmic, N, fs, Nf, 'modal' );
[~, Hreg] = get_regularization_filters( N, Nf, fs, [90,680,1650,2600]'*(0.042/rmic) );
freq = (0:Nf-1)'/Nf*fs;
fc = 3e3;
Hlp = 1./sqrt( 1 + (freq/fc).^6 );

H = Hrad .* Hreg .* Hlp;
h = ifft(H,[],1,'symmetric');

beamforming_filters = radial_filter_bank(N, h, blockSize);
a_n = get_max_rE_weights( N )';

%% Beamformer irány-mátrixok
Nfi = 360;
fi = (0:Nfi-1)'/Nfi*2*pi;
beamformer_polar = getSpherHarmMx( pi/2*ones(size(fi)), fi, 3, 'real') .* a_n';

Nazim = 180/2;
Nelev = 90/2;
azim = (0:Nazim-1)'/Nazim*2*pi;
elev = (0:Nelev)'/Nelev*pi;
[Az, El] = meshgrid(azim, elev);
beamformer_map = getSpherHarmMx( El(:), Az(:), 3, 'real') .* a_n';

%% Figure felépítése
fig = figure('units','normalized','outerposition',[0 0 1 1], ...
    'Name','Offline Beamforming','NumberTitle','off');

% --- Impulzusválasz megjelenítése (felső panel) ---
ax_ir = axes('Units','normalized','Position',[0.06 0.72 0.88 0.22]);
plot(t*1000, rir(:,1), 'Color', [0.2 0.2 0.2]);
hold on
env = sqrt(mean(rir.^2, 2));
plot(t*1000, env, 'Color', [0.1 0.5 0.8], 'LineWidth', 1.5);
xlabel('Idő [ms]')
ylabel('Amplitúdó')
title('Impulzusválasz – Jelölj ki egy szegmenst az egérrel!')
grid on
xlim([0 t(end)*1000])
legend('1. csatorna', 'RMS burkoló', 'Location','northeast')

h_patch = patch(ax_ir, [0 0 0 0], [-1 1 1 -1]*max(abs(rir(:)))*1.2, ...
    [0.5 0.5 0.5], 'FaceAlpha', 0.3, 'EdgeColor', 'none', 'Visible', 'off');

% Kijelölt szegmens kiemelése a görbén (piros overlay)
h_highlight = plot(ax_ir, NaN, NaN, 'Color', [0.9 0.2 0.2], 'LineWidth', 2);

% --- Kijelölt szegmens megjelenítése ---
ax_seg = axes('Units','normalized','Position',[0.06 0.38 0.35 0.25]);
p_seg = plot(0, 0, 'Color', [0.2 0.2 0.2]);
xlabel('Idő [ms]')
ylabel('Amplitúdó')
title('Kijelölt szegmens')
grid on

% --- Polar plot ---
ax_polar = axes('Units','normalized','Position',[0.5 0.38 0.22 0.25]);
s_polar = zeros(size(fi));
p_polar = polarplot(fi, abs(s_polar));
rlim([0 1])
title('Beamforming (horizontális)')

% --- Térkép ---
ax_map = axes('Units','normalized','Position',[0.72 0.38 0.25 0.25]);
s_map = zeros(size(beamformer_map,1),1);
p_map = pcolor(azim*180/pi, elev*180/pi, ...
    20*log10(abs(reshape(s_map, size(El,1), size(El,2))) + eps));
set(gca, 'YDir','reverse')
xlabel('Azimut [°]')
ylabel('Zenit [°]')
title('Beamforming térkép [dB]')
shading flat
axis equal tight
colorbar
caxis([-20 0])

% --- Info szöveg ---
ax_info = axes('Units','normalized','Position',[0.06 0.02 0.88 0.08], 'Visible','off');
h_info = text(0.5, 0.5, 'Kattints és húzd az egeret az impulzusválaszon a szegmens kijelöléséhez!', ...
    'HorizontalAlignment','center','FontSize',12,'Color',[0.2 0.2 0.8]);

%% Állapot eltárolása a figure UserData-jába
S = struct();
S.fs = fs;
S.N = N;
S.blockSize = blockSize;
S.numChannels = numChannels;
S.Nsamples = Nsamples;
S.rir = rir;
S.micArray = micArray;
S.beamforming_filters = beamforming_filters;
S.beamformer_polar = beamformer_polar;
S.beamformer_map = beamformer_map;
S.El = El;
S.ax_ir = ax_ir;
S.ax_seg = ax_seg;
S.h_patch = h_patch;
S.h_highlight = h_highlight;
S.t_ms = t * 1000;
S.p_seg = p_seg;
S.p_polar = p_polar;
S.p_map = p_map;
S.ax_map = ax_map;
S.h_info = h_info;
S.h = h;
S.start_ms = [];
S.dragging = false;

fig.UserData = S;

%% Callback-ok beállítása
set(fig, 'WindowButtonDownFcn',   @onMouseDown);
set(fig, 'WindowButtonMotionFcn', @onMouseMove);
set(fig, 'WindowButtonUpFcn',     @onMouseUp);

%% === Lokális függvények (a fájl végén, semmi más kód utánuk) ===

function onMouseDown(fig, ~)
    S = fig.UserData;
    cp = get(S.ax_ir, 'CurrentPoint');
    xl = get(S.ax_ir, 'XLim');
    yl = get(S.ax_ir, 'YLim');
    x = cp(1,1); y = cp(1,2);
    if x >= xl(1) && x <= xl(2) && y >= yl(1) && y <= yl(2)
        S.start_ms = x;
        S.dragging = true;
        set(S.h_patch, 'XData', [x x x x], 'Visible', 'on');
        fig.UserData = S;
    end
end

function onMouseMove(fig, ~)
    S = fig.UserData;
    if ~S.dragging, return; end
    cp = get(S.ax_ir, 'CurrentPoint');
    x_now = cp(1,1);
    set(S.h_patch, 'XData', [S.start_ms x_now x_now S.start_ms]);
end

function onMouseUp(fig, ~)
    S = fig.UserData;
    if ~S.dragging, return; end
    S.dragging = false;
    fig.UserData = S;

    cp = get(S.ax_ir, 'CurrentPoint');
    x_end = cp(1,1);

    t_start_ms = min(S.start_ms, x_end);
    t_end_ms   = max(S.start_ms, x_end);

    idx_start = max(1, round(t_start_ms/1000 * S.fs) + 1);
    idx_end   = min(S.Nsamples, round(t_end_ms/1000 * S.fs) + 1);

    if idx_end - idx_start < 10
        set(S.h_info, 'String', 'Túl rövid szegmens, jelölj ki nagyobbat!');
        return;
    end

    segment = S.rir(idx_start:idx_end, :);
    seg_t = (0:size(segment,1)-1)' / S.fs * 1000;

    % Kijelölt szegmens kiemelése a felső IR ploton (piros)
    t_highlight = S.t_ms(idx_start:idx_end);
    set(S.h_highlight, 'XData', t_highlight, 'YData', S.rir(idx_start:idx_end, 1));

    % Szegmens megjelenítése
    set(S.p_seg, 'XData', seg_t, 'YData', segment(:,1));
    set(S.ax_seg, 'XLim', [0 seg_t(end)]);
    title(S.ax_seg, sprintf('Szegmens: %.1f – %.1f ms', t_start_ms, t_end_ms));

    set(S.h_info, 'String', sprintf('Beamforming fut: %.1f – %.1f ms (%d minta)...', ...
        t_start_ms, t_end_ms, size(segment,1)));
    drawnow;

    % Beamforming
    run_beamforming(S, segment);

    set(S.h_info, 'String', sprintf('Kész! Szegmens: %.1f – %.1f ms. Jelölj ki újat!', ...
        t_start_ms, t_end_ms));
end

function run_beamforming(S, segment)
    segLen = size(segment, 1);

    % Új szűrőbank példány minden kijelöléshez (tiszta állapot)
    bf = radial_filter_bank(S.N, S.h, S.blockSize);

    % Zero-padding blockSize-ra
    nBlocks = ceil(segLen / S.blockSize);
    padded = zeros(nBlocks * S.blockSize, S.numChannels);
    padded(1:segLen, :) = segment;

    psi_rad_total = [];

    for b = 1:nBlocks
        idx = (b-1)*S.blockSize + (1:S.blockSize);
        data_block = padded(idx, :);

        psi = S.micArray.SHT('pinv', data_block, S.N);
        psi_rad = bf.filter(psi);
        psi_rad_total = [psi_rad_total; psi_rad]; %#ok<AGROW>
    end

  %  psi_rad_total = psi_rad_total(1:segLen, :);

    % Polar iránykarakterisztika
    s_polar_val = sum(abs(S.beamformer_polar * psi_rad_total').^2, 2);
    s_polar_val = s_polar_val / (max(abs(s_polar_val)) + eps);
    set(S.p_polar, 'RData', abs(s_polar_val));

    % 2D térkép
    s_map_val = sum(abs(S.beamformer_map * psi_rad_total').^2, 2);
    s_map_val = s_map_val / (max(abs(s_map_val)) + eps);
    map_dB = 20*log10(abs(reshape(s_map_val, size(S.El,1), size(S.El,2))) + eps);
    set(S.p_map, 'CData', map_dB);
    caxis(S.ax_map, [-20 0]);

    drawnow;
end
