function displayLog(logFile,mapFile,mapLimitsFile)
% function displayLog(logFile,mapFile,mapLimitsFile)
% Loads and displays the content of a log file, drawing the robot path and
% the laser scans. Usage example: 
%  displayLog('log.txt','OUT_LASER_SIM_GRIDMAP.BMP','OUT_LASER_SIM_GRIDMAP_LIMITS.TXT')

% Load map:
M=imread(mapFile);
M_limits=load(mapLimitsFile);
% Flip vertical:
 for i=1:(size(M,1)/2),
     d = M(i,:);
     M(i,:)=M(size(M,1)-i+1,:);
     M(size(M,1)-i+1,:)=d;
 end
fprintf('Map file loaded OK! %ix%i',size(M,1),size(M,2));

h=fopen(logFile,'rt');
if (h<0) 
    error('Cannot open log file!');
end;

fprintf('Leyendo fichero de log...\n');

primlin=fgetl(h);
if (~ischar(primlin)) 
    error('Formato incorrecto.');
end;

t_segs=0;
t_microsegs=0;
t_total=0;
xodom=0;
yodom=0;
phiodom=0;
xreal=0;
yreal=0;
phireal=0;
vellineal=0;
velang=0;
lasertam=0;
laser=0;

logind=1;
while (~feof(h))
    % lee e interpreta una linea de datos separados por espacios
    lin=fgetl(h);
    if (~ischar(lin))
        error('Error leyendo linea de datos de fichero.\n');
    end;
    lo=length(lin);
    f=1;
    ind=1;
    pos=1;
    vals=zeros(1,10);
    while (f<=lo)
     while ((f<=lo) & (lin(f)~=' ')) 
         f=f+1;
     end;
     n=lin(pos:f-1);
     vals(ind)=eval(n);
     ind=ind+1;
     if (ind>10) 
         ind=10;
         f=lo;
     end;
     pos=f+1;
     f=f+1;
    end; 
    if (ind~=10)
        error('Error de formato en linea de datos.');
    end;
    t_segs(logind)=vals(1);
    t_microsegs(logind)=vals(2);
    t_total(logind)=t_segs(logind)+t_microsegs(logind)/1e6;
    xodom(logind)=vals(3);
    yodom(logind)=vals(4);
    phiodom(logind)=vals(5);
    xreal(logind)=vals(6);
    yreal(logind)=vals(7);
    phireal(logind)=vals(8);
    vellineal(logind)=vals(9);
    velang(logind)=vals(10);
    
    lin=fgetl(h);
    if (~ischar(lin))
        error('Error leyendo linea de laser de fichero.');
    end;
    lo=length(lin);
    f=1;
    while ((f<=lo) & (lin(f)~='('))
        f=f+1;
    end;
    if (f>lo)
        error('Error de formato en linea de laser.');
    end;
    pos=f+1;
    f=f+1;
    while ((f<=lo) & (lin(f)~='p'))
        f=f+1;
    end;
    if (f>lo)
        error('Error de formato en linea de laser.');
    end;
    lasertam(logind)=eval(lin(pos:f-1));
    while ((f<=lo) & (lin(f)~=' '))
        f=f+1;
    end;
    if (f>lo)
        error('Error de formato en linea de laser.');
    end;
    f=f+1;
    pos=f;
    las=0;
    for x=(1:lasertam(logind))
      while ((f<=lo) & (lin(f)~=' '))
          f=f+1;
      end;
      if (f>lo)
          error('Error de formato en linea de laser.');
      end;
      las(x)=eval(lin(pos:f-1))/100;  % viene dado en centimetros  
      f=f+1;
      pos=f;
    end;
    laser(logind,1:lasertam(logind))=las;
    
    fprintf('Leida linea de log #%d\n',logind);
    logind=logind+1;
end;

fprintf('Fichero leido.\n');
fclose(h);

% Display background map:
figure; hold on;
xs = linspace(M_limits(1),M_limits(2),size(M,2));
ys = linspace(M_limits(3),M_limits(4),size(M,1));
imagesc(xs,ys,M);
colormap(gray); 
axis equal; 
set(gca,'YDir','normal');

plot(xodom,yodom,'*-b');
plot(xodom(logind-1),yodom(logind-1),'ok');
plot(xreal,yreal,'*-r');
plot(xodom(logind-1),yodom(logind-1),'ok');

for g=(1:logind-1)
    ang=-pi/2;
    incang=pi/lasertam(g);  % abarca 180 grados, con el inicio a la "-Y" y el robot mirando en el eje OX+
    posxr=xreal(g);
    posyr=yreal(g);
    orr=phireal(g)*pi/180;  % orientacion del robot: 0 para el eje OY+ universal
    for f=(1:lasertam(g))
        d=laser(g,f);
        anguniv=ang+orr;
        x=d*cos(anguniv) + posxr + cos(orr)*0.095;  % 0.095: Distancia desde origen del robot al laser en eje X
        y=d*sin(anguniv) + posyr + sin(orr)*0.095;
        c=1.0; %c=(logind-g-1)/(logind-2);
        plot(x,y,'Color',[0,0,c]);
        ang=ang+incang;
    end;
end;

xlabel('X (metros)');
ylabel('Y (metros)');
grid;
title('TRAYECTORIA DEL ROBOT (en negro la marca de fin)');
