%% 按照場地均勻安排錨節點
%考慮測距誤差為正負10%
%clear
%初始化節點及存放定位信息矩陣
node_num=200;%一般節點數量
%錨節點數量大約為一般節點數量的10%，依場地情況而定
length=100;%場地長度
width=100;%場地寬度
rangemax=15;%節點間最大通信距離
[node_x,node_y]=creatnode(node_num,length,width);%返回節點的橫縱坐標
%開始生成錨節點位置,25個錨節點均勻分布
%anchor_length=sqrt(length*width*10/100);
%anchor_no=1;
%for temp_width=1:(1+width/(anchor_length-7))
%    for temp_length=1:(1+length/(anchor_length-7))
%        anchor_x(anchor_no)=(anchor_length-7)*(temp_length-1);
%        anchor_y(anchor_no)=(anchor_length-7)*(temp_width-1);
%        anchor_no=anchor_no+1;
%    end
%end
%anchor_num=anchor_no-1;%最后得到的錨節點數量
%開始生成錨節點位置,20個錨節點均勻分布
anchor_length=sqrt(length*width*10/100);
anchor_no=1;
for temp_width=1:(1+width/(anchor_length-7))
   for temp_length=1:(1+length/anchor_length)
        anchor_x(anchor_no)=anchor_length*(temp_length-1);
        anchor_y(anchor_no)=(anchor_length-7)*(temp_width-1);
       anchor_no=anchor_no+1;
    end
end
%anchor_num=40;
%[anchor_x,anchor_y]=creatnode(anchor_num,length,width);
anchor_num=anchor_no-1;%最后得到的錨節點數量
% 生成存放定位信息的矩陣
%縱坐標為到第一個錨節點的跳數，到第一個錨節點的距離和，到第二個錨節點的跳數……
%橫坐標為所需定位節點的序號，這個序號與最初生成的node序號是一致的
locateinf=ones(2*anchor_num,node_num)*inf;

ok='程序初始化'

%% 察看初始節點狀況
plot(anchor_x,anchor_y,'*',node_x,node_y,'*');

%% 程序開始
%對圖進行廣度先遍歷
abcanchor=0;
abcnode=0;
for anchor_no=1:anchor_num
    %初始化隊列
    abcanchor=abcanchor+1
    queue=zeros(1,node_num);
    front=1;
    rear=1;
    %
	for node_no=1:node_num
        %abcnode=abcnode+1
        distance=dist(anchor_x(anchor_no),anchor_y(anchor_no),node_x(node_no),node_y(node_no));
        if distance<=rangemax%用此語句判斷是否在通信可達到距離范圍內
            locateinf(2*(anchor_no-1)+1,node_no)=1; 
            %跳數  里面的兩個元素是矩陣的位置坐標
            locateinf(2*anchor_no,node_no)=distance; %距離和
            %將該點加入隊列
            queue(1,rear)=node_no;%存放
            rear=rear+1;
%上面的這一塊描述的是現實狀態下，第一跳節點的錄入情況，dist的過程是flooding，后又聚集到錨 節點信息的過程
        end
        while(front<rear)
%從隊列中取出元素,該點為known 應該理解為，所有錨節點中每一個周圍相對的第一跳節點 哈哈
            known=queue(1,front);
            front=front+1;
            %
            for node_no=1:node_num
                distance=dist(node_x(known),node_y(known),node_x(node_no),node_y(node_no));
                %試圖進行第二跳范圍內的搜捕
                if distance<=rangemax
                    if locateinf(2*(anchor_no-1)+1,node_no)>(locateinf(2*(anchor_no-1)+1,known)+1)
                        %跳數比較 奇數行比較 與路由表的概念一致 最終達到所有的點都找到離錨節點最近的跳數
                        locateinf(2*(anchor_no-1)+1,node_no)=(locateinf(2*(anchor_no-1)+1,known)+1);
                        %選跳數少的錄入
                        
                  % if   locateinf(2*anchor_no,node_no)>locateinf(2*anchor_no,known)+distance;%增加采用距離和較短的算法
                        locateinf(2*anchor_no,node_no)=locateinf(2*anchor_no,known)+distance;
                       
                        %將該點加入隊列
                        queue(1,rear)=node_no;
                        rear=rear+1;
                        %
                  % end
                   
                   end
                end
            end
        end
    end
end 

ok='完成圖的廣度先遍歷'

% 生成錨節點優先選取矩陣anchorchosen，將距離遠的錨節點序號放在后面，即從好到壞排列
for node_no=1:node_num
    node_no
    for anchor_no=1:anchor_num
        %構造temp矩陣
        temp(anchor_no,1)=anchor_no;
        temp(anchor_no,2)=locateinf((2*anchor_no),node_no);
   %（注意討論的是距離，所以用2*anchor_no）以上，對于每個node，將temp矩陣初始化（注意，node的循環還沒有結束）
    end
    %對temp矩陣進行排序處理
    for ti=1:anchor_num-1
        min=ti;
        for tj=ti+1:anchor_num%(ti+1):anchor_num
            if temp(tj,2)<temp(min,2)
               min=tj;
            end
        end
        if ti~=min%同在第二列，ti行與min行互換
            tempintemp1=temp(ti,1);
            tempintemp2=temp(ti,2);
            temp(ti,1)=temp(min,1);
            temp(ti,2)=temp(min,2);
            temp(min,1)=tempintemp1;
            temp(min,2)=tempintemp2;
        end
    end
    for anchor_no=1:anchor_num
        anchorchosen(anchor_no,node_no)=temp(anchor_no,1);%排好的temp(anchor_no,1)
    end
end

ok='生成錨節點優先選取矩陣'

%% 改進算法，選取跳數少的錨節點進行定位,節點坐標存為(locate_near_x,locate_near_y)
locate_near_x=zeros(1,node_num);
locate_near_y=zeros(1,node_num);
for node_no=1:node_num
    tempflag=0;
    node_no
    for chosen1=1:(anchor_num-2)
        for chosen2=(chosen1+1):(anchor_num-1)
            for chosen3=(chosen2+1):anchor_num
               %[x,y]=barycenter(anchor_x(anchorchosen(chosen1,node_no)),anchor_y(anchorchosen(chosen1,node_no)),locateinf(2*anchorchosen(chosen1,node_no),node_no),anchor_x(anchorchosen(chosen2,node_no)),anchor_y(anchorchosen(chosen2,node_no)),locateinf(2*anchorchosen(chosen2,node_no),node_no),anchor_x(anchorchosen(chosen3,node_no)),anchor_y(anchorchosen(chosen3,node_no)),locateinf(2*anchorchosen(chosen3,node_no),node_no));
                [x,y]=barycenter(anchor_x(anchorchosen(chosen1,node_no)),anchor_y(anchorchosen(chosen1,node_no)),locateinf(2*anchorchosen(chosen1,node_no),node_no),anchor_x(anchorchosen(chosen2,node_no)),anchor_y(anchorchosen(chosen2,node_no)),locateinf(2*anchorchosen(chosen2,node_no),node_no),anchor_x(anchorchosen(chosen3,node_no)),anchor_y(anchorchosen(chosen3,node_no)),locateinf(2*anchorchosen(chosen3,node_no),node_no));
                %求三個圓所相交部分的三個交點坐標所成三角形的重心坐標(barycenter_x,barycenter_y)
               if((isreal(x)==1)&&(isreal(y)==1)) 
                    locate_near_x(node_no)=x;
                    locate_near_y(node_no)=y;
                    tempflag=1;
                end
                if(tempflag==1) break; end
            end
            if(tempflag==1) break; end
        end
        if(tempflag==1) break; end
    end
end

ok='定位完成'

%% 計算誤差
% 距離差
difference_x=abs(locate_near_x-node_x);
difference_y=abs(locate_near_y-node_y);

%% 顯示距離差
plot(difference_x,difference_y,'+');

% 得出距離差，放在矩陣difference中
for node_no=1:node_num
    difference(node_no)=sqrt((difference_x(node_no))^2+(difference_y(node_no))^2);
end

%% 得到距離差大于值gatevalue的點，將節點序號存于badpoint中，badpoint_no為badpoint的數量
gatevalue=rangemax;
badpoint_no=1;
for node_no=1:node_num
    if difference(node_no)>gatevalue
        badpoint(badpoint_no)=node_no;
        badpoint_no=badpoint_no+1;
    end
end
badpoint_no=badpoint_no-1;

% 繪制node節點位置和計算出的位置，并用線段連接
figure(1)
plot(anchor_x,anchor_y,'*',node_x,node_y,'*',locate_near_x,locate_near_y,'+');
hold on;
for node_no=1:node_num
    line([node_x(node_no),locate_near_x(node_no)],[node_y(node_no),locate_near_y(node_no)]);
end
hold off;
%plot(anchor_x,anchor_y,'*',node_x,node_y,'*')
%hold on;
%plot(locate_near_x,locate_near_y,'.','MarkerSize',15,'Color','r');
%for node_no=1:node_num
%    line([node_x(node_no),locate_near_x(node_no)],[node_y(node_no),locate_near_y(node_no)]);
%end
%hold off;

%門限值為最大通信距離,顯示距離差大于門限值的節點個數
badpoint_no

%計算連通度，并顯示結果
count=0;
for node_no=1:node_num
    if(isnan(difference(node_no))~=1)
        count=count+1;
    end
end
connectivity=count/node_num;
connectivity

range_error=mean(difference)/rangemax
ok='全部完成'