%% 过隧道模式，标志位=1
function u= Wcontroller(i,g)
global UavTeam
global currenttunnel
global UavHighway
global Flaststate

Pcur  =  UavTeam.Team(g).Uav(i).CurrentPos;
Vcur  =  UavTeam.Team(g).Uav(i).Velocity;
ra = UavTeam.Team(g).Uav(i).ra;
UavTeam.Team(g).Uav(i).state = 7;
M = UavTeam.AvailableNumMax;
ctn = UavTeam.Team(g).Uav(i).CurrentTaskNum;
% Pdes  =  UavTeam.Team(g).Uav(i).Waypoint(:,ctn);

ctntemp = currenttunnel(M*(g-1)+i,1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 红绿灯的是否需要添加？？
% if Flaststate(M*(g-1)+i,1) == 2 || Flaststate(M*(g-1)+i,1) == 3
%     if Pdes == UavTeam.Team(g).Uav(i).Toufangdian |  UavTeam.Team(g).Uav(i).HomePos
%         temptunnel = currenttunnel -1;
%         ctntemp = temptunnel(M*(g-1)+i,1);
%     end
% end

%% 后续tunnel转为第一个
pt1 = coordinate1(ctntemp,UavHighway.group(g).Highway(ctntemp).ph1,g);
pt2 = coordinate1(ctntemp,UavHighway.group(g).Highway(ctntemp).ph2,g);
rt = UavHighway.group(g).Highway(ctntemp).rh;
pt3 = pt2 + [0 rt]';
ksitemp  = Pcur + 1/UavTeam.gain*Vcur;
ksi = coordinate1(ctntemp,ksitemp,g);
rb = ra;
rsr= 10000;
rrt= 10000;
%% 判断区域
if ksi(1)>0 && ksi(2)>rt  % Left Standby Area
    psr1 = [pt2(1) rt+rsr]';
    psr2 = [-rb rt+rsr]';
    psr3 = [-rb rt]';
    %% 转回初始状态
    psr1 = coordinate2(ctntemp,psr1,g);
    psr2 = coordinate2(ctntemp,psr2,g);
    psr3 = coordinate2(ctntemp,psr3,g);
    u = mytunnel(g,i,psr1,psr2,psr3,rsr,1,ctntemp);
    
elseif  ksi(1)>0 && ksi(2)<-rt  % Right Standby Area
    psr1p = [pt2(1) -rt-rsr]';
    psr2p = [-rb -rt-rsr]';
    psr3p = [-rb -rt]';
    %% 转回初始状态
    psr1p = coordinate2(ctntemp,psr1p,g);
    psr2p = coordinate2(ctntemp,psr2p,g);
    psr3p = coordinate2(ctntemp,psr3p,g);
    u = mytunnel(g,i,psr1p,psr2p,psr3p,rsr,1,ctntemp);
    
elseif  ksi(1)<=0 && ksi(2)>=rt % Left Ready Area
    prt1 = [-rrt rt+rrt]';
    prt2 = [-rrt rt-rb]';
    prt3 = [0    rt-rb]';
    %% 转回初始状态
    prt1 = coordinate2(ctntemp,prt1,g);
    prt2 = coordinate2(ctntemp,prt2,g);
    prt3 = coordinate2(ctntemp,prt3,g);
    u = mytunnel(g,i,prt1,prt2,prt3,rrt,1,ctntemp);
    
elseif  ksi(1)<=0 && ksi(2)<=-rt % Left Ready Area
    prt1p = [-rrt rt+rrt]';
    prt2p = [-rrt rt-rb]';
    prt3p = [0    rt-rb]';
    %% 转回初始状态
    prt1p = coordinate2(ctntemp,prt1p,g);
    prt2p = coordinate2(ctntemp,prt2p,g);
    prt3p = coordinate2(ctntemp,prt3p,g);
    u = mytunnel(g,i,prt1p,prt2p,prt3p,rrt,1,ctntemp);
    
else% Tunnel and Tunnel Extension
    %% 转回初始状态
    pt1 = coordinate2(ctntemp,pt1,g);
    pt2 = coordinate2(ctntemp,pt2,g);
    pt3 = coordinate2(ctntemp,pt3,g);
    u = mytunnel(g,i,pt1,pt2,pt3,rt,1,ctntemp);
end