function [ ] = nickFunction02( dataCell, fs, program_accel, program_velocity, trimPointsStart, movingAverageSize )

% step 0: input cellArray of IMU data with all rows using the Matlab inport tool
% step 1: convert to matrix of floats
% step 2: detrend the data to remove DC offset
% step 2: obtain welch spectrum, plot both time and freq domains
% step 3: apply low pass filter, plot both time and freq domains
% step 4: integrate the filtered data to obtain velocity estimate
close all

% Input data format
% 1. sample #
% 2. accel x 
% 3. accel y
% 4. accel z
% 5. ...

%   Detailed explanation goes here



%% Step 1: convert cellArray to floatArray

% the number of columns to read from cellarray
readColumns = [1,2,3];

% convert from cell array to matrix
dataImport = cell2mat(dataCell(trimPointsStart:end, readColumns));
numSamples = linspace(0,size(dataImport,1),size(dataImport,1))';

% [sample#, accel x, accel y, accel z....]
dataFloat = [numSamples, dataImport];

accel = zeros(size(dataImport,1),3);

% convert from m/sec^2 into mm/sec^2
for i = 1:3
    accel(:,i) = dataFloat(:,i+1).*-1000;
end

% figure(1)
% plot(numSamples,accel(:,1), 'r-', numSamples, accel(:,2), 'g-', numSamples, accel(:,3),'b-')
% title('Raw Sensor Data')
% xlabel('Samples');
% ylabel('Acceleration Magnitude (mm/s^2)');
% legend('X accel', 'Y accel', 'Z accel', 'best');

program_accel_line = ones(1,size(dataImport,1)).*program_accel;
program_velocity_line = ones(1,size(dataImport,1)).*program_velocity;

%% Step 2: detrend the data
accel_detrend = zeros(size(dataImport,1),3);

for i = 1:3
    accel_detrend(:,i) = detrend(accel(:,i));
end

figure(2)
plot(numSamples,accel_detrend(:,1), 'r-', numSamples, accel_detrend(:,2), 'g-', numSamples, accel_detrend(:,3),'b-')
title('Raw Sensor Data')
xlabel('Samples');
ylabel('Acceleration Magnitude (mm/s^2)');
legend('X accel', 'Y accel', 'Z accel', 'best');


%% Step 2: obtain welch spectrum

% sampling freq of signal
%fs = 45; %[Hz]

% length of DFT
nfft = 256;

% time values form input signal
time = 1/fs.*numSamples; % [sec]

% length of window 
window = hamming(nfft);

% number of overlapped samples
noverlap = length(window)/2;

% PDS of raw and detrend data
for j =1:3
    %[accel_welch(:,j),f] = pwelch(accel(:,j), window, noverlap, nfft, fs);
    [accel_detrend_welch(:,j),f_detrend] = pwelch(accel_detrend(:,j), window, noverlap, nfft, fs);
end

% figure(3)
% plot(f, 10*log10(accel_welch(:,1)), 'r-', f, 10*log10(accel_welch(:,2)), 'g-', f, 10*log10(accel_welch(:,3)), 'b-')
% title('Welch Method - Raw Data');
% xlabel('Frequency (Hz)')
% ylabel('Magnitude (dB)')
% legend('X axis', 'Y axis', 'Z axis', 'best');
% grid

figure(4)
plot(f_detrend, 10*log10(accel_detrend_welch(:,1)), 'r-', f_detrend, 10*log10(accel_detrend_welch(:,2)), 'g-', f_detrend, 10*log10(accel_detrend_welch(:,3)), 'b-')
title('Welch Method - Detrend Data');
xlabel('Frequency (Hz)')
ylabel('Magnitude (dB)')
legend('X axis', 'Y axis', 'Z axis', 'best');
grid


%% Step 03: Filter data

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

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

figure(10)
plot(F,10*log10(H),'-');
xlabel('Frequency (Hz)')
ylabel('Magnitude (dB)')
title('Frequency Response of Moving Average Filter');

% Apply moving average filter to detrend accel data
accel_detrend_filtered = filter(b,a,accel_detrend);

figure(5)
hold on 
plot(numSamples,accel_detrend_filtered(:,1), 'r-', numSamples, accel_detrend_filtered(:,2), 'g-', numSamples, accel_detrend_filtered(:,3),'b-')
plot(numSamples, program_accel_line, 'k--');
hold off
xlabel('Samples');
ylabel('Acceleration Magnitude (mm/s^2)');
title('Filtered Sensor Data')
legend('X accel', 'Y accel', 'Z accel','Programmed Accel',  'best');

%% Step 04: Numerical integration

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

velocity = cumtrapz(accel_detrend).*(1/fs);
velocity_filtered = cumtrapz(accel_detrend_filtered).*(1/fs);

% figure(6)
% hold on
% plot(time, velocity(:,1), 'r-',time, velocity(:,2), 'g-', time, velocity(:,3), 'b-')
% plot(time, program_velocity_line, 'k--');
% hold off
% xlabel('Time (sec)')
% ylabel('Velocity (mm/s)')
% title('Numerical Integration - Trapezoid Rule');
% legend('X velocity', 'Y velocity', 'Z velocity', 'Programmed Velocity',  'best');

figure(7)
hold on
plot(time, velocity_filtered(:,1), 'r-',time, velocity_filtered(:,2), 'g-', time, velocity_filtered(:,3), 'b-')
plot(time, program_velocity_line, 'k--');
hold off
xlabel('Time (sec)')
ylabel('Velocity (mm/s)')
title('Numerical Integration Filtered - Trapezoid Rule');
legend('X velocity', 'Y velocity', 'Z velocity', 'best');



%% Step 05: total magnitude 

% calculate the magnitude from individual velocity estimates
velocity_magnitude = sqrt(velocity(:,1).^2 + velocity(:,2).^2 + velocity(:,3).^2);
velocity_magnitude_filtered = sqrt(velocity_filtered(:,1).^2 + velocity_filtered(:,2).^2 + velocity_filtered(:,3).^2);

figure(8)
hold on
plot(time, velocity_magnitude, 'r-', time, velocity_magnitude_filtered, 'b-')
plot(time, program_velocity_line, 'k--');
hold off
xlabel('Time (sec)')
ylabel('Velocity (mm/s)')
title('Velocity Magnitude');
legend('Velocity - raw', 'Velocity - filtered', 'Programmed Velocity', 'best');


end