ODE_Handle_GUI.asv

function [t_all, r_all, v_all, delr, delT, Z_all, DV_tot, a_all, e_all] = ODE_Handle_GUI(rA0, vA0, thrust, array_num, sc_num, dt, mass_fuel, mass_sc, Isp, M_A, R_A, d, theta, orbit_window, infront, THRUST_ON)

% rA0 and vA0 come from the inital data
% Thrust will stay constant for now
% dt is the stepsize
% array_num is the number of thrusters that are on the spacecraft pointed towards the asteroid
% sc_num is the number of identical space crafts, participating. 
% mass_fuel is the mass of the xenon fuel on board space craft
% mass_sc is the total mass of the space craft including the fuel
% Isp is the specific impulse in seconds
% M_A is the mass of the asteroid (in kilograms)
% R_A is the radius of the asteroid (in meters)
% d is the stand off distance between the space craft and the asteroid (in meters)
% theta is the Ion Beam Divergence Angle (in degrees)
% orbit_window is the window in the orbit that the thrusters will turn on, 0 is perihelion and 180 is aphelion. This number is in degrees.
% infront is a sign variable to indicate whether the spacecraft is infront of or behind the asteroid's motion, changes sign accordingly within propagate functions

% fprintf("\n--- PYTHON INPUT SANITY CHECK ---\n");
% 
% disp(rA0)
% disp(vA0)
% disp(thrust)
% disp(array_num)
% disp(sc_num)
% disp(dt)
% disp(mass_fuel)
% disp(mass_sc)
% disp(Isp)
% disp(M_A)
% disp(R_A)
% disp(d)
% disp(theta)
% disp(orbit_window)
% disp(infront)
% disp(THRUST_ON)
% 
% fprintf("--- END INPUT CHECK ---\n\n");


mu = 1.32712E+11; % km^3/s^2
tol = 1e-12; % acceptable tolerance
options = odeset('RelTol', tol, 'AbsTol', tol); % ODE45 options
g = 0.00980665; % km/s^2

thrust = thrust*array_num/1000; % to convert to kg*km/s^2 (kN) and account for how many thrusters used per space craft

total_thrust = thrust*sc_num; % account for number of space craft

mdot = thrust*2 / (g * Isp); % mass flow rate of the ion thruster per space craft

% Changes the Mode of Simulation
end_condition_num = 0;
% if SIM_MODE == 2
%     end_condition_num = SIM_TIME;  % simulate for a specific time
% else
%     disp("SIM_MODE is until fuel runs out!");
% end
SIM_MODE = 1; % until fuels run out

% Ion Beam coupling efficiency
% F = IBFraction(R_A, d, theta); commented out for now to verify
F = .95;
fprintf("Beam coupling fraction F = %.3f\n", F);

chunkSize = 10000;
step = 1;
r_all = zeros(chunkSize, 3);
v_all = zeros(chunkSize, 3);
t_all = zeros(chunkSize, 1);
a_all = zeros(chunkSize, 1);
e_all = zeros(chunkSize, 1);
delT = zeros(chunkSize, 1);
Z_all = zeros(chunkSize, 1);

%Calculate starting o_angle
fprintf("before orbit")
disp(rA0)
disp(vA0)

[a0_scalar, e0_scalar, e0_vec] = orbit_elements(rA0, vA0, mu);

e_unit = e0_vec/norm(e0_vec);
r_unit = rA0/norm(rA0);
o_angle = acosd((dot(e_unit,r_unit)));

% Set up for Displacement Calculations
T = 2 * pi * sqrt(a0_scalar^3/mu); % period of undeflected orbit

% Intialization
t_tot = 0;
total_thrust_time = 0;
i_total = 0;
Z_accumulated = 0;
first_loop = true;
angle_window = orbit_window;

fprintf('[t = %6d s] mass_sc = %.2f kg, a_thrust = %.2e km/s^2\n', t_tot, mass_sc, total_thrust / mass_sc);
% Condition Variable
SIM_ON = true;

% Updates Displacement of asteroid from original positions
b = real(a0_scalar * sqrt(1 - e0_scalar^2)); % semi-minor axis
x = (a0_scalar^2 - b^2);
C = real(pi * (a0_scalar + b) * (1 + (3*x^2)/(10 + sqrt(4-(3*x^2))))); % circumference of undeflected orbit

% Setup for numerical sweep
thetas = linspace(-pi, pi, 100000); % true anomaly in radians
dtheta = thetas(2) - thetas(1);
burn_angle = deg2rad(angle_window);

% Integrate dt over orbit
burn_time = 0;

for i = 1:length(thetas)
    th = thetas(i);
    r = a0_scalar * (1 - e0_scalar^2) / (1 + e0_scalar * cos(th));
    h = sqrt(mu * a0_scalar * (1 - e0_scalar^2));
    v = h / r;

    dt_orbit = (r^2 / h) * dtheta;  % dt = r^2 / h * dtheta  [from orbital mechanics]

    if abs(wrapToPi(th)) <= burn_angle  % within burn window
        burn_time = burn_time + dt_orbit;
    end
end

fuel_burn_per_orbit = burn_time * mdot;
N_orbits = mass_fuel / fuel_burn_per_orbit;
D_time = N_orbits * T;

% % B-Plane Deflection Set up
%D_time = 277689600; %time until it hits earth from arrival date 7/6/2032 -> 275721360, 
% 
% ref_date_str = '24-Apr-2041 16:16:00';  % Reference date
% ref_date = datetime(ref_date_str, 'InputFormat', 'dd-MMM-yyyy HH:mm:ss');
% 
% seconds_offset = seconds(D_time);
% 
% impact_date = ref_date - seconds_offset;
% 
% eart_RV = get_horizons_rv('399', impact_date, '1d')
% ast_RV = get_horizons_rv('-937019', impact_date, '1d')

V_A = [3.613283059240811E+01  6.179879692544576E+00  3.859614075692800E+00]'; % IC for earth at impact date km/s ast_RV(4:6,:);
V_E = [2.589142592452448E+01  1.370997475158599E+01 -2.070447412918064E-03]'; % IC for asteroid at impact date km/s eart_RV(4:6,:);

V_inf = V_A - V_E;
x_hat = cross(V_E, V_inf)/norm(cross(V_E, V_inf));
y_hat = V_inf/norm(V_inf);
z_hat = cross(x_hat, y_hat)/norm(cross(x_hat, y_hat));
T_HCI_B = [x_hat'; y_hat'; z_hat'];
V_E_B = T_HCI_B * V_E;
V_E_B_xz = sqrt((V_E_B(1,1)^2) + (V_E_B(3,1)^2));

THRUST_ON = 1;

fprintf("before loop")
disp(rA0)
disp(vA0)

% --- Start of the Propagation simulation ---
while SIM_ON

    %fprintf('Time: %.2f days | Fuel left: %.2f kg | Z = %.2f km\n', t_tot/86400, mass_fuel, Z_total);
    %fprintf('Angle to perihelion: %.2f\n', o_angle);
    %fprintf('Fuel left: %.2f kg', mass_fuel);

    % Checks angle and applies thrust only at the angle provided
    if THRUST_ON && o_angle <= angle_window
        
        fprintf("before propagate")
        disp(rA0)
        disp(vA0)
        disp([rA0; vA0])

        rA0 = rA0'; % significant DEBUG process to get here
        vA0 = vA0';

        [~, RV] = ode45(@(t, y) propagate_WT(t, y, mu, total_thrust, M_A, F, mass_sc*sc_num, d, infront), [0 dt], [rA0; vA0], options);
        % fprintf('thrust applied!');
        

        % Updating Mass of the Space Craft and the Fuel
        fuel_used = mdot * dt;

        if fuel_used > mass_fuel
            dt_actual = mass_fuel / mdot;
        else
            dt_actual = dt;
        end

        % updates mass of singular spacecraft
        fuel_used = mdot * dt_actual;
        mass_fuel = mass_fuel - fuel_used;
        mass_sc = mass_sc - fuel_used; 

        if mass_fuel <= 0
            fprintf('Fuel exhausted\n');
            THRUST_ON = 0;  % disables thrust globally
        end

        %fprintf('Fuel left: %.2f\n', mass_fuel);

        total_thrust_time = total_thrust_time + dt_actual;

        % Updates ΔV imparted to asteroid
        i_total = i_total + F * total_thrust * dt_actual;

    else

	    [~, RV] = ode45(@(t, y) propagate_WOT(t, y, mu, mass_sc*sc_num, d, 0), [0 dt], [rA0; vA0], options);
        dt_actual = dt;
    end

    % Updating State
    fprintf('after propagte')
	rA0 = RV(end,1:3)'
	vA0 = RV(end,4:6)'

    t_tot = t_tot + dt_actual; % might need to come back to this for orbit window

    if step > size(r_all, 1)
        r_all(end+1:end+chunkSize, :) = 0;
        v_all(end+1:end+chunkSize, :) = 0;
        t_all(end+1:end+chunkSize, 1) = 0;
        a_all(end+1:end+chunkSize, 1) = 0;
        e_all(end+1:end+chunkSize, 1) = 0;
        delT(end+1:end+chunkSize, 1) = 0;
        Z_all(end+1:end+chunkSize, 1) = 0;
    end

    % Updates and Extract time step for thrusted/simulation propagation
    r_all(step,:) = RV(end,1:3);
    v_all(step,:) = RV(end,4:6); 

    t_all(step,:) =  t_tot; % time steps

    % fprintf("DEBUG: rA0 = %.3f, vA0 = %.3f\n", rA0, vA0)

    [a_step, e_step, e_vec] = orbit_elements(rA0, vA0, mu);
    a_all(step,:) = a_step;
    e_all(step,:) = e_step;

    % This updates the angle of the orbit
    r_vec = r_all(step, :)';
    e_unit = e_vec / norm(e_vec);
    r_unit = r_vec / norm(r_vec);
    o_angle = acosd((dot(e_unit,r_unit)));


    af_step = a_step;
    dela_step = af_step - a0_scalar;
    delr_step = 1.5 * C * (dela_step/a0_scalar) * (1/T);

    % B-Plane Deflection
    Delta_T_total = (2 * pi * sqrt(af_step^3/mu)) - T;
    delT(step,:) = Delta_T_total;
    % fprintf("Delta T: %.3e s\n", delT(step));

    Z_total = Delta_T_total*(dt_actual/T)*V_E_B_xz;
    Z_all(step) = Z_accumulated + Z_total;
    Z_accumulated = Z_all(step,:);
    %fprintf('B-Plane Deflection: %.3f km\n', Z_accumulated);

    % Updating Step
    step = step + 1;

    % Sim runs until fuel runs out
    if SIM_MODE == 1
        if (mass_fuel <= 0)
            SIM_ON = false;
        end
    end

    % Sim runs until time runs out
    % if SIM_MODE == 2
    % 
    %     if t_tot > end_condition_num
    %         disp("sim ending b/c of time constraint")
    %         SIM_ON = false;
    %     end
    % 
    % end

    % if Z_all(end) >= 7370
    %     disp("sim ending b/c deflection reached")
    %     fprintf('B-Plane Deflection: %.3f km\n', Z_all(end));
    %     break
    % end

    % if t_tot >= 631152000  % For Low it is 65318400, For 50th it is 60048000,For Heavy its 86313600
    %     disp("sim ending b/c the world has ended")
    %     break
    % end

    if first_loop
    fprintf('Initial a_thrust: %.3e km/s²\n', total_thrust / (mass_sc*sc_num));
    fprintf('Initial mdot: %.3e kg/s\n', mdot);
    fprintf('Initial fuel: %.2f kg\n', mass_fuel);
    fprintf('Initial mass_sc: %.2f kg\n', mass_sc);
    first_loop = false;
    end
    
    if step == 300
        SIM_ON =0;
    end

    % SIM_ON = 0;

end

% Array Truncation
r_all = r_all(1:step-1, :);
v_all = v_all(1:step-1, :);
t_all = t_all(1:step-1, 1);
a_all = a_all(1:step-1, 1);
e_all = e_all(1:step-1, 1);
delT  = delT(1:step-1, 1);
Z_all = Z_all(1:step-1, 1);

% Finds Displacement of asteroid, over time
delr = delr_step * t_all(end,1);

% Find total ΔV imparted on asteroid
DV_tot = i_total / (M_A);


% D_remaining = max(631152000 - t_tot, 0);
% Z_total = delT(end)*(D_remaining/T)*V_E_B_xz;
% fprintf('B-Plane Deflection Coasting END: %.3f km\n', Z_total);

% % Verification calculation of max ideal delta V (Print Out)
fprintf("Total Zeta Deflection: %.3f \n", Z_all(end));
fprintf("Total Thrust Time (days): %.3f \n",  total_thrust_time/86400);
return