%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%                       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 = [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];

% 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 + (12-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 = [10 11];

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

%%% 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 = [10 20 30 40]; 

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