function[] =  pathTrajectory()

% 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

close all

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

t1 = 10; % sec
t2 = t1+2; % sec
t3 = t2+10; % sec

del_t1 = t1 - 0; % sec 
del_t2 = t2 - t1; % sec
del_t3 = t3 - t2; % sec

% True profile
f = [0*ones(del_t1*fs,1);accel*ones(del_t2*fs,1);0*ones(del_t3*fs,1)];

time = linspace(0,t3,size(f,1))';

figure(1)
plot(time, f, 'r')
axis([min(time) max(time) -accel-100 accel+100]);
xlabel('Time (sec)');
ylabel('Acceleration (mm/sec^2)');
title('Original');

% Add linear offset
offset = 0; % mm/sec^2
f_offset = f + offset;

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

figure(2)
plot(time, g, 'g')
axis([min(time) max(time) -accel-100 accel+100]);
xlabel('Time (msec)');
ylabel('Acceleration (mm/sec^2)');
title('Noisy');


%% Denoise signal

fc = denoisetv(g,.5);
figure(3);
plot(time, fc,'b');
title('De-noised');
axis([min(time) max(time) -accel-100 accel+100]);
xlabel('Time (msec)');
ylabel('Acceleration (mm/sec^2)');


    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


%% Velocity Estimation

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

vel_estimate = cumtrapz(fc).*(1/fs);

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

figure(4);
plot(time, vel_profile, 'b', time, vel_estimate, 'r');
%plot(time, vel_estimate, 'b');
%axis([min(time) max(time) -vel_terminal vel_terminal+100]);
xlabel('Time (sec)');
ylabel('Velocity (mm/sec)');
title('Velocity Estimate');
legend('vel profile', 'vel estiamte');

% Error plot
error_percent = (vel_estimate - vel_profile);
figure(5);
plot(error_percent);
%axis([min(time) max(time) -100 100]);
xlabel('Time (sec)');
ylabel('Percent Error (%)');
title('Error');


end

