% Copyright 2019 FEDOR BAKLANOV
% Permission is hereby granted, free of charge, to any person obtaining 
% a copy of this software and associated documentation files
% (the "Software"), to deal in the Software without restriction, including
% without limitation the rights to use, copy, modify, merge, publish,
% distribute, sublicense, and/or sell copies of the Software, and to permit
% persons to whom the Software is furnished to do so, subject to the
% following conditions:
% 
% The above copyright notice and this permission notice shall be included
% in all copies or substantial portions of the Software.
% 
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
% OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
% MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN
% NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
% DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
% OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
% USE OR OTHER DEALINGS IN THE SOFTWARE.

%% Function Name: extractNexData
%
% Assumptions: None
%
% Inputs:
%   filePath - string containing a full path .nex file
%
% Outputs:
%   imageTimestamps - an array of image timestamps in nanoseconds
%
% $Date: September 8, 2019
% _________________________________________________________________________
function [imageTimestamps] = extractNexData(filePath)
    imageTimestamps = [];
    
    try
        fileId = fopen(filePath,'r');
        
        if fileId == -1
            disp('Cannot open desired file!');
            return;
        else
            outputDirName = strcat(pwd,'\images');
            [res,~,~] = mkdir(outputDirName);
            
            if ~res
                disp('Cannot create output directory!');
                return;
            end            
        end
        
        frameCount = 0;
        
        while ~feof(fileId)
            marker1 = uint8(0);
            marker2 = uint8(0);
            
            [tmp, cnt] = fread(fileId,1,'uint8=>uint8');
            
            if cnt == 1
                marker1 = tmp;
            end
            
            if marker1 == uint8(hex2dec('4E'))
                [tmp, cnt] = fread(fileId,1,'uint8=>uint8');
                if cnt == 1
                    marker2 = tmp;
                end
            end
            
            if marker2 == uint8(hex2dec('45'))
                msgGroup = fread(fileId,1,'uint8=>uint8');
                msgType = fread(fileId,1,'uint8=>uint8');
                crcType = fread(fileId,1,'uint8=>uint8');
                payloadLen = fread(fileId,1,'uint32=>uint32',0,'l');
                payload = fread(fileId,payloadLen,'uint8=>uint8');
                
                if msgGroup == 3 && msgType == 0
                    frameCount = frameCount + 1;
                    fprintf('Processing frame %d...\n', frameCount);
                    timestamp = typecast(payload(1:8),'uint64');
                    imageBytes = payload(9:end);
                    
                    imageFileName = sprintf('%d.jpg',timestamp);
                    imageFileId = fopen(strcat(outputDirName,'\',imageFileName),'wb');

                    if imageFileId == -1
                        fprintf('Cannot create file %s !!!\n', imageFileName);
                    else
                        cnt = fwrite(imageFileId,imageBytes);
                        if cnt ~= length(imageBytes)
                            fprintf('Failed to save frame %d to %s !!!\n', frameCount, imageFileName);
                        else
                            imageTimestamps = [imageTimestamps, timestamp];
                        end                                 
                        fclose(imageFileId); 
                    end  
                else
                    % do nothing, continue
                end                                                         
            end
        end        
        
        fprintf('\n\n##########\n %d frames successfully extracted! \n##########\n\n', frameCount);
        fclose(fileId);
    catch
        disp('Unknown exception happened!');
    end
end