%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%                        Function punto_2_metodo_2                        %%%
%%%                                                                         %%%
%%%  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, scelti L nodi ancora in modo casuale, si procede ad   %%%
%%%  una prima stima della posizione del nodo mobile mediante una media     %%%
%%%  pesata delle stime ottenute dai L nodi ancora (il peso risulta         %%%
%%%  proporzionale al RSSI ricevuto dal nodo mobile).                       %%%
%%%  Infine, il nodo mobile seleziona la stima della posizione in cui si    %%% 
%%%  trova in un determinato istante dal nodo ancora ad esso più vicino     %%%
%%%  (come posizione del nodo mobile viene utilizzata la stima calcolata al %%%
%%%  punto precedente).                                                     %%% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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_2(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

%--------------------------------------------------------------------------
%%% Stima della posizione del nodo mobile ottenuta mediante media pesata di
%%% L stime, ciascuna calcolata da un nodo ancora. La scelta dei L nodi
%%% ancora è di tipo random

L = 15; % numero di nodi ancora utilizzati per la stima della posizione del NM

coeff_nor = zeros(length(test_X),1); % coefficiente di normalizzazione per la media pesata
w = []; % matrice dei pesi (dim length(test_X)xL)

for u=1:length(test_X)
    % Scelta random di L nodi ancora
    while coeff_nor(u) == 0 % ciclo necessario se tutti i L NA selezionati in modo casuale sono guasti
        scelta = [];
        count = 1;       
        for k=1:L
            scelta(count) = round(1 + (62-1)*rand(1)); % ID nodo ancora determinato in modo random
            chek = 0;
            for t=1:length(NG)
                if scelta(count) == NG(t)
                    chek = 1;
                    break;
                end
            end
            if chek == 0        % ID nodo ancora determinato in modo random non è nodo guasto
                coeff_nor(u) =  coeff_nor(u) + abs(71+test_X(u,scelta(count)));
                count = count + 1;
            end
        end
    end
    % Calcolo pesi w(u,k)
    for k=1:length(scelta)
        w(u,k)= abs(71+test_X(u,scelta(k)))/coeff_nor(u); % peso corrispondente al passo u del nodo mobile e al nodo ancora scelta(k)
    end
end

stima_x = zeros(length(test_X),1);
stima_y = zeros(length(test_X),1);
St2_x = zeros(length(test_X),1);
St2_y = zeros(length(test_X),1);

% Media pesata delle stime corrispondenti ai nodi ancora scelti precedentamente
for u=1:length(test_X)
    for k=1:length(scelta)
        stima_x(u) = stima_x(u) + w(u,k)*fstar_x(u,scelta(k));
        stima_y(u) = stima_y(u) + w(u,k)*fstar_y(u,scelta(k));
        St2_x(u) = St2_x(u) + S2_x(u,scelta(k));
        St2_y(u) = St2_y(u) + S2_y(u,scelta(k));
    end
    St2_x = St2_x/length(scelta);
    St2_y = St2_y/length(scelta);
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 ad esso più vicino non guasto (come posizione del nodo mobile viene
%%% utilizzata la stima calcolata al punto precedente)

% Posizione dei nodi ancora lungo l'asse x,y (in metri)
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];

vett_dist = [];
[N M] = size(posizioneNA);
for u=1:length(test_X)
    for k=1:N
        chek = 0;
        for t=1:length(NG)
            if k == NG(t)
                chek = 1;
                break;
            end
        end
        if chek == 0    % ID nodo ancora determinato in modo random non è nodo guasto
            vett_dist(k)= sqrt((posizioneNA(k,1)-stima_x(u))^2 + (posizioneNA(k,2)-stima_y(u))^2); % distanza tra il NA k e il NM al passo u
        else
            vett_dist(k) = 100;
        end
    end
    indice = 1;
    minimo = vett_dist(1);
    for i=2:length(vett_dist)
        if vett_dist(i)<minimo
            minimo = vett_dist(i);
            indice = i;   % ID nodo ancora più vicino non guasto al nodo mobile al passo u
        end
    end
    stima_x(u) = fstar_x(u,indice);
    stima_y(u) = fstar_y(u,indice);
    St2_x(u) = S2_x(u,indice); 
    St2_y(u) = S2_y(u,indice); 
end

end