clc
clear
close all
%%
addpath(genpath(pwd))
%%
kappa = 1;
%%
[Mission_Param, Environment_Param, Normalized_Param] = paraSet(); % 全局变量
X = load("XSolve.mat").X;
[X, f] = fsolve(@(X) constain_Gauss(X, kappa, Mission_Param, Environment_Param), X);
%%

%%
tf        = X(7)*1000;

alpha = 1.2;
sigma_max = tf/3;
sig       = sigma_max*(1-kappa);
theta = sig;

% if alpha<0
%     alpha = 0;
% end

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

m0 = Environment_Param.m0;
tscale = Normalized_Param.tscale;

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

%%
state0        = zeros(26,1);
state0(1:13)  = [P0 ex0 ey0 hx0 hy0 L0 X(1) X(2) X(3) X(4) X(5) X(6) m0]';
state0(14:26) = state0(1:13)*erf(tf/(sqrt(2)*sig));
% state0(14:26) = state0(1:13)*gammainc((tf)/theta, alpha)/gamma(alpha);

% timespan  = [0,tf];
deltat = 100/tscale;
timespan = [0:deltat:tf, tf];
% timespan = linspace(0, tf, 15000);
options   = odeset('RelTol',3e-14,'AbsTol',1e-14);
[t,state] = ode113(@dynamicModel_Gauss,timespan,state0,options,alpha,theta,sig,tf, Environment_Param);
%%
thrust_dire = zeros(size(t,1),3);
for ii = 1:size(t,1)
    thrust_dire(ii, :) = thrust_direction_trans(state(ii,1:13), Environment_Param)';
end
%%
Data = [state(:, 1:13), thrust_dire];
%%
DataFileName = './DataLib/Data.csv';
writematrix(Data, DataFileName, Delimiter=',');