function [psd, vec] = psdCalcMLS(sig, winLen, nLap, nfft, fs)
% psdCalcMLS 
%   psdCalcMLS calculate Pwelch PSD

%   Project: IMPACT
%   Title of Code: psdCalcMLS
%   Relevant additional documentation:
%   Developer: Carlos Gonzalez
%   External Dependencies: NA
%
%   Disclaimer
%   CAUTION: It is strongly recommended that changes to this code
%   should be avoided without first consulting with authors.


psd = [];
vec = [];

% error checking
if winLen > length(sig)
    errordlgMLS('PSD window length is greater than number of data points.');
    return;
end


% change signal into a column vector
sig = sig(:);
% window length
winLen = round(winLen);
% nfft
nfft = round(nfft);
% generate window data
winData = getWindow(winLen);

% Divide sig into k segments
k = (length(sig)- nLap)./(winLen - nLap);
k = floor(k);

pGram = zeros(1, nfft); % periodograms
noLap = winLen - nLap; % no overlap
for m=1:k
    startInd = (m-1)*noLap+1;
    endInd = startInd+winLen-1;
    vecInd = round(startInd:endInd); % round to integer
    sigSeg = sig(vecInd); % signal segment

    % windowed data
    sigW = sigSeg.*winData;

    % auto spectrum
    if length(sigW) > nfft,
        sigW = wrapData(sigW,nfft);
    end
    sigFFT = fft(sigW,nfft);
    
    coeff = winData'*winData; % coefficient for auto spectrum
    P = sigFFT.*conj(sigFFT)/coeff;      % Auto spectrum.
    pGram = pGram + P';
end
% mean periodogram
pGramM = pGram./k;
vec = getPsdFreq(nfft, fs);
% psd = pGramM./fs;
[psd, vec] = getPsd(pGramM', vec, nfft, fs);


function sig = wrapData(sig, nRows)

nSig = numel(sig);
if rem(nSig, nRows)
   nCols = ceil(nSig/nRows);
   nSigN = nRows*nCols;
   sig(nSig+1:nSigN) = zeros(1, nSigN-nSig);
end

sig = reshape(sig, nRows, []);
sig = sum(sig, 2);




function [Pxx, w] = getPsd(Sxx,w,nfft,Fs)

w = w(:); % Make sure we always returns a column vector for frequency

if rem(nfft,2),
    select = 1:(nfft+1)/2;  % ODD
    Sxx_unscaled = Sxx(select,:); % Take only [0,pi] or [0,pi)
    Sxx = [Sxx_unscaled(1,:); 2*Sxx_unscaled(2:end,:)];  % Only DC is a unique point and doesn't get doubled
else
    select = 1:nfft/2+1;    % EVEN
    Sxx_unscaled = Sxx(select,:); % Take only [0,pi] or [0,pi)
    Sxx = [Sxx_unscaled(1,:); 2*Sxx_unscaled(2:end-1,:); Sxx_unscaled(end,:)]; % Don't double unique Nyquist point
end
w = w(select);

% Compute the PSD [Power/freq]
Pxx = Sxx./Fs; % Scale by the sampling frequency to obtain the psd


function w = getPsdFreq(nfft, Fs)

% Compute the whole frequency range [0,2pi) - this avoids round off errors.
wend = Fs;

w = wend/nfft*(0:nfft-1);

% There can still be some minor round off errors.  Fix the known points,
% those near pi and 2pi.
if rem(nfft, 2)
    w((nfft+1)/2) = wend/2-wend/(2*nfft);
    w((nfft+1)/2+1) = wend/2+wend/(2*nfft);
else
    % Make sure we hit pi or Fs/2 exactly for the 1/2 nyquist point.
    w(nfft/2+1) = wend/2;
end
w(nfft) = wend-wend/nfft;
w = w(:);  % Return a column vector.


function winData = getWindow(winLen)

if ~rem(winLen,2)
    % Even length window
    half = winLen/2;
    winData = calcWindow(half,winLen);
    winData = [winData; winData(end:-1:1)];
else
    % Odd length window
    half = (winLen+1)/2;
    winData = calcWindow(half,winLen);
    winData = [winData; winData(end-1:-1:1)];
end


function w = calcWindow(m,n)

%    w = (54 - 46*cos(2*pi*(0:m-1)'/(n-1)))/100;
a0 = 0.54;
a1 = 0.46;
a2 = 0;
a3 = 0;
a4 = 0;

x = (0:m-1)'/(n-1);
w = a0 - a1*cos(2*pi*x) + a2*cos(4*pi*x) - a3*cos(6*pi*x) + a4*cos(8*pi*x);
