%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%                       MAIN - ALGORITMO CENTRALIZZATO                       %%%
%%%                                                                            %%%
%%%  Date le posizioni del nodo mobile e le potenze ricevute durante le        %%%
%%%  rilevazioni (suddivise in training data teste data), scelta la funzione   %%%
%%%  covarianza più appropiata, si determinano le stime di posizione del       %%%
%%%  mobile lungo l'asse x,y e relative varianze.                              %%%
%%%  E' possibile inoltre testare l'algoritmo nel caso di più nodi ancora      %%%
%%%  guasti.                                                                   %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all;
clc;
close all;

%%% Inizializzazione:

% Si caricano le posizioni del nodo mobile durante le rilevazioni e potenze
% ricevute
load posizioniNM 
load RSSI_rilevazioni

% Lettura pianta del piano terra del DeiA
F=imread ('floorDEI.jpg');

% Conversione posizione da metri a pixel
posizioneNA = [19.79 -11.10; 22.93 -11.10; 29.42 -11.13; 32.64 -11.13; 11.09 -3.81; 11.07 -5.28; 34.81 -1.08; 34.81 -3.00; 14.75 -13.70; 17.92 -13.70; 21.16 -13.70; 24.25 -13.70; 27.25 -13.73; 29.76 -13.73; 32.29 -13.73; 34.95 -13.73; 14.75 -9; 17.92 -9; 21.16 -9; 34.81 -4.94; 27.24 -9; 29.65 -9.01; 32.31 -9.02; 34.97 -9.03; 33.86 -6.20; 16.65 -6.20; 18.85 -6.20; 21.25 -6.20; 24 -6.20; 26.70 -6.20; 27.15 -3.92; 27.15 -1.22; 0.88 -13.37; 0.88 -11.57; 0.88 -9.78; 0.88 -8.01; 0.88 -6.30; 0.88 -4.52; 0.88 -2.79; 0.88 -1.04; 30.37 -1.06; 30.37 -2.99; 30.37 -4.90; 29.80 -6.20; 5.52 -8.08; 5.52 -9.85; 5.52 -11.62; 5.52 -13.37; 10.16 -10.56; 10.95 -10.56; 11.75 -10.56; 12.60 -10.56; 8.71 -9.18; 8.71 -10.63; 8.71 -12.09; 8.71 -13.55; 8.71 -1.04; 8.71 -2.42; 8.71 -3.81; 8.71 -5.20; 11.13 -0.88; 11.11 -2.38];
posizioneNA(:,1) = 14 + (1/0.059)*posizioneNA(:,1);
posizioneNA(:,2) = 157 + (-1/0.059)*posizioneNA(:,2);
posizioneTraining_x = 14 + (1/0.059)*training_Y_x;
posizioneTraining_y = 157 + (-1/0.059)*training_Y_y;
posizioneTest_x = 14 + (1/0.059)*test_Y_x;
posizioneTest_y = 157 + (-1/0.059)*test_Y_y;

% Numero nodi ancora utilizzati
numNA = 62;

%--------------------------------------------------------------------------

%%% Simulazione di G nodi guasti:

G = 0; % numero nodi guasti (se G=0 allora tutti i nodi ancora utilizzati funzionano)
NG = []; % contiene gli ID dei nodi guasti
if G == 0
    NG = 0;
else
    NG = round(1 + (62-1)*rand(1,G)); % ID nodi guasti determinati in modo casuale
end

% per Picco: commentare la parte sopra fino a %%% Simulazione di G nodi guasti:
% e togliere commento qui sotto
%NG = [18 34 61 45 52 27 30 35 17 47];

if NG(1) ~=0
    for k=1:length(NG)
        test_X(:,NG(k))= -70*ones(length(test_X(:,1)),1); % aggiornamento matrice test_X di RSSI
    end
end

%--------------------------------------------------------------------------

%%% Scalatura dati:

mean_training_X = mean(training_X);

for k=1:numNA
    training_X(:,k) = (training_X(:,k) - mean_training_X(k));
end

offset_x = mean(training_Y_x);
offset_y = mean(training_Y_y);

training_Y_x = training_Y_x - offset_x;
training_Y_y = training_Y_y - offset_y;

mean_test_X = mean(test_X);

for k=1:numNA
     test_X(:,k) = (test_X(:,k) - mean_test_X(k));
end

%--------------------------------------------------------------------------

%%% Scelta funzione covarianza:

% 3 Parametri

% covfunc = {'covSum', {'covMatern3iso','covNoise'}};
% covfunc = {'covSum', {'covMatern5iso','covNoise'}};
% covfunc = {'covSum', {'covNNone','covNoise'}};

% logtheta0 = 5*[1; 1; 1];
% logtheta0 = 1*[1; 1; 1];
% logtheta0 = 1*[1; 1; 1];

% L Parametri

covfunc = {'covSum', {'covSEard','covNoise'}};
% covfunc = {'covSum', {'covLINard','covNoise'}};
% covfunc = {'covSum', {'covRQard','covNoise'}};

[N M]=size(training_X);
temp = ones(M,1);
 
logtheta0 = 6*[temp; 1; 1];
% logtheta0 = 1*[temp; 1];
% logtheta0 = 1*[temp; 1; 1; 1];

%--------------------------------------------------------------------------

%%% Determinazione del Kernel scelto:

[logtheta_x, fvals_x, iter_x] = minimize(logtheta0, 'gpr', -100, covfunc, training_X, training_Y_x);
[logtheta_y, fvals_y, iter_y] = minimize(logtheta0, 'gpr', -100, covfunc, training_X, training_Y_y);

figure
plot(fvals_x,'o-')
hold on;
plot(fvals_y,'ro-');
grid on;
legend('Asse x','Asse y');

disp(' ')
disp('Parametri della funzione covarianza relativa all asse x')
for k =1:length(logtheta0)
    fprintf(1, ['ell_ ' num2str(k) '\t\t%12.6f\n'],exp(logtheta_x(k)));
end
disp(' ')

disp(' ')
disp('Parametri della funzione covarianza relativa all asse y')
for k =1:length(logtheta0)
    fprintf(1, ['ell_ ' num2str(k) '\t\t%12.6f\n'],exp(logtheta_y(k)));
end
disp(' ')

%--------------------------------------------------------------------------

%%% Calcolo delle stime di posizioni lungo l'asse x e y del nodo mobile

[fstar_x S2_x] = gpr(logtheta_x, covfunc, training_X, training_Y_x, test_X);
fstar_x = fstar_x + offset_x;
[fstar_y S2_y] = gpr(logtheta_y, covfunc, training_X, training_Y_y, test_X);
fstar_y = fstar_y + offset_y;

%--------------------------------------------------------------------------

%%% Calcolo dei residui, scarto quadratico medio, errore medio, 
%%% errore massimo di posizione, mean predictive log likelihood lungo 
%%% l'asse x e y del nodo mobile e visualizzazione dei risultati ottenuti

res_x = fstar_x-test_Y_x;
res_y = fstar_y-test_Y_y;

mse_x = mean(res_x.^2);
mse_y = mean(res_y.^2);

em_x = mean(abs(res_x)); 
em_y = mean(abs(res_y)); 

max_x = max(abs(res_x));
max_y = max(abs(res_y));

pll_x = -0.5*mean(log(2*pi*S2_x)+res_x.^2./S2_x);
pll_y = -0.5*mean(log(2*pi*S2_y)+res_y.^2./S2_y);

disp(' ')
disp('Risultati relativi all`asse x');
fprintf(1,'The mean squared error is %10.6f\n', mse_x);
fprintf(1,'Errore medio è %10.6f\n', em_x);
fprintf(1,'L errore massimo è %10.6f\n', max_x);
fprintf(1,'and the mean predictive log likelihood is %6.4f.\n', pll_x);
disp(' ')
disp(' ')
disp('Risultati relativi all`asse y');
fprintf(1,'The mean squared error is %10.6f\n', mse_y);
fprintf(1,'Errore medio è %10.6f\n', em_y);
fprintf(1,'L errore massimo è %10.6f\n', max_y);
fprintf(1,'and the mean predictive log likelihood is %6.4f.\n', pll_y);
disp(' ')

a = round(max(max(abs(res_x),abs(res_y))));
b = round(max(max(sqrt(S2_x),sqrt(S2_y))));

figure
subplot(2,2,1), plot(res_x,'.'), grid minor ,ylim([-a a]), xlim([0 105]), ylabel('residui asse x'), xlabel('test case')
subplot(2,2,2), plot(sqrt(S2_x),'.'), grid minor, ylim([0 b]), xlim([0 105]), ylabel('deviazione standard asse x'), xlabel('test case')
subplot(2,2,3), plot(res_y,'.'), grid minor, ylim([-a a]), xlim([0 105]),ylabel('residui asse y'), xlabel('test case')
subplot(2,2,4), plot(sqrt(S2_y),'.'), grid minor , ylim([0 b]), xlim([0 105]),ylabel('deviazione standard asse y'), xlabel('test case')

%--------------------------------------------------------------------------

%%% Visualizzazione delle posizioni dei nodo mobile (1° figura) e confronto
%%% tra le posizioni stimate e reali (2° figura)

figure
imshow(F);
hold on;
plot(posizioneTraining_x,posizioneTraining_y,'.')
hold on;
plot(posizioneTest_x,posizioneTest_y,'.r')
legend('Posizione Training set','Posizione Test set','Location','NorthOutside','Orientation','horizontal');
hold off;

figure
for k=1:length(test_Y_x)
    imshow(F);
    hold on;
    plot(posizioneNA(:,1),posizioneNA(:,2),'.');
    hold on;
    plot(14 + (1/0.059)*fstar_x(k),157 + (-1/0.059)*fstar_y(k),'*r');
    hold on;
    plot(14 + (1/0.059)*test_Y_x(k),157 + (-1/0.059)*test_Y_y(k),'*g');
    hold on;
    e = plot_ellipse(14 + (1/0.059)*fstar_x(k),157 + (-1/0.059)*fstar_y(k),((1/0.059)^2)*9*[S2_x(k) 0; 0 S2_y(k)]);
    hold on;
    legend('Posizione nodi ANCORA utilizzati','Stima posizione nodo MOBILE','Posizione reale nodo MOBILE','Location','NorthOutside','Orientation','horizontal');
    pause(0.5)
end

save stimeC NG fstar_x fstar_y


