%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%                        Function punto_2_metodo_1                        %%%
%%%                                                                         %%%
%%%  Ciascun nodo ancora stima la posizione del nodo mobile utilizzando     %%%
%%%  la propria informazione (RSSI ricevuto dal nodo mobile) e quella dei   %%% 
%%%  suoi vicini (1 hope).                                                  %%%
%%%  Successivamente il nodo mobile seleziona la stima della posizione in   %%% 
%%%  cui si trova in un determinato istante dal nodo ancora dal quale ha    %%%
%%%  ricevuto maggior potenza del segnale.                                  %%% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% input:  Matrice nella quale sono definiti i vicini di ciascun nodo ancora
%         (matrix_near), vettore dei nodi guasti (NG)        
% output: Stime delle posizioni del nodo mobile e relative varianze lungo
%         l'asse x,y


function [stima_x stima_y St2_x St2_y] = punto_2_metodo_1(matrix_near,NG)

%%% Inizializzazione:

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

load posizioniNM 
load RSSI_rilevazioni

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

%%% Simulazione di G nodi guasti:

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

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

%%% Stima della posizione del nodo mobile calcolata da ogni nodo ancora, 
%%% ciascuno utilizzando anche l'informazione dei propri vicini(1 hop)

fstar_x = []; % stime posizione asse x 
fstar_y = []; % stime posizione asse y
S2_x = [];    % varianza asse x
S2_y = [];    % varianza asse y

for u = 1:length(matrix_near(:,1))
    temp_training_X = [];
    temp_test_X = [];
    for y = 1:length(matrix_near(1,:))
        if matrix_near(u,y)~=0
            chek = 0;
            if matrix_near(u,y)~=u
                for t=1:length(NG)
                    if matrix_near(u,y) == NG(t)
                        chek = 1;
                        break;
                    end
                end
            end
            if chek == 0
                % Matrice definita dagli RSSI del NA u e dei propri vicini, utilizzata come training data
                temp_training_X = [temp_training_X training_X(:,matrix_near(u,y))]; 
                % Matrice definita dagli RSSI del NA u e dei propri vicini, utilizzata come test data
                temp_test_X = [temp_test_X test_X(:,matrix_near(u,y))];   
            end
        end
    end
    % Stime delle posizioni del NM e relative varianze calcolate dal NA u utilizzando la propria informazione e quella dei vicini
    [fstar_x(:,u) S2_x(:,u)] = stima(temp_training_X, temp_test_X, training_Y_x);
    [fstar_y(:,u) S2_y(:,u)] = stima(temp_training_X, temp_test_X, training_Y_y);
end

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

%%% Selezione da parte del nodo mobile, per ogni sua posizione, del nodo
%%% ancora dal quale richiedere la stima. Viene selezionato ogni volta il
%%% nodo ancora dal quale si è ricevuto il segnale di maggior potenza

for k=1:length(test_X)
    indice = 1;
    massimo = test_X(k,1);
    for i=2:length(test_X(k,:))
        if test_X(k,i)>massimo
            massimo = test_X(k,i);
            indice = i;     %ID del nodo ancora dal quale si è ricevuta maggior potenza al passo k
        end
    end
    stima_x(k,1) = fstar_x(k,indice);
    stima_y(k,1) = fstar_y(k,indice);
    St2_x(k,1) = S2_x(k,indice); 
    St2_y(k,1) = S2_y(k,indice); 
end

end

