function[ ] = offsetCalcAccel( )
close all
clear all
clear fig

% Description: import accel data and calcualte the calibration constands 


%--------------------------------------------------------------------------
% Import experimental data set
%--------------------------------------------------------------------------
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 = 'H:\01-Jun-2011-00-03-24'; % all six directions - missing an +X
%fileName = 'H:\01-Jun-2011-00-03-00';

fileName = 'H:\01-Jun-2011-00-11-08'; % trapezoid segment



     %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'));
    
    calibrationData = [double(accelX_int16), double(accelY_int16), double(accelZ_int16)];

%--------------------------------------------------------------------------
% Plot the calibration data
%--------------------------------------------------------------------------
fprintf('Plot raw accelerometer data...\n');
samples = linspace(0, size(accelX_double, 1), size(accelX_double, 1))';

figure(1)
hold on
plot(calibrationData(:,1), 'r-')
plot(calibrationData(:,2), 'g-')
plot(calibrationData(:,3), 'b-')
title('Raw Accelerometer Calibration Data');
xlabel('Samples');
ylabel('Acceleration (m/sec^2)');
legend('x-axis', 'y-axis', 'z-axis', 'Location', 'best');
hold off

%% Identify the data ranges for each calbration position
fprintf('Identify regions when axes are aligned with gravity vector...\n');
index = [
            150, 1626;
            59300, 59700;
            60220, 60610;
%             5999, 7500;
%             8470, 9800;
%             10570, 12550;
            ];
        
%% Calcualte averages
fprintf('Calcualte linear offset valyes...\n');
data_avg = zeros(6,3);

for i = 1:size(index,1)
    
    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



%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
        

end
