%% 在tunnel就判断，u（i，6）是free，u（i，7）是wait。
function ifwait(g1,j)
global UavTeam
global U order % 红绿灯等待不需要order
global Flaststate
global priority_yes

M = UavTeam.AvailableNumMax;
ctn = UavTeam.Team(g1).Uav(j).CurrentTaskNum;
Pdes  =  UavTeam.Team(g1).Uav(j).Waypoint(:,ctn);
NextPdes  =  UavTeam.Team(g1).Uav(j).Waypoint(:,ctn+1);

%% 如果是投递点
if UavTeam.Team(g1).Uav(j).state == 1 & NextPdes == UavTeam.Team(g1).Uav(j).Toufangdian % 在tunnel里等待看的下一个点，只有等与不变
    % 1就等，0就走
    tempTLY = Toufangdian_load(j,g1,UavTeam.Team(g1).Uav(j).Toufangdian); % 投递点有无状态飞机
    tempLagT = Lag(j,g1,UavTeam.Team(g1).Uav(j).Toufangdian,3); % tunnel尽头有无状态飞机
    if tempTLY == 1 || tempLagT == 1
        U(M*11*(g1-1)+j+6*M,1)=1; % wait
        fprintf('%d组',g1);
        fprintf('%d飞机',j);
        fprintf('在tunnel等待投递区域f1\n'); % task不d动
        UavTeam.Team(g1).Uav(j).state = 7;
        Flaststate(M*(g1-1)+j,1) = 3; % 表明和投递点相关
        order = order + 1;
        UavTeam.Team(g1).Uav(j).priority = order;
    end
end
if UavTeam.Team(g1).Uav(j).state == 1 & Pdes == UavTeam.Team(g1).Uav(j).Toufangdian % 在tunnel里free看的当前点，有等与free
    % 1就等，0就走
    tempTLY = Toufangdian_load(j,g1,UavTeam.Team(g1).Uav(j).Toufangdian);
    if tempTLY == 0
        U(M*11*(g1-1)+j+5*M,1)=1; % free
%         UavTeam.Team(g1).Uav(j).CurrentTaskNum =
%         UavTeam.Team(g1).Uav(j).CurrentTaskNum +1 ; %%%%%%%%%
%         task不+1？？因为Tcontroller内部有+1
        fprintf('%d组',g1);
        fprintf('%d飞机',j);
        fprintf('离开tunnel去投递点，任务%d \n',UavTeam.Team(g1).Uav(j).CurrentTaskNum);
        UavTeam.Team(g1).Uav(j).state = 6;
        Flaststate(M*(g1-1)+j,1) = 3; % 表明和投递点相关
        priority_yes(g1,1) = 0; % 恢复，该分组已经选出一个最优先的，直到他完成任务才会轮到下一个，第1列是投递点
    else
        U(M*11*(g1-1)+j+6*M,1)=1; % wait
        fprintf('%d组',g1);
        fprintf('%d飞机',j);
        fprintf('在tunnel等待投递区域f2\n'); % task不d动
        UavTeam.Team(g1).Uav(j).state = 7;
        Flaststate(M*(g1-1)+j,1) = 3; % 表明和投递点相关
        order = order + 1;
        UavTeam.Team(g1).Uav(j).priority = order;
    end
end
    %% 如果是家点
if UavTeam.Team(g1).Uav(j).state == 1 & Pdes == UavTeam.Team(g1).Uav(j).HomePos % 在tunnel里free看的当前点，有等与free
    % 1就等，0就走
    tempHSUY = HomPos_see_unload(j,g1,UavTeam.Team(g1).Uav(j).HomePos);
    if tempHSUY == 0
        U(M*11*(g1-1)+j+5*M,1)=1; % free
%         UavTeam.Team(g1).Uav(j).CurrentTaskNum =
%         UavTeam.Team(g1).Uav(j).CurrentTaskNum +1 ; %%%%%%%%%
%         task不+1？？因为Tcontroll内部有+1
        fprintf('%d组',g1);
        fprintf('%d飞机',j);
        fprintf('离开tunnel去家点，任务%d \n',UavTeam.Team(g1).Uav(j).CurrentTaskNum);
        UavTeam.Team(g1).Uav(j).state = 6;
        Flaststate(M*(g1-1)+j,1) = 2; % 表明和家点相关
        priority_yes(g1,2) = 0; % 恢复，该分组已经选出一个最优先的，直到他完成任务才会轮到下一个，第2列是家点
    else
        U(M*11*(g1-1)+j+6*M,1)=1; % wait
        fprintf('%d组',g1);
        fprintf('%d飞机',j);
        fprintf('在tunnel等待家点区域f3 \n');
        UavTeam.Team(g1).Uav(j).state = 7;
        Flaststate(M*(g1-1)+j,1) = 2; % 表明和家点相关
        order = order +1;
        UavTeam.Team(g1).Uav(j).priority = order;
    end
end
if UavTeam.Team(g1).Uav(j).state == 1 & NextPdes == UavTeam.Team(g1).Uav(j).HomePos % 等待看的下一个点，只有等与不动的选择
    % 1就等，0就走
    tempHSUY = HomPos_see_unload(j,g1,UavTeam.Team(g1).Uav(j).HomePos); % 家点有无状态飞机
    tempLagH = Lag(j,g1,UavTeam.Team(g1).Uav(j).HomePos,2); % tunnel尽头有无状态飞机
    if tempHSUY == 1 || tempLagH == 1
        U(M*11*(g1-1)+j+6*M,1)=1; % wait
        fprintf('%d组',g1);
        fprintf('%d飞机',j);
        fprintf('在tunnel等待家点区域f4 \n');
        UavTeam.Team(g1).Uav(j).state = 7;
        Flaststate(M*(g1-1)+j,1) = 2; % 表明和家点相关
        order = order +1;
        UavTeam.Team(g1).Uav(j).priority = order;
    end
end