%% ------------------------------ Note ------------------------------------
%  这是原程序的并行计算版本
%  不要出现任何的全局变量，以免出现内存访问错误，因为并行池之间的内存不是共享的
%% ------------------------------ END--------------------------------------
close all;clear all;clc
[Mission_Param, Environment_Param, Normalized_Param] = paraSet(); % 全局变量
% global Normalized_Param
tscale = Normalized_Param.tscale;
guesstf = 5; % 时间的初始猜想
x0 = [0;0;0;1;1;1;guesstf]; % initial guesses

%% ------------The pseudo-arclength continuation method-------------
tic
[X,~,param] = arclength('constain_Gauss',x0,-1,Mission_Param, Environment_Param);
% [X, ~, param] = arclength_continuation2('constain_Gauss', x0, 1, Mission_Param, Environment_Param);
f = constain_Gauss(X,param,Mission_Param, Environment_Param); %
toc
figPlot;
%% ---------------Test---------------
% N_iter = 100;
% RandMax = 0.2;
% Success_InverseGauss = 0.0;
% Success_Gauss = 0.0;
% Success_Flag = zeros(N_iter, 2);
% for iter = 1:N_iter
% 
%     x0 = [1;1;1;0;0;0;guesstf]; % initial guesses
%     x0_Rand = 1 + RandMax*rand*2-RandMax;
%     x0 = x0 .* x0_Rand;
% 
%     fun_name_InverseGauss = 'constain_InverseGauss';
%     fun_name_Gauss = 'constain_Gauss';
%     Options_fsolve  = optimset('TolX',1e-12','TolFun',1e-12,'MaxIter',300,...
%     'MaxFunEvals',10000,'FunValCheck ','on' ,'LargeScale','off',...
%     'Algorithm','trust-region-dogleg','Display','iter',...
%     'PrecondBandWidth',1,'UseParallel',true);
%     [~,f_InverseGauss] = fsolve(fun_name_InverseGauss,x0,Options_fsolve,0.0,Mission_Param, Environment_Param);
%     [~,f_Gauss] = fsolve(fun_name_Gauss,x0,Options_fsolve,0.0,Mission_Param, Environment_Param);
%     if sum(abs(f_InverseGauss))<1e-6
%         Success_InverseGauss = Success_InverseGauss+1;
%         Success_Flag(iter,1) = 1;
%     end
%     if sum(abs(f_Gauss))<1e-6
%         Success_Gauss = Success_Gauss+1;
%         Success_Flag(iter, 2) = 1;
%     end
% 
%     disp(['测试进度：', num2str(100*iter/N_iter), '%  ==============  InverseGauss成功率：', num2str(100 * Success_InverseGauss/iter), '%  ==============  Gauss成功率：', num2str(100 * Success_Gauss/iter)])
% 
% end
%%
% save(['RandMax', num2str(RandMax),'.mat'])