% testCanceller - test script for adaptive canceller

%% Parameters
ff = 1200;       % frequency of noise sources
Tb = 25e-3;      % block length
fs = 10e3;       % sampling rate
duration = 60;   % length of test

state_canceller = [];

c = 340.29;      % speed of sound

% phasors for sources and initial values for speakers
phasor_sources = 100*ones(2,1);
phasor_speakers = zeros(4,1);

% accuracy of phasor measurements
noise_pwr = 0.000;   % <--- Try changing

% geometry parameters   <--- Try changing
source_off = 10; % x-offset of sources

seat_off_y = 10; % seat center coordinates (z=0)
seat_off_x = -1;

speaker_x = 0.25; % relative speaker positions
speaker_z = 0.125;

mic_x = 0.1;      % relative mic positions
mic_z = 0.05;

%% Coordinates of sources, speakers, and mics
sources = [+source_off, 0, 0; ...
           -source_off, 0, 0];
      
seat = [seat_off_x, seat_off_y, 0];

speakers = ones(4,1)*seat + ...
    [+speaker_x, 0 , +speaker_z; ...
     +speaker_x, 0 , -speaker_z; ...
     -speaker_x, 0 , +speaker_z; ...
     -speaker_x, 0 , -speaker_z];
 
mics = ones(2,1)*seat + ...
    [+mic_x, 0, mic_z; ...
     -mic_x, 0, mic_z];
 
 % compute distances between all source/speaker and mic pairs
 d_source_mic = zeros(2, 2);
 d_speaker_mic = zeros(2, 4);
 
 for nn = 1:2
     p_mic = mics(nn,:);
     
     for kk = 1:2
         p_source = sources(kk,:);
         
         d_source_mic(nn, kk) = norm(p_mic - p_source);
     end
     
     for kk = 1:4
         p_speaker = speakers(kk,:);
         
         d_speaker_mic(nn,kk) = norm(p_mic - p_speaker);
     end
 end
 
 %% "Channels"
 % amplitude decays proportional to 1/distance
 % phase changes according to -2*pi*ff*distance/speed_of_sound
 H_sources = 1./d_source_mic .* exp(-1j*2*pi*ff*d_source_mic/c);
 H_speakers = 1./d_speaker_mic .* exp(-1j*2*pi*ff*d_speaker_mic/c);
 
 %% the main loop
 % initialize arrays
 nBlocks = round(duration/Tb);
 samplesPerBlock = round(fs*Tb);
 tt = (0:samplesPerBlock-1)/fs;
 comp_exp = exp(1j*2*pi*ff*tt);
 
 resPowers = zeros(1,nBlocks);
 allSamples = zeros(nBlocks*samplesPerBlock, 2);
 
 for nn = 1:nBlocks
     % phasor addition of sinusoids from sources and speakers
     phasor_mics = H_sources*phasor_sources + H_speakers*phasor_speakers;
     
     % record powers and samples
     resPowers(nn) = norm(phasor_mics)^2; % power of residual
     allSamples((nn-1)*samplesPerBlock+1:nn*samplesPerBlock,:) = ...
         real(phasor_mics * comp_exp).';
     
     % measured phasor is noisy
     phasor_measured = phasor_mics + sqrt(noise_pwr/2)*(randn(2,1) + 1j*randn(2,1));
     
     % adaptation
     [phasor_speakers, state_canceller] = adaptCanceller(phasor_measured, state_canceller);
 end
 
 %% post processing: plot and sound
 figure(777)
 plot((1:nBlocks)*Tb, 10*log10(resPowers));
 grid on
 xlabel('Time (s)')
 ylabel('Power (dB)')
 
 % for some reason, MATLAB doesn't always play the entire 60 seconds
 sound(allSamples(:,1), fs);