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

    temp = inv(A'*inv(R)*A)*A'*inv(R)*b;
    
    
    stima_x(u) = temp(1);
     stima_y(u) = temp(2);
     St2_x = 1;%inv(A'*inv(R)*A)???
     St2_y = 1;%
end
    
    
    
