% Calculating theta base on start and target points
theta = atand((target(2) - start(2))/(target(1) - start(1)));
% Parameter to control size of bounds
delta_d = 0;
% Number of wolfs
N = 50;
% Number of point generation for each wolf
d = 10;
% Constant for Cost_Function
mhio = 0.4;
% Optimization itteration
t_max = 100;
%% Initialization
% Transofrm point and threats to the new coordination
start_transform = Coordinate_Transfromation(start, start, theta);
target_transform = Coordinate_Transfromation(target, start, theta);
threats_tranform = threats;
for i = 1:size(threats, 1)
    threats_tranform(i, 1:3) = Coordinate_Transfromation(threats(i, 1:3), start, theta);
end
% Calculating bounds for initialization and producing N random intial points
[P_min, P_max] = UL_Bounds(threats_tranform, delta_d);
wolfs_positions = Initialization(N, d, target_transform, start_transform, P_min, P_max);
% calculating fittness of each wolf
fitness = Inf;
for i=1:N
    % wolfs_positions(:, :, i) is a set of d points' coordinates for the ith wolf
    fitness_wolf = Cost_Function(wolfs_positions(:, :, i),  threats_tranform, mhio, d);
    if fitness_wolf < fitness
        fitness = fitness_wolf;
        X_alpha = wolfs_positions(:, :, i);
        X_alpha_index = i;
    end
end
% Generating matrix for Xi(t). 
X_t = zeros(d, 3, N, t_max);
for i = 1:N
    for t = 1:t_max
        if t==1
            % We have Xi(1) for different all i. So we fill the X_t(:,:,i,1)
            % with the known positions.
            X_t(:,:,i,1) = wolfs_positions(:, :, i);
        else
            % Also since all the time for all wolfs, the x coordinate
            % should be the same, we fill it so !
            X_t(:,1,i,t) = wolfs_positions(:, 1, i);
            % Also the last row (d) for all X_t should be the target
            X_t(d,:,i,t) = target_transform;
        end
    end
end