%
% RUNLOCALIZATION(SIMOUTFILE, MAPFILE, SENSORPOSE)
%
% 
function ekf_locate(simoutfile, mapfile, sensorpose)

if nargin < 1
  disp('Usage: runlocalization(simoutfile, opt:mapfile, opt:sensorpose)');
  return
end

if nargin < 2
  mapfile = 'map-goals.txt';
end

if nargin < 3
  sensorpose = zeros(3,1);
end

clf
if ~isempty(mapfile) 
  d = load(mapfile);
  margin = 2.5;
  xmin = min(d(:,2)) - margin;
  xmax = max(d(:,2)) + margin;
  ymin = min(d(:,3)) - margin;
  ymax = max(d(:,3)) + margin;

  drawLandmarkMap(mapfile);

  landmarks = d;
else
  disp('Need a map file!')
  return
end

hold on

axis([xmin xmax ymin ymax])

fid = fopen(simoutfile,'r');
if fid <= 0
  disp(sprintf('Failed to open simoutput file "%s"\n',simoutfile));
  return
end

h = [];

title('Circels: landmarks, red cross: true pos, blue dots: odom pos')

inited = 0;
enc_last = [];

% We know exactly where we are to begin with
P = 1e-9*eye(3);

rR = 0.025; % Right wheel radius [m]
rL = 0.025; % Left wheel radius [m}
B = 0.16;   % wheel base [m]
encRes = 8806;   % Ticks/rev for encoder

while 1
  line = fgetl(fid);
  if ~ischar(line)
    break
  end

  values = sscanf(line, '%f');
  t = values(1);
  odom = values(2:4);
  enc = values(5:6);
  truepose = values(7:9);
  n = values(10);
  if (n > 0) 
    bearings = values(12:3:end);
    ranges = values(13:3:end);
    ids = values(11:3:end);
  else
    bearings = [];
    ranges = [];
    ids = [];
  end

  if ~inited
    disp('Initializing xhat and enc_last')

    enc_last = enc;

    % We assume that we know exactly where we are to begin with
    xhat = truepose;

    inited = 1;
  end

  % Here you could stick you localization code which should update
  % estpose = (x,y,theta) in some other way than to copy the odometry

  % PREDICTION

  denc = enc - enc_last;

  dDr = denc(1) * 2*pi/encRes * rR;
  dDl = denc(2) * 2*pi/encRes * rL;

  dD = 0.5*(dDr + dDl);
  dA = (dDr - dDl) / B;

  xhat = xhat + [dD*cos(xhat(3));
                 dD*sin(xhat(3));
                 dA]

  F = [1 0 -dD*sin(xhat(3));
       0 1  dD*cos(xhat(3));
       0 0 1];

  G = [0.5*cos(xhat(3)) 0.5*cos(xhat(3));
       0.5*sin(xhat(3)) 0.5*sin(xhat(3));
       1/B -1/B];

  kr = 0.01;
  kl = 0.01;
  Q = [kr*abs(dDr) 0; 0 kl*abs(dDl)];

  P = F*P*F'+G*Q*G';

  % UPDATE

  % Loop through each of the measurements
  for k = 1:n
    % Get the index of the measured landmark
    li = ids(ids(k));

    % Get position of the landmark
    xl = landmarks(li,2);
    yl = landmarks(li,3);   

    % Do something with this
    %
    % If you use the bearings be careful when you calculate the
    % innovations so that you do not get innovation angles that are larger
    % than pi or smaller than -pi. 
    % Remember 1.99*pi is the same angle as -0.01*pi!!

    r = sqrt((xl-xhat(1))^2+(yl-xhat(2))^2);
    Hr = [-(xl-xhat(1))/r -(yl-xhat(2))/r 0];
    innov_r = ranges(k) - r; 
    Rr = (0.05)^2;
  
    beta = atan2(yl-xhat(2),xl-xhat(1))-xhat(3);
    Hb = [(yl-xhat(2))/r^2 -(xl-xhat(1))/r^2 -1];
    innov_b = bearings(k) - beta;
    while (innov_b > pi) innov_b = innov_b - 2*pi; end
    while (innov_b <-pi) innov_b = innov_b + 2*pi; end
    Rb = (5*pi/180)^2;    

    if 1
      H = [Hr; Hb];
      innov =  [innov_r; innov_b];
      R = diag([Rr Rb]);
    elseif 0
      H = Hr;
      innov = innov_r;
      R = Rr;
    else
      H = Hb;
      innov = innov_b;
      R = Rb;
    end      

    S = H*P*H'+R;
    K = P*H'*inv(S);

    xhat = xhat + K * innov;

    P = P - K*H*P;
    
  end

  estpose = xhat;

  plot(odom(1), odom(2), 'b.', ...
       truepose(1), truepose(2), 'rx', ...
       estpose(1), estpose(2), 'k.')  

  if n > 0

    RO = [cos(odom(3)) -sin(odom(3)); 
          sin(odom(3)) cos(odom(3))];
    RW = [cos(truepose(3)) -sin(truepose(3)); 
          sin(truepose(3)) cos(truepose(3))];
    RE = [cos(estpose(3)) -sin(estpose(3)); 
          sin(estpose(3)) cos(estpose(3))];

    xsO = odom(1:3) + [RO * sensorpose(1:2); sensorpose(3)];
    xsW = truepose(1:3) + [RW * sensorpose(1:2); sensorpose(3)];
    xsE = estpose(1:3) + [RE * sensorpose(1:2); sensorpose(3)];
  
    for k = 1:length(h)
      delete(h(k))
    end
    h = [];  

    for k = 1:n
      h1 = plot(xsO(1)+[0 ranges(k)*cos(xsO(3)+bearings(k))], ...
                xsO(2)+[0 ranges(k)*sin(xsO(3)+bearings(k))], 'b');
      h2 = plot(xsW(1)+[0 ranges(k)*cos(xsW(3)+bearings(k))], ...
                xsW(2)+[0 ranges(k)*sin(xsW(3)+bearings(k))], 'r');
      h3 = plot(xsE(1)+[0 ranges(k)*cos(xsE(3)+bearings(k))], ...
                xsE(2)+[0 ranges(k)*sin(xsE(3)+bearings(k))], 'k');
      h = [h h1 h2 h3];
    end


    xy = uncEllipse(xhat(1:2),P(1:2,1:2),100,1);
    h4 = plot(xy(1,:),xy(2,:));
    h = [h h4];

  end

  enc_last = enc;

  axis([xmin xmax ymin ymax])      

  drawnow

  pause
end

fclose(fid);