%%%%  飞机1红2橙3黄蓝4绿5青6蓝7紫8朱9墨10黑
%%%%  state： 0准备  1tunnel  2隐身  4降落  5投递  6free  7wait  10起飞 最后一位11留给over判断quit

global UavTeam
global UavHighway
global taskdone
global gcount
global takeoffM
global temp1 tempin tempout temporder
global bug Tlight
global currenttunnel
global U N
global Flaststate

gcount=gcount+1;
M = UavTeam.AvailableNumMax;
% 判断条件u初始化，u=[1-go/quit/deliver/land/load/free/wait/wait2free/wait2tunnel/takeoff/OVER,2-~,3-~.....M-~]，11位用于终止simulink

%% ……………………………… 红绿灯逻辑还没完善
Tlight = Tlight +1;   % 初始化 Tlight = 0,按次数变化
if Tlight >= 15000
    turnlight();
    Tlight = 0;     % 次数归零
    fprintf('第1组第1个红绿灯是%d颜色\n',UavHighway.group(1).Highway(2).light);
    fprintf('第2组第1个红绿灯是%d颜色\n',UavHighway.group(2).Highway(2).light);
end
%% ……………………………… 起飞（i,10）是takeoff
% 现在有多个飞机逐一起飞（每跑bug多少次，执行一个起飞命令）
if takeoffM <= M    % 初始化 takeoffM = 1
    bug = bug +1;   % 初始化 bug = 0
    if bug >= 2000
        % 每一组的第takeoffM个飞机都准备
        for g4=1:N
                U(M*11*(g4-1)+9*M+takeoffM,1)=1;     % takeoff置1
                UavTeam.Team(g4).Uav(takeoffM).state = 10; % 进入tunnel状态
                UavTeam.Team(g4).Uav(takeoffM).CurrentTaskNum = UavTeam.Team(g4).Uav(takeoffM).CurrentTaskNum +1;
        end
        fprintf('%d飞机',takeoffM);
        fprintf('在%d s',77.36);
        fprintf('准备任务完成，下一任务起飞%d \n',UavTeam.Team(g4).Uav(takeoffM).CurrentTaskNum);
        takeoffM = takeoffM + 1;
        bug = 0;
    end
end
%% 去Tunnel和降落和投递。（i,1/2/5）分别是go/quit/load
for ggg=1:N
    for i=1:M
        ctn = UavTeam.Team(ggg).Uav(i).CurrentTaskNum;
        Pdes = UavTeam.Team(ggg).Uav(i).Waypoint(:,ctn);
        % NEXT部分逻辑问题
        if UavTeam.Team(ggg).Uav(i).state == 2 && ctn ~= 1
        else
            NextPdes  =  UavTeam.Team(ggg).Uav(i).Waypoint(:,ctn+1);
            % LAST部分逻辑问题
            if ctn == 1
                LastPdes = [inf;inf];
            else
                LastPdes  =  UavTeam.Team(ggg).Uav(i).Waypoint(:,ctn-1);
            end
            % 如果任务是正数第2，也就是takeoff状态，悬停5s出发;
            if temp1(M*(ggg-1)+i,1) == 0 && UavTeam.Team(ggg).Uav(i).state == 10
                temp1(M*(ggg-1)+i,1) = 1;
                temporder(M*(ggg-1)+i,1) = 2; % 起飞
                tempin(M*(ggg-1)+i,1) = 77.36;
            end
            if temporder(M*(ggg-1)+i,1) == 2 && 77.36 == tempin(M*(ggg-1)+i,1)+5
                U(M*11*(ggg-1)+i+0*M,1) = 1;    % go=1;
                UavTeam.Team(ggg).Uav(i).state = 1; % 进入tunnel状态
                UavTeam.Team(ggg).Uav(i).CurrentTaskNum = UavTeam.Team(ggg).Uav(i).CurrentTaskNum +1;
                fprintf('%d组',ggg);
                fprintf('%d飞机',i);
                fprintf('在%d s',77.36);
                fprintf('起飞任务完成，下一任务tunnel%d \n',UavTeam.Team(ggg).Uav(i).CurrentTaskNum);
                temp1(M*(ggg-1)+i,1)=0;
                temporder(M*(ggg-1)+i,1) = inf;
            end
            % 如果上一目标和当前目标均是投递点，悬停5s模拟投递;
            if temp1(M*(ggg-1)+i,1) == 0 & Pdes == UavTeam.Team(ggg).Uav(i).Toufangdian & LastPdes == UavTeam.Team(ggg).Uav(i).Toufangdian
                temp1(M*(ggg-1)+i,1)=1;
                temporder(M*(ggg-1)+i,1) = 3;%投递
                tempin(M*(ggg-1)+i,1)=77.36;
            end
            if temporder(M*(ggg-1)+i,1) == 3 && 77.36 == tempin(M*(ggg-1)+i,1)+5
                U(M*11*(ggg-1)+i+4*M,1)=0;    % load=0!!
                UavTeam.Team(ggg).Uav(i).state = 1; % 进入tunnel状态
                UavTeam.Team(ggg).Uav(i).CurrentTaskNum = UavTeam.Team(ggg).Uav(i).CurrentTaskNum +1;
                fprintf('%d组',ggg);
                fprintf('%d飞机',i);
                fprintf('在%d s',77.36);
                fprintf('投递任务完成，下一任务tunnel%d \n',UavTeam.Team(ggg).Uav(i).CurrentTaskNum);
                temp1(M*(ggg-1)+i,1)=0;
                temporder(M*(ggg-1)+i,1)= inf;
            end
            % 如果任务是倒数第2，也就是landing状态，悬停5s退出;
            if temp1(M*(ggg-1)+i,1) == 0 & Pdes == UavTeam.Team(ggg).Uav(i).HomePos & LastPdes == UavTeam.Team(ggg).Uav(i).HomePos & NextPdes == UavTeam.Team(ggg).Uav(i).HomePos
                temp1(M*(ggg-1)+i,1) = 1;
                temporder(M*(ggg-1)+i,1) = 4;% 降落
                tempout(M*(ggg-1)+i,1) = 77.36;
            end
            if temporder(M*(ggg-1)+i,1) == 4 && 77.36 == tempout(M*(ggg-1)+i,1)+5
                U(M*11*(ggg-1)+i+1*M,1)=1;    % quit=1;
                UavTeam.Team(ggg).Uav(i).state = 2; % 进入隐身状态
                UavTeam.Team(ggg).Uav(i).CurrentTaskNum = UavTeam.Team(ggg).Uav(i).CurrentTaskNum +1;
                fprintf('%d组',ggg);
                fprintf('%d飞机',i);
                fprintf('在%d s',77.36);
                fprintf('降落任务完成，总任务数%d \n',UavTeam.Team(ggg).Uav(i).CurrentTaskNum);
                temporder(M*(ggg-1)+i,1) = inf;
                U(M*11*(ggg-1)+i+10*M,1) = 1; % U最后一位为了终止simulink
            end
        end
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 到达目标点。去投递和去降落判断在FFcontroller进行。（i，3/4）分别是deliver/land
for gggg=1:N
    for i1=1:M
        %% 如果是free状态
        if UavTeam.Team(gggg).Uav(i1).state == 6
            if taskdone(M*(gggg-1)+i1,3) == 1    %%%%%%%%%%%%%%%%%%%%%%%%%%%% deliver
                U(M*11*(gggg-1)+i1+2*M,1)=1; % deliver置1
                UavTeam.Team(gggg).Uav(i1).CurrentTaskNum = UavTeam.Team(gggg).Uav(i1).CurrentTaskNum + 1;
                fprintf('%d组',gggg);
                fprintf('%d飞机',i1);
                fprintf('在%d s',77.36);
                fprintf('free2投递点，下一任务投递%d \n',UavTeam.Team(gggg).Uav(i1).CurrentTaskNum);
                taskdone(M*(gggg-1)+i1,3) = 0; % 还原
                UavTeam.Team(gggg).Uav(i1).state = 5; % 进去投递状态
                
            elseif taskdone(M*(gggg-1)+i1,4) == 1   %%%%%%%%%%%%%%%%%%%%%%%%%%%%% land
                U(M*11*(gggg-1)+i1+3*M,1)=1; % land置1
                UavTeam.Team(gggg).Uav(i1).CurrentTaskNum = UavTeam.Team(gggg).Uav(i1).CurrentTaskNum + 1;
                fprintf('%d组',gggg);
                fprintf('%d飞机',i1);
                fprintf('在%d s',77.36);
                fprintf('free2家点，下一任务降落%d \n',UavTeam.Team(gggg).Uav(i1).CurrentTaskNum);
                taskdone(M*(gggg-1)+i1,4)=0; % 还原
                UavTeam.Team(gggg).Uav(i1).state = 4; % 进入landing状态
            end
        end
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 在tunnel中，离开还是等待。（i，6）是free，（i，7）是wait。
for g1=1:N
    for j=1:M
        % 在tunnel就判断
        if  UavTeam.Team(g1).Uav(j).state == 1
            % 判断是否等区域
            ifwait(g1,j);
            
            % 判断是否等红绿灯
            temptunnel = currenttunnel(M*(g1-1)+j,1);
            if  UavTeam.Team(g1).Uav(j).state == 1 && UavHighway.group(g1).Highway(temptunnel).nextconnectedlight == 3 % 如果下一个tunnel是红灯。
                U(M*11*(g1-1)+j+6*M,1)=1; % wait置1
                fprintf('%d组',g1);
                fprintf('%d飞机',j);
                fprintf('在%d s',77.36);
                fprintf('在tunnel等待红绿灯 \n');
                UavTeam.Team(g1).Uav(j).state = 7; % 等待状态
                Flaststate(M*(g1-1)+j,1) = 1; % 表明处于等红绿灯状态
            end
        end
    end
end
%% 在等待中，free还是tunnel。u（i，8）是wait2free，u（i，9）是wait2tunnel。
for g2=1:N
    for k=1:M
        %% 如果在等待状态
        if UavTeam.Team(g2).Uav(k).state == 7
            % 等红绿灯？
            temptunnel = currenttunnel(M*(g2-1)+k,1);
            % 如果处于等红绿灯状态
            if Flaststate(M*(g2-1)+k,1) == 1;
                % 如果下一个tunnel有灯，并且是绿灯
                if UavHighway.group(g2).Highway(temptunnel).nextconnectedlight == 2
                    U(M*11*(g2-1)+k+8*M,1)=1; % wait2tunnel
                    fprintf('%d组',g2);
                    fprintf('%d飞机',k);
                    fprintf('在%d s',77.36);
                    fprintf('结束等待，下一任务通过红绿灯%d \n',UavTeam.Team(g2).Uav(k).CurrentTaskNum);
                    UavTeam.Team(g2).Uav(k).state = 1; % 回到tunnel状态
                    Flaststate(M*(g2-1)+k,1) = inf; % 回到最初
                end
            end
        end
    end
end
%% 判断区域
wait2what();
%% U结束情况 u=[1-go/quit/deliver/land/load/free/wait/wait2free/wait2tunnel/takeoff,2-~,3-~.....M-~]
for g5=1:N
    for kk=1:M
        %%%%%%%%%%%%%%%%%%%%%%%%%%% takeoff结束
        if U(M*11*(g5-1)+kk+0*M,1) == 1
            U(M*11*(g5-1)+kk+9*M,1) = 0;
        end
        %%%%%%%%%%%%%%%%%%%%%%%%%%% quit.land.go.load结束可以不管
        %%%%%%%%%%%%%%%%%%%%%%%%%%% deliver结束
        if U(M*11*(g5-1)+kk+4*M,1) == 0 % 注意load与其他相反
            U(M*11*(g5-1)+kk+2*M,1) = 0;
        end
        %%%%%%%%%%%%%%%%%%%%%%%%%%% wait结束
        if U(M*11*(g5-1)+kk+8*M,1) == 1
            U(M*11*(g5-1)+kk+6*M,1) = 0;
        end
        %%%%%%%%%%%%%%%%%%%%%%%%%%% free结束
        if U(M*11*(g5-1)+kk+2*M,1) == 1 || U(M*11*(g5-1)+kk+3*M,1) == 1
            U(M*11*(g5-1)+kk+5*M,1) = 0;
        end
        %%%%%%%%%%%%%%%%%%%%%%%%%%% wait2tunnel结束
        if U(M*11*(g5-1)+kk+5*M,1) == 1 || U(M*11*(g5-1)+kk+6*M,1) == 1
            U(M*11*(g5-1)+kk+8*M,1) = 0;
        end
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 画图
if gcount>=1000
    for g3=1:N
        for ii=1:M
            if UavTeam.Team(g3).Uav(ii).state ~= 2
                figure(1);
                o = [UavTeam.Team(g3).Uav(ii).CurrentPos(1) UavTeam.Team(g3).Uav(ii).CurrentPos(2)]';
                mydrawcolorball(o,ii);
            end
        end
    end
    gcount = 0;
end

u=U;