%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%                                 IMMAGINI                                   %%%
%%%                                                                            %%%
%%%  In questo file vengono visualizzate le stime, residui, scarto quadratico  %%%
%%%  medio, errore medio, errore massimo di posizione, mean predictive log     %%% 
%%%  likelihood, lungo l'asse x e y del nodo mobile relativi all'algoritmo     %%%
%%%  distribuito e ad un valore del raggio r selezionati dall'utente.          %%%
%%%  Vengono inoltre mostrate le posizioni del nodo mobile stimate e reali     %%%
%%%  al fine di un confronto visivo.                                           %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
                      
clear all;
close all;
clc;

%%% Inizializzazione:

% Si caricano le posizioni del nodo mobile durante le rilevazioni, potenze
% ricevute, le stime di posizione e relative varianze ottenute mediante 
% i vari algorimi distribuiti 
load posizioniNM 
load RSSI_rilevazioni
load valori

% Lettura pianta del piano terra del DeiA
F=imread ('floor.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;

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

%%% Visualizzazione delle stime, residui, scarto quadratico medio,
%%% errore medio, errore massimo di posizione, mean predictive log likelihood,
%%% lungo l'asse x e y del nodo mobile relativi all'alg. distibuito selezionato 
%%% e ad un particolare valore del raggio r

metodo = input('Inserire il numero del metodo (21, 22, 31, 32) che si vuole visualizzare: ');
raggio = input('Inserire il valore del raggio r (5, 15, 25, 35) che determina i vicini: ');

if ((metodo == 21) || (metodo == 22) || (metodo == 31) || (metodo == 32)) && ((raggio == 5) || (raggio == 15) || (raggio == 25) ||(raggio == 35))
    if metodo == 21
        temp_stima_x = stima_x21;
        temp_stima_y = stima_y21;
        temp_St2_x =  St2_x21;
        temp_St2_y = St2_y21;
    end
    if metodo == 22
        temp_stima_x = stima_x22;
        temp_stima_y = stima_y22;
        temp_St2_x =  St2_x22;
        temp_St2_y = St2_y22;
    end
    if metodo == 31
        temp_stima_x = stima_x31;
        temp_stima_y = stima_y31;
        temp_St2_x =  St2_x31;
        temp_St2_y = St2_y31;
    end
    if metodo == 32
        temp_stima_x = stima_x32;
        temp_stima_y = stima_y32;
        temp_St2_x =  St2_x32;
        temp_St2_y = St2_y32;
    end
    for t=1:length(r)
        if raggio == r(t)
            break
        end
    end
    
    % Chiamata alla funzione grafici
    grafici(temp_stima_x(:,t), temp_stima_y(:,t), temp_St2_x(:,t), temp_St2_y(:,t), metodo, raggio);
    
    % Confronto tra le posizioni stimate e reali
    figure
    for k=1:length(test_Y_x)
            imshow(F);
            title(['Risultati relativi all utilizzo del metodo ' num2str(metodo) ' con un raggio di ' num2str(raggio) ' metri.']);
            hold on;
            plot(posizioneNA(:,1),posizioneNA(:,2),'.');
            hold on;
            plot(14 + (1/0.059)*temp_stima_x(k,t),157 + (-1/0.059)*temp_stima_y(k,t),'*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)*temp_stima_x(k,t),157 + (-1/0.059)*temp_stima_y(k,t),((1/0.059)^2)*9*[temp_St2_x(k,t) 0; 0 temp_St2_y(k,t)]);
            hold on;
            legend('Posizione nodi ANCORA utilizzati','Stima posizione nodo MOBILE','Posizione reale nodo MOBILE','Location','NorthOutside','Orientation','horizontal');
            pause(0.5)
   end
else
    disp('Metodo e/o raggio selezionato non corretto');
end

