% Script to reconstruct light sensor measurements taken with Webots
% EDM, 7 April 2014

function ReconstructionWebots
original_signal=load('controllers/e-puck_ls/ls_values.txt');
original_signal(original_signal(:,1)>10,:)=[];  % keep only 10 initial seconds
t=original_signal(:,1);
y=original_signal(:,2);
y=y-mean(y);
fs=1/(t(2)-t(1));

% Plot original signal
figure;
subplot(3,1,1);
stem(t,y);
xlabel('Time [s]');
title('Original signal');

% Reconstruct signal
reconstructFreq=1000;
tr=0:1/reconstructFreq:max(t);
yr=linearSignalReconstruction(tr,t,y);
%yr=wsSignalReconstruction(tr,t,y);

% Plot reconstructed signal
subplot(3,1,2);
plot(tr,yr);
xlabel('Time [s]');
title('Reconstructed signal');

% Plot FFT of reconstructed signal
subplot(3,1,3);
fty=abs(fft(yr)/length(yr));
f=reconstructFreq*linspace(0,1,round(length(yr)/2));
fty=fty(1:length(f));
Fmax=20;
maxIndex=find(f>Fmax,1);
plot([-f(maxIndex:-1:2) f(1:maxIndex)], [fty(maxIndex:-1:2) fty(1:maxIndex)]);
xlabel('Frequency [Hz]');
title('FFT of reconstructed signal');


%Function to reconstruct the signal
function yr=wsSignalReconstruction(tr,ts,ys)
%tr      Resulting timeline
%ts     Sampled timeline
%ys     Sampled signal
if(length(tr)<5*length(ts))
    error('Reconstructed signal must have a frequency 5 times bigger than the sampling frequency');
end
Ts=(ts(2)-ts(1));
yr=zeros(1,length(tr));
for j=1:length(tr)
    for k=1:length(ys)        
        shift=tr(j)-ts(k);
        yr(j)=yr(j)+sinc(shift/Ts)*ys(k);
    end
end

%Function to reconstruct the signal
function yr=linearSignalReconstruction(tr,ts,ys)
%tr      Resulting timeline
%ts     Sampled timeline
%ys     Sampled signal
if(length(tr)<5*length(ts))
    error('Reconstructed signal must have a frequency 5 times bigger than the sampling frequency');
end
k=0;
Ts=(ts(2)-ts(1));
Tr=(tr(2)-tr(1));
yr=zeros(1,length(tr));
for j=1:length(ts)-1
    for k=k+1:k+round(Ts/Tr)
        yr(k)=ys(j)+(ys(j+1)-ys(j))*(tr(k)-ts(j))/Ts;
    end
end



        
