%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%                        Function punto_3_metodo_2                        %%%
%%%                                                                         %%%
%%%  Ciascun nodo ancora stima la posizione del nodo mobile utilizzando     %%%
%%%  la propria informazione (RSSI ricevuto dal nodo mobile), quella dei    %%% 
%%%  suoi vicini e l'RSSI dei vicini dei suoi vicini (2 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_3_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 e quella
%%% dei vicini dei suoi vicini (2 hope)

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

% Costruzione di una matrice temporanea nella quale in ogni riga k vi sono
% gli ID dei nodi ancora vicini al NA k e gli ID dei nodi ancora vicini ai
% suoi vicini
matrix_temp = []; 
for k = 1:length(matrix_near(:,1))
    temp = matrix_near(k,:);
    for h = 2:length(matrix_near(1,:))
        if matrix_near(k,h)~=0
            temp = [temp matrix_near(matrix_near(k,h),:)];
        else
            temp = [temp zeros(1,length(matrix_near(1,:)))];
        end
    end
    temp = sort(temp);
    matrix_temp(k,:)=temp;
end

% Definizione della matrice dei vicini nel senso 2 hop
matrix_near2 = zeros(length(matrix_near(1,:)),length(matrix_near(1,:)));
for k=1:length(matrix_near(:,1))
    indice = 1;
    val = 0;
    for h =1:length(matrix_temp(1,:))
         if matrix_temp(k,h)>val
            matrix_near2(k,indice)=matrix_temp(k,h);
            indice = indice+1;
            val = matrix_temp(k,h);
         end
    end
end

for u = 1:length(matrix_near2(:,1))
   temp_training_X = [];
   temp_test_X = [];
   for y = 1:length(matrix_near2(1,:))
       if matrix_near2(u,y)~=0
            chek = 0;
            if matrix_near2(u,y)~=u
                for t=1:length(NG)
                    if matrix_near2(u,y) == NG(t)
                        chek = 1;
                        break;
                    end
                end
            end
            if chek == 0
                % Matrice definita dagli RSSI del NA u e dei propri vicini nel senso 2 hope, utilizzata come training data
                temp_training_X = [temp_training_X training_X(:,matrix_near2(u,y))];
                % Matrice definita dagli RSSI del NA u e dei propri vicini nel senso 2 hope, utilizzata come test data
                temp_test_X = [temp_test_X test_X(:,matrix_near2(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 nel senso 2 hope
   [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 = 3; % 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 + (12-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 (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 = [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];

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
