% jleonard 6/21/96 
% pull the times out of the array 'data' for use with LBLpath
% for now, assuming operation 'haro96'

speed_of_sound=1482;		
turn_around_time = 0.003;
MAX_DEV = 20;

ind = find(data(:,m_lbl_2t)>0);
tt = data(ind,m_present_time);
t1 = data(ind,m_lbl_1t);
t2 = data(ind,m_lbl_2t);
t3 = data(ind,m_lbl_3t);
t4 = data(ind,m_lbl_4t);
tn = data(ind,m_dr_north);
te = data(ind,m_dr_east);
n = length(t1);

lbl_data = [tt t1 t2 t3 t4 tn te];
%% save -ascii lbl_data.tmp ttt;


t1=(lbl_data(:,2)-turn_around_time)/2; 	% correct times for filter delay and round trip
t2=(lbl_data(:,3)-turn_around_time)/2;
t3=(lbl_data(:,4)-turn_around_time)/2;
t4=(lbl_data(:,5)-turn_around_time)/2;

r1 = t1*speed_of_sound;
r2 = t2*speed_of_sound;
r3 = t3*speed_of_sound;
r4 = t4*speed_of_sound;

% dr = zeros(n,2);
dr=[lbl_data(:,6) lbl_data(:,7)];



x0=[data(1,m_dr_north) data(1,m_dr_east) 0]; % get initial position from dr
pos = zeros(n,3);
pos(1,:)=x0;
xstart = x0;


x1 = [34686 23697 245];         % beacon locations: 6/20/96
x2 = [36585 25251 178];
x3 = [nan nan nan];
%x3 = [35839 22102 223];
x4 = [37671 23650 217];


n = length(r1);

crit = zeros(n,1);
crit(1)=0;

% save results to compare don and john's fix calculation algorithm
xy12 = zeros(n,2);
xy23 = zeros(n,2);
xy13 = zeros(n,2);
xy14 = zeros(n,2);
xy24 = zeros(n,2);
xy34 = zeros(n,2);

xy12(1,:) = x0(1,1:2);
xy23(1,:) = x0(1,1:2);
xy13(1,:) = x0(1,1:2);
xy14(1,:) = x0(1,1:2);
xy24(1,:) = x0(1,1:2);
xy34(1,:) = x0(1,1:2);


total_fixes_used = 0;

num_fixes = zeros(n,1);

for j=2:n

    xstart = x0;  
    xx0(j,:) = xstart(:,1:2);


    [xj12(1), xj12(2)] = closest_intersect(xstart(1), xstart(2), x1(1), x1(2),  x2(1), x2(2), r1(j), r2(j));

    [xj23(1), xj23(2)] = closest_intersect(xstart(1), xstart(2), x2(1), x2(2),  x3(1), x3(2), r2(j), r3(j));

    [xj13(1), xj13(2)] = closest_intersect(xstart(1), xstart(2), x1(1), x1(2),  x3(1), x3(2), r1(j), r3(j));

    [xj14(1), xj14(2)] = closest_intersect(xstart(1), xstart(2), x1(1), x1(2),  x4(1), x4(2), r1(j), r4(j));

    [xj24(1), xj24(2)] = closest_intersect(xstart(1), xstart(2), x2(1), x2(2),  x4(1), x4(2), r2(j), r4(j));

    [xj34(1), xj34(2)] = closest_intersect(xstart(1), xstart(2), x3(1), x3(2),  x4(1), x4(2), r3(j), r4(j));

    xy12(j,:) = xj12(:,1:2);     % may need transpose
    xy23(j,:) = xj23(:,1:2);
    xy13(j,:) = xj13(:,1:2);
    xy14(j,:) = xj14(:,1:2);
    xy24(j,:) = xj24(:,1:2);
    xy34(j,:) = xj34(:,1:2);

% Now eliminate outliers one-by-one

fix=[xj12(1,1:2); xj23(1,1:2); xj13(1,1:2); xj14(1,1:2); xj24(1,1:2); xj34(1,1:2)];
[tfix,num_fixes(j)] = compute_fix(fix, MAX_DEV);

if (length(tfix)>0)
    pos(j,1:2)=tfix;
    crit(j)=1;
else 
    pos(j,1:2)=pos(j-1,1:2)+(dr(j,:)-dr(j-1,:));    
    crit(j)=0;
    num_fixes(j) = 0;
end

x0=pos(j,:);
	
end  % main for loop


disp(['Percent fixes accepted = ', num2str(floor(length(find(crit~=0))/length(crit)*100)), '%']);

% Plot  vehicle trajectory

figure(1)	
clg
hold on 
plot(x1(2),x1(1),'ro')
plot(x2(2),x2(1),'go')
plot(x3(2),x3(1),'bo')
plot(x4(2),x4(1),'yo')


plot(pos(find(crit==0), 2), pos(find(crit==0),1), 'g.');
plot(pos(find(crit==1), 2), pos(find(crit==1),1), 'r+');

title(['LBL vehicle track (Pairwise fix method) ' run_name])

% plot(pos(:,2), pos(:,1))

xlabel('East [m]')
ylabel('North [m]')
grid on

disp(['elapsed time ', num2str(toc), ' seconds']);


aa0 = (length(find(num_fixes==0))/length(num_fixes))*100;
aa1 = (length(find(num_fixes==1))/length(num_fixes))*100;
aa2 = (length(find(num_fixes==2))/length(num_fixes))*100;
aa3 = (length(find(num_fixes==3))/length(num_fixes))*100;
aa4 = (length(find(num_fixes==4))/length(num_fixes))*100;
aa5 = (length(find(num_fixes==5))/length(num_fixes))*100;
aa6 = (length(find(num_fixes==6))/length(num_fixes))*100;

disp([' 0 fixes used ', num2str(aa0), '%']);
disp([' 1 fix   used ', num2str(aa1), '%']);
disp([' 2 fixes used ', num2str(aa2), '%']);
disp([' 3 fixes used ', num2str(aa3), '%']);
disp([' 4 fixes used ', num2str(aa4), '%']);
disp([' 5 fixes used ', num2str(aa5), '%']);
disp([' 6 fixes used ', num2str(aa6), '%']);

return

figure(2)
clf
plot(num_fixes, '+')
hold on
grid on
ylabel('number of fixes used');
xlabel('LBL cycle count');
