%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PURPOSE: Plot range and computed altitude.
% AUTHOR:  rsm, following MIT code.
% DATE:    00/1/5
% MODS:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%

% Backward compatibility
if( exist('dataES') == 1 )
    es.data     = dataES;
    es.status   = ESstatus;
    es.range    = ESrange;
    % for( i=0:499 )
    %   eval([ 'es.raw' num2str(i) ' = ESraw'  num2str(i) ';']);
    %   eval([ 'clear ESraw'  num2str(i) ';']);
    % end
    clear dataES ESstatus ESrange;
end

global noReturnRange
noReturnRange=65;

%
% Plot hand-exctracted SAN times, if available.  Create SanTimes.txt with
%
% grep "SAN - t" syslog | cut -d" " -f6 > SanTimes.txt
%
if( exist('SanTimes.txt') )
    sanTimes=textread('SanTimes.txt','%f,') - startTime;
end

global titleStr;
global vehicleName;

if exist( 'h_alt' ) ~= 1
    h_alt = figure;
    figure( h_alt );
else
    figure( h_alt );
end

set( gcf, 'name', [vehicleName,' Altimeter'] );

%mask = find( alt_m > 0 & alt_m < 99 );
%if exist( 'medfilt1' ) == 2 & ~isempty(mask)
%    alt_c = medfilt1( alt_m(mask), 5 );
%else
%    alt_c = alt_m(mask);
%end
if( exist('fontsize') == 0), fontsize = 14; end
if( fontsize ==12 ), linewidth = .5;end
fontweight = 'normal';
if( exist('plotSwIce') ~= 1 ) plotSwIce = 0; end
if( exist('postprocalt') ~= 1 ) postprocalt = 1; end
if( exist('plotSwEcho') ~= 1 ) plotSwEcho = 0; end
set(gca,'fontsize',fontsize,'fontweigh',fontweight)

winpos( position(1,:) );

altpstruct.strip(1)=subplot( 2, 1, 1 );


%
%  set scaleFix to 1 when the code is fixed.
%  scaleFix = 1/cos(pi/6) = 2/sqrt(3).
%
datecol = sscanf(DirName,'%d.%d.%d');
if( exist('dataSim') ~= 1 & ( datecol(1) < 2010 | (datecol(1) == 2010 & ...
						   datecol(2) <= 334 ) ) )
    dvlScaleFix = 2/sqrt(3);
else
    dvlScaleFix = 1;
end
fprintf('dvlScaleFix = %.2f.\n', dvlScaleFix);

time = dataNav(:,1);
%
% Get rid of the 1000 meter default for plotting purposes.
%
procAlt = dataNav(:,mAltitude)*dvlScaleFix;
%Indx = find(procAlt >= 290);
Indx = find(procAlt >= 200 | (procAlt >=100 & dataNav(:,1) < 10 ) );
procAlt(Indx) = 0*ones(length(Indx),1);
%procAlt(Indx) = NaN(length(Indx),1);    %Fill won't fill with NaN's.
%
%
%if( exist('dvlRange')==1 ),
%  dvlAlt2 = dataDvl(:,dvlRange);
%  indx2 = find( dvlAlt2 == 0. );  
%  dvlAlt2(indx2) = NaN(length(indx2),1);
%  dvlRangeInterp = interp1(dataDvl(:,1),dvlAlt2,time);
%  if( postprocalt) 
%    procAlt = dvlRangeInterp .* cos(dataNav(:,mPhi)) .* ...
%	cos(dataNav(:,mTheta));
%  end
%end
%
% Compute distance made good (x-y plane arc length)
if( exist('posx') & exist('posy') )
    %
    % For 2004.174.10, the ones() is 24kx24k, which exceeds an internal size limit.
    %arclen=tril( ones(length(posx), length(posx)) )*...
    %	 [0; sqrt( abs(diff(posx).*diff(posy)) ) ];
    fprintf('Starting ''for'' loop to compute arc length... \n');
    tic
    arcsegs = [0; sqrt( diff(posx).^2 + diff(posy).^2 ) ];
    arclen = 0*arcsegs;
    for( i=2:length(arcsegs) )
        arclen(i) = arclen(i-1) + arcsegs(i);
    end
    fprintf('''For'' loop finished.\n');
    toc
end
if( exist('dmgSw') == 0 ) dmgSw = 1;end
if( length(arclen) == length(dataNav(:,1)) & dmgSw )
    navxaxis = arclen;
    if( exist('dataDvl') == 1 )
        dvlxaxis    = interp1(dataNav(:,1), navxaxis, dataDvl(:,1) );
    end
    if( exist('dataPSA') == 1 & 0)
        psaxaxis    = interp1(dataNav(:,1), navxaxis, psa916(:,1) );
    end
    if( exist('PA200LR') == 1 )
        pa200xaxis    = interp1(dataNav(:,1), navxaxis, PA200LR(:,1) );
    end
    if( exist('es') == 1 )
        echoxaxis     = interp1(dataNav(:,1), navxaxis, es.data(:,1) );
    end
    if( exist('sanTimes') == 1 )
        stArcLenx     = interp1(dataNav(:,1), navxaxis, sanTimes );
    end
    dcaxis     = interp1(dataNav(:,1), navxaxis, dataDC(:,1) );
    useArcLen = 1;
else
    navxaxis = time;
    dcaxis  = time;
    useArcLen = 0;
end


if( plotSwIce == 0 )
    plot( navxaxis, dataNav(:,mDepth), 'm', ...
          navxaxis, dataNav(:,mDepth) + procAlt, 'b' );
    %if(useArcLen == 1) axis equal; end
    area(navxaxis, dataNav(:,mDepth) + procAlt, 'facecolor', [0 .75 .85]);
    if( exist('mDvlAltitude') == 1 )
        dvlAltitude = dataNav(:,mDvlAltitude)*dvlScaleFix;
        indx = find(dvlAltitude > 250);
        dvlAltitude(indx) = NaN(length(indx),1);

        line( navxaxis(5:end), ...
              dataNav(5:end,mDepth) + dvlAltitude(5:end), ...
              'color','m','linestyle','--');
        %       dataNav(5:end,mDepth) + dataNav(5:end,mDvlAltitude), ...
    end

    hold on

    if( exist('alt2') == 1 )
        line( navxaxis, dataNav(:,mDepth) + alt2, ...
              'color','m','linestyle','--'); 
    end

    if( exist( 'dataSim' ) == 1 & exist( 'sWaterDepth' ) == 1 )
        indxStuck=find( diff(dataSim(:,1)) == 0 );
        if( ~isempty(indxStuck) )
            fprintf('WARNING dataSim(:,1) has dropouts at:\n');
            fprintf('  %d\n',indxStuck);
            for( i=indxStuck ) 
                if( i== 1 ) 
                    dataSim(1,1)=.1;
                else
                    dataSim(i,1) = dataSim(i-1,1)+.2;
                end
            end

        end
        simxaxis    = interp1(dataNav(:,1), navxaxis, dataSim(:,1) );
        interpDepth = interp1(dataSim(:,1), dataSim(:,sWaterDepth), navxaxis );
        line( simxaxis, dataSim(:,sWaterDepth),...
              'linewidth', linewidth, 'Color','g' );
    end


elseif( plotSwIce == 1 )
    plot( navxaxis, dataNav(:,mDepth), 'm', ...
          navxaxis, dataNav(:,mDepth) - procAlt, 'b' );
    area(navxaxis, dataNav(:,mDepth), 'facecolor', [0 .75 .85]);
    %area(time, dataNav(:,mDepth)-procAlt, ...
    %      'facecolor','g');
    %     'facecolor', [0 .75 .85]);
    %     'facecolor', [0 .75 .5] );
end %if( plotSwIce == 0 )

set( gca, 'YDir', 'reverse' );
set(gca,'fontsize',fontsize,'fontweigh',fontweight)
altpXlim = get(gca,'XLim');
line( navxaxis, dataNav(:,mDepth), 'color', [1 0.5 0], 'linewidth', ...
      linewidth );
%title( ['Measured Depth and Calculated Bottom: ' titleStr] );
title( ['Vehicle and Bottom Depth: ' titleStr] );
ylabel( 'Depth, Meters' );

npts = length(navxaxis);
cax  = [1 npts]; 
%cmp  = colormap(jet(npts));
cmp  = jet(npts);



if 0
    if( exist('mEchoAltitude') == 1 )
        echoAltitude = dataNav(:,mEchoAltitude);
        indxA = find(echoAltitude > 90);
        echoAltitude(indxA) = NaN(length(indxA),1);
    end

    if( exist('mEchoHorizRange') == 1 )
        echoRange    = dataNav(:,mEchoHorizRange);
        indxR = find(echoRange > 90);
        echoRange(indxR)    = NaN(length(indxR),1);
        fprintf('Starting ''for'' loop to plot echo sounder hits... \n');
        tic
        for( i=npts:-1:1 )
            plot( navxaxis(i)+echoRange(i), dataNav(i,mDepth)+echoAltitude(i),...
                  '+','color',cmp(i,:));
            plot( navxaxis(i), dataNav(i,mDepth),'d','color',cmp(i,:));
        end
        fprintf('''For'' loop finished.\n');
        toc
    end


elseif( plotSwEcho )


    if( exist('mEchoAltitudeLast') == 1 )
        echoAltitudeLast = dataNav(:,mEchoAltitudeLast);
        echoAltitude     = dataNav(:,mEchoAltitude);
        indxA = find( echoAltitudeLast < 90 & echoAltitudeLast ~= 0 );
    elseif(exist('mEchoAltitude') == 1 )
        echoAltitude     = dataNav(:,mEchoAltitude);
        indxA = find( echoAltitude < 90 & echoAltitude ~= 0 );
    end

    if( exist('mEchoHorizRangeLast') == 1 )
        echoRangeLast    = dataNav(:,mEchoHorizRangeLast);
        echoRange        = dataNav(:,mEchoHorizRange);
        indxR = find( echoRangeLast < 90 & echoRangeLast ~=0 );
        lenIndxR = length(indxR);
        fprintf('Starting ''for'' loop to plot echo sounder hits... \n');
        tic
        for( i=lenIndxR:-1:1 )
            %
            % Beware: this particular plot doesn't make sense when navxaxis is
            % in seconds, because echoRangeLast is always meters.
            %
            plot( navxaxis(indxR(i))+echoRangeLast(indxR(i)),...
                  dataNav(indxR(i),mDepth)+echoAltitudeLast(indxR(i)),...
                  '+','color',cmp(indxR(i),:));
            plot( navxaxis(indxR(i)), dataNav(indxR(i),mDepth),'d','color',cmp(indxR(i),:));
        end
        fprintf('''For'' loop finished.\n');
        toc
        indxRm = find( echoRange < 65 & echoAltitude < 65 & echoRange ~=0 );
        plot( navxaxis(indxRm)+echoRange(indxRm),...
              dataNav(indxRm,mDepth)+echoAltitude(indxRm),...
              'o', 'markersize',4,'color','k','markerfacecolor','k');
        
        fprintf('Starting ''for'' loop to plot echo sounder vectors... \n');
        tic
        for( i=lenIndxR:-1:1 )
            plot( [navxaxis(indxR(i))  navxaxis(indxR(i))+echoRangeLast(indxR(i))],...
                  [dataNav(indxR(i),mDepth)  dataNav(indxR(i),mDepth)+echoAltitudeLast(indxR(i))],...
                  'color',cmp(indxR(i),:));
        end
        fprintf('''For'' loop finished.\n');
        toc
        
    elseif( exist('mEchoHorizRange') == 1 )
        %
        % For backward compatibility:
        echoRange        = dataNav(:,mEchoHorizRange);
        indxR = find( echoRange < 65 & echoRange ~=0 );
        lenIndxR = length(indxR);
        fprintf('Starting ''for'' loop to plot echo sounder hits... \n');
        tic
        for( i=lenIndxR:-1:1 )
            plot( navxaxis(indxR(i))+echoRange(indxR(i)),...
                  dataNav(indxR(i),mDepth)+echoAltitude(indxR(i)),...
                  '+','color',cmp(indxR(i),:));
            plot( navxaxis(indxR(i)), dataNav(indxR(i),mDepth),'d','color',cmp(indxR(i),:));
        end
        fprintf('''For'' loop finished.\n');
        toc
        
        fprintf('Starting ''for'' loop to plot echo sounder vectors... \n');
        tic
        for( i=lenIndxR:-1:1 )
            plot( [navxaxis(indxR(i))  navxaxis(indxR(i))+echoRange(indxR(i))],...
                  [dataNav(indxR(i),mDepth)  dataNav(indxR(i),mDepth)+echoAltitude(indxR(i))],...
                  'color',cmp(indxR(i),:));
        end
        fprintf('''For'' loop finished.\n');
        toc
        
    end

end  



if( exist( 'dataSim' ) == 1 )
    altps.pOn  = find( diff(dataSim(:,sOmega_P)) > 0 );
    altps.pOff = find( diff(dataSim(:,sOmega_P)) < 0 );
    %if( altps.pOn ~= [] )
    line( navxaxis(altps.pOn), dataNav(altps.pOn,mDepth), ...
          'color', 'g', 'linestyle','x', 'linewidth', linewidth );
    %end
    %if( altps.pOff ~= [] )
    line( navxaxis(altps.pOff), dataNav(altps.pOff,mDepth), ...
          'color', 'r', 'linestyle','x', 'linewidth',  linewidth );
    %end
end

if(useArcLen)
    xlabel( 'Distance Travelled, Meters' );
else
    xlabel( 'Elapsed Time, Seconds' );
end
zoom on
grid on
hold off
%
% Turn this off for now.
%
if( exist('dvlRange')==1 & 0),
    dvlRangeInterp = interp1(dataDvl(:,1),dataDvl(:,dvlRange),navxaxis);
    dvlAlt = dvlRangeInterp .* cos(dataNav(:,mPhi)) .* cos(dataNav(:,mTheta));
    [xx,yy]  = stairs( navxaxis, dataNav(:,mDepth)+dvlAlt );
    line( xx, yy, 'color','g')
end







altpstruct.strip(2) = subplot( 2, 1, 2 );
if(useArcLen) xaxisD = dvlxaxis;
else          xaxisD = dataDvl(:,1);
end

if( exist('es') == 1 )
    if(useArcLen) xaxisE = echoxaxis;
    else          xaxisE = es.data(:,1);
    end
end

if( exist('sanTimes') == 1 )
    if(useArcLen) xaxisST = stArcLenx;
    else          xaxisST = sanTimes;
    end
end
%  plot(xaxisD, dataDvl(:,[dvlRange dvlBeam1 dvlBeam2 dvlBeam3 dvlBeam4]), ...
%      navxaxis, procAlt)
lstr = {};
if( exist('dvlBeam1') == 1 ),
    handle=plot(xaxisD, dataDvl(:,[dvlBeam1 dvlBeam2 dvlBeam3 dvlBeam4]), ...
                navxaxis, procAlt);
    lstr(:,1) = { 'Beam1','Beam2','Beam3','Beam4','Altitude'};
    set(handle(5),'linewidth',3);
    hold on
    if( Indx )
        plot( navxaxis(Indx), 0*procAlt(Indx), 'mx','markersize',10);
        lstr(length(lstr)+1,1) = { 'Invalid Altitude' };
    end
else
    handle=plot(navxaxis, procAlt);
    lstr(:,1) = {'Altitude'};
    set(handle,'linewidth',3);
end
hold on

plot(navxaxis, dvlAltitude,'--');
lstr(length(lstr)+1,1) = {'Dvl Altitude'};

if( exist('idt')==1 && ~isfield(idt,'vertMt') ) idt.vertMt == 1;end
if( exist('idt')==1 && isfield(idt,'data') && idt.vertMt == 1)
    mbIndx = find(dataNav(:,multibeamAltitude) >= 1000);
    mbAltitude = dataNav(:,multibeamAltitude);
    mbAltitude(mbIndx) = 0*ones(length(mbIndx),1);
    if(mbIndx)
        plot(navxaxis(mbIndx), 0*mbAltitude(mbIndx),'kd','markersize',5);
        lstr(length(lstr)+1,1) = {'Invalid Imagenex Altitude'};
    end
    plot(navxaxis, mbAltitude,'k--');
    lstr(length(lstr)+1,1) = {'Imagenex Altitude'};
end

if( exist('mEchoAltitude') == 1 )
    echoAltitude = dataNav(:,mEchoAltitude);
    indxInval = find(echoAltitude >= 1000);
    indx = find(echoAltitude > 100 );
    %echoAltitude(indx) = NaN(length(indx),1);
    echoAltitude(indx) = 100*ones(length(indx),1);
    plot(navxaxis, echoAltitude,'g--');
    lstr(length(lstr)+1,1) = {'Imagenex Horizon'};
    if( indxInval )
        plot(navxaxis(indxInval), 0*echoAltitude(indxInval),'gd','markersize',5);
        lstr(length(lstr)+1,1) = {'Invalid Imagenex Horizon'};
    end
    indxHoriz=find(dataNav(:,mEchoHorizRange)<30);
    plot(navxaxis(indxHoriz), echoAltitude(indxHoriz),'r--');
    lstr(length(lstr)+1,1) = {'Imagenex Horizon < 30'};
    if( exist('wpb') )
        plot(wpb.data(:,1), wpb.data(:,wpb.filtHoriz), 'g-.')
        lstr(length(lstr)+1,1) = {'Filtered Imagenex Horizon < avoidRange'};
    end
end

if( exist('plotSwAlt') == 0 ) plotSwAlt = 0;end
if( plotSwAlt )
    plot(dcaxis, dataDC(:,cDepth),'r--')
    lstr(length(lstr)+1,1) = {'Altitude Cmd'};
end



set(altpstruct.strip(2),'XLim',altpXlim);
set(altpstruct.strip(2),'fontsize',fontsize,'fontweigh',fontweight);
if( exist('psa916') == 1 )
    line( navxaxis, dataNav(:,mPsaRange), 'color','k');
    lstr(length(lstr)+1,1) = {'Psa916 Range'};
end
if( exist('es') == 1 && plotSwEcho )
    indxES = find(es.data(:,es.range) ~= 0);
    line( xaxisE(indxES), es.data(indxES,es.range)/100, 'color','k','linestyle','.');
    lstr(length(lstr)+1,1) = {'Scanning Imagenex Range'};
end
if( exist('PA200LR') == 1 )
    line( pa200xaxis, PA200LR(:,pa200range), 'color','k');
    lstr(length(lstr)+1,1) = {'Pa200LR Range'};
end
if( exist('sanTimes') == 1 )
    altSan     = interp1(dataNav(:,1), procAlt, sanTimes );
    plot( xaxisST, altSan, 'h', 'markersize',10,'color','k','markerfacecolor','k');
    lstr(length(lstr)+1,1) = {'SAN Updates'};
end
if(useArcLen)
    xlabel( 'Distance Travelled, Meters' );
    xaxisD = dvlxaxis;
else
    xaxisD = dataDvl(:,1);
    xlabel( 'Elapsed Time, Seconds' );
end
legend(lstr);
altpTitleStr = sprintf( 'Dvl Range. dvlScaleFix = %.2f', dvlScaleFix );
title( altpTitleStr );
ylabel('Range, Meters')
zoom on
grid on
hold off

linkaxes(altpstruct.strip,'x');

