%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%                                 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 lab. di Legnaro
F=imread ('legnaro.jpg');

% Conversione posizione da metri a pixel
posizioneNA = [18.5 18; 20.5 35; 26.5 22.2; 29.5 14; 36.5 20; 17.6 5; 33.8 27; 14.5 27; 6.8 10.8; 6.5 25; 33.5 35; 34.5 6];
posizioneNA(:,1) = 62 + 12.3*posizioneNA(:,1);
posizioneNA(:,2) = 485 - 12.3*posizioneNA(:,2);
posizioneTraining_x = 62 + 12.3*training_Y_x;
posizioneTraining_y = 485 - 12.3*training_Y_y;
posizioneTest_x = 62 + 12.3*test_Y_x;
posizioneTest_y = 485 - 12.3*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 (10, 20, 30, 40) che determina i vicini: ');

if ((metodo == 21) || (metodo == 22) || (metodo == 31) || (metodo == 32)) && ((raggio == 10) || (raggio == 20) || (raggio == 30) ||(raggio == 40))
    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(62,485,'yo');
            hold on;
            plot(posizioneNA(:,1),posizioneNA(:,2),'.');
            hold on;
            plot(62 + 12.3*temp_stima_x(k,t),485 - 12.3*temp_stima_y(k,t),'*r');
            hold on;
            plot(62 + 12.3*test_Y_x(k),485 - 12.3*test_Y_y(k),'*g');
            hold on;
            e = plot_ellipse(62 + 12.3*temp_stima_x(k,t),485 - 12.3*temp_stima_y(k,t),(12.3^2)*9*[temp_St2_x(k,t) 0; 0 temp_St2_y(k,t)]);
            hold on;
            legend('Origine degli assi x,y','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

