function[] =  pathTrajectoryCreator()

% Description: 
% Create an acceleration profile with a step input that repeats for a specific numer of cycles.
% Then add an offset and white noise before determining PSD.
% After filtering, use numerical integration to estiamte the velocity and
% compare to true value.

% 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
clear fig
clc
%% Create signal
fs = 100;           % Hz
accel = 150;        % mm/sec^2
vel_term = 200;     % mm/sec
vel_init = 0;
x_init = 10; % [mm]
x_end = 590;
delay = 5; % [sec]

% Part 1 - ramp up
t_1 = (vel_term - vel_init)/accel;
x_2 = x_init + vel_init*t_1 + 0.5*accel*t_1^2;
del_x1 = x_2 - x_init;

%Part 3 - ramp down
t_3 = t_1;
x_4 = x_end;
del_x3 = del_x1; % [mm]

% Part 2 - constant velocity
x_3 = x_4 - del_x3; % [mm]
t_2 = (x_3 - x_2)/vel_term; % [sec]
del_x2 = t_2 * vel_term; % [mm/sec]

totalTime = delay*2 + t_1 + t_2 + t_3;
totalSamples = fs*totalTime; % samples


time =     [delay, (delay+t_1), (delay+t_1+t_2), (delay+t_1+t_2+t_3)]'; 
position = [x_init, x_2, x_3, x_4, x_4]';
velocity = [vel_init, vel_term, vel_term, vel_init, vel_init]';

figure(1);
plot(position, velocity);

figure(2)
plot(time, velocity);
axis([0 max(time) min(velocity) max(velocity)]);


%% 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_init;

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