function view_data(data_set)
%JIN May 2004
%This file is not doing any kind of filtering, the only purpose of the file
%is to illustrate how to use the datasets from the UTE. The user has to add 
%the filtering part ... good luck!



    load(data_set);
    load('landmarks_pos110');  %landmarks data set, if there is any!
    global hhh;
    FrontEnd(GPS1,landmarks_pos);
    

    %Init variables
    nn=length(Index);
    xdr=[GPS1(1,3),GPS1(1,2),-110*(pi/180)];  %init position for the dead reckoning integration
    iglobal=1;
    dt_sensors=0.025;
    
    %---main program-----------------------------------------------------------------------------------------------------------
    for i=1:nn
        sensor_id=Sensor(iglobal);  
        time=Time(iglobal);
     
        if (sensor_id==1)
            %GPS
            gps_position=[GPS1(Index(iglobal),3),GPS1(Index(iglobal),2)];
            set(hhh(1),'xdata',gps_position(1),'ydata',gps_position(2)) ;
            
        elseif (sensor_id==2)
            %Encoder\Steering sensors
            
            xdr=DRModel(xdr,Steering1(Index(iglobal)),Velocity1(Index(iglobal)),dt_sensors,1);
            set(hhh(2),'xdata',xdr(1),'ydata',xdr(2)) ;
            set(hhh(3),'xdata',xdr(1),'ydata',xdr(2)) ;
            
        elseif (sensor_id==3)
            %LASER
            xp=[0,0,pi/2];       %xp is the vehicle pose  
            plot_laser(xp,Laser1(Index(iglobal),:),Intensity1(Index(iglobal),:));
            
        elseif (sensor_id==4)
            %INS

        end
        printInPlot(time,gps_position);
        iglobal=iglobal+1;
    end
    
return;
%-----------------------------end of main-----------------------------------------------------------------------------------------
%---------------------------------------------------------------------------------------------------------------------------------
function plot_laser(xp,RR,a)
  %This function is only to plot the laser view
    global hhh;
    aa = [0:360]*pi/360 ;
    
    ii=find(RR<50 & RR>1) ;
    aa2=aa(ii) ; xx = RR(ii).*cos(aa2+xp(3)-pi/2) ;yy = RR(ii).*sin(aa2+xp(3)-pi/2) ;     %All points
    set(hhh(11),'XData',xx+xp(1),'YData',yy+xp(2)); 

    ii=find(a>0) ;
    if ~isempty(ii)
        aa2=aa(ii) ; xx = RR(ii).*cos(aa2+xp(3)-pi/2) ;yy = RR(ii).*sin(aa2+xp(3)-pi/2) ;     %High intensity points
        set(hhh(12),'XData',xx+xp(1),'YData',yy+xp(2)); 
    end
    
    pause(0.01);
              
 return;
%---------------------------------------------------------------------------------------------------------------------------------
function FrontEnd(GPS1,landmarks_pos)

    global hhh hcc;
    
%---figure 2--------------------------------------------------------------------------------------------
    a2=figure(2); clf; %axis([xmin xmax ymin ymax]); 
    hold on; 
    box on; axis([-40 40 0 40]);
    xlabel('(metres)');ylabel('(metres)'); 
    title('Laser View');
    set(a2,'Name','LASER', 'NumberTitle','off');
   

%----plots-----------------------------------------------------------------------------------------------

    hhh(11)=plot(0,0,'b.','erasemode','xor') ;                      %laser: all points
    hhh(12)=plot(0,0,'r*','erasemode','xor') ;                      %laser: high intensity points
    legend('All points','High Intensity')
    hold off;

    
%---figure 1--------------------------------------------------------------------------------------------
    a1=figure(1); clf; %axis([xmin xmax ymin ymax]); 
    hold on; 
    box on; axis equal;
    xlabel('East (metres)');ylabel('North (metres)'); 
    title('GPS - Dead Reckoning');
    set(a1,'Name','GPS - Dead Reckoning', 'NumberTitle','off');
    
%---control buttons------------------------------------------------------------------------------------

  	hcc(1) = uicontrol('Style','text','Position',[ 110 15 200 20]);
    
%----plots-----------------------------------------------------------------------------------------------
    plot(landmarks_pos(:,1),landmarks_pos(:,2),'r*')
    plot(GPS1(:,3),GPS1(:,2),'b');

    hhh(1)=plot(0,0,'ko','markersize',10,'markerfacecolor','r','erasemode','xor') ;     %robot position
    hhh(2)=plot(0,0,'ko','markersize',10,'markerfacecolor','b','erasemode','xor') ;     %robot position
    hhh(3)=plot(0,0,'r','erasemode','none') ;  %dead reckoning position
    legend('Landmarks','GPS','GPS','DR') 
    hold off;
    

return;
%----------------------------------------------------------------------------------------------------------------------------------
function xp=DRModel(xp,steering,speed, dt,UteModel)
%Dead reckoning model for the ute: ackerman model with the encoder in the left wheel
    cosx=cos(xp(3)); sinx=sin(xp(3)); tans=tan(steering);
    L=2.83 ;h=0.76; b=1.21-1.42/2; a=3.78;
    if UteModel                %ute model with the laser in front
        speed=speed/(1-tan(steering)*h/L);                
        ds = speed*dt; vcL=ds/L;  
        xp(1) = xp(1) + ds*cosx-(vcL*tans)*(a*sinx+b*cosx) ;
        xp(2) = xp(2) + ds*sinx+(vcL*tans)*(a*cosx-b*sinx) ;
        xp(3) = xp(3) + vcL*tans ;
    else                            %point model
        ds = speed*dt;
        xp(1) = xp(1) + ds*cosx ;   
        xp(2) = xp(2) + ds*sinx ;
        xp(3) = xp(3) + ds*tans/L ;
    end
    xp(3) = NormalizeAngle(xp(3));
return;
%----------------------------------------------------------------------------------------------------------------------------------
function printInPlot(tt,xgps);
global hcc;

    tx1 = sprintf('time=%.2f secs',tt) ;

    tx2 = ['    pos:',sprintf('[%.2f]',[xgps(1),xgps(2)])];
    set(hcc(1),'string',[tx1,tx2])    ;
return;
%-----------------------------------------------------------
function a = NormalizeAngle(a)             %to normalize angle between -pi and pi
    pi2 = 2*pi ;                            
    ii=find(a>=pi); a(ii)=a(ii)-pi2;
    ii=find(a<-pi); a(ii)=a(ii)+pi2;
return ; 
%----------------------------------------------------------------------------------------------------------------------------------
