function HW4
% 
%  ENGN0040 2018
%  Solution to Homework #4 Problem 4

   close all
   filter_constant = 0.5;      % Filter coefficient
   car_mass = 1875;            % Mass of vehicle (kg)

   data = csvread('test1.csv');
   times = data(:,1);
   pos = data(:,2);
   vel = diff(pos)*1000;    % Raw velocities (we assume 1000 fps)
   accel = diff(vel)*1000;  % Raw accelerations
   % Filter the velocities
   filtered_vel(1) = vel(1);
   for i=2:length(vel)
     filtered_vel(i) = (1-filter_constant)*filtered_vel(i-1) + filter_constant*vel(i);
   end
   
   % Accelerations calculated from the filtered velocities
   accel2 = diff(filtered_vel)*1000;
   filtered_accels(1) = 0;
   for i=2:length(accel)
     filtered_accels(i) = (1-filter_constant)*filtered_accels(i-1) + filter_constant*accel2(i);
   end
   
   
    % Plot both the raw and filtered velocities on the same figure
    figure;
    hold on
    grid on 
    plot1 = plot(times(1:end-1),vel(:));
    plot2 = plot(times(1:end-1),filtered_vel(:));  
    ylabel('Speed (cm/s)');
    xlabel('Time (seconds)');
    title('Speed -v- time');  
    set(plot1(1),'DisplayName','Measured velocity');
    set(plot2(1),'DisplayName','Filtered velocity');
    axes1 = gca;
    legend1 = legend(axes1,'show');
    set(legend1,...
    'Position',[0.587500160695629 0.775803898024184 0.193856342540406 0.116949725164222],...
    'FontSize',14.4);    

    % Plot the raw and filtered accelerations on the same figure
    figure;
    hold on
    grid on
    plot1 = plot(times(2:end-1),accel(:)/100);         % the 100 is converting to m/s^2
    plot2 = plot(times(2:end-1),filtered_accels/100);
    ylabel('Acceleration (m/s^2)');
    xlabel('Time (seconds)');
    title('Acceleration -v- time');  
    set(plot1(1),'DisplayName','Measured acceleration');
    set(plot2(1),'DisplayName','Filtered acceleration');
    axes1 = gca;
    legend1 = legend(axes1,'show');
    set(legend1,...
    'Position',[0.587500160695629 0.775803898024184 0.193856342540406 0.116949725164222],...
    'FontSize',14.4); 

    % Calculate the force on the car (=ma) and integrate it
    impulse = car_mass*trapz(times(2:end-1),filtered_accels/100);
    str = compose('Impulse calculated from forces (Ns) %5g',impulse);
    disp(str)


end