%------基本粒子群優化算法（Particle Swarm Optimization）-----------
function result = PSO(dim)
%--------聲明全局---------------------------
global N;
global m;
global MaxDT;
global Qmax;
global Kmax;
global w;
global c1;
global c2;
global curpoint;
global segR;
global pathta;
global pamoveta;
global numta;
global pathpoint;
global hadsteps;
global curstep;
global pointIndex;
%-----------------------
%------初始化種群的個體(可以在這里限定位置和速度的范圍)------------
result = true;

%psosued = ones(1,N);         %粒子群成功
Xmax = dim*pathta;                    %最大x值
Xmin = (dim-1)*pathta;                    %最小x值
Vmax = (Xmax - Xmin)*0.1;

for i=1:N
    [temp,psosued(i)] = initX(Xmax,Xmin);
     %如果初始化不成功則不做任何的v變化
    if psosued(i)
        x(i,:) = temp;
        for j=1:m
            v(i,j) = -Vmax+2*rand*Vmax;
        end
    end
end

%判斷是否有條可走路徑
for i=1:N
    if psosued(i)
        break;
    end
end

if psosued(i) == false
    result = false;
    return;
end

%------先計算各個粒子的適應度，并初始化Pi和Pg----------------------
%如果最大的
best_fitness = 10000;

for i=1:N
    %如果粒子群初始化不成功的話將其適應值改為10000
    if psosued(i)
        p(i)=fitness(x(i,:),m);
        y(i,:)=x(i,:);  %y記錄前一次迭代，粒子的位置
        if p(i)<best_fitness
            pg = x(i,:);
            best_fitness = p(i);
        end
    end
end


%------進入主要循環，按照公式依次迭代，直到滿足精度要求------------

for t=1:MaxDT

    for i=1:N
        %如果psosued為false的話，就讓這個粒子不迭代
        if psosued(i)
            %----cjss更新x
            for q = 1:Qmax
                %保護現場如果不通，直接返回
                yuanx = x(i,:);
                yuanv = v(i,:);
                for j = 1:m
                    for k=1:Kmax
                        %保護現場，如果更新后不連通，返回原來的x，v值
                        tempv = v(i,j);
                        tempx = x(i,j);
                        %%%粒子群更新公式%%%%%%%%%%%
                        v(i,j)=w*v(i,j)+c1*rand*(y(i,j)-x(i,j))...
                            +c2*rand*(pg(j)-x(i,j));
                        %看v是否越界
                        if v(i,j)>Vmax
                            v(i,j) = Vmax;
                        else if v(i,j)<-Vmax;
                                v(i,j) = -Vmax;
                            end
                        end

                        x(i,j)=x(i,j)+v(i,j);
                        %看x是否越界
                        if x(i,j)>Xmax
                            x(i,j) = Xmax;
                        else if x(i,j)<Xmin
                                x(i,j) = Xmin;
                            end
                        end
                        %%%%%%%%%%%%%%%%%%%%%%%%

                        %如果j是1的話就將curpoint賦值給purPoint
                        %如果不是的話講將前一個的tempPoint賦值給purPoint
                        if 1 == j
                            purPoint = curpoint;
                        else
                            purPoint = tempPoint;
                        end

                        %將極坐標轉換為直角坐標
                        [tempPoint(1),tempPoint(2)]=plorTozhijiao(j*segR,x(i,j));

                        %判斷這個點和前面的點是否相通
                        r = Conn(purPoint(1),purPoint(2),tempPoint(1),tempPoint(2));
                        if r
                            break;
                        else
                            v(i,j) = tempv;
                            x(i,j) = tempx;
                        end
                    end
                    %如果k太大的話，說明此路不通，直接重新開始
                    if r == false
                        break ;
                    end
                end
                if r
                    break;
                else
                    %保護現場如果不通，直接返回
                    x(i,:)=yuanx;
                    v(i,:)=yuanv;
                end
            end

            %---更新x
            % x(i,:)=x(i,:)+v(i,:);

            if fitness(x(i,:),m)<p(i)
                p(i)=fitness(x(i,:),m);
                y(i,:)=x(i,:);
            end
            if p(i)<fitness(pg,m)
                pg=y(i,:);
            end
        end
    end
    %記錄每次迭代的最好fitness
    Pbest(t)=fitness(pg,m);
end

%----------更新鏈表
%將這一維的字賦值維2,然后初始化下一個節點
pamoveta(curstep,dim) = 2;%2表示已經走過了
curstep = curstep+1;%將走的點賦值給當前值，繼續搜索
pamoveta(curstep,:)= 1;
%將下一段的維值
tempta = mod(numta/2+dim,numta);
if  tempta == 0
    tempta = 6;
end
pamoveta(curstep,tempta) = 3;%3表示來的路
%------------
%------更新點-------------
for j=1:m
    [tempPoint(1),tempPoint(2)]=plorTozhijiao(j*segR,pg(j));
    pointIndex = pointIndex+1;
    pathpoint(pointIndex,1) = tempPoint(1);       %路徑規劃得出的點
    pathpoint(pointIndex,2) = tempPoint(2);
end
pathpoint
curpoint(1) = tempPoint(1);
curpoint(2) = tempPoint(2);
hadsteps(curstep,1) = curpoint(1);
hadsteps(curstep,2) = curpoint(2);

% %------最后給出計算結果
%
% disp('*************************************************************')
% disp('函數的全局最優位置為：')
% Solution=pg'
% disp('最后得到的優化極值為：')
Result=fitness(pg,m)
% disp('*************************************************************')
%
% %------算法結束---DreamSun GL & HF-----------------------------------