% fclose(instrfind); <-- Use this to reset serial ports

function serialtest()

global serial_inbuffer;

% Record/Save functionality
global current_seq;
global record_matrix;
global record;

serial_inbuffer = [];

record_matrix = [];
record = false;
current_seq = 0;

s = serial('COM1','BaudRate',115200,'DataBits', 8,'Parity','none','StopBits',1);

s.BytesAvailableFcnMode = 'terminator';
s.Terminator = 3;
s.BytesAvailableFcn = @incoming_data;

fopen(s)
 
while true
  %incoming_data(s,0);
  out = input('S >> ', 's');
  
  [command, c, e, nextindex] = sscanf(out, '%s', 1);
  
  switch lower(command) 
    case 'exit'
      break;
    case 'record'
      record = true;
      record_matrix = [];
      current_seq = 0;
    case 'stop'
      record = false;
    case 'save'
      n = nextindex;
      [file, c, e, nextindex] = sscanf(out(n:end), '%s', 1);
      save(file, 'record_matrix');
  end
end

fclose(s);
delete(s);
clear s;


% ------------------------------------------------------------------------
% SERIAL INPUT HANDLER
% ------------------------------------------------------------------------

% *****
% Decode a serially transmitted dword value
function r = decode_dword(v)

r = v(1) + bitshift(v(2),8) + bitshift(v(3), 2*8) + bitshift(v(4), 3*8);

% *****
% Decode a serially transmitted word value
function r = decode_word(v)

r = v(1) + bitshift(v(2),8);

% *****
% Encode a word value for serial transmission
function a = encode_word(w)

a = [bitand(w,255) bitand(bitshift(w,-8),255)];

% *****
% Event triggered on incoming data
function incoming_data(obj, event) 

% Input buffer 
global serial_inbuffer;
global record;
global record_matrix;
global current_seq;

% Ignore spurious events (won't happen)
if obj.BytesAvailable 
  
  % Wait for start byte 
  if length(serial_inbuffer) == 0 
    while obj.BytesAvailable > 0 
      A = fread(obj,1,'uint8');
      if A == 2 
        serial_inbuffer = [serial_inbuffer; A];
        break;
      end 
    end
  end
  
  % Read type and size fields
  if length(serial_inbuffer) < 2
    if ( obj.BytesAvailable >= 1 )
      A = fread(obj,1,'uint8');
      serial_inbuffer = [serial_inbuffer; A];
    
      if ( length(A) ~= 1 )
        serial_inbuffer = [];
        fprintf(1, 'INVALID PACKET RECEIVED [A]\n');
        return;
      end
    else
      return;
    end
  end
  
  % Read the data
  if length(serial_inbuffer) >= 2  
    size = serial_inbuffer(2);
  
    if obj.BytesAvailable > 0  
      A = fread(obj, size + 3 - length(serial_inbuffer), 'uint8');
      serial_inbuffer = ...
        [serial_inbuffer; A];
    
      % Process the packet if it is complete
      if length(serial_inbuffer) == size + 3
        
        % Check for invalid packets (missing stop byte)
        if serial_inbuffer(end) ~= 3  
          fprintf(1, 'INVALID PACKET RECEIVED; MISSING STOP BYTE\n');
          serial_inbuffer = [];
          return;
        end
      
        % Extract data from the packet 
        DATA = serial_inbuffer(3:(3+size)); 
      
        seq = decode_word(DATA(1:4));
        slot = decode_word(DATA(5:6));
        
        if ( seq ~= current_seq ) 
          % Fill in the blanks with zeros if packets are missing
          if ( seq - current_seq > 1 )
            record_matrix = ...
              [record_matrix; zeros(4 * (seq - current_seq - 1), 18)];
          end 
          
          current_seq = seq;
          record_matrix = [record_matrix; zeros(4,18) ];
        end
        
        if record
          acc = [];
          magn = [
            decode_word(DATA(31:32)) ...
            decode_word(DATA(33:34)) ...
            decode_word(DATA(35:36)) ...
          ];
       
          for n = 0:3
            offset = n*6;
            acc = [...
              decode_word(DATA((offset+7):(offset+8))) ...
              decode_word(DATA((offset+9):(offset+10))) ...
              decode_word(DATA((offset+11):(offset+12))) ...
            ];
          
            record_matrix((end-3)+n, ((slot-1)*6+1):((slot-1)*6+6)) = [ ... 
              typecast(uint16(acc), 'int16') ...
              typecast(uint16(magn), 'int16') ...
            ];
          end
        end
         
        serial_inbuffer = [];
      end 
    end
  end
end
