%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PURPOSE: Matlab-based iUsbl driver.
% FIRST AUTHOR:  Rob McEwen  06/7/19
% CHANGELOG:     (1)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% First open the serial port:
sd = serial( 'COM1', 'baudrate', 9600, 'databits', 8, 'parity', 'no', ...
    'stopbits', 1, 'flowcontrol', 'hardware')
fopen(sd)
%
% First  set up the Usbl:
%
usblSetup{1}   = readwrite( sd, 'PG3' );
usblSetup{2}   = readwrite( sd, 'PL0' );
usblSetup{3}   = readwrite( sd, 'LW3' );    %Not accepted?
usblSetup{4}   = readwrite( sd, 'SV' );
usblSetup{5}   = readwrite( sd, 'RG' );     %Not accepted?
usblSetup{6}   = readwrite( sd, 'PDT' );
usblSetup{7}   = readwrite( sd, 'MRPL2' );
usblSetup{8}   = readwrite( sd, 'UAL' );
usblSetup{9}   = readwrite( sd, 'VJSE' );
posCmd = 'POS V7,7'
npings = 5;
%
% Begin looping and collecting data:
%
clear posStr posdStr;
t0 = clock;
for( i=1:npings )
  tic 
  response  = readwrite( sd, posCmd);
  tat = toc;
  time = etime(clock,t0)
  %
  % Save the raw data:
  %
  posStr{i,1} = time;
  posStr{i,2} = response;
  posStr{i,3} = readwrite( sd, 'POSD');
  %
  % Now begin parsing and processing.  Example strings:
  %
  % FV V8.03 HMF,XC,LB,USBL(GDT),PT
  % POS V7,7 R2.66,X0.02,Y0.10,Z-2.66
  % POSD S107.5,N16.0,D-30,U8,Q0.002187
  %
  posData  = sscanf(response, [posCmd, ' R%f,X%f,Y%f,Z%f'],[1,4]);
  rlen = length(posData);
  posdData    = sscanf(posStr{i,3}, 'POSD%*cS%f,N%f,D%f,U%f,Q%f',[1,5]);
  if( sscanf(posStr{i,3}, '%cS') =='+' ) clipped = 1;
  else clipped = 0;
  end
  %
  % Build up the usbllog array.  Insert error strings in the raw data array
  % where appropriate.
  %
  if( rlen==0 )
    status = 3;
    posStr{i,2} = 'Timed out';
    usbllog(i,:) = [time NaN(1,4) tat status NaN(1,6) ];
  elseif( rlen==1 )
    status = 2;
    usbllog(i,:) = [time posData NaN(1,3) tat status posdData clipped];
  elseif( rlen==4 )
    status = 1;
    usbllog(i,:) = [time posData tat status posdData clipped];
  else
    status = 0;
    errstr = sprintf('Error - Unexpected response!\n');
    posStr{i,2} = errstr;
    usbllog(i,:) = [NaN(1,4) tat status NaN(1,6) ];
  end
end
%
% Generate the column-based indirect addresses:
%

lbls  = {
'sonardynetime'
'sonardyneRange'
'sonardyneX'
'sonardyneY'
'sonardyneZ'
'sonardyneTAT'
'sonardyneStatus'
'sonardyneSigLevel'
'sonardyneDetectLevel'
'sonardyneDoppler'
'sonardyneAcceptance'
'sonardyneQuality'
'sonardyneClipped'
};
i = 1;
while( i <= length(lbls) ),
  eval([ char(lbls(i))  ' = '  num2str(i) ';']);
  i = i + 1;
end

nameStr = datestr(clock,30);
eval([ 'save ', nameStr, ' usblSetup posStr usbllog sonardyne*' ])

fclose(sd);
%delete(sd);
%clear sd

