ODE_Handle.m

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

% 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

global THRUST_ON SIM_MODE SIM_TIME

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

Isp = 1455;
thrust = 235e-3; %N
array_num = 10; % number of solar arrays

thrust = thrust*thrust_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

% 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
[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 = acos((dot(e_unit,r_unit)));   
o_angle = rad2deg(o_angle);

% 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));

%POWER HANDLING VARIABLES (reference top of code for more info)
P0 = 6853; %Watts, reference value (max throttle power)
V0 = 1800; %Volts
P_sc = 2000; %Volts
syms ib;

% NEXT PM Thruster - High Thrust Boundary (Table B.3-1)
% Coefficients: [1, P0^1, P0^2, P0^3, P0^4]
thrust_coeffs = [1.19388817e-02,  1.60989424e-02,  1.14181412e-02, ...
                -2.04053417e-03,  1.01855017e-04];

mdot_coeffs   = [2.75956482e-06, -1.71102132e-06,  1.21670237e-06, ...
                -2.07253445e-07,  1.10213671e-08];

% High Isp boundary (alternative test)
thrust_coeffs_hiIsp = [3.68945763e-03,  4.05432510e-02, -7.91621814e-03, ...
                        1.72548416e-03, -1.11563126e-04];

mdot_coeffs_hiIsp   = [2.22052155e-06, -1.80919262e-07,  2.77715756e-08, ...
                        2.98873982e-08, -2.91399146e-09];

% Physical constants
% m_Xe = 2.1801e-25;  % kg, Xenon ion mass
% e_charge = 1.602e-19;  % C

% C_F from: F = eta_d * sqrt(2 * m_Xe / e) * I_B * sqrt(V_b)
% C_F = sqrt(2 * m_Xe / e_charge);  % ≈ 1.65e-3

% c1 = ; c2 = ; c3 = ; %edit!


% --- 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);
    
    % POWER HANDLING
    r_AU = norm(rA0)/(1.496e8);
    
    a1 = 1.4229; a2 = 0.6139; a3 = 0.0038; a4 = -0.2619; a5 = 0.0797;

    V_rel = ((a1 + a2*r_AU^-1 + a3*r_AU^-2)/(1 + a4*r_AU + a5*r_AU^2)); % b = a if b not given

    P_rel = 1/(r_AU^2) * V_rel;

    P_aval = P0 * P_rel * array_num; % W, total available power

    V_aval = V0 * V_rel * array_num; % V, avaiable beam Voltage

    P_in_aval = (P_aval - P_sc)/(thrust_num*2); % thruster pair (one forward and other one backwards to stay stationary)
    
    % I_B = solve(c1 + c2*ib - 0.0915*ib^2 == mdot/2);
    % 
    % V_beam = P_in_aval / I_B; %actual beam voltage needed
    % 
    % if V_beam > V_aval
    %     V_beam = V_aval;
    %     I_B = P_in_aval / V_aval;
    % end
    % 
    % % Gamma = divergence efficiency * utilization efficiency * I_B
    % Gamma = 0.987 * 0.018 * I_B;
    % 
    % F_thrust = C_F * Gamma * sqrt(I_B * P_in_aval); % N, per thruster
    % 
    % total_thrust = F_thrust * thrust_num * sc_num / 1000 ; % kN

     % Power available per thruster
    P0_kW = P_in_aval / 1000; %kW

    % Clamp to valid throttle table range (TL01-TL40)
    P0_min = 0.545; P0_max = 6.853;  % kW, from table
    if P0_kW < P0_min
        warning('P0 = %.3f kW below min throttle level', P0_kW);
        P0_kW = P0_min;
    elseif P0_kW > P0_max
        P0_kW = P0_max;
    end

    % Evaluate polynomial fits
    F_thrust = polyval(flip(thrust_coeffs), P0_kW);  % N, per thruster
    mdot     = polyval(flip(mdot_coeffs),   P0_kW) * thrust_num*2;  % kg/s, per thruster

    % Scale to full mission thrust
    total_thrust = F_thrust * thrust_num * sc_num / 1000;  % kN


    % Checks angle and applies thrust only at the angle provided
    if THRUST_ON && o_angle <= angle_window

        [~, 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 = false;  % 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
	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

    [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_all(step));

    % 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

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);
fprintf('Remaining Fuel %.3f kg\n', mass_fuel);

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