path = 'C:\\Users\\Gerard Jensen Olmos\\Desktop\\Arbejde\\CORE-SAFE\\Android Recs\\2023-08-02_103206.bin';
[ACC0X,ACC0Y,ACC0Z,ACC0,MIC0,ECG0,RR_0Z] = read_bin(path,false);

ECG_fs = 500; % Hz
MIC_fs = 1000; % Hz
ACC_fs = 50; % Hz

ECG_x = (0:(length(ECG0)-1))/ECG_fs;
MIC_x = (0:(length(MIC0)-1))/MIC_fs;
ACC_x = (0:(length(ACC0)-1))/ACC_fs;

ax_ecg = subplot(3,1,1);
plot(ECG_x,ECG0);
ax_mic = subplot(3,1,2);
plot(MIC_x,MIC0);
ax_acc = subplot(3,1,3);
plot(ACC_x,ACC0);

linkaxes([ax_ecg,ax_mic, ax_acc],'x');

function [ACC0X,ACC0Y,ACC0Z,ACC0,MIC0,ECG0,RR_0Z] = read_bin(path,hasComments)
    raw_bytes = fread(fopen(path),inf,'uint8');

    if(hasComments)
    indexOfCommentsHeader = getFirstStringHeaderIndex([newline newline '--Comments--'], raw_bytes);
    else
        indexOfCommentsHeader = length(raw_bytes);
    end
    
    byte_read_index = 1;
    
    ACC0Header = [65, 67, 67, 48]'; % ACC0 in utf8
    MIC0Header = [77, 73, 67, 48]'; % MIC0 in utf8
    RR_0Header = [82, 82, 95, 48]'; % RR_0 in utf8
    ECG0Header = [69, 67, 71, 48]'; % ECG0 in utf8
    
    ACC0PacketBytesLen = findPacketLen("ACC0",raw_bytes); 
    disp(convertCharsToStrings(strcat("Using ",string(ACC0PacketBytesLen)," bytes per ACC0 packet")));
    MIC0PacketBytesLen = findPacketLen("MIC0",raw_bytes); 
    disp(convertCharsToStrings(strcat("Using ",string(MIC0PacketBytesLen)," bytes per MIC0 packet")));
    
    ECG0PacketBytesLen = findPacketLen("ECG0",raw_bytes); 
    disp(convertCharsToStrings(strcat("Using ",string(ECG0PacketBytesLen)," bytes per ECG0 packet")));
    
    ACC0X = [];
    ACC0Y = [];
    ACC0Z = [];
    
    MIC0 = [];
    
    RR_0X = [];
    RR_0Y = [];
    RR_0Z = [];
    
    ECG0 = [];
    
    ACC0SampleCnt = 0;
    MIC0SampleCnt = 0;
    RR_0SampleCnt = 0;
    ECG0SampleCnt = 0;
    lostByteCnt = 0;
    
    while byte_read_index < indexOfCommentsHeader
        header = raw_bytes(byte_read_index:byte_read_index+3,1);
        if header == ACC0Header
            ACC0SampleCnt = ACC0SampleCnt+1;
            byte_read_index = byte_read_index + 4;
%             disp(convertCharsToStrings(strcat("I ",string(byte_read_index),' A')));
            ACC0X_p = typecast(uint8([raw_bytes(byte_read_index) raw_bytes(byte_read_index+1)]),'int16');
            ACC0Y_p = typecast(uint8([raw_bytes(byte_read_index+2) raw_bytes(byte_read_index+3)]),'int16');
            ACC0Z_p = typecast(uint8([raw_bytes(byte_read_index+4) raw_bytes(byte_read_index+5)]),'int16');
            
            ACC0X(ACC0SampleCnt) = ACC0X_p;
            ACC0Y(ACC0SampleCnt) = ACC0Y_p;
            ACC0Z(ACC0SampleCnt) = ACC0Z_p;
            
            byte_read_index = byte_read_index + 6;
             % disp(convertCharsToStrings(strcat("E ",string(byte_read_index),' A')));
            
        elseif header == MIC0Header
            byte_read_index = byte_read_index + 4;
%             disp(convertCharsToStrings(strcat("I ",string(byte_read_index),' M')));
            for i = 1:MIC0PacketBytesLen/2
                MIC0SampleCnt = MIC0SampleCnt +1;
                MIC0_p = typecast(uint8([raw_bytes(byte_read_index) raw_bytes(byte_read_index+1)]),'int16');
                byte_read_index = byte_read_index+2;
                MIC0(MIC0SampleCnt) = MIC0_p;
            end
%             disp(convertCharsToStrings(strcat("E ",string(byte_read_index),' M')));
        elseif header == RR_0Header
            RR_0SampleCnt = RR_0SampleCnt+1;
            byte_read_index = byte_read_index + 4;
%             disp(convertCharsToStrings(strcat("I ",string(byte_read_index),' R')));
            
            RR_0X_p = raw_bytes(byte_read_index);
            RR_0Y_p = raw_bytes(byte_read_index+1);
            RR_0Z_p = raw_bytes(byte_read_index+2);
            
            RR_0X(RR_0SampleCnt) = RR_0X_p;
            RR_0Y(RR_0SampleCnt) = RR_0Y_p;
            RR_0Z(RR_0SampleCnt) = RR_0Z_p;
            
            byte_read_index = byte_read_index + RR_0PacketLen;
%             disp(convertCharsToStrings(strcat("E ",string(byte_read_index),' R')));
    
        elseif header == ECG0Header
            byte_read_index = byte_read_index + 4;
%             disp(convertCharsToStrings(strcat("I ",string(byte_read_index),' E')));
            for i = 1:ECG0PacketBytesLen/3
                ECG0SampleCnt = ECG0SampleCnt +1;
                ECG0_p = raw_bytes(byte_read_index) + raw_bytes(byte_read_index+1)*256 + raw_bytes(byte_read_index+2)*256*256;
                if ECG0_p >= 8388608 %0x800000
                    ECG0_p = ECG0_p - 16777216; % 0x1000000;
                end
                byte_read_index = byte_read_index+3;
                ECG0(ECG0SampleCnt) = ECG0_p;
            end
%             disp(convertCharsToStrings(strcat("E ",string(byte_read_index),' E')));
        else
            byte_read_index = byte_read_index+1;
            %In case of a lost packet
            lostByteCnt = lostByteCnt+1;
             disp(byte_read_index);
            disp(convertCharsToStrings(char(header)));
        end
        
        
        disp(byte_read_index/length(raw_bytes)*100);
        
    end
    ACC0 = ACC0Z./sqrt(ACC0X.^2+ACC0Y.^2+ACC0Z.^2);
    ACC1 = ACC0Z./sqrt(ACC0Y.^2+ACC0Z.^2);
    
    connection_type_index = getFirstStringHeaderIndex('Connection type: ', raw_bytes);
    if(connection_type_index == 0)
        disp("Failed getting connection type and rec length. Probably old recoarding");
    else
    rec_length_index = getFirstStringHeaderIndex('Rec length: ', raw_bytes);
    end_rect = getStringHeaderIndexAfter(" s",raw_bytes,rec_length_index);
    
    connection_mode = (convertCharsToStrings(char(raw_bytes(connection_type_index+17:rec_length_index-2))));
    rec_length = str2double(convertCharsToStrings(char(raw_bytes(rec_length_index+12:end_rect-1))));
    
    disp(convertCharsToStrings(strcat("Connection mode: ",connection_mode)));
    fprintf(convertCharsToStrings(strcat("Rec length: ",string(rec_length)," s")));
    end
end

function index = getFirstStringHeaderIndex(txt, raw_bytes)
    header = uint8(convertStringsToChars(txt))';
    index = 0;
    n = length(header)-1;
    for i = length(raw_bytes)-n:-1:1
        piece = raw_bytes(i:i+n,1);   
        if piece == header
            index = i;
            break;
        end
    end
end

function index = getStringHeaderIndexAfter(txt, raw_bytes,N)
    header = uint8(convertStringsToChars(txt))';
    index = 0;
    n = length(header)-1;
    for i = N:1:length(raw_bytes)-n
        piece = raw_bytes(i:i+n,1);   
        if piece == header
            index = i;
            break;
        end
    end
end

function len = findPacketLen(header,raw_bytes)
    ACC0Header = [65, 67, 67, 48]'; % ACC0 in utf8
    MIC0Header = [77, 73, 67, 48]'; % MIC0 in utf8
    RR_0Header = [82, 82, 95, 48]'; % RR_0 in utf8
    ECG0Header = [69, 67, 71, 48]'; % ECG0 in utf8
    

   firstECGHeaderIndex = getStringHeaderIndexAfter(header,raw_bytes,1);
    ECG0PacketBytesLen = 0;
    existingHeader = false;
    while(~existingHeader)
        ECG0PacketBytesLen = ECG0PacketBytesLen +1;
        header = raw_bytes(firstECGHeaderIndex+ECG0PacketBytesLen+4:firstECGHeaderIndex+4+ECG0PacketBytesLen+3);
        existingHeader = isequal(header,ACC0Header) | isequal(header,MIC0Header) | isequal(header,ECG0Header)| isequal(header,RR_0Header);
    end
    
    len = ECG0PacketBytesLen;
end

