%% vincoli non lineari trasferta diretta NRHO-LEO
function [c,ceq] = vincoli_LLO(X,t0,H,errH,inc,errI)                       % se H = [h,apo] impongo max apo

if nargin < 5
     inc = [];
end

% propagazione traiettoria
[Y_t,t_t,Xf_t,~,ie] = propagazione_traj(X,t0);
Xfin = Xf_t(end,:)';                                                       % estraggo stato di arrivo

% calcolo dV impulso circolarizzante
dV_fin = circolarizzazione(Xfin,2,t_t(end),inc);

r = H(1) + 1738.4;                                                            % posizione target
s_moon = cspice_spkpos('moon',t_t(end),'j2000','none','earth');
s_rel = Xfin(1:3)-s_moon;
dpos = abs(norm(s_rel)-r);                                                 % calcolo errore all'arrivo

c = dpos-errH;

if ~isempty(ie)
    c = [c;1];
else
    c = [c;-1];
end

if ~isscalar(H)
    for i = 1:length(t_t)
        Y_rel(i,:) = Y_t(i,:)- cspice_spkezr('moon',t_t(i),'j2000','none','earth')';
    end
    rrel_apo = max(vecnorm(Y_rel(:,1:3),2,2));
    apo_min = H(2);
    c = [c;apo_min-rrel_apo];
    if length(H) == 3
        apo_max = H(3);
        c = [c;rrel_apo-apo_max];
    end
end

if ~isempty(inc)
    XLLO = Xfin + [zeros(3,1); dV_fin];                                         % stato iniziale dell'orbita LLO
    kep_fin = state2kep(XLLO,2,t_t(end));                                  % estraggo parametri orbitali
    inc_fin = kep_fin(3)*180/pi;                                           % vincolo inclinazione
    dinc = norm(inc-inc_fin);
    c = [c;(dinc-errI)*10];
end

ceq = [];
