% Generate 3D view of town
figure;

subplot(2,3,1);
hold on;

title('Original','FontSize',16);

plotcube([99,99,0],[0,0,0],1,[1,1,1]) % Ground
plotcube([99,9,0],[0,45,.01],1,[0,0,0]) % Ave A
plotcube([9,99,0],[9,0,.01],1,[0,0,0]) % Ave B
plotcube([9,99,0],[81,0,.01],1,[0,0,0]) % Ave C
plotcube([15,15,9],[23,15,0],.5,[.5,.5,.5]) % House 1
plotcube([15,15,9],[43,15,0],.5,[.5,.5,.5]) % House 2
plotcube([15,15,9],[63,15,0],.5,[.5,.5,.5]) % House 3
plotcube([15,15,30],[23,69,0],.5,[.5,.5,.5]) % Office 1
plotcube([15,15,60],[43,69,0],.5,[.5,.5,.5]) % Office 2
plotcube([15,15,90],[63,69,0],.5,[.5,.5,.5]) % Office 3

subplot(2,3,4);
hold on;

plotcube([99,99,0],[0,0,0],1,[1,1,1]) % Ground
plotcube([99,9,0],[0,45,.01],1,[0,0,0]) % Ave A
plotcube([9,99,0],[9,0,.01],1,[0,0,0]) % Ave B
plotcube([9,99,0],[81,0,.01],1,[0,0,0]) % Ave C
plotcube([15,15,9],[23,15,0],.5,[.5,.5,.5]) % House 1
plotcube([15,15,9],[43,15,0],.5,[.5,.5,.5]) % House 2
plotcube([15,15,9],[63,15,0],.5,[.5,.5,.5]) % House 3
plotcube([15,15,30],[23,69,0],.5,[.5,.5,.5]) % Office 1
plotcube([15,15,60],[43,69,0],.5,[.5,.5,.5]) % Office 2
plotcube([15,15,90],[63,69,0],.5,[.5,.5,.5]) % Office 3

N = 100;
loc = zeros(N,1);
dist = zeros(N,1);
pos = zeros(N,3);
pos_pca = zeros(N,3);
pos_mod = zeros(N,3);

for pp=1:N;
    loc(pp) = randi(9);
    dist(pp) = rand;
    if loc(pp) == 1        
        pos(pp,1) = round(99*dist(pp));
        pos(pp,2) = 47+(5*(rand>.5));
        pos(pp,3) = 0;
    elseif loc(pp) == 2
        pos(pp,1) = 11+(5*(rand>.5)); 
        pos(pp,2) = round(99*dist(pp));
        pos(pp,3) = 0;
    elseif loc(pp) == 3
        pos(pp,1) = 83+(5*(rand>.5)); 
        pos(pp,2) = round(99*dist(pp));
        pos(pp,3) = 0;
    elseif loc(pp) == 4
        pos(pp,1) = 23+randi(15)-1; 
        pos(pp,2) = 15+randi(15)-1;
        pos(pp,3) = floor(dist(pp)*3)*3;
    elseif loc(pp) == 5
        pos(pp,1) = 43+randi(15)-1; 
        pos(pp,2) = 15+randi(15)-1;
        pos(pp,3) = floor(dist(pp)*3)*3;
    elseif loc(pp) == 6
        pos(pp,1) = 63+randi(15)-1; 
        pos(pp,2) = 15+randi(15)-1;
        pos(pp,3) = floor(dist(pp)*3)*3;
    elseif loc(pp) == 7
        pos(pp,1) = 23+randi(15)-1; 
        pos(pp,2) = 69+randi(15)-1;
        pos(pp,3) = floor(dist(pp)*10)*3;
    elseif loc(pp) == 8
        pos(pp,1) = 43+randi(15)-1; 
        pos(pp,2) = 69+randi(15)-1;
        pos(pp,3) = floor(dist(pp)*20)*3;
    elseif loc(pp) == 9
        pos(pp,1) = 63+randi(15)-1; 
        pos(pp,2) = 69+randi(15)-1;
        pos(pp,3) = floor(dist(pp)*30)*3;

    end
    subplot(2,3,1);
    plot3(pos(pp,1),pos(pp,2),pos(pp,3),'*');
    subplot(2,3,4);
    plot3(pos(pp,1),pos(pp,2),pos(pp,3),'*');
end

subplot(2,3,1);
view(25,35);
zlim([-10 100]);
xlim([-20 120]);
ylim([-20 120]);
subplot(2,3,4);
view(90,0);
zlim([-10 100]);
xlim([-20 120]);
ylim([-20 120]);

% Generate rotated principal components
[loadings, scores, eig] = pca(zscore(pos));

pVar = eig/sum(eig); % percent variance accounted for by each component
Npc = sum(eig>1); % Number of principal components

% Plot PCA fit to data

subplot(2,3,2);
hold on;

title('PCA','FontSize',16);

plotcube([99,99,0],[0,0,0],1,[1,1,1]) % Ground
plotcube([99,9,0],[0,45,.01],1,[0,0,0]) % Ave A
plotcube([9,99,0],[9,0,.01],1,[0,0,0]) % Ave B
plotcube([9,99,0],[81,0,.01],1,[0,0,0]) % Ave C
plotcube([15,15,9],[23,15,0],.5,[.5,.5,.5]) % House 1
plotcube([15,15,9],[43,15,0],.5,[.5,.5,.5]) % House 2
plotcube([15,15,9],[63,15,0],.5,[.5,.5,.5]) % House 3
plotcube([15,15,30],[23,69,0],.5,[.5,.5,.5]) % Office 1
plotcube([15,15,60],[43,69,0],.5,[.5,.5,.5]) % Office 2
plotcube([15,15,90],[63,69,0],.5,[.5,.5,.5]) % Office 3

subplot(2,3,5);
hold on;

plotcube([99,99,0],[0,0,0],1,[1,1,1]) % Ground
plotcube([99,9,0],[0,45,.01],1,[0,0,0]) % Ave A
plotcube([9,99,0],[9,0,.01],1,[0,0,0]) % Ave B
plotcube([9,99,0],[81,0,.01],1,[0,0,0]) % Ave C
plotcube([15,15,9],[23,15,0],.5,[.5,.5,.5]) % House 1
plotcube([15,15,9],[43,15,0],.5,[.5,.5,.5]) % House 2
plotcube([15,15,9],[63,15,0],.5,[.5,.5,.5]) % House 3
plotcube([15,15,30],[23,69,0],.5,[.5,.5,.5]) % Office 1
plotcube([15,15,60],[43,69,0],.5,[.5,.5,.5]) % Office 2
plotcube([15,15,90],[63,69,0],.5,[.5,.5,.5]) % Office 3

for pp=1:N
    
    pos_pca(pp,1) = (scores(pp,1:2)*loadings(1,1:2)')*std(pos(:,1))+mean(pos(:,1));
    pos_pca(pp,2) = (scores(pp,1:2)*loadings(2,1:2)')*std(pos(:,2))+mean(pos(:,2));
    pos_pca(pp,3) = (scores(pp,1:2)*loadings(3,1:2)')*std(pos(:,3))+mean(pos(:,3));
    subplot(2,3,2);
    plot3(pos_pca(pp,1),pos_pca(pp,2),pos_pca(pp,3),'*');
    subplot(2,3,5);
    plot3(pos_pca(pp,1),pos_pca(pp,2),pos_pca(pp,3),'*');
    RMSE_pca(pp) = sqrt(mean((pos_pca(pp,:)-pos(pp,:)).^2)); 
end

subplot(2,3,2);
view(25,35);
zlim([-10 100]);
xlim([-20 120]);
ylim([-20 120]);
subplot(2,3,5);
view(90,0);
zlim([-10 100]);
xlim([-20 120]);
ylim([-20 120]);

% Plot model fit to data

subplot(2,3,3);
hold on;

title('Theory','FontSize',16);

plotcube([99,99,0],[0,0,0],1,[1,1,1]) % Ground
plotcube([99,9,0],[0,45,.01],1,[0,0,0]) % Ave A
plotcube([9,99,0],[9,0,.01],1,[0,0,0]) % Ave B
plotcube([9,99,0],[81,0,.01],1,[0,0,0]) % Ave C
plotcube([15,15,9],[23,15,0],.5,[.5,.5,.5]) % House 1
plotcube([15,15,9],[43,15,0],.5,[.5,.5,.5]) % House 2
plotcube([15,15,9],[63,15,0],.5,[.5,.5,.5]) % House 3
plotcube([15,15,30],[23,69,0],.5,[.5,.5,.5]) % Office 1
plotcube([15,15,60],[43,69,0],.5,[.5,.5,.5]) % Office 2
plotcube([15,15,90],[63,69,0],.5,[.5,.5,.5]) % Office 3

subplot(2,3,6);
hold on;

plotcube([99,99,0],[0,0,0],1,[1,1,1]) % Ground
plotcube([99,9,0],[0,45,.01],1,[0,0,0]) % Ave A
plotcube([9,99,0],[9,0,.01],1,[0,0,0]) % Ave B
plotcube([9,99,0],[81,0,.01],1,[0,0,0]) % Ave C
plotcube([15,15,9],[23,15,0],.5,[.5,.5,.5]) % House 1
plotcube([15,15,9],[43,15,0],.5,[.5,.5,.5]) % House 2
plotcube([15,15,9],[63,15,0],.5,[.5,.5,.5]) % House 3
plotcube([15,15,30],[23,69,0],.5,[.5,.5,.5]) % Office 1
plotcube([15,15,60],[43,69,0],.5,[.5,.5,.5]) % Office 2
plotcube([15,15,90],[63,69,0],.5,[.5,.5,.5]) % Office 3

for pp=1:N
    if loc(pp) == 1        
        pos_mod(pp,1) = round(99*dist(pp));
        pos_mod(pp,2) = 50;
        pos_mod(pp,3) = 0;
    elseif loc(pp) == 2
        pos_mod(pp,1) = 16; 
        pos_mod(pp,2) = round(99*dist(pp));
        pos_mod(pp,3) = 0;
    elseif loc(pp) == 3
        pos_mod(pp,1) = 88; 
        pos_mod(pp,2) = round(99*dist(pp));
        pos_mod(pp,3) = 0;
    elseif loc(pp) == 4
        pos_mod(pp,1) = 30; 
        pos_mod(pp,2) = 22;
        pos_mod(pp,3) = floor(dist(pp)*3)*3;
    elseif loc(pp) == 5
        pos_mod(pp,1) = 50; 
        pos_mod(pp,2) = 22;
        pos_mod(pp,3) = floor(dist(pp)*3)*3;
    elseif loc(pp) == 6
        pos_mod(pp,1) = 70; 
        pos_mod(pp,2) = 22;
        pos_mod(pp,3) = floor(dist(pp)*3)*3;
    elseif loc(pp) == 7
        pos_mod(pp,1) = 30; 
        pos_mod(pp,2) = 76;
        pos_mod(pp,3) = floor(dist(pp)*10)*3;
    elseif loc(pp) == 8
        pos_mod(pp,1) = 50; 
        pos_mod(pp,2) = 76;
        pos_mod(pp,3) = floor(dist(pp)*20)*3;
    elseif loc(pp) == 9
        pos_mod(pp,1) = 70; 
        pos_mod(pp,2) = 76;
        pos_mod(pp,3) = floor(dist(pp)*30)*3;

    end
    
    subplot(2,3,3);
    plot3(pos_mod(pp,1),pos_mod(pp,2),pos_mod(pp,3),'*');
    subplot(2,3,6);
    plot3(pos_mod(pp,1),pos_mod(pp,2),pos_mod(pp,3),'*');
    RMSE_mod(pp) = sqrt(mean((pos_mod(pp,:)-pos(pp,:)).^2)); 
end
        
subplot(2,3,3);
view(25,35);
zlim([-10 100]);
xlim([-20 120]);
ylim([-20 120]);
subplot(2,3,6);
view(90,0); 
zlim([-10 100]);
xlim([-20 120]);
ylim([-20 120]);

clear pos_rand RMSE_rand;

for tt = 1:100;
    pos_rand = randi(99,100,3);
    for pp=1:100
        RMSE_rand(pp,tt) = sqrt(mean((pos_rand(pp,:)-pos(pp,:)).^2));
    end
end

mean(RMSE_rand(:))
min(RMSE_rand(:))
max(RMSE_rand(:))