function[ ] =  pathTrajectoryCreator()

    % optional outputs: rawSensorY, accel 
    
% Creator: Nick Raymond
% Organization: MBARI
% Date: Jul 8, 2016
% Version: 1.0.0

%xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
%
%      DESCRIPTION
%
%xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx

% This is a program to simulate and compare acceleration and velocoty profiles from 
% simualtion and experimental data collected on linear translation stage.

% Step 1: 
% Simulate an acceleration profile by specifying constant accel, 
% terminal velocity, and simulation run time. A perfect acceleration 
% profile (no AWGN or offset) is created from 9 discrete points calcaulted 
% from the kinematics of particle motion. This pure signal is then used to  
% create a larger data set that represents an acceleration profile obtained
% from an accelerometer sensor over the full simulation time, having many 
% more intermediate points data points as specifed by a desired simulation sampling freq.

% Step 2: 
% An estimate of the PSD for simulated acceleration signal is 
% created using the Welch method to look at the freq spectru. Then the
% simulated accel signal is numerically intergrated using the trapezoid rule
% to obtain a pure velocity profile (no AWGN or offset). This is the 
% simulated velocioty profile that will be compared to the epxerimental results in
% Step ##  when performing error analysis. The information from this
% simulation is used to inform the opperator about setting up the
% experiment on the translation slide such as position values, and total
% number of samples needed for programming the DAQ.

% Step 3: 
% Import the experimental data. Detrend the data, look at the PSD.

%Step 4: 
% Filter the experimental data, numerically integrate and apply a freq.
% selective filter to obtain the final experimental velocity estimate.

% Step 5:
% Compare the simulation data with the experimental data and calcualte the
% percent error as a function of time.

close all
clear fig
clc

%xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
%
%     SIMULATION DATA
%
%xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
%-------------------------------------------------------------------------
% Create simulation of acceleration and velocity profiles
%-------------------------------------------------------------------------
fs = 100;           % [Hz]
accel_const = 1.0;  % [mm/sec^2]
vel_term    = 16.0;  % [mm/sec]
vel_init    = 0;    % [mm/sec]
x_init      = 10;   % [mm]
x_end       = 590;  % [mm]
delay       = 5;    % [sec]

% File name of experimental data
fileName = 'H:\01-Jun-2011-20-24-59';

% Input the length of path simulation, in seconds
testDuration = 10*60; % [sec]

% Trajectory Part 1 - velocity ramp up
del_t1 = (vel_term - vel_init)/accel_const;
x_2 = x_init + vel_init*del_t1 + 0.5*accel_const*del_t1^2;
del_x1 = x_2 - x_init;

%Trajectory Part 3 - velocity ramp down
del_t3 = del_t1;                % [sec]
x_4 = x_end;                    % [mm]
del_x3 = del_x1;                % [mm]

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

% Part 4 - return trapezoid
del_x4 = del_x2; % [mm]
del_x5 = del_x1; % [mm]
del_x6 = del_x2; % [mm]
del_x7 = del_x3; % [mm]
del_x8 = del_x2; % [mm]


del_t4 = delay; % [sec]
del_t5 = del_t1; % [sec]
del_t6 = del_t2; % [sec]
del_t7 = del_t3; % [sec]
del_t8 = del_t2; % [sec]

t0 = 0;             % [sec]
t1 = delay;         % [sec]
t2 = t1 + del_t1;   % [sec]
t3 = t2 + del_t2;   % [sec]
t4 = t3 + del_t3;   % [sec]
t5 = t4 + del_t4;   % [sec]
t6 = t5 + del_t5;   % [sec]
t7 = t6 + del_t6;   % [sec]
t8 = t7 + del_t7;   % [sec]

x1 = x_init;        % [mm]
x2 = x1 + del_x1;   % [mm]
x3 = x2 + del_x2;   % [mm]
x4 = x3 + del_x3;   % [mm]
x5 = x4 + del_x4;   % [mm]
x6 = x5 + del_x5;   % [mm]
x7 = x6 + del_x6;   % [mm]
x8 = x7 + del_x7;   % [mm]

time_delta = [ delay, del_t1, del_t2, del_t3, del_t4, del_t5, del_t6, del_t7]'; % [sec]
time_oneCycle =     [t0, t1, t2, t3, t4, t5, t6, t7, t8]';  % [sec]
position_oneCycle = [x_init, x1, x2, x3, x4, x5, x6, x7, x8]'; % [mm]
velocity_oneCycle = [vel_init, vel_init, vel_term, vel_term, vel_init, vel_init, -vel_term, -vel_term, vel_init]';

% Initialize simulation timespan
timeSIM = time_oneCycle; 

% Initialize simulation velocity profile
velocity = velocity_oneCycle;

% Calcualte the numer of repeated cycles based on total simulation time
numCycles = ceil(testDuration/max(time_oneCycle));

% True acceleration profile made up of eight points
accel_oneCycle = [0*ones(delay*fs,1);
                  accel_const*ones(del_t1*fs,1);
                  0*ones(del_t2*fs,1);
                 -accel_const*ones(del_t3*fs,1);
                  0*ones(del_t4*fs,1);
                 -accel_const*ones(del_t5*fs,1);
                  0*ones(del_t6*fs,1);
                  accel_const*ones(del_t7*fs,1);
                 ];
              
% time span
timeAccel_oneCycle = linspace(0,t8,int16(fs*t8))'; % [sec]
accel = accel_oneCycle;
timeAccelLong = timeAccel_oneCycle;

for i=1:numCycles-1
    
    oldTime = timeSIM(end,1);
    
    for j = 1:size(time_oneCycle,1)-1
        newTime = oldTime + time_delta(j,1);
        timeSIM = [timeSIM; newTime];
        oldTime = newTime;
    end
    
    velocity = [velocity; velocity_oneCycle(2:end, 1)];
    accel = [accel; accel_oneCycle];
    timeAccelLong = [timeAccelLong; timeAccelLong(end,1)+timeAccel_oneCycle];

end

velocity_estiamte = cumtrapz(accel).*(1/fs); %[mm/sec^2]


%-------------------------------------------------------------------------
% Print out helpful information for data logging
%-------------------------------------------------------------------------
fprintf('Time for one cycle:                 %.2f [sec]\n', time_oneCycle(end,1));
fprintf('Numer of samples per cycle:         %.f [samples] \n',time_oneCycle(end,1)*fs);
fprintf('Total numer of samples:             %.f [samples]\n', timeSIM(end,1)*fs); 
fprintf('Total time required:                %.2f [sec]\n', timeSIM(end,1));
fprintf('Number of cycles in %.2f minutes:  %.f [cycles]\n\n ', testDuration/60, numCycles);
fprintf('Program Path profile: \n');
fprintf('   Accel = %.3f mm/sec^2 \n', accel_const); 
fprintf('   Vel   = %.3f mm/sec \n', vel_term); 
fprintf('   x1 = %.3f mm \n', x1);
fprintf('   x2 = %.3f mm \n', x2);
fprintf('   x3 = %.3f mm \n', x3);
fprintf('   x4 = %.3f mm \n', x4);
fprintf('   x5 = %.3f mm \n', x3);
fprintf('   x6 = %.3f mm \n', x2);
fprintf('   x7 = %.3f mm \n\n', x1);


%--------------------------------------------------------------------------
% Plot the simulated resultes
%--------------------------------------------------------------------------
fprintf('Plotting simulation results...\n');

% Custom colors for plotting
r1 = [255/255 000/255 00/255];
g1 = [000/255 204/255 051/255];
b1 = [051/255 102/255 255/255];

r2 = [0.6902 0.0275 0.2706];
g2 = [042/255 098/255 070/255];
b2 = [000/255 000/255 153/255];

r3 = [255/255 153/255 153/255];
g3 = [102/255 255/255 102/255];
b3 = [000/255 255/255 255/255];

% Control plotting for simulation results
simPlots = false;

if simPlots == true;
    figure(1);
        plot(position_oneCycle, velocity_oneCycle,'b-')         
        title('Simulated Velocity Trapezoid Profile');
        xlabel('Linear Translation Stage Position (mm)');
        ylabel('Velocity (mm/sec)');
        axis([0, 600 vel_init-10 max(velocity_oneCycle)+10]);

    figure(2)
        plot(time_oneCycle, velocity_oneCycle, 'r-');
        title('Simulated Velocity Profile - One Cycle');
        xlabel('Time (sec)');
        ylabel('Velocity (mm/sec)');
        axis([0, max(time_oneCycle) min(velocity_oneCycle)-10 max(velocity_oneCycle)+10]);

    % figure(3)
        % plot(timeSIM, velocity, 'r-');
        % title('Simulated Velocity Profile');
        % xlabel('Time (sec)');
        % ylabel('Velocity (mm/sec)');
        % axis([0, max(timeSIM) min(velocity)-5 max(velocity)+5]);

    figure(4)
        plot(timeAccel_oneCycle(1:size(accel_oneCycle,1),1),accel_oneCycle, 'b-');
        title('Simulated Acceleration Profile - One Cycle');
        xlabel('Time (sec)');
        ylabel('Acceleration (mm/sec^2)');
        axis([0, max(timeAccel_oneCycle) min(accel_oneCycle)-5 max(accel_oneCycle)+5]);

    figure(5)
        plot(timeAccelLong(1:size(accel,1),1),accel, 'b-');
        title('Simulated Acceleration Profile');
        xlabel('Time (sec)');
        ylabel('Acceleration (mm/sec^2)');
        axis([0, max(timeAccelLong(1:size(accel,1))) min(accel)-5 max(accel)+5]);

    figure(6);
        hold on
        plot(timeSIM, velocity, 'r-');
        plot(timeAccelLong(1:size(velocity_estiamte,1),1), velocity_estiamte, 'k--');
        hold off
        title('Velocity Estimate from Simulated Acceleration Input');
        xlabel('Time (sec)');
        ylabel('Velocity (mm/sec)');
        axis([0, max(timeAccelLong(1:size(velocity_estiamte,1),1)) min(velocity)-5 max(velocity)+5]);
        legend('Simulated velocity profile', 'Numerical velocity estimate', 'Location', 'best');
end

%--------------------------------------------------------------------------
% Determine PSD for simulated acceleration profile
%--------------------------------------------------------------------------
 fprintf('Compute PSD of simulated acceleration data...\n');

% Length of DFT
nfft = 8192;

% Length of window
windowWelch = hamming(nfft);

% Mumber of overlapped samples
noverlap = length(windowWelch)/2;

% PSD estimate of simulated acceleration data
[accelWelch,f_accelWelch] = pwelch(accel, windowWelch, noverlap, nfft, fs);

figure(7)
    plot(f_accelWelch, 10*log10(accelWelch), 'b')
    title('Power Spectral Density Estimate of Simulated Acceleration Profile - Y axis');
    xlabel('Frequency (Hz)')
    ylabel('Magnitude (dB)')
    xlim([0 50]); % [Hz]
    grid;
        


%%

%xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
%
%     EXPERIMENTAL DATA
%
%xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx

%--------------------------------------------------------------------------
% Import experimental data set from accelerometer
%--------------------------------------------------------------------------
fprintf('Importing experimental data...\n');
%Name of file to import

% Source file on computer >>> \\atlas.shore.mbari.org\projectlibrary\CPF\Interns\NichRaymond\Data\26_Jul_2016_Linear Translation Stage_Trapezoid\Run A_v200_a150\03_Aug_2016    
%fileName = 'C:\Users\nraymond\Downloads\Accel_Data\01-Jun-2011-00-11-08';
     %fileName = 'C:\Users\nraymond\Downloads\01-Jun-2011-02-00-36';
     
     [accelX_int16, accelX_double] = bytes2float(strcat(fileName,'_accelX.txt'));
     [accelY_int16, accelY_double] = bytes2float(strcat(fileName,'_accelY.txt'));
     [accelZ_int16, accelZ_double] = bytes2float(strcat(fileName,'_accelZ.txt'));
     
     % Raw sensor output
     accelEXP = [accelX_double, -1*accelY_double, accelZ_double]*1000; % [mm/sec^2];
      rawSensorY = -1*accelY_double;
     % Sample rate for data
     fs = 100;
     
     % Time span for experimental data
     timeEXP = linspace(0, size(accelEXP,1), size(accelEXP,1))'/fs; % [sec]

figure(8);
     hold on
     plot(timeEXP, accelEXP(:,1), 'Color', r1);
     plot(timeEXP, accelEXP(:,2), 'Color', g1);
     plot(timeEXP, accelEXP(:,3), 'Color', b1);
     title('Raw Accelerometer Output');
     xlabel('Time (sec)');
     ylabel('Acceleration (mm/sec^2)');
     legend('X - axis', 'Y - axis', 'Z - axis', 'Location', 'best');
     xlim([0, max(timeEXP)]);
     grid
    
    
%--------------------------------------------------------------------------
% Determine PSD for experimental acceleration data
%--------------------------------------------------------------------------
fprintf('Compute PSD of experimental acceleration data...\n');

% detrend the data
accelExp_X = detrend(double(accelEXP(:,1)));
accelExp_Y = detrend(double(accelEXP(:,2)));
accelExp_Z = detrend(double(accelEXP(:,3)));

% estimate PSD using Welch method
[accelExp_X,f_accelExpX] = pwelch(accelExp_X, windowWelch, noverlap, nfft, 100);
[accelExp_Y,f_accelExpY] = pwelch(accelExp_Y, windowWelch, noverlap, nfft, 100);
[accelExp_Z,f_accelExpZ] = pwelch(accelExp_Z, windowWelch, noverlap, nfft, 100);
 
figure(9)
    plot(f_accelExpX, 10*log10(accelExp_X), 'Color', r1)
    title('Power Spectral Density Estimate of Experimental Acceleration Input - X axis');
    xlabel('Frequency (Hz)')
    ylabel('Magnitude (dB)')
    xlim([0 50]);   % [Hz]
    ylim([-20 50]); % [Hz]
    grid;

figure(10)
    plot(f_accelExpY, 10*log10(accelExp_Y), 'Color', g1)
    title('Power Spectral Density Estimate of Experimental Acceleration Input - Y axis');
    xlabel('Frequency (Hz)')
    ylabel('Magnitude (dB)')
    xlim([0 50]);   % [Hz]
    ylim([-20 50]); % [Hz]
    grid;

figure(11)
    plot(f_accelExpZ, 10*log10(accelExp_Z), 'Color', b1)
    title('Power Spectral Density Estimate of Experimental Acceleration Input - Z axis');
    xlabel('Frequency (Hz)')
    ylabel('Magnitude (dB)')
    xlim([0 50]);   % [Hz]
    ylim([-20 50]); % [Hz]
    grid;

%--------------------------------------------------------------------------
% Remove offsets from experimental accleration data
%--------------------------------------------------------------------------
fprintf('Applying offset and scaling corrections to experimental data...\n');
% reference: http://cache.freescale.com/files/sensors/doc/app_note/AN4399.pdf

g = 9810; % [mm/sec^2]

Gf = double(accelEXP); % accel data in int16 format

% Calibration values obtained from calibration in six positions
Gfx = [948.6, -989.1];  % [mg]
Gfy = [1126.5, -793.5]; % [mg]
Gfz = [984.4, -981.4];  % [mg]

% Gain value
Wx = 1; % 2 / (Gfx(1)/1000 - Gfx(2)/1000);
Wy = 1; % 2 / (Gfy(1)/1000 - Gfy(2)/1000);
Wz = 1; %2 / (Gfz(1)/1000 - Gfz(2)/1000);

% Linear offset value
Vx = -(Gfx(1) + Gfx(2))/(Gfx(1) - Gfx(2))*g;
Vy = -(Gfy(1) + Gfy(2))/(Gfy(1) - Gfy(2))*g;
Vz = -(Gfz(1) + Gfz(2))/(Gfz(1) - Gfz(2))*g;

Gx = zeros(length(accelX_double),1);
Gy = zeros(length(accelX_double),1);
Gz = zeros(length(accelX_double),1);

% Apply gain and offsets
Gx(:,1) = Wx.*Gf(:,1) + Vx;
Gy(:,1) = Wy.*Gf(:,2) + Vy;
Gz(:,1) = Wz.*Gf(:,3) + Vz;

% Plot the corrected data
% figure(12);
%     hold on
%     plot(timeEXP, Gx, 'Color', r1);
%     plot(timeEXP, Gy, 'Color', g1);
%     plot(timeEXP, Gz, 'Color', b1);
%     title('Experimental Accelerometer Data after Applying Offsets (no detrend)');
%     xlabel('Time (sec)');
%     ylabel('Acceleration (m/sec^2)');
%     legend('X - axis', 'Y - axis', 'Z - axis', 'Location', 'best');
%     xlim([0, max(timeEXP)]);
%     grid
%     hold off

figure(13);
    hold on
    plot(timeEXP, detrend(Gx), 'Color', r1);
    plot(timeEXP, detrend(Gy), 'Color', g1);
    plot(timeEXP, detrend(Gz), 'Color', b1);
    title('Experimental Accelerometer Data after Applying Offsets (with detrend)');
    xlabel('Time (sec)');
    ylabel('Acceleration (m/sec^2)');
    legend('X - axis', 'Y - axis', 'Z - axis', 'Location', 'best');
    xlim([0, max(timeEXP)]);
    grid
    hold off

%--------------------------------------------------------------------------    
% Filter the experimental acceleration data
%--------------------------------------------------------------------------
fprintf('Filtering experimental acceleration data...\n');
% De - noise filter
mu =  0.00025;
rho = 41;

% Find optimal denois filter setting
%denoiseParameterTest(timeEXP, detrend(Gy),timeAccelLong(1:size(accel,1),1), accel, .0011, -0.0002, rho)
%movingAverageParameterTest(timeEXP, detrend(Gy),timeAccelLong(1:size(accel,1),1),accel, 7000, 500);

DNx = denoiseFilter(detrend(Gx), mu, rho);
DNy = denoiseFilter(detrend(Gy), mu, rho);
DNz = denoiseFilter(detrend(Gz), mu, rho);

% Moving average filter

N = 7000;
MVAx = movingAverageFilter(detrend(Gx), N);
MVAy = movingAverageFilter(detrend(Gy), N);
MVAz = movingAverageFilter(detrend(Gz), N);

% figure(14);
%     hold on
%     plot(timeEXP, detrend(Gx),'Color', r1);
%     plot(timeEXP, MVAx, 'Color', r3);
%     plot(timeEXP, DNx, 'Color', r2);
%     hold off
%     title('Experimental Accelerometer Output - X axis');
%     xlabel('Time (sec)');
%     ylabel('Acceleration (mm/sec^2)');
%     legend('Sensor output', 'Moving average filter', 'Denoise filter', 'Location', 'Best');
%     xlim([0, max(timeEXP)]);
%     grid

figure(15);
    hold on
    plot(timeEXP, detrend(Gy),'Color', g1);
    plot(timeEXP, MVAy, 'Color', g3);
    plot(timeEXP, DNy, 'Color', g2); 
    hold off
    title('Experimental Accelerometer Output - Y axis');
    xlabel('Time (sec)');
    ylabel('Acceleration (mm/sec^2)');
    legend('Sensor output','Moving average filter', 'Denoise filter', 'Location', 'Best');
    xlim([0, max(timeEXP)]);
    grid

% figure(16);
%     hold on
%     plot(timeEXP, detrend(Gz), 'Color', b1);
%     plot(timeEXP, MVAz, 'Color', b3);
%     plot(timeEXP, DNz, 'Color', b2);
%     hold off
%     title('Experimental Accelerometer Output - Z axis');
%     xlabel('Time (sec)');
%     ylabel('Acceleration (mm/sec^2)');
%     legend('Sensor output', 'Moving average filter', 'Denoise filter', 'Location', 'Best');
%     xlim([0, max(timeEXP)]);
%     grid

%--------------------------------------------------------------------------         
% Numerical estimation of velocity profile from acceleration data
%--------------------------------------------------------------------------    
%reference: http://www.mathworks.com/help/matlab/ref/cumtrapz.html
fprintf('Numerical integration of experimental data...\n');

% Numerically integrate acceleration
velx = [cumtrapz(DNx).*(1/fs), cumtrapz(MVAx).*(1/fs)]; % [mm/sec]
vely = [cumtrapz(DNy).*(1/fs), cumtrapz(MVAy).*(1/fs)]; % [mm/sec]
velz = [cumtrapz(DNz).*(1/fs), cumtrapz(MVAz).*(1/fs)]; % [mm/sec]

velx_detrend = detrend(velx); % [mm/sec]
vely_detrend = detrend(vely); % [mm/sec]
velz_detrend = detrend(velz); % [mm/sec]


fprintf('Remove nonlinear trend from velocity estimate...\n');
    % Remove nonlinear trend from velocity estimate
        dt_velx = [detrendNonLinear(velx(:,1),12), detrendNonLinear(velx(:,2),12)];    
        dt_vely = [detrendNonLinear(vely(:,1),12), detrendNonLinear(vely(:,2),12)];
        dt_velz = [detrendNonLinear(velz(:,1),12), detrendNonLinear(velz(:,2),12)];


        
% Control the plotting of velocity estiamtion results
plotVelocityEstimate = false;

if plotVelocityEstimate == true
    figure(18);
        hold on
        plot(timeEXP, velx(:,1),'Color', r2);
        plot(timeEXP, velx(:,2),'Color', r3);
        title('Experimental Velocity Estimate - X axis');
        xlabel('Time (sec)');
        ylabel('Velocity (mm/sec)');
        legend('Velocity estimate - denoise', 'Velocity estimate - MVA', 'Location', 'best');
        xlim([0, max(timeEXP)]);
        grid
        hold off

    figure(19);
        hold on
        plot(timeEXP, vely(:,1),'Color', g2);
        plot(timeEXP, vely(:,2),'Color', g3);
        title('Experimental Velocity Estimate - Y axis');
        xlabel('Time (sec)');
        ylabel('Velocity (mm/sec)');
        legend('Velocity estimate - denoise', 'Velocity estimate - MVA', 'Location', 'best');
        xlim([0, max(timeEXP)]);
        grid
        hold off

    figure(20);
        hold on
        plot(timeEXP, velz(:,1),'Color', b2);
        plot(timeEXP, velz(:,2),'Color', b3);
        title('Experimental Velocity Estimate - Z axis');
        xlabel('Time (sec)');
        ylabel('Velocity (mm/sec)');
        legend('Velocity estimate - denoise', 'Velocity estimate - MVA', 'Location', 'best');
        xlim([0, max(timeEXP)]);
        grid
        hold off

    figure(21)
        hold on
        plot(timeEXP,dt_velx(:,1), 'Color', r2)
        plot(timeEXP,dt_vely(:,1), 'Color', g2)
        plot(timeEXP,dt_velz(:,1), 'Color', b2)

        plot(timeEXP,dt_velx(:,2), 'Color', r3)
        plot(timeEXP,dt_vely(:,2), 'Color', g3)
        plot(timeEXP,dt_velz(:,2), 'Color', b3)

        title('Experimental Velocity Estimate (selective freq. filtering)');
        xlabel('Time (sec)');
        ylabel('Velocity (mm/sec)');
        xlim([0, max(timeEXP)]);
        legend('X - axis', 'Y - aixs', 'Z - axis' , 'Location', 'best');
        grid;
        hold off
end

    
%%

%xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
%
%     COMPARE RESULTS
%
%xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
fprintf('Compare simulation and experimental results...\n');

figure(22)

    % determine the horizontal offset for acceleration profiles    
    shift = true;
    
    if shift == true
        crossSIM = 41.24;
        crossDN = 25.45;
        crossMVA = 29.85;
    else
        crossSIM = 0;
        crossDN = 0;
        crossMVA = 0;
    end
        
    shift_DN = (crossDN - crossSIM); % [sec] default set to "0" for no shift
    shift_MVA =  (crossMVA - crossSIM);  % [sec] default set to "0" for no shift
    hold on
    
    plot(timeEXP-shift_DN, DNy, 'Color', g1);
    plot(timeEXP-shift_MVA, MVAy, 'Color', b1);
    plot(timeAccelLong(1:size(accel,1),1),accel, 'Color', r1);
    
    title('Comparison of Simulation vs Experimental Acceleration Profiles - Y axis');
    xlabel('Time (sec)');
    ylabel('Acceleration (mm/sec^2)');
    legend('Accel. estimate - denoise', 'Accel. estimate - MVA', 'Simulated accel.', 'Location', 'best');
    xlim([0, max(timeEXP)]);
    %whitebg(22,'k')
   % grid
    hold off
    
    % determine the horizontal offset for velocity plots
    shift = true;
    
    if shift == true
        crossSIM = 30.87;
        crossDN = 126.4;
        crossMVA = 0;
    else
        crossSIM = 0;
        crossDN = 0;
        crossMVA = 0;
    end
    
    shift_DN = (crossDN - crossSIM); % [sec] default set to "0" for no shift
    shift_MVA =  (crossMVA - crossSIM);  % [sec] default set to "0" for no shift
 
    
    figure(23)
    hold on
    plot(timeEXP-shift_DN, dt_vely(:,1),'Color', g1);
    plot(timeEXP-shift_MVA, dt_vely(:,2),'Color', b1);
    plot(timeAccelLong(1:size(velocity_estiamte,1),1), velocity_estiamte, 'Color', r1);
     
    title('Comparison of Simulation vs Experimental Velocity Profiles - Y axis');
    xlabel('Time (sec)');
    ylabel('Velocity (mm/sec)');
    legend('Velocity estimate - denoise', 'Velocity estimate - MVA','Simulated velocity', 'Location', 'best');
    xlim([0, max(timeEXP)]);
   % grid
    %whitebg(23,'k') % turn background black
    hold off

%-------------------------------------------------------------------------
% Calcualte error in velocity estimate and plot
%-------------------------------------------------------------------------

errorDN = dt_vely(:,1) - velocity_estiamte(1:length(dt_vely),1);
errorMVA = dt_vely(:,2) - velocity_estiamte(1:length(dt_vely),1);

percentErrorDN = (1/2).*(dt_vely(:,1) - velocity_estiamte(1:length(dt_vely),1))./(dt_vely(:,1) + velocity_estiamte(1:length(dt_vely),1));
percentErrorMVA = (1/2).*(dt_vely(:,2) - velocity_estiamte(1:length(dt_vely),1))./(dt_vely(:,2) + velocity_estiamte(1:length(dt_vely),1));

figure(24)
hold on
plot(timeEXP-shift_DN, errorDN, 'r-');
%plot(timeEXP-shift_MVA, errorMVA, 'b');
xlabel('Time (sec)');
ylabel('Acceleration (mm/sec^2)');
hold off
title('Absolute Error in Velocity Estimate');

figure(25)
hold on
plot(timeEXP-shift_DN, percentErrorDN, 'r-');
%plot(timeEXP-shift_MVA, percentErrorMVA, 'b');
hold off
title('Error in Velocity Estimate');
xlabel('Time (sec)');
ylabel('Percent Error (%)');
   
fprintf('Program END.\n');




%%

%xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
%
%     FUNCTION DEFINITIONS
%
%xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx

%--------------------------------------------------------------------------
% Import experimental data - output float array
%--------------------------------------------------------------------------
    function[data_int16, data_double] = bytes2float(filePath)
        
        % SD Card directory
        %filePath = strcat('H:\', fileName);
        fileID = fopen(filePath);
        
        % Read the file, assuming signed 8 bit integers
        A = fread(fileID);
        
        % Close the file
        fclose('all');
        
        numSamples = length(A)/2;
        
        %Data_byte = zeros(numSamples,2);
        data_double = zeros(numSamples,1);
        
        index = 1;
        for q =1:2:numSamples*2
            MyByteArray = [A(q) A(q+1)];
            MyByteArray = uint8(MyByteArray);
            MySingle = typecast(MyByteArray,'int16');
            accelDouble = double(MySingle)/100.0; % [m/sec^2]
            
            data_int16(index,1) = MySingle;
            data_double(index,1) = accelDouble;
            
            index = index + 1;
        end
        
    end
        
% %--------------------------------------------------------------------------
% % Calibration Sequence
% %--------------------------------------------------------------------------
%     function[ ] = offsetCalcAccel(calibrationData)
%         
%         % Description: calcualte the calibration constands for accel
%         
%         % Plot the calibration data
%         samples = linspace(0, size(calibrationData, 1), size(calibrationData, 1))';
%         
%         figure(1)
%         plot(samples, calibrationData(:,1), 'r-', samples, calibrationData(:,2), 'g-', samples, calibrationData(:,3), 'b-' );
%         xlabel('Samples');
%         ylabel('Acceleration (m/sec^2)');
%         title('Accelerometer Calibration');
%         legend('x-axis', 'y-axis', 'z-axis', 'Location', 'best');
%         
%         % Identify the data ranges for each calbration position - manually input
%         index = [
%             50, 1800;
%             2345, 3750;
%             4260, 5675;
%             6228, 7575;
%             8061, 9651;
%             10030, 11870;
%             ];
%         
%         % Calcualte averages
%         data_avg = zeros(6,3);
%         
%         for i = 1:6
%             
%             for j = 1:3
%                 
%                 data_avg(i,j) = mean(calibrationData(index(i,1):index(i,2),j));
%             end
%         end
%         
%         data_avg
%         
%         % Calcualte offset values
%         for i=1:3
%             temp  = 0;
%             for j = 1:6
%                 if data_avg(j,i) < 900 && data_avg(j,i) > -900
%                     temp = temp + data_avg(j,i);
%                 end
%             end
%             offset(1,i) = temp/4;
%         end
%         
%         offset
%     end
    
%--------------------------------------------------------------------------
% Denoise signal function
%--------------------------------------------------------------------------
    function [ fs ] = denoiseFilter( g, mu, rho )
        
        % Apply the de-noise filter
        fs = denoisetv(g,mu, rho);
        
        function [f] = denoisetv(g,mu, rho)
            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
        
        
    end
    
%--------------------------------------------------------------------------
% Apply moving average filter
%--------------------------------------------------------------------------
    function [ mva ] = movingAverageFilter(inputSignal, movingAverageSize)

        % Define moving average filter
        a = 1;
        b = ones(1,movingAverageSize)./movingAverageSize;
        
        % Frequency response of filter
%         [H,F] = freqz(b,a,1024,fs);
%         figure;
%         plot(F,H);
        
        % Apply moving average filter
        mva = filter(b,a,inputSignal);
    end

%--------------------------------------------------------------------------
% Eliminate nonlinear trend, fit a low-order polynomial to the signal and subtract
%--------------------------------------------------------------------------
    function [dt_inputData] = detrendNonLinear(inputData, opol)
        % Reference: http://www.mathworks.com/help/signal/ug/remove-trend-from-data.html
        
        timeSpan = (1:length(inputData))';
        [p,s,nu] = polyfit(timeSpan,inputData(:,1),opol);
        f_y = polyval(p,timeSpan,[],nu);
        
        dt_inputData = inputData - f_y;
       
    end

    
%--------------------------------------------------------------------------
% Plot output from denoise filter over a range of mu values
%--------------------------------------------------------------------------
    function[ ] = denoiseParameterTest(inputX, inputY, trueX, trueY, muStart, muDelta, rhoTest)

    muTest = muStart;
   
    outputY = zeros(length(inputX), 5);

    for p = 1:5
        disp(muTest);
       % build a column matrix of filtered values
       outputY(:,p) = denoiseFilter(inputY, muTest, rhoTest);

       % increment the filter parameter
       muTest = muTest + muDelta;

    end

    figure(50);
    hold on
    plot(inputX, outputY);
    plot(trueX, trueY, 'Color', [0/255 0/255 0/255]);
    title('Denoise filter - test filter settings');
    xlabel('Time (sec)');
    ylabel('Acceleration (mm/sec^2)');
    legend('a', 'b', 'c', 'd', 'e', 'true accel profile');
    whitebg(50,'k')

    end
    
    
%--------------------------------------------------------------------------
% Plot output from denoise filter over a range of mu values
%--------------------------------------------------------------------------
    function[ ] = movingAverageParameterTest(inputX, inputY, trueX, trueY, nStart, nDelta)

    nTest = nStart;
   
    outputY = zeros(length(inputX), 5);

    for r = 1:5
        disp(nTest);
       % build a column matrix of filtered values
       outputY(:,r) = movingAverageFilter(inputY, nTest);

       % increment the filter parameter
       nTest = nTest + nDelta;

    end

    figure(50);
    hold on
    plot(inputX, outputY);
    plot(trueX, trueY, 'Color', [0/255 0/255 0/255]);
    title('Moving average filter - test filter settings');
    xlabel('Time (sec)');
    ylabel('Acceleration (mm/sec^2)');
    legend('a', 'b', 'c', 'd', 'e', 'true accel profile');
    whitebg(50,'k')

    end
    
end

