clear all
close all
clc

% T = [ 30 60 90 200 300 400 500 600] ;
T = [90] ; 
eta_A = [];
eta_T = [];
eta_P = [];
for t = T
    %% System parameters
    % Defining the most important system parameters
    K = 6 ;       % Number of users
    M = 2 ;       % Number of UAVs
    Nr = 1  ;     % Number of realizations

    L = 2e3 ;     % Coverage area length (m)
    W = 2e3 ;     % Coverage area Width (m)
    H = 100 ;     % UAV flight height (m)

    sigma2 = 10^(-110/10)/1e3 ;% Noise power -110 dBm
    do = 1 ;                   % refrence distance
    rho = 10^(-60/10);         % channel power at refrence distance (-60 dBm)

    Pmax = 0.1 ;               % UAV max power
    Vmax = 50  ;               % UAV max speed (m/s)
    dmin = 100 ;               % UAVs min separation distance

    tau = 100;                 % time-slot factor
    epislon_o = 1e-4;          % algorithim stopping criteria
    % T = 210 ;                  % Maximum period time (s)
    % N = 11 ;
    % epislon_max = ceil(Vmax*T / (H*N)) ;
    % Smax = 6000; % calcualted from the inital trajectory to make the problem valid
    % Smax = epislon_max * H ;
    epislon_max = 1;
    N = ceil(Vmax*t/(H*epislon_max)) ;
    delta_t = t/N;
%     Smax = delta_t * Vmax ;
    Smax = epislon_max * H;
%     if ((Smax/H)>epislon_max)
%         Smax = epislon_max * H;
%     end
    

    % generating the user's location realization
    % w = [ L*rand(K,1) W*rand(K,1) ] ;     % generating a random realization
    w = [-1200 570 ; -450 400;-700 -600 ; ...
        600 1100 ; 150 200;750 -400]; % realization used in the paper

    % Initialization (M=1)
    Po  = ones(N,M)*Pmax ;         % Initial power
    theta_n = 2*pi*([0:N-2 ,0])./(N-1) ;

    C = mean(w) ;                  % center for the trajectory
    R_max = Vmax*t/(2*pi) ;        % Maximum radius of the trajectory
    R_cg = max(sqrt(sum(((kron(C,ones(K,1))- w).^2),2))) ;
    R = min(R_cg,R_max) ;
    Qo(:,:,1)  = [ C(1)+R*cos(theta_n) ;  C(2)+R*sin(theta_n) ]' ;
    Qo(:,:,2)  = [ C(1)+R_max*cos(theta_n) ;  C(2)+R_max*sin(theta_n) ]' ;
    C_1 = mean(w(4:6,:));
    R_1 = R/2;
    Qo(:,:,3)  = [ C_1(1)+R_1*cos(theta_n) ;  C_1(2)+R_1*sin(theta_n) ]' ;
    Qo(:,:,4)  = [ C_1(1)+(R_1/2)*cos(theta_n) ;  C_1(2)+(R_1/2)*sin(theta_n) ]' ;
    % % C = mean(w(1:3,:));
    C_2 = mean(w(1:3,:));
    R_2 = R/2;
    Qo(:,:,5)  = [ C_2(1)+R_2*cos(theta_n) ;  C_2(2)+R_2*sin(theta_n) ]' ;
    Qo(:,:,6)  = [ C_2(1)+(R_2/2)*cos(theta_n) ;  C_2(2)+(R_2/2)*sin(theta_n) ]' ;

    eta_o = 0 ;
    delta_eta = 1;
    P = Po ;
    r = 0 ;
    Q(:,:,1) = Qo(:,:,4);
    Q(:,:,2) = Qo(:,:,6);
%     P(1:10,1) = 0;

    figure(1)
    grid on
    hold on
    plot(w(:,1) , w(:,2),'b o')
    xlabel('x(m)')
    ylabel('y(m)')
    plot(Qo(:,1,1),Qo(:,2,1),'r -- ')
%     plot(Qo(:,1,2),Qo(:,2,2),'r - ')  
    plot(Qo(:,1,3),Qo(:,2,3),'b -- ')
    plot(Qo(:,1,4),Qo(:,2,4),'b - ')
    plot(Qo(:,1,5),Qo(:,2,5),'g -- ')
    plot(Qo(:,1,6),Qo(:,2,6),'g - ')
    plot(C(1),C(2),'r x')
    C = mean(w);
    plot(C_1(1),C_1(2),'b x')
    plot(C_2(1),C_2(2),'g x')
    plot([C_1(1) C_2(1)],[C_1(2) C_2(2)],'b --')

    figure(2)
    hold on
    grid on
    plot(Q(:,1,1),Q(:,2,1),'r -- ')
    plot(Q(:,1,2),Q(:,2,2),'b -- ')
    plot(w(:,1) , w(:,2),'k x')
    
    
    
    while delta_eta > epislon_o
        [eta_Alloc(r+1) , A, gamma] = optimize_Allocation(Q,P,w,rho,H,sigma2,M,K,N);    % initialization
         eta_Alloc(r+1)
        [eta_Traj_underEst(r+1),Q] = optimize_Trajectory_M_2_1(A,Q,P,N,K,M,sigma2,rho,H,w,Smax,dmin);
        eta_traj(r+1) = min(Rate_Calculation(SINR_calculation(Q,P,w,rho,H,sigma2,M,K,N),A,K,M,N))
       
        [eta_Pow_upperbound(r+1),P] = power_optimization(Pmax,A,Q,P,M,N,K,rho,w,H,sigma2);
        
        eta_pow(r+1) = min(Rate_Calculation(SINR_calculation(Q,P,w,rho,H,sigma2,M,K,N),A,K,M,N))
        
        delta_eta = eta_pow(r+1)- eta_o;
        
        if (delta_eta < epislon_o)
            eta_o = 0;
        end
        eta_o = eta_pow(r+1);
        r = r + 1
    end
    eta_A = [eta_A;eta_Alloc(r)]
  
    eta_P = [eta_P;eta_pow(r)]
    eval(['save t=' num2str(t) 'M=2'])
    clear Qo Q
end
figure
plot(T,eta_A)
