%% 悬停
function u = Scontroller(i,g) % in表示状态，=0表示landing ex
global UavTeam N

Pcur  =  UavTeam.Team(g).Uav(i).CurrentPos;
Vcur  =  UavTeam.Team(g).Uav(i).Velocity;
ctn = UavTeam.Team(g).Uav(i).CurrentTaskNum;
Pdes  =  UavTeam.Team(g).Uav(i).Waypoint(:,ctn);
vmax = UavTeam.Team(g).Uav(i).vmax;
rm = UavTeam.Team(g).Uav(i).r;
ra = UavTeam.Team(g).Uav(i).ra;
vmax = UavTeam.Team(g).Uav(i).vmax;
% 
% % 部分逻辑问题
% if ctn == 1
%     LastPdes = [inf;inf];
%     LLastPdes = [inf;inf];
% elseif ctn == 2
%     LastPdes  =  UavTeam.Team(g).Uav(i).Waypoint(:,ctn-1);
%     LLastPdes = [inf;inf];
% else
%     LastPdes  =  UavTeam.Team(g).Uav(i).Waypoint(:,ctn-1);
%     LLastPdes  =  UavTeam.Team(g).Uav(i).Waypoint(:,ctn-2);
% end

% % 结束任务，隐身啦
% if Pdes == UavTeam.Team(g).Uav(i).HomePos & LastPdes == UavTeam.Team(g).Uav(i).HomePos & LLastPdes == UavTeam.Team(g).Uav(i).HomePos
%     UavTeam.Team(g).Uav(i).state = 2;
%     UavTeam.Team(g).Uav(i).CurrentTaskNum = UavTeam.Team(g).Uav(i).CurrentTaskNum +1;
% end

% Velocity to waypoint
k1 = 1;
ksi_w  = Pcur + 1/UavTeam.gain*Vcur - Pdes;
Vw = - mysat(k1*ksi_w,vmax);

% Velocity away other multicopters
Vm = [0;0];
for ggg = 1:N
    for l = 1:UavTeam.AvailableNumMax
        if i~=l && UavTeam.Team(ggg).Uav(l).state ~= 2
            k2 = 1; e = 0.000001; rs = e;
            ksimil = (Pcur - UavTeam.Team(ggg).Uav(l).CurrentPos) + 1/UavTeam.gain*(Vcur - UavTeam.Team(ggg).Uav(l).Velocity);
            temp = (1+e)*norm(ksimil) - (2*rm)*mys(norm(ksimil)/(2*rm),rs);
            bil = k2*dmysigma(norm(ksimil),2*rm,ra)/temp ...
                - k2*mysigma(norm(ksimil),2*rm,ra)*((1+e)-dmys(norm(ksimil)/(2*rm),rs))/(temp^2);
            % 分子为零的危险
            if ksimil == 0
            else
                Vm = Vm - bil*(ksimil/norm(ksimil));
            end
        end
    end
end

% Sum of all velocities
u = mysat(Vw+Vm,vmax);