1. runge-kutta integrator with vop and without?
2. fuel mass


FS-9
6889.4908134881943624 km
190.2497292231479378 km
-16.5468474743937080 km
0.0453559761031717 km/sec
-0.9866437758147498 km/sec
7.5404616924289787 km/sec
x_cross = 2237487; % unit: mm2
y_cross = 2141441; % unit: mm2
z_cross = 4801999; % unit: mm2
sa_max_cross = 3055854; % unit: mm2
sa_y_cross = 22965.7; % unit: mm2
dm.area = [x_cross; x_cross; y_cross+sa_y_cross; ... % a1~a3 unit: mm2
y_cross+sa_y_cross; z_cross; z_cross; ... % a4~a6 unit: mm2
sa_max_cross;sa_max_cross;sa_max_cross;sa_max_cross];
% normal vector of each area
dm.norV = [ 1 0 0; % n1
-1 0 0; % n2
0 1 0; % n3
0 -1 0; % n4
0 0 1; % n5
0 0 -1; % n6
1 0 0; % n7
-1 0 0; % n8
0 0 1; % n9
0 0 -1]; % n10

clc;
clear;
close all;
format long
warning('off', 'all')

% Constants
G = 6.67430e-20; % Gravitational constant in km^3/(kg * s^2)
m1 = 5.97219e24; % Mass of the Earth in kg
RE = 6378.12; % Earth's radius in km
mu = 398600.44189; % Earth's gravitational parameter in km^3/s^2
Cd = 2.2; % Drag coefficient
A = 2.120773439350248; % Cross-sectional area of the satellite in square meters

% Initial conditions (circular orbit)
r0 = [0 7000.000000000003 0]; % Initial position vector in km
v0 = [-7.546053287267834 0 0]; % Initial velocity vector in km/s
Y0 = [r0 v0]; % Initial state vector
    

% Time settings
t0 = 0; % Initial time in seconds
tf = 86400*0.2; % Final time in seconds (2 day)
step_size = 1; % Time step in seconds
thrust_duration = 5828.51662847387; % Thrust duration in seconds

% Propagate the orbit using a custom RK4 solver with J2 and drag
[t, Y] = ode4(@(t, Y) relative_motion_drag_J2(t, Y, thrust_duration), t0, step_size, tf, Y0);

% Extract position and velocity vectors
rvec = Y(:, 1:3);
vvec = Y(:, 4:6);

% Calculate altitude
rmag = vecnorm(rvec, 2, 2);
altitude = rmag - RE;

% Calculate specific orbital energy and angular momentum
specific_energy = zeros(length(t), 1);
specific_angular_momentum = zeros(length(t), 1);
for i = 1:length(t)
    r_i = rvec(i, :);
    v_i = vvec(i, :);
    
    % Calculate the magnitude of the position and velocity vectors
    r_mag = norm(r_i);
    v_mag = norm(v_i);
    
    % Specific orbital energy
    specific_energy(i) = (v_mag^2 / 2) - (mu / r_mag);
    
    % Specific angular momentum magnitude
    h_vec = cross(r_i, v_i);
    specific_angular_momentum(i) = norm(h_vec);
    % Specific orbital energy and semi-major axis
    xi = (norm(v_i)^2 / 2) - (mu / norm(r_i));
    semi_major_axis(i) = -mu / (2 * xi);
    % Calculate semi-minor axis
    %b = semi_major_axis(i) * sqrt(1 - eccentricity(i)^2);
    % Orbital period calculation
    period(i) = 2 * pi * sqrt(semi_major_axis(i)^3 / mu);
end

% Plot specific orbital energy over time
figure;
plot(t / 86400, specific_energy);
xlabel('Time (days)');
ylabel('Specific Orbital Energy (km^2/s^2)');
title('Specific Orbital Energy over Time');
grid on;

% Plot specific angular momentum over time
figure;
plot(t / 86400, specific_angular_momentum);
xlabel('Time (days)');
ylabel('Specific Angular Momentum (km^2/s)');
title('Specific Angular Momentum over Time');
grid on;


% Plot altitude over time
figure;
plot(t / 86400, altitude);
xlabel('Time (days)');
ylabel('Altitude (km)');
title('Altitude over Time');
grid on;

% Create 3D orbit plot
figure;
[xx, yy, zz] = sphere(200);
surf(RE * xx, RE * yy, RE * zz);
colormap(light_blue);
clim([-RE / 100 RE / 100]);
shading interp;

hold on;
plot3(rvec(:, 1), rvec(:, 2), rvec(:, 3), 'k');
plot3(rvec(1, 1), rvec(1, 2), rvec(1, 3), 'go'); % Starting point
plot3(rvec(end, 1), rvec(end, 2), rvec(end, 3), 'ro'); % Ending point
grid on;
axis equal;
xlabel('X (km)');
ylabel('Y (km)');
zlabel('Z (km)');
title('3D Orbit with Thrust Directions');
hold off;

% Theoretical verification
% 理論上的速度增量計算
thrust = 0.032; % 推力 (N)
satellite_mass = 500; % 衛星質量 (kg)

% 推力加速度
thrust_acceleration = thrust / satellite_mass * 1e-3; % km/s^2

% 理論上的速度增量
delta_v_theoretical = thrust_acceleration * thrust_duration; % km/s
fprintf('理論上的速度增量: %.6f km/s\n', delta_v_theoretical);

% 理論上的新速度
v_initial = norm(v0); % 初始速度大小 (km/s)
v_new = v_initial + delta_v_theoretical; % 理論上的新速度大小

% 理論上的新的比機械能
r_initial = norm(r0); % 初始軌道半徑 (km)
epsilon_theoretical = (v_new^2 / 2) - (mu / r_initial);

% 理論上的新的半長軸
a_theoretical = -mu / (2 * epsilon_theoretical); % 新的半長軸 (km)
fprintf('理論上的新的半長軸: %.6f km\n', a_theoretical);

% 計算模擬的最終半長軸
r_final = norm(rvec(end, :)); % 最終軌道半徑 (km)
specific_energy_simulation_final = specific_energy(end);
a_simulation_final = -mu / (2 * specific_energy_simulation_final); % 模擬的最終半長軸 (km)
fprintf('模擬的新的半長軸: %.6f km\n', a_simulation_final);

% 比較半長軸變化
fprintf('理論與模擬半長軸的誤差: %.6f m\n', abs(a_theoretical - a_simulation_final)*1000);
% Helper function for color map
function map = light_blue
    r = 0.8;
    g = r;
    b = 1.0;
    map = [r g b; 0 0 0; r g b];
end

% ODE function that includes drag, J2 perturbation effects, and thrust
function Ydot = relative_motion_drag_J2(t, Y, thrust_duration)
    % Constants
    %J2 = 0.001082626724393;
    J2 = 0;
    mu = 398600.44189; % km^3/s^2
    RE = 6378.12; % Earth's radius in km
    
    % State vector decomposition
    rvector = Y(1:3);
    vvector = Y(4:6);
    
    % Distance from Earth's center
    r = norm(rvector);
    
    % J2 perturbation calculation
    Z = rvector(3);
    Z2 = Z^2;
    r2 = r^2;
    
    ax_J2 = (-mu * rvector(1) / r^3) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 1));
    ay_J2 = (-mu * rvector(2) / r^3) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 1));
    az_J2 = (-mu * rvector(3) / r^3) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 3));
    a_J2 = [ax_J2; ay_J2; az_J2];
    
    % Thrust parameters
    thrust = 0.032; % Thrust in Newtons
    satellite_mass = 500; % Mass of the satellite in kg
    thrust_acceleration = thrust / satellite_mass * 1e-3; % km/s^2
    
    % Apply thrust only within thrust duration
    if t <= thrust_duration
        % Apply thrust along velocity direction
        v = norm(vvector); % Magnitude of velocity
        if v > 0
            v_hat = vvector / v; % Unit vector in velocity direction
        else
            v_hat = [0; 0; 0];
        end
        a_thrust = thrust_acceleration * v_hat; % Thrust acceleration vector
    else
        a_thrust = [0; 0; 0]; % No thrust after thrust duration
    end
    
    % Total acceleration (gravitational + J2 + thrust)
    a_total = a_J2 + a_thrust;
    
    % Return rate of change of position and velocity
    Ydot = [vvector; a_total];
end

clc;
clear;
close all;
format long
warning('off','all')

% Constants
G = 6.67430e-20; % Gravitational constant in km^3/(kg * s^2)
m1 = 5.97219e24; % Mass of the Earth in kg
RE = 6378.12; % Earth's radius in km
mu = 398600.44189; % Earth's gravitational parameter in km^3/s^2
Cd = 2.2; % Drag coefficient
A = 2.120773439350248; % Cross-sectional area of the satellite in square meters

% Initial conditions
%[r_ijk,v_ijk] = keplerian2ijk(6915.843310968788,0.05,15,0,0,45);
r0 = [0 7000.000000000003 0]; % Initial position vector in km
v0 = [-7.546053287267834 0 0]; % Initial velocity vector in km/s
Y0 = [r0 v0]; % Initial state vector



% Time settings
t0 = 0; % Initial time in seconds
tf = 86400*7; % Final time in seconds (2 days)

% Global indices for burn1_done and burn2_done
global burn1_done_index burn2_done_index a_trans;
burn1_done_index = [];
burn2_done_index = [];

% Propagate the orbit using 4th order Runge-Kutta method
[t, Y] = ode4(@relative_motion_drag_J2, t0, 1, tf, Y0);

% Extract position and velocity vectors
rvec = Y(:, 1:3);
vvec = Y(:, 4:6);

% Calculate altitude
rmag = vecnorm(rvec, 2, 2);
altitude = rmag - RE;

% Calculate eccentricity at the final state
r_final = rvec(end, :)';
v_final = vvec(end, :)';
r_mag_final = norm(r_final);
v_mag_final = norm(v_final);
e_vec = (cross(v_final, cross(r_final, v_final)) / mu) - (r_final / norm(r_final));
eccentricity = norm(e_vec);

fprintf('Final Orbit Eccentricity: %.12f\n', eccentricity);


% Calculate specific orbital energy
xi = (v_mag_final^2 / 2) - (mu / r_mag_final);

% Calculate the semimajor axis using the relation a = -mu / (2 * xi)
a_final = -mu / (2 * xi);

fprintf('Final Orbit Semimajor Axis: %.12f km\n', a_final);
fprintf('模擬轉移軌道半長軸與理論差距: %.12f km\n', abs(a_final-a_trans));

% Plot altitude over time
figure;
plot(t / 86400, altitude);
xlabel('Time (days)');
ylabel('Altitude (km)');
title('Altitude over Time');
hold on;

% Mark burn start points on the altitude plot
if ~isempty(burn1_done_index)
    plot(burn1_done_index / 86400, altitude(find(t == int64(burn1_done_index))), 'bo', 'MarkerFaceColor', 'b', 'DisplayName', 'Burn 1 Start');
end

legend;

% Print indices for burn1_done and burn2_done
disp('Times for burn1_done:');
disp(burn1_done_index);
index1 = find(t == int64(burn1_done_index));



% Create a 3D plot of the orbit
figure;
[xx, yy, zz] = sphere(200);
surf(RE * xx, RE * yy, RE * zz)
colormap(light_blue)
clim([-RE / 100 RE / 100])
shading interp

% Draw and label the X, Y, and Z axes
line([0 2 * RE], [0 0], [0 0]);
text(2 * RE + RE / 10, 0, 0, 'x')
line([0 0], [0 2 * RE], [0 0]);
text(0, 2 * RE + RE / 10, 0, 'y')
line([0 0], [0 0], [0 2 * RE]);
text(0, 0, 2 * RE + RE / 10, 'z')

hold on
plot3(rvec(1:index1, 1), rvec(1:index1, 2), rvec(1:index1, 3), 'b')
plot3(rvec(index1:end, 1), rvec(index1:end, 2), rvec(index1:end, 3), 'r')
plot3(rvec(1, 1), rvec(1, 2), rvec(1, 3), 'go'); % Starting point
plot3(rvec(index1, 1), rvec(index1, 2), rvec(index1, 3), 'b+'); % Burn 1 start point

plot3(rvec(end, 1), rvec(end, 2), rvec(end, 3), 'ro'); % Ending point



% Set graph properties
grid on
axis equal
xlabel('km')
ylabel('km')
zlabel('km')
title('3D Orbit');
legend;

% Custom colormap function
function map = light_blue
    r = 0.8;
    g = r;
    b = 1.0;
    map = [r g b; 0 0 0; r g b];
end

% Function to compute the derivatives of the state vector
function Ydot = relative_motion_drag_J2(t, Y)
    % Declare global variables
    global burn1_done_index burn2_done_index a_trans;

    % Earth's J2 coefficient and gravitational parameter
    %J2 = 0.001082626724393;
    J2 = 0;
    mu = 398600.44189; % km^3/s^2
    RE = 6378.12; % Earth's radius in km
    
    % Extract position and velocity vectors
    rvector = Y(1:3);
    vvector = Y(4:6);
    
    % J2 perturbation calculation
    r = norm(rvector);  
    Z = rvector(3);
    Z2 = Z^2;
    r2 = r^2;
    ax_J2 = (-mu * rvector(1) / r^3) * (1 - (3 / 2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 1));
    ay_J2 = (-mu * rvector(2) / r^3) * (1 - (3 / 2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 1));
    az_J2 = (-mu * rvector(3) / r^3) * (1 - (3 / 2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 3));
    a_J2 = [ax_J2; ay_J2; az_J2];
    
    % Thrust calculation
    a_thrust = [0; 0; 0];
    thrust =0.032; % Thrust in Newtons
    satellite_mass = 475; % Mass of the satellite in kg
    thrust_acceleration = thrust / satellite_mass * 1e-3; % in km/s^2
    persistent burnStartTime burn_time1 burn_time2 burn1_done burn2_done; % Persistent variable to record burn start time
    if t >=5723.671751785554   
        if isempty(burnStartTime) 
            burn1_done = false;
            burn2_done = false;
            target_altitude = 700; % Target orbit altitude in km
            r1 = norm(rvector); % Initial orbit radius in km
            fprintf("R1:%.3f Km\n",r1-RE);
            r2 = RE + target_altitude; % Final orbit radius in km
            fprintf("R2:%.3f Km\n",r2-RE);
            % Calculate the velocities for the Hohmann transfer
            a_trans = (r1 + r2) / 2;
            fprintf("a_trans:%.12f Km\n",a_trans);
            v1 = sqrt(mu / r1);
            v1 = norm(vvector); % Initial orbit velocity
            fprintf("V1:%f Km/s\n",v1);
            v2 = sqrt(mu / r2); % Final orbit velocity
            fprintf("V2:%f Km/s\n",v2);
            va = sqrt((2 * mu / r1) - (mu / a_trans)); % Transfer orbit apogee velocity
            fprintf("Va:%f Km/s\n",va);
            vb = sqrt((2 * mu / r2) - (mu / a_trans)); % Transfer orbit perigee velocity
            fprintf("Vb:%f Km/s\n",vb);
            % Delta-v for the transfer
            delta_v1 = va - v1; % First burn
            fprintf("delta_v1:%f m/s\n",delta_v1*1000);
            % Thrust parameters
            %thrust =15000; % Thrust in Newtons
            Isp = 1244; % Specific impulse in seconds
            g0 = 9.81e-3; % Standard gravity in km/s^2
            satellite_mass = 475; % Mass of the satellite in kg
            mdot = thrust/(g0*Isp); % mass flow rate kg/s
            v_e = Isp * g0; % exhaust speed km/s
         
            % Calculate burn times for the Hohmann transfer
            burn_time1 = delta_v1 / thrust_acceleration; % Time for first burn in seconds
            fprintf("burn_time1:%f s\n",burn_time1);
            burnStartTime = t; % Record the start time of the burn
        end

        % Calculate true anomaly for periapsis (burn 1) and apoapsis (burn 2)    
        if ~burn1_done
            % Calculate true anomaly for periapsis
            e_vec = cross(vvector, cross(rvector, vvector)) / mu - rvector / norm(rvector);
            e = norm(e_vec);
            true_anomaly = acos(dot(e_vec, rvector) / (e * norm(rvector)));
            if dot(rvector, vvector) < 0
                true_anomaly = 2 * pi - true_anomaly;
            end
            % Start burn 1 at periapsis
            if abs(true_anomaly) <= 1.745329251994330e-04
                burnStartTime = t; % Start time for burn 1
                burn1_done = true;
                burn1_done_index = [burn1_done_index; t]; % Store time
                fprintf("True Anomaly (Burn 1): %.12f\n", rad2deg(true_anomaly));
                fprintf("burnStartTime: %.5f\n", burnStartTime);
                fprintf("burn1_done: %d\n", burn1_done);
            end
       
        end
        
        if burn1_done && (t >= burnStartTime && t <= burnStartTime + burn_time1)
            vvector_normalized = vvector / norm(vvector);
            a_thrust = (thrust_acceleration * vvector_normalized);
        end
    
        
    end
    % Total acceleration
    a_total = a_J2 + a_thrust;
    
    % Output derivative of state vector
    Ydot = [vvector; a_total];
end