%  how to sync sonar times?
%  ok dataNav(:,mDepth)
%  idt_fwd have 120 or 60 beams?
%  verify that 1st 60 beams in idt_fwd = THE 60 beams
%  need logic that allows the AUV to surface (real returns) if we filter
%  out surface returns.
%  At start, on surface, need logic to ignore forward hits.
%  Have a learning phase to determine which beams are ok (consistency)
%  The sonar hits fall in a thick band -- due to a theta offset?  ID and
%  claibrate?
%  use mPos_x and mPos_y??
%  loadData seems to lose dataNav  (does my isnan check make it "work")


%  14 Nov 2014
%  my horizon calculation is a little different that Rob's -- but that
%  might be because my lookahead and range are different.


%  AI:  Downlooking sonar beams limited to 30-90. What did I do about forward beams?

    numberofsonars=2;

    lookahead=150;

    %
    baseline=0;
    filter=1;
    %
    goodstart=30;
    goodend=90;
    if baseline==1,
        goodstart=1;
        goodend=120;
        filter=0;
    end
    
    altstart=30;
    altend=90;

usemat=0;
if usemat==1
load 201417109.mat
else
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% I had trouble with loadData.  It only subtracted the startTime for
% dataNav.  Hack is to exit loadData early and for me to throw away the
% data
%if numberofsonars==2, loadData_mod_sr; end
end
dd1=idt.data;
if numberofsonars==2, dd2=idt_fwd.data; end
if numberofsonars==1, dd2=dd1; end
%dd2=dd1;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%





npts1=max(size(dd1));
tsonar1=dd1(:,1);

npts2=max(size(dd2));
tsonar2=dd2(:,1);

RobsMultAlt=dataNav(:,multibeamAltitude);
auvTheta=dataNav(:,mTheta);
auvDepth=dataNav(:,mDepth);
tNav=dataNav(:,1);
robHorizon=dataNav(:,mEchoAltitude);

xpos=dataNav(:,mPos_x);
ypos=dataNav(:,mPos_y);
xpos=xpos-xpos(1)*ones(size(xpos));
ypos=ypos-ypos(1)*ones(size(ypos));
pos=sqrt(xpos.*xpos+ypos.*ypos);

%velocity signal has dropouts
velx=dataDvl(:,dvlBotVelx);
vely=dataDvl(:,dvlBotVely);
vel=sqrt(velx.*velx+vely.*vely);
tdvl=dataDvl(:,1);

tmax=max(tNav);
dt=.1;              % the smallest ping interval >0.1s.  Use dt=0.1 to see all the beams (slow)
tsonar=0:dt:tmax;   % the time between pings varies from 0.1 to 0.4s (about)
npts=max(size(tsonar));



%npts=npts1;
%tsonar=tsonar1;
%if npts2<npts1,
%    npts=npts2;
%    tsonar=tsonar2;
%end

RobsImagenexAlt=interp1(tNav,RobsMultAlt,tsonar);
auvvel=interp1(tdvl,vel,tsonar);
theta=interp1(tNav,auvTheta,tsonar);
depth=interp1(tNav,auvDepth,tsonar);
robHor=interp1(tNav,robHorizon,tsonar);
%AUVxout=interp1(tNav,pos,tsonar);
AUVxout=zeros(1,npts);
altitude=zeros(1,npts);
for i=1:npts
    if isnan(theta(i)), theta(i)=0; end
    if isnan(depth(i)), depth(i)=0; end
    if isnan(AUVxout(i)), AUVxout(i)=0; end
    if isnan(auvvel(i)), auvvel(i)=0;  end
    if auvvel(i)>1.75, auvvel(i)=1.5; end
end


angs1=-150:1:-31;   %beam angles wrt AUV
angs2=90*ones(1,120);
%angs2(1:60)=-30:1:29;
angs2(1:120)=-60:1:59;
angs1=angs1*pi/180;
angs2=angs2*pi/180;
if numberofsonars==1, angs2=angs1; end

nptssonar1=max(size(tsonar1));
nptssonar2=max(size(tsonar2));
beam1datatemp=dd1(1:nptssonar1,4:123);
beam2datatemp=dd2(1:nptssonar2,4:123);


dtsonar1=zeros(1,nptssonar1);
dtsonar2=zeros(1,nptssonar2);
for i=2:nptssonar1
    dtsonar1(i)=tsonar1(i)-tsonar1(i-1);
    if dtsonar1(i)<=0, tsonar1(i)=tsonar(i)+.001; end   %  11/13/2014
end
for i=2:nptssonar2
    dtsonar2(i)=tsonar2(i)-tsonar2(i-1);
    if dtsonar2(i)<=0, tsonar2(i)=tsonar(i)+.001; end
end

if( 0 )
    figure(10)
    plot(tsonar1,dtsonar1,'.')
    title('time between pings:  Down-Sonar')
    xlabel('mission time (s)')
    ylabel('dt (s)')
    figure(11)
    plot(tsonar2,dtsonar2,'.')
    title('time between pings:  FWD-Sonar')
    xlabel('mission time (s)')
    ylabel('dt (s)')
end


%
%%% demand 3 pings in row before declaring a beam to be good
beam1data=zeros(size(beam1datatemp));
beam2data=zeros(size(beam2datatemp));

npersist=1;
nptssonars=min(nptssonar1,nptssonar2);
for i=1:nptssonars-npersist
    for j=1:120
    C=1;
        for k=1:npersist
            if beam1datatemp(i+k-1,j)==0, C=0*C; end
        end
    if C==1, beam1data(i,j)=beam1datatemp(i,j); end
    end
end
for i=1:nptssonars-npersist
    for j=1:120
    C=1;
        for k=1:npersist
            if beam2datatemp(i+k-1,j)==0, C=0*C; end
        end
    if C==1, beam2data(i,j)=beam2datatemp(i,j); end
    end
end   



%beam1data=beam1datatemp;
%beam2data=beam2datatemp;

beams1=interp1(tsonar1,beam1data,tsonar,'nearest');
beams2=interp1(tsonar2,beam2data,tsonar,'nearest');

if numberofsonars==1, beams2=0*beams2; end

maxrange=150;  %only used for plotting

%  View which beams are "active" in each sonar
ploton=0;
if ploton==1.
for i=1:npts
    plot(beams1(i,:),'*b')
    axis([0 120 0 200])
    hold on
    plot(beams2(i,:),'*r')
    hold off
    pause(0.01)
end
end

if( 0 )
 if( exist('position') ~=1 ) 
   npposition = [4 32 1100 900];
 else
   npposition = position;
 end
end

firstfirst = 1;
        
% if(exist('altpstruct') && isfield(altpstruct,'strip'))
%     set(altpstruct.strip(1),'nextplot','replace');
% end
clear dotHandle jj tss

if( exist( 'movingSonarFig' ) ~= 1 )
    movingSonarFig = figure;
    %set(gcf,'position',npposition)
    set(gcf, 'Name', 'twoSonar', 'DeleteFcn', 'clear movingSonarFig' );
    set(gca,'fontsize',fontsize,'fontweigh',fontweight)
    set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)
    
    PauseButton_h = uicontrol('Style', 'pushbutton',...
                              'String', 'Click to Pause','FontSize',16,...
                              'BackgroundColor','g',...
                              'Position', [20 20 180 50 ],...
                              'Callback',@pause_Callback);
    % 
    % Beware that the string 'Click to Pause' is also the state.  If you change
    % this string, you must also change it in pause_Callback.m
    % and in the waitfor() below.
    %
else
    try
        figure(movingSonarFig);
    catch
        %
        % The figure has apparently been deleted, but the
        % handle remains.  Delete the handle and start over.
        %
        clear movingSonarFig
        movingSonarFig = figure;
    end
end


%loop twice:  1st time plot whole trajectory
%             2nd time do animation
%for iloop=1:2
for iloop=2
    ploton=1;
    if iloop==1, ploton=0; end

    % if iloop==2,
    %     figure(20)
    %     plot(AUVxout,-depth,'.',AUVxout,bottom,'.')
    %     xlabel('AUV x position (m)')
    %     title('AUV and Bottom profiles')
    % end

    trail=600;

    bottom=zeros(1,npts);
    altitude=zeros(1,npts);
    xrange1=zeros(npts,120);
    zrange1=zeros(npts,120);
    xrange2=zeros(npts,120);
    zrange2=zeros(npts,120);

    horizonout=zeros(1,npts);
    zout1=zeros(npts,120);
    xout1=zeros(npts,120);
    zout2=zeros(npts,120);
    xout2=zeros(npts,120);
    bottom=zeros(1,npts);
    AUVx=0;
    horizon=-depth(1);  %ic for LPF
    horizonin=horizon;

    %    i = 1;
    %    while( i<npts && ~paused )
    

    for i=1:npts-1
        for j=1:120;
            xrange1(i,j)=beams1(i,j)*cos(theta(i)+angs1(j));
            zrange1(i,j)=beams1(i,j)*sin(theta(i)+angs1(j));
            xrange2(i,j)=beams2(i,j)*cos(theta(i)+angs2(j));
            zrange2(i,j)=beams2(i,j)*sin(theta(i)+angs2(j));    
        end
        
        %AUVx=AUVxout(i);
        attx=[AUVx-3*cos(theta(i)),AUVx+3*cos(theta(i))];
        attz= [-depth(i)-3*sin(theta(i)),-depth(i)+3*sin(theta(i))];
        AUVxout(i)=AUVx;
        zout1(i,1:120)=-depth(i)*ones(1,120)+zrange1(i,:);
        xout1(i,1:120)=AUVx*ones(1,120)+xrange1(i,:);
        zout2(i,1:120)=-depth(i)*ones(1,120)+zrange2(i,:);
        xout2(i,1:120)=AUVx*ones(1,120)+xrange2(i,:);
        


        
        altitude(i)=0;
        alts=zeros(1,120);

        k=0;
        for j=altstart:altend
            if beams1(i,j)>0,
                k=k+1;
                alts(k)=-zout1(i,j);
            end
        end
        if k>0, altitude(i)=-depth(i)+median(alts(1:k)); end
        
        
        % 11/15/2014  initialize horizons to the altitude
        %horizons=-150*ones(1,240);
        horizons=-(depth(i)+altitude(i))*ones(1,240);
        for j=1:120
            %if and(and(j>goodstart,j<goodend),beams1(i,j)>0); horizons(j)=zout1(i,j); end
            %if beams2(i,j)>0, horizons(120+j)=zout2(i,j); end

            if and(and(j>goodstart,j<goodend),and(beams1(i,j)>0,and(xrange1(i,j)<lookahead,xrange1(i,j)>0))); horizons(j)=zout1(i,j); end 
            if and(beams2(i,j)>0,and(xrange2(i,j)<lookahead,xrange2(i,j)>0)),horizons(120+j)=zout2(i,j); end

        end
        
        horizoninlast=horizon;
        horizonin=max(horizons);
        
        %LPF
        tau=3;
        T=0.3;
        a=1/tau;
        c1=1/(a+2/T);
        c2=-(a-2/T);
        horizon=c1*(  c2*horizon + a*horizonin + a*horizoninlast);
        if filter==0, horizon=horizonin; end
        horizonout(i)=horizon;
        bottom(i)=-depth(i)-altitude(i);
        
        bottom(i)=-depth(i)-RobsImagenexAlt(i);
        
        horx=[AUVx-maxrange,AUVx+maxrange];
        horz= [horizon,horizon];
        altx=[AUVx,AUVx];
        altz=[-depth(i),bottom(i)];
        
        robHorx=horx;
        robHorz=[-robHor(i)-depth(i),-robHor(i)-depth(i)];
        %robHorz=[horizon horizon];
        %
        
        if ploton==1,
            trailon=1;
            if trailon==1,
                if( ~exist('tss') )
                    tss.blackStars = plot(xout1(i,:),zout1(i,:),'k*');
		    title( ['Vehicle Altitude and Sonar Data: ' titleStr] );
		    ylabel( 'Depth, Meters' );
		    xlabel( 'Distance, Meters' );
                    hold on
                    tss.blueStars = plot(xout1(i,goodstart:goodend),...
                                         zout1(i,goodstart:goodend),'b*');
                    tss.redStars  = plot(xout2(i,:),zout2(i,:),'r*');
                    tss.attitude  = plot(attx,attz,'k');
                    tss.greenLine = plot(horx,horz,'g');
                    tss.vertLine  = plot(altx,altz,'k');
                    tss.blackLine = plot(horx,robHorz,'k');
                    hold off
                else
                    if( i>trail)
                        if( ~isfield(tss,'trail') )
                            hold on
                            tss.trail = plot(AUVxout(i-trail:i),...
                                             bottom(i-trail:i),'.g');
                            hold off
                        else
                            tss.trail.XData = AUVxout(i-trail:i);
                            tss.trail.YData = bottom(i-trail:i);
                        end
                    end
                    tss.blackStars.XData = xout1(i,:);
                    tss.blackStars.YData = zout1(i,:);
                    tss.blueStars.XData  = xout1(i,goodstart:goodend);
                    tss.blueStars.YData  = zout1(i,goodstart:goodend);
                    tss.redStars.XData   = xout2(i,:);
                    tss.redStars.YData   = zout2(i,:);
                    tss.attitude.XData   = attx;
                    tss.attitude.YData   = attz;

                    tss.greenLine.XData   = horx;
                    tss.greenLine.YData   = horz;
                    tss.vertLine.XData    = altx;
                    tss.vertLine.YData    = altz;
                    tss.blackLine.XData   = horx;
                    tss.blackLine.YData   = robHorz;
                end
            else
                plot(xout1(i,:),zout1(i,:),'k*',...
                     xout1(i,goodstart:goodend),zout1(i,goodstart:goodend),'b*',...
                     xout2(i,:),zout2(i,:),'r*',attx,attz,'k',horx,horz,'g',...
                     altx,altz,'k',horx,robHorz,'k')    
            end
            
            axis([AUVx-100 AUVx+100 -125 10])
            %AUVx=AUVx+auvvel(i)*(tsonar(i+1)-tsonar(i))*cos(theta(i));

            pause(0.01)    

            waitfor( PauseButton_h, 'String','Click to Pause' );
            %
            % Check to see if the user has deleted the figure.  If so,
            % exit the loop
            try
                figure(movingSonarFig);
            catch
                %
                % The figure has apparently been deleted, but the
                % handle remains.  Delete the handle and bust out.
                %
                clear movingSonarFig;
                break;
            end

        end % ploton

        AUVx=AUVx+auvvel(i)*(tsonar(i+1)-tsonar(i))*cos(theta(i));


        if( 0 )
            if(exist('altpstruct','var') && isfield(altpstruct,'strip'))
                if(exist('dotHandle','var'))
                    %if(firstfirst==0)
                    while(AUVx > navxaxis(jj) && jj < length(navxaxis) ) jj=jj+1,end
                    %set(dotHandle, 'XData', navxaxis(jj));
                    %set(dotHandle, 'YData', dataNav(jj, mDepth));
                    dotHandle.XData=navxaxis(jj);
                    dotHandle.YData=dataNav(jj, mDepth);
                    jj
                    %drawnow
                else    
                    jj=1;
                    hold(altpstruct.strip(1),'on')
                    dotHandle = plot(altpstruct.strip(1), ...
                                     navxaxis(1), dataNav(1, mDepth),'mo');
                    hold(altpstruct.strip(1),'off')
                    firstfirst=0;
                end
            end
            %figure(movingSonarFig);
        end
        
        
    end  % i=1:npts

end   % iloop

if( 0 )
    figure(2)
    for i=1:npts-1
        plot(xout1(i,:),zout1(i,:),'b*',xout2(i,:),zout2(i,:),'r*')
        hold on
    end
    hold off
end

%figure(3)
%plot(AUVxout,-(depth'+altitude),AUVxout,horizonout)
%xlabel('AUV x position (m)')
%ylabel('(m)')
%title('Altitude and Horizon')
