clc;
clear all;

global UavTeam
global UavHighway % 关联分组
global gcount gfigure % 画图temp
global temp1 temporder tempin tempout % 时间T=5s，temp
global currenttunnel % 正在或者已经过了几个tunnel
global takeoffM % 已经起飞的飞机个数
global taskdone
global bug Tlight turnTlight % 按时间起飞有个bug没修复，pass了，换成按次数起飞 % 红绿灯按次数变换颜色 % 红绿灯按次数该变哪个颜色
global N % 分组个数
global order % 排队号数
global Flaststate
global priority_yes % 已经选出一个要先走的

turnTlight = 0;
Tlight = 0;
bug = 0;
order = 0;
takeoffM = 1;
gcount = 0;
gfigure = 1;

rm = 7;
l  = 5;
vmax = 10;
ro = 0; % 没有静态障碍

%%………………………………%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 人工设置Highway参数
N = 2; % 分组个数
UavHighway = HighwayInitialization(); % 具体tunnel参数
%%………………………………%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  人工M个数
M = 4; % 每组飞机个数
taskdone = zeros(M*N,4);
temp1 = zeros(M*N,1);
temporder = inf * ones(M*N,1);
currenttunnel= ones(N*M,1);
tempin = inf * ones(M*N,1);
tempout = - inf * ones(M*N,1);
Flaststate = inf * ones(M*N,1);
priority_yes = zeros(N,2);
%%………………………………%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 人工初始化U!!!记得11位是否变化
global U
U = zeros(11*M*N,1);
for g=1:N
    for iii=1:M
        U(M*11*(g-1)+4*M+iii,1)=1; % load逻辑相反
    end
end
%%………………………………%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 人工设置uav参数
UavTeam = UAVInitialization(M,rm,l,vmax);

for gg=1:N
    for mm = 1:M
        UavTeam.Team(gg).Uav(mm).priority = inf; % 优先级，闲置无穷
    end
end

% Draw 2D map
figure(1);
% subplot(4,4,1)
MyMap();

%% YAC论文里的画图
% for k = 1: M
%     o = [UavTeam.Uav(k).CurrentPos(1) UavTeam.Uav(k).CurrentPos(2)]';
%     mydrawcolorball(o,k);
% end

'start'
sim('EMS_situation22_test9.mdl')
'over'

%% 画图函数还没修改过
% figure(2)
% subplot(2,1,1)
% plot(mindis(:,1),mindis(:,2),'-'); hold on;
% ksm = find(mindis(:,2)==min(mindis(:,2)));
% text(mindis(ksm(1),1),mindis(ksm(1),2),['\leftarrow' num2str(mindis(ksm(1),2))],'HorizontalAlignment','left')
% plot(mindis(:,1),2*rm*ones(size(mindis(:,1))),'r-.');
% legend('Between two multicopters')
% xlabel('t(sec)')
% ylabel('Minimum distance(m)')
%
% subplot(2,1,2)
% plot(mindis(:,1),mindis(:,4),'-'); hold on;
% ksh = find(mindis(:,4)==min(mindis(:,4)));
% text(mindis(ksh(1),1),mindis(ksh(1),4),['\leftarrow' num2str(mindis(ksh(1),4))],'HorizontalAlignment','left')
% plot(mindis(:,1),rm*ones(size(mindis(:,1))),'r-.');
% xlabel('t(sec)')
% ylabel('minimum distance(m)')
% legend('Between multicopter and tunnel edge')
%
% hold off