
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%                  %%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%   Triangolazione.  %%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%                  %%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [stima_x stima_y St2_x St2_y] = triangolazione(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

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

%%% Determinazione della distanza tra il nodo mobile e i
%%% diversi nodi ancora grazie al modello di canale.

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];

P_tx = -1;  
Aa = -42.5;     % Dati ricavati sperimentalmente
gamma = 1.8;    %

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);


for u = 1:length(test_X(:,1))
    vicini = [];
    distanze = [];
    indice = 1;
    [Y,I] = max(test_X(u,:));
    for y = 1:length(matrix_near(1,:))
        if matrix_near(I,y)~=0
            chek = 0;
            if matrix_near(I,y)~=I
                for f=1:length(NG)
                    if matrix_near(I,y) == NG(f)
                        chek = 1;
                        break;
                    end
                end
            end
            if chek == 0
               vicini(indice) = matrix_near(I,y);
               indice = indice + 1;
            end
        end
    end
    
    for y = 1:length(vicini)
        distanze(y) = 10^( (P_tx + Aa - test_X(u,vicini(y)))/(10*gamma) );
    end
    
    %%% Costruzione delle matrici per la triangolazione (vedi Parolini)
    A = [];
    b = [];
    R = ones(length(vicini)-1, length(vicini)-1);  
    for j = 2:length(vicini)
        A(j-1,:) = [posizioneNA(vicini(1),1)-posizioneNA(vicini(j),1) posizioneNA(vicini(1),2)-posizioneNA(vicini(j),2)];
        b(j-1,:) = [distanze(j)^2-distanze(1)^2-posizioneNA(vicini(j),1)^2+posizioneNA(vicini(1),1)^2-posizioneNA(vicini(j),2)^2+posizioneNA(vicini(1),2)^2];
    end
    b = 0.5*b;
    R = R*(1/4)*distanze(1)^4*exp(2*gamma)*(exp(4*gamma)-1);
    for j = 1:length(vicini)-1
        R(j,j)=(1/4)*distanze(1)^4*exp(2*gamma)*(exp(4*gamma)-1)+distanze(j+1)^4*exp(2*gamma)*(exp(4*gamma)-1);
    end
    
    %%% Calcolo della stima della posizione del nodo mobile 
    temporary = pinv(A'*inv(R)*A)*A'*inv(R)*b;
    
    stima_x(u) = temporary(1);
    stima_y(u) = temporary(2);
    St2_temp = pinv(A'*inv(R)*A);
    St2_x(u) = St2_temp(1,1);
    St2_y(u) = St2_temp(2,2);
end  
    
