%-----路徑規劃函數--------------------------------------
function result = pathplanning(void)
%---聲明全局變量-------
global segR;
global V;
global curpoint;
global pathused;
global curstep;
global pamoveta;
global goalta;
global pathta;
global numta;
global g;
global movelen
global searchsued;      %是否搜索成功
global robotv;
global ploR;
global hadsteps;
%------
result = false;

%判斷是否找到目標點了
%%如果找到則searchsued為true
if distance(curpoint(1),curpoint(2),g(1),g(2))<=robotv
    searchsued = true;
    %判斷當前點到目標點的距離是否小于傳感器范圍
    %%如果小于的話就重新設置機器人的移動距離
else if distance(curpoint(1),curpoint(2),g(1),g(2))<ploR
        movelen = distance(curpoint(1),curpoint(2),g(1),g(2));
    else
        movelen = ploR
    end
end

%判斷搜索是否成功，成功的話就退出
if searchsued
    result = true;
    return;
end

%---接下來就是搜索主題了---
%求出當前點與目標點的方程
gaolta = straightLine(curpoint(1),curpoint(2),g(1),g(2));

%求出下一個點在第幾維空間中
dim = ceil(gaolta/pathta)

%從維數中依此搜索路徑
%一個for循環
for i=1:numta
    %求出在第幾維中
    if mod(dim+i-1,numta) == 0
        dimtemp = numta;
    else
        dimtemp = mod(dim+i-1,numta);
    end

    %算出機器人的速度(矢量)
    V(1) = movelen*cos(gaolta+(i-1)*pathta);
    V(2) = movelen*sin(gaolta+(i-1)*pathta);
    %求出下一個
    nextpoint(1) = curpoint(1)+V(1);
    nextpoint(2) = curpoint(2)+V(2);
    
    %先判斷這一維是否可以走通
    if pamoveta(curstep,dimtemp) == 1
        %判斷在這一維中是否與目標點相連
        %%如果相連的話
        if Conn(curpoint(1),curpoint(2),nextpoint(1),nextpoint(2))
            %將這一維的字賦值維2,然后初始化下一個節點
            pamoveta(curstep,dimtemp) = 2;%2表示已經走過了
            curstep = curstep+1;
            pamoveta(curstep,:)= 1;
            %將下一段的維值
            tempta = mod(numta/2+dimtemp,numta);
            if  tempta == 0
                tempta = 6;
            end
            pamoveta(curstep,tempta) = 3;%3表示來的路
            %將走的點賦值給當前值，繼續搜索
            curpoint(1) = nextpoint(1);
            curpoint(2) = nextpoint(2);
            hadsteps(curstep,1) = curpoint(1);
            hadsteps(curstep,2) = curpoint(2);
            if pathplanning()
                result = true;
                return;
            end
        else
            pamoveta(curstep,dimtemp) = 0;%2表示已經走過了
        end
        %如果不能走通的話，繼續循環（什么都不用做）
    end
end

if i==numta && pamoveta(curstep,i)~=1
    curstep = curstep-1;
end






% %----
% %求出機器人的預計下一個點
% nextpoint(1) = curpoint(1)+V(1);
% nextpoint(2) = curpoint(2)+V(2);
% %判斷當前點和下一個點之間的連線是否有障礙物點
% %如果有，用PSO，沒有直接返回x
% %如果區域連通則讓pamoveta在相應的區域為2，
% %讓下一個點的pamoveta對應域為3.
% if  Conn(curpoint(1),curpoint(2),nextpoint(1),nextpoint(2))
%     k = ceil(goalta/pathta); %求在第幾個區域
%     pamoveta(curstep,k) = 2;%2表示已經走過了
%     pamoveta(curstep+1,:)= 1;
%     pamoveta(curstep+1,mod(numta/2+k,numta)) = 3;%3表示來的路
%     x(1) = nextpoint(1);
%     x(2) = nextpoint(2);
%     return;
% else
%     pg = PSO();
%     %如果PSO成功就進行PSO，否則將當前的值賦值給x
%     if pathused
%         for i = 1:length(pg)
%             [x(i,1),x(i,2)]=plorTozhijiao(i*segR,pg(i));
%         end
%     else
%         %不成功返回值%有問題
%         x(1) = curpoint(1);
%         x(2) = curpoint(2);
%     end
% end