load beijing.mat;
load beijing_angles.mat;
load beijing_graph.mat;
%load beijing_config.mat;

cov = [ 0.25, 0, 0; 0, 0.25, 0; 0, 0, 0.01];


factors = Pose2Graph;
factors2 = Pose2Graph;
ord = Ordering();
ord2 = Ordering();

[~, idx] = sort(way_levels, 'descend');

for i=1:length(idx)
    if mod(i,500) == 0
        fprintf(1, 'processing way %d\n', i);
    end
    for j=1:length(ways{idx(i)})-1
        id1 = ways{idx(i)}(j);
        id2 = ways{idx(i)}(j+1);
        key1 = sprintf('x%d', id1);
        key2 = sprintf('x%d', id2);

        delta_x = points(id1,:) - points(id2,:);
        delta_angle = angles(id1) - angles(id2);
        measured = Pose2(delta_x(1), delta_x(2), delta_angle);

        if pred(id1) == id2 || pred(id2) == id1 %% in the spanning tree
            factor=Pose2Factor(key1,key2,measured, cov);
            factors.push_back(factor);
            ord.push_back(key1);
            ord.push_back(key2);
        else %% not in the spanning tree
            factors2.push_back(Pose2Factor(key1,key2,measured, cov));
            ord2.push_back(key1);
            ord2.push_back(key2);
        end
    end
end
ord.unique();
ord2.unique();
% ord.reverse();
% ord2.reverse();

if 1
    config=Pose2Config();
    n=size(points,1);
    for j=1:n
        pose=Pose2(points(j,1),points(j,2),angles(j));
        key = sprintf('x%d', j);
        config.insert(key,pose);
        if mod(j,1000) == 0
            key
        end
    end
    save('beijing_config.mat','config');
end

amd_ord=factors.getOrdering_(); % does not work
LFG=factors.linearize_(config);
ijs=LFG.sparse(ord);
A=sparse(ijs(1,:),ijs(2,:),ijs(3,:)); 
figure(1)
spy(A);
%save('beijing_factors.mat', 'factors');

LFG2=factors2.linearize_(config);
ijs2=LFG2.sparse(ord2);
A2=sparse(ijs2(1,:),ijs2(2,:),ijs2(3,:)); 
figure(2)
spy(A2);

% show R factor
R = qr(A,0);
figure(3)
spy(R)

% show re-ordered R factor
P = colamd(A);
figure(4)
spy(A(:,P))
R = qr(A(:,P),0);
figure(5)
spy(R)