clear all;
close all;

samplerate = 10; %Hz
samples = 10000;
rectime = samples/samplerate;

% Find a serial port object.
obj1 = instrfind('Type', 'serial', 'Port', 'COM3', 'Tag', '');

% Create the serial port object if it does not exist
% otherwise use the object that was found.
if isempty(obj1)
    obj1 = serial('COM3');
else
    fclose(obj1);
    obj1 = obj1(1)
end

% Connect to instrument object, obj1.
fopen(obj1);

% Configure instrument object, obj1.
set(obj1, 'BaudRate', 115200);
set(obj1, 'Terminator', {'LF','LF/CR'});

% Communicating with instrument object, obj1.
i=0;

data={};

 %figure
    subplot(2,3,1)
    title('Acc X')
    subplot(2,3,2)
    title('Acc Y')
    subplot(2,3,3)
    title('Acc Y')
    subplot(2,3,4)
    title('GYR X')
    subplot(2,3,5)
    title('GYR Y')
    subplot(2,3,6)
    title('GYR Z')


while (i <7)
    
    fscanf(obj1,'%c');
    
    i = i+1;
end

x_co = 1;

while (i < 108)
    
    gyr_box = fscanf(obj1,'%c');
    gyr_box_array = strsplit(gyr_box,',');
    acc_box = fscanf(obj1,'%c');
    acc_box_array = strsplit(acc_box,',');
    
%     ax_c = acc_box_array(2);
%     ax(x_co) = str2double(ax_c);
%     ay_c = acc_box_array(3);
%     ay(x_co) = str2double(ay_c);
%     az_c = acc_box_array(4);
%     az(x_co) = str2double(az_c);
%     gx_c = gyr_box_array(2);
%     gx(x_co) = str2double(gx_c);
%     gy_c = gyr_box_array(3);
%     gy(x_co) = str2double(gy_c);
%     gz_c = gyr_box_array(4);
%     gz(x_co) = str2double(gz_c);
%     
    
    ax(x_co) = str2double(acc_box_array(2))/8192;
   
    ay(x_co) = str2double(acc_box_array(3))/8192;
   
    az(x_co) = (str2double(acc_box_array(4))-1340)/8192;
   
    gx(x_co) = str2double(gyr_box_array(2));
   
    gy(x_co) = str2double(gyr_box_array(3));
    
    gz(x_co) = str2double(gyr_box_array(4));
    
    %figure
    drawnow;
    subplot(2,3,1)
    plot(ax,'r')
    %title('Acc X')
    subplot(2,3,2)
    plot(ay,'b')
    %title('Acc Y')
    subplot(2,3,3)
    plot(az,'g')
    %title('Acc Y')
    subplot(2,3,4)
    plot(gx,'r')
    %title('GYR X')
    subplot(2,3,5)
    plot(gy,'b')
    %title('GYR Y')
    subplot(2,3,6)
    plot(gz,'g')
    %title('GYR Z')
    
    data = [data;{acc_box}];
    data = [data;{gyr_box}];
%data=[data;{fscanf(obj1,'%c')}];

i=i+1
x_co = x_co+1;
end

while (i < samples)
    
    gyr_box = fscanf(obj1,'%c');
    gyr_box_array = strsplit(gyr_box,',');
    acc_box = fscanf(obj1,'%c');
    acc_box_array = strsplit(acc_box,',');
    
%     ax_c = acc_box_array(2);
%     ax(x_co) = str2double(ax_c);
%     ay_c = acc_box_array(3);
%     ay(x_co) = str2double(ay_c);
%     az_c = acc_box_array(4);
%     az(x_co) = str2double(az_c);
%     gx_c = gyr_box_array(2);
%     gx(x_co) = str2double(gx_c);
%     gy_c = gyr_box_array(3);
%     gy(x_co) = str2double(gy_c);
%     gz_c = gyr_box_array(4);
%     gz(x_co) = str2double(gz_c);
%     
    
    ax(x_co) = str2double(acc_box_array(2))/8192;
   
    ay(x_co) = str2double(acc_box_array(3))/8192;
   
    az(x_co) = (str2double(acc_box_array(4))-1340)/8192;
   
    gx(x_co) = str2double(gyr_box_array(2));
   
    gy(x_co) = str2double(gyr_box_array(3));
    
    gz(x_co) = str2double(gyr_box_array(4));
    
    %figure
    drawnow;
    subplot(2,3,1)
    plot(ax(length(ax)-100:1:length(ax)),'r')
    %title('Acc X')
    subplot(2,3,2)
    plot(ay(length(ax)-100:1:length(ax)),'b')
    %title('Acc Y')
    subplot(2,3,3)
    plot(az(length(ax)-100:1:length(ax)),'g')
    %title('Acc Y')
    subplot(2,3,4)
    plot(gx(length(ax)-100:1:length(ax)),'r')
    %title('GYR X')
    subplot(2,3,5)
    plot(gy(length(ax)-100:1:length(ax)),'b')
    %title('GYR Y')
    subplot(2,3,6)
    plot(gz(length(ax)-100:1:length(ax)),'g')
    %title('GYR Z')
    
    data = [data;{acc_box}];
    data = [data;{gyr_box}];
%data=[data;{fscanf(obj1,'%c')}];

i=i+1
x_co = x_co+1;
end

save data; 

fclose(obj1);