function[] =  pathTrajectory01()

% Description: 
% create an acceleration profile with a step input, then add an offset and white noise before filtering
% after filtering with the denoise function, use numerical integration to calcaute the velocity

% The input acceleration signal is an alternating sqaure wave to produce a trapezoid
% velicty profile that should return to zero velocity at the end.
close all

%% Create signal
fs = 100;           % Hz
accel = 150;        % mm/sec^2
vel_terminal = 200;     % mm/sec
vel_initial = 0;

pulse_on = 25;
pulse_off = 10;

% time series for input signal
t1 = 10; % sec
t2 = t1 + pulse_on;         % [sec]
t3 = t2 + 2*pulse_off;      % [sec]
t4 = t3 + pulse_on;         % [sec]
t5 = t4 + 2*pulse_off;      % [sec]
t6 = t5 + pulse_on;         % [sec]
t7 = t6 + 2*pulse_off;      % [sec]
t8 = t7 + pulse_on;         % [sec]
t9 = t8 + pulse_off;        % [sec]

% segment time series to create step inputs
del_t1 = t1 - 0;            % [sec] - no motion
del_t2 = t2 - t1;           % [sec] -  constant accel step input
del_t3 = t3 - t2;           % [sec] - no accel, constant velocity
del_t4 = t4 - t3;           % [sec] - constant deccel step input
del_t5 = t5 - t4;           % [sec] - no accel, constant velocity (zero)
del_t6 = t6 - t5;           % [sec] -  constant accel step input
del_t7 = t7 - t6;           % [sec] - no accel, constant velocity
del_t8 = t8 - t7;           % [sec] - constant deccel step input
del_t9 = t9 - t8;           % [sec] - no accel, constant velocity (zero)

% True acceleration profile
f = [0*ones(del_t1*fs,1);
     accel*ones(del_t2*fs,1);
     0*ones(del_t3*fs,1);
     -accel*ones(del_t4*fs,1);
     0*ones(del_t5*fs,1);
     -accel*ones(del_t6*fs,1);
     0*ones(del_t7*fs,1);
     accel*ones(del_t8*fs,1);
     0*ones(del_t9*fs,1)
     ];

% time span
time = linspace(0,t9,size(f,1))'; % [sec]


%% Add offset and noise to input signal

% Add linear offset
offset = 00.0;          % [mm/sec^2]
f_offset = f + offset;

% Sensor RMS noise
nu = 0.478;

% Add white noise
g = f_offset + nu*randn(length(f),1);



figure(1)
plot(time,  g, 'm', time, f, 'k')
axis([min(time) max(time) min(accel)-10 max(accel)+10]);
title('Input Signals');
xlabel('Time (msec)');
ylabel('Acceleration (mm/sec^2)');
legend('Noisy signal', 'True signal', 'Location', 'best');


%% Denoise signal
mu = 0.1;

% Apply the de-noise filter
accel_denoise = denoisetv(g,mu);

    function f = denoisetv(g,mu)
    I = length(g);
    u = zeros(I,1);
    y = zeros(I,1);
    rho = 10;

    eigD = abs(fftn([-1;1],[I 1])).^2;
    
        for k=1:100
            f = real(ifft(fft(mu*g+rho*Dt(u)-Dt(y))./(mu+rho*eigD)));
            v = D(f)+(1/rho)*y;
            u = max(abs(v)-1/rho,0).*sign(v);
            y = y - rho*(u-D(f));
        end
    end

    function y = D(x)
    y = [diff(x);x(1)-x(end)];
    end

    function y = Dt(x)
    y = [x(end)-x(1);-diff(x)];
    end

%% Moving Average Filter

% Number of sample points to use in moving average filter
movingAverageSize = 250;

% Define moving average filter
a = 1;
b = ones(1,movingAverageSize)./movingAverageSize;

% Frequency response of filter
[H,F] = freqz(b,a,1024,fs);

% Apply moving average filter
accel_MVA = filter(b,a,g);

figure(2);
plot( time, accel_MVA, 'b', time, accel_denoise,'r', time, f, 'k');
title('Filtering');
axis([min(time) max(time) min(accel)-10 max(accel)+10]);
xlabel('Time (msec)');
ylabel('Acceleration (mm/sec^2)');
legend('De-noise', 'Moving Average', 'True Signal', 'Location', 'best');


%% True velocity profile

% Set initial velocity condition
vel_profile(1,1) = vel_initial;

for i = 2:size(g,1)
    vel_profile(i,1) =  vel_profile(i-1,1) + f(i,1)*(time(i,1) - time(i-1, 1));
end

%% Velocity Estimation

%reference: http://www.mathworks.com/help/matlab/ref/cumtrapz.html

% Numerically integrate acceleration
vel_estimate = [cumtrapz(accel_denoise).*(1/fs), cumtrapz(accel_MVA).*(1/fs), cumtrapz(f).*(1/fs)]; % [mm/sec]
 
% Plot velocity profile and velocity estimates
figure(3);
plot(time, vel_profile, 'k', time, vel_estimate(:,1), 'r', time, vel_estimate(:,2), 'b');
title('Velocity');
xlabel('Time (sec)');
ylabel('Velocity (mm/sec)');
legend('vel profile', 'vel estiamte - denoise', 'vel estimate - MVA', 'Location', 'best');


%% Velocity Estimate Error

% Calcualte the velcity error
error_percent = [vel_estimate(:,1) - vel_profile, vel_estimate(:,2) - vel_profile, vel_estimate(:,3) - vel_profile];


% Plot the error in the velocity estiamte
figure(4);
plot(time, error_percent(:,1), 'r', time, error_percent(:,2), 'b');
%axis([min(time) max(time) -100 100]);
title('Velocity Estimate Error');
xlabel('Time (sec)');
ylabel('Error (mm/sec)');
legend('De-noise', 'Moving Average', 'Location', 'best');

error_RMS_denoise = rms(detrend(error_percent(:,1)))
error_RMS_MVA = rms(detrend(error_percent(:,2)))


end

