function[] =  pathTrajectory02(input)

% 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

%% Custom colors for plotting

%Dark red
r2 = [0.6902 0.0275 0.2706];
g2 = [59/255 69/255 150/255];
b2 = [7/255 176/255 159/255];

%% Create signal
fs = 100;           % Hz

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

% rename the variable
g = input;

% figure(1)
% plot(time,  g, 'm', time, input, '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.005;

% 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 = 150;

% 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, input, 'k', time, accel_MVA, 'b', time, accel_denoise,'r');
title('Filtering');
%axis([min(time) max(time) min(accel)-10 max(accel)+10]);
xlabel('Time (msec)');
ylabel('Acceleration (mm/sec^2)');
legend('Input Signal', 'Moving Average', 'De-noise', '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)]; % [mm/sec]
 
% Plot velocity profile and velocity estimates
figure(3);
plot(time, vel_estimate(:,1), 'r', time, vel_estimate(:,2), 'b');
title('Velocity');
xlabel('Time (msec)');
ylabel('Velocity (mm/sec)');
legend('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

