%% funzione per le ottimizzazioni
function [X_GA,X_sqp,Inc,Quota,DV_tot,DV,Dt] = ...
         ottimizzazione(t0,f_opt,vincoli,H,inc,N,errH,errI,pop_size,gen_max,dV_max,t_span,plt,ref,X0)

dV = dV_max*ones(1,3*(N-1));                                               % modulo massimo velocità
l = [0 1e-8*ones(1,N-1) -dV];                                              % intervallo temporale di 30 giorni
u = [30 t_span(1)*ones(1,N-1) dV];
nvar = length(u);                                                          % lunghezza vettore da ottimizzare

% vincoli lineari
if isscalar(t_span)
    b = t_span;                                                            % t_span = t_max
    A = [0 ones(1,N-1) zeros(1,length(dV))];                               % t(end)-t(1)<t_max
else
    b = [t_span(1);-t_span(2)];                                            % t_span = [t_max, -t_min]
    A = [0 ones(1,N-1) zeros(1,length(dV)); 
        0 -ones(1,N-1) zeros(1,length(dV))];                               % -(t(end)-t(1))<-t_min
end

%% OTTIMIZZAZIONE GENETICA DELLA TRASFERTA
if ~isempty(gen_max)
    % opzioni dell'ottimizzazione
    options = optimoptions('ga', ...
        'Display','iter');
    if nargin == 15
        options.InitialPopulationMatrix = X0;
    end
    % OTTIMIZZAZIONE GENETICA
    if ~isscalar(pop_size)
        dpos_ad = 1e3;                                                             % normalizzo posizione
        dinc_ad = 30;                                                              % normalizzo inclinazione
        dt_ad = 5;                                                                 % normalizzo tempo
        w = [1 100/dpos_ad 80/dinc_ad 0/dt_ad];                                       % pesi
        options.PopulationSize = pop_size(2);
        options.MaxGenerations = gen_max*2;
        X_GA_p = ga(@(x) fitness_smooth(x,t0,ref,w,H,inc),nvar,A,b,[],[],l,u,[],options);
        f_opt(X_GA_p,t0,inc)
        options.InitialPopulationMatrix = X_GA_p;
    end
    options.PopulationSize = pop_size(1);
    options.MaxGenerations = gen_max;
    exitflag = -2;
    attempt = 0;
    if length(pop_size) ~= 3
    while exitflag == -2 && attempt < 10
        attempt = attempt + 1;
        [X_GA,~,exitflag] = ga(@(x) f_opt(x,t0,inc),nvar,A,b,[],[],l,u,@(x) vincoli(x,t0,H,errH,inc,errI),options);
    end
    else 
        X_GA = X_GA_p;
    end
end

%% OTTIMIZZAZIONE SQP DI RIFINIMENTO DELLA SOLUZIONE A PARTIRE DA X0 DELLA GENETICA
% opzioni dell'ottimizzazione sqp
optionsSQP = optimoptions('fmincon','Algorithm','sqp', ...
    'Display','iter','MaxFunctionEvaluations',100000, ...
    'StepTolerance', 1e-12 ,'MaxIterations',10000);
if isempty(gen_max)
    X_GA = X0;
end

% OTTIMIZZAZIONE SQP
X_sqp = fmincon(@(x) f_opt(x,t0,inc),X_GA,A,b,[],[],l,u,@(x) vincoli(x,t0,H,errH,inc,errI),optionsSQP);

%% POST PROCESSING DEI RISULTATI DELL'OTTIMIZZAZIONE GENETICA

[dVf_GA,kepF_GA] = post_processing(X_GA,t0,plt,ref,inc);
dv_GA = X_GA(N+1:end);
dV_GA = reshape(dv_GA,3,[]);

%% %% POST PROCESSING DEI RISULTATI DELL'OTTIMIZZAZIONE SQP

[dVf_sqp,kepF_sqp] = post_processing(X_sqp,t0,plt,ref,inc);
dv_sqp = X_sqp(N+1:end);
dV_sqp = reshape(dv_sqp,3,[]); 

%%
Inc = [kepF_GA(3)*180/pi,kepF_sqp(3)*180/pi];
r = [6378,1738.4];
Quota = [kepF_GA(1),kepF_sqp(1)]-r(ref);
DV_tot = [norm(dVf_GA)+sum(vecnorm(dV_GA,2,1)),norm(dVf_sqp)+sum(vecnorm(dV_sqp,2,1))];
Dt = [sum(X_GA(2:N)),sum(X_sqp(2:N))];
DV = [dV_GA,dVf_GA,dV_sqp,dVf_sqp];
fprintf('\n%-12s %20s %20s\n', '', 'Ottimizzazione GA', 'Ottimizzazione SQP');
fprintf('%s\n', repmat('-', 1, 54));
fprintf('%-12s %20.4f %20.4f\n', 'Inc',    Inc(1),    Inc(2));
fprintf('%-12s %20.4f %20.4f\n', 'Quota',  Quota(1),  Quota(2));
fprintf('%-12s %20.4f %20.4f\n', 'DV_tot', DV_tot(1), DV_tot(2));
fprintf('%-12s %20.4f %20.4f\n', 'Dt',     Dt(1),     Dt(2));