%MATLAB EMSplot.m
%Written by Aaron Ho and Tim Hu (adh78, tyh9)
%as part of Exercise Monitor System project for ECE 4760, SP'12
%Version 1 - April 26, 2012
%-1st version - basic implementation
%
%Version 1.1 - April 29, 2012
%-corrections to axes
%-added ability to handle null data sets
%-improved graphics. I guess.
%
%Version 2 - April 30, 2012
%-reformat for y data instead of x data
%-change for standard orientation of accelerometer
%-changed for different metric of steps (still being determined)
%
%Version 2.1 - May 1, 2012
%-adding graphing running average of forward acceleration
%-terminology change to be oriented around body, not accelerometer (because
%of the addition of a second adxl)
%
%Version 3 - May 2
%time, acc, step, raw, acc, raw, freq, avgfreq
%
%
%
%3.1
% Raw accelerations
%
%3.2 
%   Running average of the z axis data
%
%3.3
%   jerk
%
%3.4
%   close to final?
%   pretty much prettying it up for the demo and for 'final' data output
%
%
%
%Script should be stored onto whatever SD device is to be used for the
%project.  This can be done after or before data acquisition.
%
%
%EMSplot.m script does the following:
%
%-Open as many log files as the user indicates;
%-For each file:
%   -load the data
%   -graph forward data
%   -graph vertical data
%   -if variable is set, use raw data to calculate out numbers
%       and compare raw integrated data with noise calibrated data
%
%EMSplot.m assumes:
%
%-log files are in the same directory as script
%   (NOTE: this is really the only important assumption for the script- the
%   following should be maintained by the MCU code and should not 
%   be handled by MATLAB script)
%-log files have the name 'EMSlog___.csv', where ___ is a 3 digit number
%   enumerating the log files
%-files consist of data delimited by spaces or commas
%-a line of data contains the following:
%   [time(ms), fAcc(m/s^2), fVel(m/s), fDis(m), avgSp(m/s), vAcc(m/s^2),
%   vVel(m/s), step(), fAccumulator(LSB), vAccumulator(LSB), fAccRun(m/s^2)]  
%   where
%
%   time = time in ms when sample is acquired
%   fAcc = forward acceleration data (=0 when sitting still on a table)
%   fVel = forward velocity relative to the accelerometer's orientation
%   fDis = forward distance traveled
%   avgSp = average speed to travel the distance
%   vAcc = vertical acceleration data (=0 when sitting still on a table)
%   vVel = vertical velocity
%   step = number of steps taken, as defined in Pedometer.c
%   fAccumulator = forward acceleration data that is averaged for 8 samples,
%       but does not account for noise threshold
%   vAccumulator = vertical acceleration data that is averaged for 8 samples,
%       but does not account for noise threshold
%   (NOTE: yAccumulator and zAccumulator will be stored in yRaw and zRaw,
%   respectively for this script)
%   fAccRun = forward acceleration data, but as a running average

%define: raw: 1 to analyze raw data in addition to plots, 0 for only 
%        plotting the data.
%        gain: scales data from LSB to g to m/s^2.
%        
raw =0;
gain=.0039*9.8;
%data arrays:
%arrays correspond to data listed above. Big change: time will be in s, NOT
%ms like it is in the log file
%time; yAcc; yVel; yDis; avgSp; zAcc; zVel; step; yRaw; zRaw;

%calculation arrays to be used if(raw)
%yVRaw;yDRaw;avgRaw;zVRaw;stepRaw;

ovfl=0;
FileName=[];
PathName=[];
FilterIndex=[];
DATA=[];
%open files
[FileName,PathName,FilterIndex] = uigetfile('*.*','Select log file','MultiSelect','on');
[a len] = size(FileName);
if (ischar(FileName))
    len=1;
end
%for each file, parse out the data, and 
for i=1:len
    %clear out related data arrays
    time=[];yAcc=[];yVel=[];yDis=[];avgSp=[];zAcc=[];zVel=[];step=[];
    yRaw=[];zRaw=[];fname=[];yAccRun=[]; zAc2=[]; zVe2=[]; zRa2=[]; stepHz=[];
    stepAvg=[];stepAvg2=[];jerk1=[];jerk2=[];jerkavg1=[];jerkavg2=[];
    xRaw=[];yRa2=[];xRa2=[];dist=[];
    zAccRun=[];cursp=[];dist=[];
    zAccRun2=[];
    close all
    if(raw)
        %if we're analyzing raw Data, clear out the calculated arrays too
        yVRaw=[];yDRaw=[];avgRaw=[];zVRaw=[];stepRaw=[];
    end
    if (ischar(FileName))
        fname=FileName;
    else
        fname=FileName{i};
        
        end
    lognum=fname( length(fname)-6 : length(fname)-4 );
    tmpdata=load(fname);
    DATA{i}=tmpdata; %no real purpose to saving in DATA except as a centralized location of data
    if(size(tmpdata))
        time=tmpdata(:,1)./1000;
        dist=tmpdata(:,3);
        step=tmpdata(:,2);
        %zRaw=tmpdata(:,4);
        cursp=tmpdata(:,4);
        %zRa2=tmpdata(:,6);
        zAcc=tmpdata(:,5);
        zAc2=tmpdata(:,6);
        %zAccRun=tmpdata(:,9);
        %zAccRu2=tmpdata(:,10);
        zAccRun=tmpdata(:,7);
        zAccRun2=tmpdata(:,8);
        jerk1=tmpdata(:,9);
        jerk2=tmpdata(:,10);
        jerkavg1=tmpdata(:,11);
        jerkavg2=tmpdata(:,12);
        stepHz=tmpdata(:,13);
        stepAvg=tmpdata(:,14);
        %xRaw=tmpdata(:,9);
        %xRa2=tmpdata(:,10);
        %yRaw=tmpdata(:,11);
        %yRa2=tmpdata(:,12);
        %yAcc=tmpdata(:,2);
        %yVel=tmpdata(:,3);
        %yDis=tmpdata(:,4);
        %avgSp=tmpdata(:,5);
        %zAcc=tmpdata(:,6);
        %zVel=tmpdata(:,7);
        %step=tmpdata(:,8);
        %yRaw=tmpdata(:,9).*gain;
        %zRaw=tmpdata(:,10).*gain;
        %yAccRun=tmpdata(:,11);
        if(~raw)
            %if raw bit is NOT set - simply plot data 
            numSamples=length(time);
            ovfl=0;
             for j=2:numSamples
                if (time(j-1)>65 && time(j)<.2)
                   ovfl=ovfl+1; 
                end
                time(j)=time(j)+ovfl*65.536;
             end
            
            %x data: plot 4 graphs top to bottom:
            %   Accleration
            %   Velocity
            %   Distance Traveled
            %   Average Speed
            fig = figure(1);
            set(fig,'units','normalized','outerposition',[0 0 1 1]);
            subplot(4,1,1);
            axis equal
            plot(time,zAcc);%,'b',time,yAccRun,'k');
            %y_leg=legend('Calibrated','Running Average');
            %set(y_leg, 'Position',[.835 .945 .05 .05])
            ylabel('zAcc1 [m/s{^2}]');
            title('Vertical Acceleration (Foot 1)');
            pause(.2)
            subplot(4,1,2);
            plot(time,zAc2);
            ylabel('zAcc2 [m/s{^2}]');
            title('Vertical Acceleration (Foot 2)');
            pause(.2)
            subplot(4,1,3);
            axis equal
            plot(time,step);
            ylabel('Steps taken');
            title('Steps taken');
            %axis([0 max(time) 0 max(yDis)]);
            pause(.2)
            subplot(4,1,4);
            axis equal
            plot(time,stepHz, 'b',time, stepAvg,'r');
            ylabel('Steps per second [Hz]');
            xlabel('time [s]');
            l=legend('frequency','running average','Location','BestOutside');
            title('Pace');  
            %axis([0 max(time) 0 max(avgSp)]);
            saveas(fig,['stepdata',lognum,'.jpg']);
            pause(.2)
            %z data: plot 3 graphs top to bottom:
            %   Accleration
            %   Velocity
            %   Steps Taken
            
            fig = figure(2);
            set(fig,'units','normalized','outerposition',[0 0 1 1]);
            subplot(2,1,1)
            plot(time,jerk1,'b',time,jerkavg1,'r')
            subplot(2,1,2)
            plot(time,jerk2,'b',time,jerkavg2,'r')
            saveas(fig,['jerk',lognum,'.jpg']);

            %%%%
            fig=figure(3);
            set(fig,'units','normalized','outerposition',[0 0 1 1]);
            subplot(3,1,1)
            axis equal
            plot(time,step);
            ylabel('Steps taken');
            title('Steps taken');
            pause(.05)
            subplot(3,1,2)
            axis equal
            plot(time,stepHz, 'b',time, stepAvg,'r');
            ylabel('Steps per second [Hz]');
            pause(.05)
            subplot(3,1,3);
            axis equal
            plot(time,dist);
            ylabel('Distance traveled [m]');
            xlabel('time [s]')
            title('Run data');
            saveas(fig,['distance',lognum,'.jpg']);
             
            fig=figure(4);
            set(fig,'units','normalized','outerposition',[0 0 1 1]);
            axis([0 20 -100 100])
            subplot(3,1,1)
            plot(time,zAcc,'b',time, zAccRun+.3,'r',time, (zAccRun+.3).*-1,'r')
                        axis([0 20 -100 100])
            %legend('x','y','z')
            ylabel('acceleration [m/s{^2}]')
            title('Foot 1 raw accelerations (already sampled)')
            subplot(3,1,2)
            axis([0 20 -100 100])
            plot(time,zAc2,'b',time, zAccRun2+.3,'r',time, (zAccRun2+.3).*-1,'r')
                        axis([0 20 -100 100])
            %plot(time,xRa2,'b',time,yRa2,'r',time,zRa2,'k')
            title('Foot 2 raw accelerations (already sampled)');
            subplot(3,1,3);
 %           axis equal
  

             plot(time,step);
             axis([0 20 0 40])
             ylabel('Steps taken');
            title('Steps taken');
            xlabel('time [s]')
            saveas(fig,['rawsteps',lognum,'.jpg']);
            
            fig=figure(5);
            set(fig,'units','normalized','outerposition',[0 0 1 1]);
            subplot(3,1,1)            
            pause(.05)
            axis equal
            plot(time,stepHz, 'b',time, stepAvg,'r');
            ylabel('Steps per second [Hz]');
            pause(.05)
            subplot(3,1,2)
            axis equal
            plot(time,cursp)
            ylabel('Current speed [m/s])')
            pause (0.5)
            subplot(3,1,3);
            axis equal
            plot(time,dist);
            ylabel('Distance traveled [m]');
            xlabel('time [s]')
            title('Run data');
            saveas(fig,['speed',lognum,'.jpg']);
            
        else
            %raw bit is set - calculate the data and THEN plot
            numSamples=length(time);
            yVRaw(1:numSamples)=0;
            yDRaw(1:numSamples)=0;
            avgRaw(1:numSamples)=0;
            zVRaw(1:numSamples)=0;
            stepRaw(1:numSamples)=0;
            for j=2:numSamples
                if (time(j-1)>time(j))
                   ovfl=ovfl+1; 
                end
                time(j)=time(j)+ovfl*65536;
                dt=time(j)-time(j-1);
                yVRaw(j)=yVRaw(j-1)+dt*.5*(yRaw(j)+yRaw(j-1));
                yDRaw(j)=yDRaw(j-1)+abs(dt*.5*(yVRaw(j)+yVRaw(j-1)));
                avgRaw(j)=yDRaw(j)/time(j);
                zVRaw(j)=zVRaw(j-1)+dt*.5*(zRaw(j)+zRaw(j-1));
                if(abs(zRaw(j))>8)
                    stepRaw(j)=stepRaw(j-1)+1;
                else
                    stepRaw(j)=stepRaw(j-1);
                end
            end
                    %x data: plot 4 graphs top to bottom:
            %   Accleration
            %   Velocity
            %   Distance Traveled
            %   Average Speed
            fig = figure(1);
            set(fig,'units','normalized','outerposition',[0 0 1 1]);
            subplot(4,1,1);
            %axis equal
            plot(time,yAcc,'b',time,yRaw,'r',time,yAccRun,'k');
            y_leg=legend('Calibrated','w/ Noise','Running Average');
            set(y_leg, 'Position',[.835 .945 .05 .05])
            ylabel('yAcc [m/s{^2}]');
            title('Lateral Acceleration');
            subplot(4,1,2);
            %axis equal
            plot(time,yVel,'b',time,yVRaw,'r');
            ylabel('yVel [m/s]');
            title('Lateral Velocity');
            subplot(4,1,3);
            axis equal
            plot(time,yDis,'b',time,yDRaw,'r');
            ylabel('yDis [m]');
            title('Distance Traveled');
            %axis([0 max(time) 0 max(yDRaw)]);
            subplot(4,1,4);
            axis equal
            plot(time,avgSp,'b',time,avgRaw,'r');
            ylabel('avgSp [m/s]');
            xlabel('time [s]');
            title('Average Speed');
            %axis([0 max(time) 0 max(avgRaw)]);

            saveas(fig,['yData_w_noise',lognum,'.jpg']);

            %z data: plot 3 graphs top to bottom:
            %   Accleration
            %   Velocity
            %   Steps Taken
            fig = figure(2);
            set(fig,'units','normalized','outerposition',[0 0 1 1]);
            subplot(3,1,1);
            plot(time,zAcc,'b',time,zRaw,'r');
            z_leg=legend('Calibrated','w/ Noise');
            set(z_leg, 'Position',[.835 .945 .05 .03])
            ylabel('zAcc [m/s{^2}]');
            title('Vertical Acceleration');
            subplot(3,1,2);
            plot(time,zVel,'b',time,zVRaw,'r');
            ylabel('zVel [m/s]');
            title('Vertical Velocity');
            subplot(3,1,3);
            plot(time,step,'b',time,stepRaw,'r');
            ylabel('step');
            title('Steps Taken');
            xlabel('time [s]');
            %axis([0 max(time) 0 max(step)]);
            saveas(fig,['zData_w_noise',lognum,'.jpg']);
        
        
            fig=figure(2);
            set(fig,'units','normalized','outerposition',[0 0 1 1]);
            axis([0 20 -100 100])
            subplot(3,1,1)
            plot(time,zAcc,'b',time, zAccRun+.3,'r',time, (zAccRun+.3).*-1,'r')
                        axis([0 20 -100 100])
            %legend('x','y','z')
            ylabel('acceleration [m/s{^2}]')
            title('Foot 1 raw accelerations (already sampled)')
            subplot(3,1,2)
            axis([0 20 -100 100])
            plot(time,zAc2,'b',time, zAccRu2+.3,'r',time, (zAccRu2+.3).*-1,'r')
                        axis([0 20 -100 100])
            %plot(time,xRa2,'b',time,yRa2,'r',time,zRa2,'k')
            title('Foot 2 raw accelerations (already sampled)');
            subplot(3,1,3);
 %           axis equal
  
             axis([0 20 -100 100])
             plot(time,step);
            ylabel('Steps taken');
            title('Steps taken');
            xlabel('time [s]')
                        axis([0 20 -100 100])
            saveas(fig,['rawsteps',lognum,'.jpg']);
        
        end
        
        
    end
    
    
end


