%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This script is a supplementary material for the paper:                  %
% G.Firtha and P.Fiala, Theory and Implementation of 2.5D WFS of moving   %
%    sources with arbitrary trajectory, in proceedings of DAGA2018        %
%                                                                         %
%  The script generates driving functions for a circular SSD from an      %
%  arbitrary input sound file, being the excitation signal of a moving    %
%  point source on an arbitrary trajectory                                %
%  Outputs:                                                               %
%   d_wfs: i.-th column contains the i.-th loudspeaker's driving signal   %
% (c) 2018 by Gergely Firtha                                              %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear
close all
addpath('Files')
c = 343.1;
%% User defined parameters
% Virtual source properties
v = 10;                                       % Virtual source velocity [m/s]
[in,fs] = audioread('Samples/fire.mp3');      % Source excitation signal

% Anchor points for source trajectory
x_a = [  3   3  3  2.5   1.5    0  -50;  %x_coordinates [m]
        -50  -1   0   1.5    2.5    3   3 ]; %y_coordinates [m]


% SSD properties
N_ssd =   90;                                 % Number of loudspeakers
Rssd  =   2;                                  % Radius of circular SSD
Rref  =   0.1;                                % Radius of reference circle
AA_filter = 'off';                            % Antialiasing filtering enabled (on/off)
AA_type   = 'freq_domain';                    % AA filter type: freq_domain/time_domain

subsamp = 200;                                % subsampling parameter of source trajectory
% Create circular SSD
fi = (0:2*pi/N_ssd:2*pi-2*pi/N_ssd)';
x0 =  [ cos(fi)  sin(fi) ]*Rssd;
n0 = -[ cos(fi)  sin(fi) ];
v0 =  [ sin(fi) -cos(fi) ];
% Get actual driving function lengths
[ p,xp,yp ] = make_path( x_a(1,:), x_a(2,:), 150 );
Tsim = p(end)/v;
Nt = floor(Tsim*fs/subsamp)*subsamp;
t = (0:Nt-1)'/fs;
ts = t(1:subsamp:end);
in = sum(in(1:Nt),1)';
% Get source trajectory as the function of time t
xs = get_trajectory( p,xp,yp, ts, v );
f = figure('units','normalized','outerposition',[0 0 1 1]);
p1 = plot(xs(:,1),xs(:,2));
hold on
draw_ssd( p1, x0(1:1:end,:), n0(1:1:end,:), 0.04 );
axis equal tight
grid on
xlim( [-Rssd-10,Rssd+10] );
ylim( [-Rssd-10,Rssd+10] );
plot(x_a(1,:),x_a(2,:),'ok')
ps = plot(xs(1,1),xs(1,2),'.k','MarkerSize',20);
title('SSD geometry and source trajectory')
xlabel('x -> [m]')
ylabel('y -> [m]')
drawnow
clear xp yp p
%%
% Calculate initial source position/propagation time delay for each SSD element at t = 0
Tau0  = get_initial_position( v,c, x_a, x0 );
% Get amplitudes and delays at time instants ts
[ A, Tau, wc ] = get_amps_and_taus( ts, x0,n0,v0, xs, Tau0,c, Rref );
% Filter input signal with ideal WFS prefilters and apply amplitude and delays
w = 2*pi*fftshift( (-Nt/2:Nt/2-1)'/(Nt)*fs );
s_wfs = ifft( sqrt(1i*w/(c*2*pi)).*fft(in) );
%%
d_wfs = zeros(subsamp*size(Tau,1),length(x0));
wb = waitbar(0,'Calculating driving functions');
for n = 1 : length(x0)
    waitbar(n/length(x0),wb);
    d_wfs(:,n) = interp1( t,in, t-interp1(ts,Tau(:,n),t), 'linear','extrap' ).*interp1(ts,A(:,n),t, 'linear','extrap');
end
close(wb);
clear A Tau w
%%
if strcmp(AA_filter,'on')
    if strcmp(AA_type,'freq_domain')
        % Frequency domain antialiasing filtering
        wlen = 2048;
        hop = wlen/4;
        nfft = wlen;
        d_wfs  = anti_aliasing_fd(fs,x0,d_wfs,ts,wc,wlen,hop,nfft);
    elseif strcmp(AA_type,'time_domain')
        % Time domain antialiasing filtering
        Nfil = 200;
        step = 4096;
        d_wfs = anti_aliasing_td( d_wfs, t,ts, wc, Nfil, step );
    end
elseif strcmp(AA_filter,'off')
end
d_wfs = real(d_wfs);
%%
SOFAstart;
hrtf = SOFAload('FABIAN_HRIR_measured_HATO_0.sofa');
fs_hrtf = hrtf.Data.SamplingRate;
hrtfData = hrtf.Data.IR;
sourcePosition = hrtf.SourcePosition;

LS_pos = [fi zeros(length(fi),1)];
indz = zeros(size(LS_pos,1),1);
for n = 1 : size(LS_pos,1)
    [~,ind] = min( sum(  (bsxfun(@minus, sourcePosition(:,1:2) ,mod(LS_pos(n,:)*180/pi,360))).^2,2  ) );
    indz(n) = ind;
end
HRTFs = hrtfData(indz,:,:);
%%
if (fs-fs_hrtf)>1e-6
    HRTF_res = zeros(size(HRTFs,1),size(HRTFs,2),ceil(size(HRTFs,3)*fs/fs_hrtf));
    for n = 1 : size(HRTFs,1)
        for m = 1 : size(HRTFs,2)
            HRTF_res(n,m,:) = resample(HRTFs(n,m,:),fs,fs_hrtf);
        end
    end
    HRTFs = HRTF_res;
end
%%
output = zeros( size(d_wfs,1), 2);
for n = 1 : size(LS_pos,1)
    output(:,1) = output(:,1) + conv( d_wfs(:,n), squeeze(HRTFs(n,1,:)) ,'same');
    output(:,2) = output(:,2) + conv( d_wfs(:,n), squeeze(HRTFs(n,2,:)) ,'same');
end
output = output/max(max(abs(output)));
output(isnan(output)) = 0;
%%
res = 10;
dt0 = mean(diff(ts))*res;
soundsc(output,fs)
for n = 1 : floor(length(ts)/res)
set(ps,'XData',xs( (n-1)*res+1,1));
set(ps,'YData',xs( (n-1)*res+1,2));
pause(dt0)
end