%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%                       MAIN - ALGORITMO DECENTRALIZZATO                     %%%
%%%                                                                            %%%
%%%  Date le posizioni del nodi ancora viene definita la matrice delle         %%%
%%%  distanze. Sucessivamente vengono stimate le posizioni del nodo mobile e   %%% 
%%%  relative varianze lungo l'asse x,y utilizzando 4 algoritmi differenti.    %%% 
%%%  Tali metodi vengono testasi per diversi valori del raggio r, delle        %%% 
%%%  circonferenze centrate in ciascun nodo ancora, che determina i propri     %%%
%%%  vicini.                                                                   %%% 
%%%  E' possibile inoltre testare gli algoritmi nel caso di più nodi ancora    %%%
%%%  guasti.                                                                   %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all;
clc;
close all;

%%% Calcolo e definizione della matrice delle distanze tra i nodi ancora

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

% Costruzione della matrice delle distanze tra i nodi ancora
matrix_dist = [];
[N M] = size(posizioneNA);
for k=1:N
    for h=1:N
        matrix_dist(h,k)= sqrt((posizioneNA(h,1)-posizioneNA(k,1))^2 + (posizioneNA(h,2)-posizioneNA(k,2))^2); % distanza tra il nodo k e h
    end
end

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

%%% Simulazione di G nodi guasti:

G = 0; % numero nodi guasti (se G=0 allora tutti i nodi ancora utilizzati funzionano)
NG = []; % contiene gli ID dei nodi guasti
if G == 0
    NG = 0;
else
    NG = round(1 + (62-1)*rand(1,G)); % ID nodi guasti determinati in modo casuale
end

% per Picco: commentare la parte sopra fino a %%% Simulazione di G nodi guasti:
% e togliere commento qui sotto
%NG = [18 34 61 45 52 27 30 35 17 47];

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

%%% Stima delle posizioni del nodo mobile e relative varianze lungo l'asse
%%% x,y utilizzando 4 algoritmi differenti e per diversi valori del raggio
%%% che determina i vicini di ciascun nodo ancora

% Vettore r contenente vari valori del raggio delle circonferenze centrate
% in ciascun nodo ancora al fine di determinare i propri vicini.
r = [5 15 25 65]; 

errore_x = [];
errore_y = [];

% Calcolo della matrice dei vicini e stime ottenute dai 4 metodi al variare
% del raggio r
for t=1:length(r)      
    matrix_near =[];
    for k=1:N
        l=1;
        test = true;
        temp = matrix_dist(k,:);
        while test     
            minimo = temp(1);
            indice =1;
            for h=2:N
                if minimo > temp(h)
                    minimo = temp(h);  % distanza tra NA k e NA h
                    indice = h;        % ID nodo ancora h
                end
            end
            if minimo<=r(t)                 % NA h è un vicino del nodo ancora k
                matrix_near(k,l) = indice;  % costruzione matrice dei vicini
                l=l+1;
                temp(indice)=100;
            else
                test =false;
            end
        end
    end
    % Chiamate ai metodi distribuiti
    [stima_x21(:,t) stima_y21(:,t) St2_x21(:,t) St2_y21(:,t)] = punto_2_metodo_1(matrix_near,NG);
    [stima_x22(:,t) stima_y22(:,t) St2_x22(:,t) St2_y22(:,t)] = punto_2_metodo_2(matrix_near,NG);
    [stima_x31(:,t) stima_y31(:,t) St2_x31(:,t) St2_y31(:,t)] = punto_3_metodo_1(matrix_near,NG);
    [stima_x32(:,t) stima_y32(:,t) St2_x32(:,t) St2_y32(:,t)] = punto_3_metodo_2(matrix_near,NG);
    [stima_x4(:,t) stima_y4(:,t) St2_x4(:,t) St2_y4(:,t)] = triangolazione(matrix_near,NG);

end

save valori NG r stima_x21 stima_y21 St2_x21 St2_y21 stima_x22 stima_y22 St2_x22 St2_y22 stima_x31 stima_y31 St2_x31 St2_y31 stima_x32 stima_y32 St2_x32 St2_y32 stima_x4 stima_y4 St2_x4 St2_y4

