
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% 
%%% Triangolazione con tecnica dei minimi quadrati.
%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

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

P_tx = 10;  
A = -10;     % Dati ricavati sperimentalmente
gamma = 2;    %

for u = 1:length(matrix_near(:,1))
    vicini = [];
    distanze = [];
    indice = 1;
    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
               vicini(u,indice) = matrix_near(u,y);
               indice = indice + 1;
            end
        end
    end
    
    for y = 1:length(vicini(u,:))
        distanze(u,y) = 10^( (P_tx + A - test_X(u,vicini(u,y)))/(10*gamma) );
    end
    
    %%% Costruzione delle matrici per i minimi quadrati
    
    A = [];
    b = [];
    R = ones(length(vicini(u,:))-1,length(vicini(u,:))-1);
    
    for j = 2:length(vicini(u,:))
        A = [A ; test_Y_x(1,vicini(u,1))-test_Y_x(1,vicini(u,j)) test_Y_y(1,vicini(u,1))-test_Y_y(1,vicini(u,j))];
        b = [b ; distanze(u,j)^2-distanze(u,1)^2-test_Y_x(1,vicini(u,j))^2+test_Y_x(1,vicini(u,1))^2-test_Y_y(1,vicini(u,j))^2+test_Y_y(1,vicini(u,1))^2];
    end
    b = 0.5*b;
    R = R*(1/4)*distanze(u,1)^4*exp(2*gamma)*(exp(4*gamma)-1);
    for j = 1:length(vicini(u,:))-1
        R(j,j)=(1/4)*distanze(u,1)^4*exp(2*gamma)*(exp(4*gamma)-1)+distanze(u,j+1)^4*exp(2*gamma)*(exp(4*gamma)-1);
    end
    
    temp = inv(A'*inv(R)*A)*A*inv(R)*b;
    
    
%     stima_x(u) = temp(1);
%     stima_y(u) = temp(2);
%     St2_x = inv(A'*inv(R)*A)???
%     St2_y
end
    
    
    
