function alpsTrajRec(spkLoc,pageSize,pageBufCount,...
    measChanList,playChanList,delSa)

% function alpsTrajRec(spkLoc,pageSize,pageBufCount,
% measChanList,playChanList,delSa)
% Records Trajectory on numSpkLoc speaker locations for measChanList
% microphones on playChanList amount of channels. The system delay time can
% be entered in samples as the last argument. 

% spkLoc need to be entered like this, for example for a 8 loudspeaker set
% up, with origin in the middle of the room, 1.6 m between loudpseakers,
% room size 4.8*2.5 meters:
% [1.25 0.8 0; 0.8 2.3 0; -0.8 2.3 0; -1.25 0.8 0; -1.25 -0.8 0; -0.8 -2.3 0; 0.8 -2.3 0; 1.25 -0.8 0]


% The trajectory of a (moving) microphone is recorded using acoustic
% localisation Time Difference of Arrival(TDoA) technique: The signal is
% recorded at a microphone after it has been played over the loudspeakers.
% Then we corelate the recorded signal with the original: The peak in the 
% correlation signal indicates the delaytime of the distributed signal. 
% Assuming the speed of sound in air to be 343 m/s, we can then estimate
% the distance. 

% The function alpsSphWaveToA.m trilaterates the position of the
% microphones based on the ToA (in samples) and returns them as positions
% (in m) on a Cartesian grid. The loudspeaker positions can be arbitrary
% but their positions need to be on the grid. For the use with VBAP, the
% origin should be roughly equidistnce to all speakers (Sweet spot)
%The Cartesian  coordinates are right handed, corresponding 
% to an anticlockwise azimuth of spherical coordinates, positive elevation
% along the positive z-axis.(See the notes on VBAP) The function ignores
% non-active loudspeakers, so in case that there is only one signal, it
% returns the loudspeakers position, indicating the position of an active
% signal.

% A simple imterpolation ramps the vaues from one loudspeaker to the next
% once a  new readimg has been received.

% The test signal is band pass filtered white noise signal, between 17 500
% and 18 000 Hz. This is ideally increased to higher frequencies if the
% loudspeakers used allow it.

%%% defaults with minimum argument spkLoc %%%


% The playrec Library can be downloaded here: 
% https://github.com/PlayrecForMatlab



if nargin < 6
    % only considers the system latency as per playrec
    [~,latency] = playrec('getPlayLatency');
    delSa = latency*playrec('getSampleRate');
    pageSize = 2048;
end
% Supposing that play channels come first, and meas channels are appended:
if nargin <5
    playChanList = (1:(size(spkLoc,1)));
end
if nargin <4
    measChanList = [1]; %Choose here
end
if nargin < 3
    pageBufCount = 4;
end
if nargin < 2
    pageSize =2048;
end


% Setting variables, empty arrays, initial delays etc.
%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%
fs = playrec('getSampleRate');
c = 343; % m/s
corrMicInputs = [];
runMaxSpeed = true;
firstTimeThrough = true;
timeFigure = figure;
drawnow;
numSpkLoc = size(spkLoc,1);
numMeasMic = length(measChanList);
ToA = zeros(numSpkLoc,numMeasMic);
smoothToA = [0];
oldToA = zeros(numSpkLoc,numMeasMic);
stableToA = zeros(numSpkLoc,numMeasMic);
measmicAndInsChans = (1:numSpkLoc + numMeasMic);
playNowSamples = nan(pageSize,numSpkLoc,pageBufCount);
measBuff = [];
% delay to compensate for the time it takes to record the first page:
nextPageSamples = nan(pageSize, numSpkLoc);
pageNumList = nan(1, pageBufCount);
aveBuff = nan(numSpkLoc,numMeasMic,pageBufCount);
dist = [];
measBuffRec = [];
stableToABuff = [];
pos = zeros(1,3,numMeasMic);
posBuff = zeros(1,3,numMeasMic);
startPoint = 1;

% The length of the noise (will be looped when over)
fileLength = fs*30;
endPoint = fileLength;
oldPos = zeros(1,3);
posNmin1 = zeros(1,3);
nanFillPos = zeros(1,3);
steps = [];
step = 0;



% Preparing the noise in nextPageSample 
startingTime = 0.5*pageSize;
noise = (randn((fileLength)+startingTime, numSpkLoc));

b = fir1(pageSize,17000/(fs/2), 'high');       % make a second one to bandpass
b2=  fir1(pageSize,20000/(fs/2), 'low');
nb = fftfilt(b,noise);
nb = fftfilt(b2,nb)*0.065;

% Here take off starting time from beginning: (avoids clips and pops)
nb = nb(startingTime+1:end,:);
startPoint = 1;
fileSizeNoise = size(nb);
fileLengthNoise = fileSizeNoise(1);

% truncating what doesn't fit into multiples of pageSize:
remainder = rem(fileLengthNoise,pageSize);
endPoint = fileLengthNoise-remainder;

% Here we fade the signal in and out, so it doesn't clip in the loop
fadeLength = pageSize; % fade-length in samples
sample = 1/pageSize;
rampUp = [sample:sample:sample*fadeLength]';
rampDown = flipud(rampUp);
rampBuffUp = repmat(rampUp,1,numSpkLoc);
rampBuffDown = repmat(rampDown,1,numSpkLoc);

fadeIn = rampBuffUp.*nb(startPoint:fadeLength,:);
fadeOut = rampBuffDown.*nb(endPoint+1-fadeLength:endPoint,:);
nb(startPoint:fadeLength,:) = fadeIn;
nb(endPoint+1-fadeLength:endPoint,:) = fadeOut;

% uncomment if you want a .wav recording of the test signal:
% wavwrite(nb, fs, 'ultraNoiseBuff');

count = 0;

%%%%%% Loop %%%%%%%
%%%%%%%%%%%%%%%%%%%


% Here is where all the playrec stuff is happening. For more details, see
% http://playrec.co.uk %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while(ishandle(timeFigure))
    % for count = 1:(fileLength/pageSize)
    %        count = count + 1;
    if(~isnan(nextPageSamples)) % If there is something in nextPageSamples
        playNowSamples(:,:,pageBufCount+1)=nextPageSamples(:,1:numSpkLoc);
    else % If there isn't anything in nextPageSamples
        playNowSamples(:,:,pageBufCount+1) = [zeros(pageSize, numSpkLoc)];
    end
    if count == numSpkLoc
        count = 0;
    end
    play = playNowSamples(:,:,pageBufCount+1);
    pageNumList = [pageNumList playrec('playrec',play,playChanList,-1,measChanList)];
    
    if(runMaxSpeed)
        while(playrec('isFinished', pageNumList(1)) == 0)
        end
    else
        playrec('block', pageNumList(1));
    end
    
    if(firstTimeThrough)
        playrec('resetSkippedSampleCount');
        firstTimeThrough = false;
    else
        if(playrec('getSkippedSampleCount'))
            fprintf('%d samples skipped!!\n', playrec('getSkippedSampleCount'));
            firstTimeThrough = true;
        end
    end
    
    % Getting what's on the measMic AND what's going to be played on the next page:
    samplesRecorded = (playrec('getRec', pageNumList(1)));
    
    if~isnan(samplesRecorded)
        % If measuring channels start with channel 1
        measBuff = samplesRecorded(:,measChanList);
    end

    % Once the file length has been reached, loops back to the begining
    if startPoint >  endPoint
        startPoint = 1;
    end

    startSample = startPoint;
    endSample = min(startSample + pageSize - 1, endPoint);
    sliceOfUltraNoise = nb(startSample : endSample,:);
    startPoint = startPoint + pageSize;
    nextPageSamples = [nextPageSamples, sliceOfUltraNoise];

    %%%%% Correlation %%%%%%%
    if( (length(measBuff) == pageSize) && (~isempty(playNowSamples)))
        for ch =1:numSpkLoc;
            playInsBuff = playNowSamples(:,ch);
            
            % Nested for-loop iterations for each microphone
            for mic = 1:numMeasMic;
                meas = measBuff(:,mic);
                corrMicInputs = real(ifft(conj(fft(meas,length(playInsBuff))).*fft(playInsBuff)));
                corrMicInputs = double(corrMicInputs);
                [peak, max_index] = max(corrMicInputs);
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                %%%%%%%%%%  Tune HERE! %%%%%%%%
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                % If peak is small, there is no signal,
                if peak < 0.0005
                    ToA(ch,mic) = NaN;
                else
                    ToA(ch,mic) = pageSize - (max_index+delSa);
                end
            end
        end
    end
    
    measBuffRec = [measBuffRec; measBuff];
    
    if ishandle(timeFigure)
        
        % plot(measBuff);
        ylim([-1 1])
        xlim([1 pageSize])
    end
    
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%  Tune HERE! %%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    % DEVIATION AVERAGING FILTER
    % adding a page to the average buffer
    aveBuff(:,:,pageBufCount+1) = ToA;
    average =  rms(aveBuff,3);
    deviation = std(aveBuff,0,3);
    % replace in ToA with NaN's
    ToA(deviation > 50) = [NaN];
    % and in the average:
    average(deviation > 20) = [NaN];
    % deleting the first page
    aveBuff = aveBuff(:,:,2:end);
    playrec('delPage', pageNumList(1));
    pageNumList = pageNumList(2:end);
    nextPageSamples(:, 1: numSpkLoc)= [];
    % playNowSamples(:, 1:numSpkLoc) = [];
    playNowSamples = playNowSamples(:,:,2:end);
    
    
    %   Filter ignoring big jumps
    absToA = abs(ToA); % keeping the original ToA
    difference = oldToA - absToA;
    difference = abs(difference);
    %   replacing NaN's with values:
    indexOfNaNs = isnan(difference);
    oldToA(indexOfNaNs) = absToA(indexOfNaNs);
    indexOfjumpySignal = difference > 50;
    absToA(indexOfjumpySignal) = [NaN]; %
    stableToA = absToA;
    indicesOfTheseNaNs = isnan(stableToA);
    stableToA(indicesOfTheseNaNs) = absToA(indicesOfTheseNaNs);
    oldToA = stableToA;
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%  Tune HERE! %%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    % Filter weighting the previous value more than new value
    smoothToA = smoothToA*0.9 + ToA*0.1; %This needs Tuning!!
    indicesOfNaNs = isnan(smoothToA);
    smoothToA(indicesOfNaNs) = ToA(indicesOfNaNs); % i.e. replacing the NaN values with unfiltered readings.
    
    % prefering a filter which has a signal over one which sends a NaN
    noSignalInStable =  isnan(stableToA);
    stableToA(noSignalInStable) = smoothToA(noSignalInStable);
    % repeat steps for the NaN's introduced by smoothToA (filtered twice)
    noSignalInStable =  isnan(stableToA);
    stableToA(noSignalInStable) = average(noSignalInStable);
    
    % Arrange readings into spkLoc columns 
    dist = stableToA/fs*c;  % Convert ToA to distance.
    
    display(dist);
    drawnow;

    
    for cha = 1:numMeasMic
        % Send only spkLoc with values!
        activeSpkLoc = find(~isnan(stableToA(:,cha)));
        if activeSpkLoc
            pos(:,:,cha) = alpsSphWaveToA(stableToA(activeSpkLoc,cha), spkLoc(activeSpkLoc,:), c);
        else
            pos(:,:,cha) = [NaN NaN NaN];
        end
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%% Smoothing %%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    

 
    % Fill NaN's with the last avaialable reading
    if step == 0 % this is not an interpolation %
        if isnan(pos)
            % we lost signal, we use the last good value we had.
            posBuff =  [posBuff;oldPos];
        else % fill the buffer, but only if
            if  isequal(oldPos,pos)
                % Maybe some caution needed? what if this new reading is humbug?
                posBuff =  [posBuff;pos]; %
            end
        end
        if ~isequal(oldPos, pos) & ~isnan(pos) % only happens when step is not zero 
            % prepare for interpolation
            aimPos = pos;
            difPos = pos - oldPos;
            % Euclidean distance:
            distDifPos= sqrt(difPos(1)^2+difPos(2)^2 +difPos(3)^2);
            steps = ceil(distDifPos *(fs/pageSize));
            fillPos = [linspace(oldPos(1),aimPos(1),steps); linspace(oldPos(2),aimPos(2),steps); linspace(oldPos(3),aimPos(3),steps)];
            fillPos = fillPos';
            step = 1; %ensuring it will not run through here for a while.
        end
        % updates oldPos if it is a number.
        if ~isnan(pos)
            oldPos = pos;
        end
        
    else % it is an interpolation
        
        posBuff = [posBuff;fillPos(step,:)];
        step = step + 1;
        
        if step >= steps % the interpolation has finished.
            step = 0; %reset
        end
    end   
end


%    end
stableToAbackup = stableToA';
stableToABuff = [stableToABuff;stableToAbackup];

if isnan(corrMicInputs)
    %  do nothing!
else
    %       display(x);
end

posBuff = posBuff(pageBufCount+2:end,:); % Why 2 ??

% The audio on the measBuff is written to a .wav file
wavwrite(measBuffRec,fs,24,'measBuffRec');
save('posBuff.mat','posBuff');
clear measBuffRec;
clear smoothToA;
clear stableBuff;
end



% alpsTrajRec records a trajectory in a multi-loudspeaker set.up using acoustic
% localisation The author of this script is Dominik Schlienger ? 2016
% Copyright 1992-2016 The FreeBSD Project. All rights reserved.
% 
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:  
% 
%     Redistributions of source code must retain the above copyright
%     notice, this list of conditions and the following disclaimer. 
%     Redistributions in binary form must reproduce the above copyright
%     notice, this list of conditions and the following disclaimer in the
%     documentation and/or other materials provided with the distribution. 
% 
% THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
% ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
% PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.  
% The views and conclusions contained in the software and documentation are
% those of the authors and should not be interpreted as representing
% official policies, either expressed or implied, of the FreeBSD Project.  