clc
close all
clear all

%% pre-allocation
recording_frequency = 1000; %% Hz approx.
time_to_record = 10; %%seconds
time_each_loop = 20; %%msec
pre_allocated_space = floor(time_to_record*recording_frequency/time_each_loop); %% 1 loop collects the data for 20 msec

FSR_data = 0*ones(pre_allocated_space*20,8);
IMU_data = 0*ones(pre_allocated_space,12);

%% recording time setup
recording_frequency = 1000; %% Hz approx.
time_to_record = 500; %%seconds
time_each_loop = 20; %%msec
loops = floor(time_to_record*recording_frequency/time_each_loop); %% 1 loop collects the data for 20 msec

%% file saving
file_index = 1;
saving_time = 30; %% sec

%% decision_making 
decision_making_time = 0.5; %%sec

%% plotting options
plotting_frequecy = 0.100; %% msec
plotting_frequecy = floor(plotting_frequecy*1000/20);
plot_counter = 0;
show_data = 2 ; %% last 2 seconds
show_data = show_data * 1000;

%% Connect AAL-Band
[Serial_Port,com_stab] = Connect_AAL_BAND();

%% reset amplifier gain
[~,~] = communicate(Serial_Port,'Reset_Calibration','NA');


%% calibrate amplifier gain
%% Close you fist with desired force before calibration starts. Keep the fist during calibration and open it when calibration is finished.
disp('Close you Fist')
pause(2)
disp('Calibrating')
pause(0.01)
[~,~] = communicate(Serial_Port,'Calibrate','NA');
disp('Calibrating finished')
counter = 1;
j = 1;
k = 1;

tic
initial_time_saving = toc;
initial_time_decision = toc;
for i = 1:loops
    
    %% start colecting data
    [Data,counter] = communicate(Serial_Port,'Start',counter); %% this command will return a data recorded in 20 msec window
    FSR_data((k-1)*20+1:k*20,:) = Data(:,1:8);
    IMU_data(k,:) = Data(end,9:20);
    
    %% allocating more space
    if(k == pre_allocated_space*j)
        
        FSR_data = [FSR_data;0*ones(pre_allocated_space*20,8)];
        IMU_data = [IMU_data;0*ones(pre_allocated_space,12)];
        j = j + 1;
        
    end
    k = k + 1;
    
    %% plotting
    plot_counter = plot_counter + 1;
    if(plot_counter == plotting_frequecy)
        
        plot_counter = 0;
        subplot(3,1,1)
        plot(FSR_data)
        ylim([0 3.5])
        xlim([k*20-show_data k*20+(show_data/5)])
        ylabel('Voltage (V)')
        subplot(3,1,2)
        plot(IMU_data(:,1:3))
        ylim([-10 10])
        xlim([k-show_data/20 k+(show_data/5)/20])
        ylabel('Gravity (m/sec^2)')
        subplot(3,1,3)
        plot(IMU_data(:,7:9))
        ylim([-10 10])
        xlim([k-show_data/20 k+(show_data/5)/20])
        ylabel('Acceleration (m/sec^2)')
        pause(0.001)
        
    end
    
    %% decision making
    if((toc - initial_time_decision) > decision_making_time)
        initial_time_decision = toc;
    end
    
    %% saving data 
    if((toc - initial_time_saving) > saving_time)
        FSR_data = FSR_data(1:(k-1)*20,:);
        IMU_data = IMU_data(1:(k-1),:);
        name = strcat('data_',int2str(file_index));
        file_index = file_index + 1;
        save(name,'FSR_data','IMU_data')
        [~,~] = communicate(Serial_Port,'Stop','NA');
        counter = 1;
        initial_time_saving = toc;
        k = 1;
        j = 1;
        FSR_data = 0*ones(pre_allocated_space*20,8);
        IMU_data = 0*ones(pre_allocated_space,12);
        
    end
end
toc
FSR_data = FSR_data(1:(k-1)*20,:);
IMU_data = IMU_data(1:(k-1),:);
name = strcat('data_',int2str(file_index));
save(name,'FSR_data','IMU_data')

%% stop collecting data
[~,~] = communicate(Serial_Port,'Stop','NA');

%% put AAL-Band to sleep mode
[~,~] = communicate(Serial_Port,'Shut_Down','NA');

subplot(5,1,1)
plot(FSR_data(1:end,1:8),'DisplayName','raw_data(:,1:8)')
subplot(5,1,2)
plot(IMU_data(:,1:3),'DisplayName','raw_data(:,9:11)')
subplot(5,1,3)
plot(IMU_data(:,4:6),'DisplayName','raw_data(:,12:14)')
subplot(5,1,4)
plot(IMU_data(:,7:9),'DisplayName','raw_data(:,15:17)')
subplot(5,1,5)
plot(IMU_data(:,10:12),'DisplayName','raw_data(:,18:end)')


%% Commands
%% 'Start'  to start collecting data
%% 'Stop'   to stop collecting data
%% 'Calibrate'  Calibration is performed to optimize FSR amplifier gain. Close your fist with desired force before starting calibration and open your fist when calibration is ended
%% 'Reset_Calibration' to reset amplifier gain
%% 'Shut_Down'  to put AAL-Band control unit to sleep mode
%%
%% Data structure FSR_data
%% columns 1 to 8 are FSR sensors (voltage) ... recording frequency -> 1000 Hz
%% Data structure IMU_data
%% columns 1 to 3 are gravity vector in x,y and z axis (m/sec2) ... recording frequency -> 50 Hz
%% columns 4 to 6 are angular velocity vector in x,y and z axis (rad/sec) ... recording frequency -> 50 Hz
%% columns 7 to 9 are linear acceleration vector in x,y and z axis (m/sec2) ... recording frequency -> 50 Hz
%% columns 10 to 12 are euler angles vector in x,y and z axis (deg) ... recording frequency -> 50 Hz
