close all;
%% 初始化参数
[Mission_Param, Environment_Param, Normalized_Param] = paraSet();
% global Mission_Param Environment_Param Normalized_Param
Re  = Environment_Param.Re;
g0  = Environment_Param.g0;
m0  = Environment_Param.m0;

P0  = Mission_Param.P0;
ex0 = Mission_Param.ex0;
ey0 = Mission_Param.ey0;
hx0 = Mission_Param.hx0;
hy0 = Mission_Param.hy0;
L0  = Mission_Param.L0;

Pf  = Mission_Param.Pf;
exf = Mission_Param.exf;
eyf = Mission_Param.eyf;
hxf = Mission_Param.hxf;
hyf = Mission_Param.hyf;

tscale = Normalized_Param.tscale;
Lscale = Normalized_Param.Lscale;

% Read = load('./data/data-200.txt');
Read = readmatrix("data\data-200.txt");
x0 = Read(end,4:10);
% x0  = X;
%% 积分
tf = x0(7);
state0    = zeros(26,1);
state0(1:13)  = [P0 ex0 ey0 hx0 hy0 L0 x0(1) x0(2) x0(3) x0(4) x0(5) x0(6) m0 ]';
timespan = [0,tf*1000];
options   = odeset('RelTol',3e-14,'AbsTol',1e-14);
Mu = 0.0;
Lambda = 0.0;
[tt1,xx1] = ode113(@dynamicModel_InverseGauss,timespan,state0,options,Mu, Lambda,tf, Environment_Param);
%各个值
P  = xx1(:,1)*Lscale;
ex = xx1(:,2);
ey = xx1(:,3);
hx = xx1(:,4);
hy = xx1(:,5);
L  = xx1(:,6);
%% modified equinoctial orbit elements  -->  orbital elements
%参数转换
e     = sqrt(ex.^2+ey.^2);
a     = P./(1-e.^2);
i     = 2*atan(sqrt(hx.^2+hy.^2));
Omega = atan2(hy,hx);
omega = atan2(ey,ex)-Omega;
f     = L-Omega-omega;
e_f     = sqrt(exf^2+eyf^2);
a_f     = Pf*Lscale/(1-e_f^2);
i_f     = 2*atan(sqrt(hxf^2+hyf^2));
Omega_f = atan2(hyf,hxf);
omega_f = atan2(eyf,exf)-Omega_f;
f_f     = -Omega_f-omega_f;

X_save = OE_to_GEI(a,e,i,Omega,omega,f);
X_f = OE_to_GEI(a_f,e_f,i_f,Omega_f,omega_f,f_f);

%% 绘制轨道根数变化
figure(1)
plot(tt1*tscale/3600,P/1000,'-','linewidth',2);grid on;xlabel('time(h)');ylabel('P(km)');
hold on
scatter(tt1(end)*tscale/3600,P(end)/1000,200,"pentagram",'filled');
legend('',"End Point",'Location','southeast')

figure(2)
plot(tt1*tscale/3600,ex,'-','linewidth',2);grid on;xlabel('time(h)');ylabel('e_x');
hold on
scatter(tt1(end)*tscale/3600,ex(end),200,"pentagram",'filled');
legend('',"End Point",'Location','southeast')

figure(3)
plot(tt1*tscale/3600,ey,'-','linewidth',2);grid on;xlabel('time(h)');ylabel('e_y');
hold on
scatter(tt1(end)*tscale/3600,ey(end),200,"pentagram",'filled');
legend('',"End Point",'Location','southeast')
figure(4)
plot(tt1*tscale/3600,hx,'-','linewidth',2);grid on;xlabel('time(h)');ylabel('h_x');
hold on
scatter(tt1(end)*tscale/3600,hx(end),200,"pentagram",'filled');
legend('',"End Point",'Location','southeast')
figure(5)
plot(tt1*tscale/3600,hy,'-','linewidth',2);grid on;xlabel('time(h)');ylabel('h_y');
hold on
scatter(tt1(end)*tscale/3600,hy(end),200,"pentagram",'filled');
legend('',"End Point",'Location','southeast')
figure(6)
plot(tt1*tscale/3600,L,'-','linewidth',2);grid on;xlabel('time(h)');ylabel('L(rad)');
hold on
scatter(tt1(end)*tscale/3600,L(end),200,"pentagram",'filled');
legend('',"End Point",'Location','southeast')

%% 绘制轨道
figure(7)
z1 = plot3(X_save(1,:)/1000,X_save(2,:)/1000,X_save(3,:)/1000,'r--','linewidth',1.5);hold on;
z2 = plot3(X_save(1,1)/1000,X_save(2,1)/1000,X_save(3,1)/1000,'b.','MarkerSize',30);hold on;
z3 = plot3(X_save(1,end)/1000,X_save(2,end)/1000,X_save(3,end)/1000,'k.','MarkerSize',30);hold on;
axis equal;
[x, y, z] = sphere(100);
x = Re * x / 1000;
y = Re * y / 1000;
z = Re * z / 1000;
surf(x, y, z);
legend([z1,z2,z3],{'transfer orbit','Initial point','End point'});
% legend([z1,z2,z3,z4],{'transfer orbit','Initial point','Target point','Engine shutdown point'});
xlabel('x(km)');ylabel('y(km)');zlabel('z(km)');grid on;

%% 卫星到地心的距离
figure(8)
plot(tt1*tscale/3600,sqrt(X_save(1,:).^2+X_save(2,:).^2+X_save(3,:).^2)/1000,'b-','linewidth',2);hold on;
plot(tt1*tscale/3600,Re*ones(size(tt1))/1000,'r-','linewidth',2);hold on;
plot(tt1*tscale/3600,(Re+100e3)*ones(size(tt1))/1000,'k--','linewidth',2);hold on;
xlabel('time(h)');ylabel('r(km)');grid on;
legend('Transfer vector diameter','Earth radius','Karman line');

%% 推力方向
figure(9)
thrust_dire = zeros(3,size(tt1,1));
for ii = 1:size(tt1,1)
    thrust_dire(:,ii) = thrust_direction_trans(xx1(ii,1:13), Environment_Param);
end
plot(tt1*tscale/3600,thrust_dire(1,:),'b-o','LineWidth',2,'markersize',3);hold on
plot(tt1*tscale/3600,thrust_dire(2,:),'r-.','LineWidth',2);hold on
plot(tt1*tscale/3600,thrust_dire(3,:),'-','LineWidth',2);hold on
grid on;
legend('axis-x','axis-y','axis-z');

figure(10)
% 第一个子图
subplot(3,1,1);
plot(tt1*tscale/3600,thrust_dire(1,:),'LineWidth',3);
xlabel('time(h)');
ylabel('T_x');
grid on;
ylim([-1,1])
% 第二个子图
subplot(3,1,2);
plot(tt1*tscale/3600,thrust_dire(2,:),'LineWidth',3);
grid on;
xlabel('time(h)');
ylabel('T_y');
ylim([-1,1])
% 第三个子图
subplot(3,1,3);
plot(tt1*tscale/3600,thrust_dire(3,:),'LineWidth',3);
grid on;
xlabel('time(h)');
ylabel('T_z');
ylim([-1,1])
%% 转移时间
figure(11)
T_f = 2*pi*sqrt(a_f^3/3.9802e+14)/3600; % Terminal orbit period (h)
trans_time = Read(:,10)* 1000 * tscale/3600; % time of engine turning on
% plot(Read(:,3),trans_time,'LineWidth',2)
scatter(Read(:,3),trans_time,'LineWidth',2)
grid on 
xlabel('\kappa')
ylabel('t_f(h)')
title('轨道转移时间随\kappa的变化')

%% kappa的变化
figure(12)
plot(Read(:,3),'-o','LineWidth',2,'MarkerSize',2)
grid on 
ylabel('\kappa')
xlabel('iteration times')
title('\kappa随迭代次数的变化')

%% 不同的sigma对应的kappa变化
figure(13)
line_styles = {'-', '-', '-', '-.', '-.', '-.'};
markers = {'o', 's', '^', 'd', 'v', '*'};
col = [1,0.5,0.1];
d_col = [0,0.4,0.4];
colors = [col;col;col;col;col;col;]; % 使用 MATLAB 内置的颜色映射，您也可以使用其他颜色映射或自定义RGB值
colors = linspace(0,1,6)'*d_col + colors;
for i = 2
    name = ['.//data//data-',int2str(i*100),'.txt'];
    data_read = readmatrix(name);
    XL = linspace(0, 1, length(data_read(:,3)));
    plot(XL, data_read(:,3), 'LineWidth', 2, 'LineStyle', line_styles{i-1}, 'Marker', markers{i-1}, 'Color', colors(i-1, :),'MarkerSize',6)
    hold on 
end
grid on
legend('\sigma_{max}=200','\sigma_{max}=300','\sigma_{max}=400','\sigma_{max}=500','\sigma_{max}=600','\sigma_{max}=700','Location','best')
ylabel('\kappa')

%% 过程中随kappa变化推力方向的变化
figure(14)
clear thrust_dire
% 第一个子图
ref = [1,4,11,27];
name = ['.//data//data-',int2str(200),'.txt'];
data_read = readmatrix(name); 
line_width = 1.5;
marker_size = 4;
for i = 1:4
    x0 = data_read(ref(i),4:10);
    tf = x0(7);
    state0    = zeros(26,1);
    state0(1:13)  = [P0 ex0 ey0 hx0 hy0 L0 x0(1) x0(2) x0(3) x0(4) x0(5) x0(6) m0 ]';
    timespan = [0,tf*1000];
    options   = odeset('RelTol',3e-14,'AbsTol',1e-14);
    % [tt1,xx1] = ode113(@dynamicModel, timespan,state0,options,0,tf*1000);
    [tt1,xx1] = ode113(@dynamicModel_InverseGauss,timespan,state0,options,Mu, Lambda,tf*1000, Environment_Param);
    %各个值
    P  = xx1(:,1)*Lscale;
    ex = xx1(:,2);
    ey = xx1(:,3);
    hx = xx1(:,4);
    hy = xx1(:,5);
    L  = xx1(:,6);
    
    n = round(200*tf);
    thrust_dire = zeros(3,n);
    for ii = 1:n
        thrust_dire(:,ii) = thrust_direction_trans(xx1(ii,1:13), Environment_Param);
    end

    subplot(3,1,1);
    plot(tt1(1:n)*tscale/3600,thrust_dire(1,:), 'LineWidth', line_width, 'LineStyle', line_styles{i}, 'Marker', markers{i}, 'Color', colors(i, :),'MarkerSize',marker_size);
    xlabel('time(h)');
    ylabel('T_x');
    hold on;grid on;
    legend('\kappa=0','\kappa=0.55','\kappa=0.86','\kappa=1')

    % 第二个子图
    subplot(3,1,2);
    plot(tt1(1:n)*tscale/3600,thrust_dire(2,:), 'LineWidth',line_width,'LineStyle', line_styles{i}, 'Marker', markers{i}, 'Color', colors(i, :),'MarkerSize',marker_size);
    hold on;grid on;
    xlabel('time(h)');
    ylabel('T_y');
    legend('\kappa=0','\kappa=0.55','\kappa=0.86','\kappa=1')

    % 第三个子图
    subplot(3,1,3);
    plot(tt1(1:n)*tscale/3600,thrust_dire(3,:), 'LineWidth',line_width, 'LineStyle', line_styles{i}, 'Marker', markers{i}, 'Color', colors(i, :),'MarkerSize',marker_size);
    hold on;grid on;
    xlabel('time(h)');
    ylabel('T_z');
    legend('\kappa=0','\kappa=0.55','\kappa=0.86','\kappa=1')
end