KoopmanESN/TrajTrans/DataGen/DataGenLib/arclength.m

function [x_L1,f,Param_out] = arclength(fun_name,x0,sign,Mission_Param, Environment_Param)
Param_0 = 0;
Param_1 = 1;
ArcLenParam.atol = 1e-10;
ds_default =1;
gg      = 40000;
epsilon =  sqrt(eps);
try
    val = ArcLenParam.ds;
catch

    ArcLenParam.ds = ds_default;%;
end
try
    val = ArcLenParam.max_iter;
catch
    ArcLenParam.max_iter = 900000000;
end
try
    val = ArcLenParam.max_corr;
catch
    ArcLenParam.max_corr =40;
end
try
    val = ArcLenParam.atol;
catch
    ArcLenParam.atol = 1e-8;
end

%存储文件
% datas = sprintf('.\\data\\data-%s.txt',int2str(200));%创建文件
% fid_sim = fopen(datas,'a+t');%保存文件索引

if(nargout >= 5)
    ido_stab = 1;
else
    ido_stab = 0;
end

N = length(x0);

count_pts  = 0;
Param_old1 = Param_0;
Param_old2 = Param_0;

Options_fsolve  = optimset('TolX',1e-12','TolFun',1e-12,'MaxIter',1000,...
    'MaxFunEvals',10000,'FunValCheck ','on' ,'LargeScale','off',...
    'Algorithm','trust-region-dogleg','Display','iter',...
    'PrecondBandWidth',1,'UseParallel',true);
[x,f] = fsolve(fun_name,x0,Options_fsolve,Param_0,Mission_Param, Environment_Param);
% 利用差分计算雅可比矩阵
Jac = ac_calc_Jac(fun_name,x,Param_0,f,epsilon,Mission_Param, Environment_Param);
x_c(:,1) = x;

lambda = 0;
Param  = (1-lambda)*Param_0 + lambda*Param_1;
if(ido_stab)
    stab_c(1) = ac_calc_stab(Jac);
end

temp_x = [count_pts,ArcLenParam.ds,Param,x(1:7)'];
% fprintf(fid_sim,'%4.11f %4.11f %4.11f %4.11f %4.11f %4.11f %4.11f %4.11f %4.11f %4.11f \n',temp_x);

            figure(2);
            plot(Param,x(N),'ro');
            hold on

Param_out = Param;
x_L1 = x;

count_pts = count_pts + 1;  % increment counter
x1 = x;
x2=x;
lambda_old = lambda;
lambda_old2=lambda;
f_old = f;
f_old2 = f;
c_rand = rand(1,N+1);

icomplete_arclength = 0;
icorr_OK=0;
nCount =1;
nx6 = 0;

for iter = 1:ArcLenParam.max_iter
    %判断迭代是否种植
    if(lambda >= 1)
        icomplete_arclength = 1;
        break;
    end
    norm_x = norm(x);
    if nCount == 1
        ArcLenParam.ds = norm_x/10;%迭代步长
        bound = norm_x/1;
    end
    df_dlambda = ac_calc_df_dlambda(fun_name,x,lambda,Param_0,Param_1,f,epsilon,Mission_Param, Environment_Param);%fun对lambda的微分

    % construct matrix for solving dz/ds
    A = zeros(N+1,N+1);
    A(1:N,1:N) = Jac;
    A(1:N,N+1) = df_dlambda;
    A(N+1,1:N+1) = c_rand;

    b = zeros(N+1,1); 
    b(N+1) = 1;
    v = A\b;
    dz_ds = sign*v / norm(v,2);

    % 修正迭代方向
    dz_old = zeros(N+1,1);
    dz_old(1:N) = x - x2;
    dz_old(N+1) = lambda - lambda_old2;
    if(dot(dz_ds,dz_old) < 0)
        dz_ds = -dz_ds;
    end
    
    if iter == 1 && dz_ds(N+1)<0
        sign = -sign;
    end

    % 显示欧拉得到下一时刻
    x1 = x;  lambda_old = lambda;
    f_old = f;
    x = x + dz_ds(1:N)*ArcLenParam.ds;
    nx6 = 0;
    for kkk = 1:N
        if abs(imag(x(kkk)))>0 || isnan(x(kkk))
            nx6 = 1;
            break;
        end
    end

    if x(7) > gg || nx6 == 1  || x(7) < 0 || (abs(abs(x1(7)) - abs(x(7))) > bound)
        icorr_OK = 0;
    else
        lambda = lambda + dz_ds(N+1)*ArcLenParam.ds;
        Param = (1-lambda)*Param_0 + lambda*Param_1;
        if abs(Param) > 1
            icorr_OK = 0;
        else
            f = feval(fun_name,x,Param,Mission_Param, Environment_Param);
            icorr_OK = 0;

            A(N+1,:) = dz_ds';
            [L,U,P] = lu(A);
            if rank(A(1:N,1:N)) < N
                rank(A(1:N,1:N))
                disp('limit point!')
                pause
            end
            for k_corr = 1:ArcLenParam.max_corr
                if(norm(f,2) <= ArcLenParam.atol )
                    icorr_OK = 1;
                    x_L1 = x;
                    break;
                end
                % 获得delta_z
                nn = [-f;0];
                v = L\(P*nn);
                delta_z= U\v;
                if abs(imag(delta_z(7))) > 0
                    icorr_OK = 0;
                    break;
                end

                x = x + delta_z(1:N);
                nx6 = 0;
                lambda = lambda + delta_z(N+1);
                % 新函数值
                Param = (1-lambda)*Param_0 + lambda*Param_1;
                for kkk=1:N
                    if abs(imag(x(kkk)))>0 || isnan(x(kkk))
                        nx6 = 1;
                        break;
                    end
                end
                xx1=dot([Param_old2,x2(7)],[Param_old1,x1(7)])/(norm([Param_old2,x2(7)])*norm([Param_old1,x1(7)]));
                xx2=dot([Param_old1,x1(7)],[Param,x(7)])/(norm([Param_old1,x1(7)])*norm([Param,x(7)]));
                xx3 = abs(abs(acos(xx1)) - abs(acos(xx2)));
                xx4 = (x1(7) -  x2(7))/(Param_old1 -Param_old2 );
                xx5 = (x(7) -  x1(7))/(Param -Param_old1 );
                xx6 = abs(atan(xx4) - atan(xx5));

                if x(7) > gg || nx6 == 1  || x(7) < 0 || (abs(abs(x1(7)) - abs(x(7))) > bound)  || xx3 > pi/10  || xx6 > pi/10  %|| atan(xx4)*atan(xx5) < 0
                    icorr_OK = 0;
                    break;
                end

                f = feval(fun_name,x,Param,Mission_Param, Environment_Param);
            end
        end
    end
    if(icorr_OK==0)
        if nCount >7
            fclose(fid_sim);
            return;
        else
            lambda=lambda_old;
            x = x1;
            f=f_old;
            Param = (1-lambda)*Param_0 + lambda*Param_1;
            Jac = ac_calc_Jac(fun_name,x,Param,f,epsilon,Mission_Param, Environment_Param);
            ArcLenParam.ds = ArcLenParam.ds/5;
            nCount = nCount+1;
        end
    elseif  icorr_OK==1
        x2 = x1;
        lambda_old2=lambda_old;
        f_old2 = f_old;
        % 有限差分计算雅可比
        Jac = ac_calc_Jac(fun_name,x,Param,f,epsilon,Mission_Param, Environment_Param);
        nCount = 1;

        x_c(:,1) = x;
        lambda_c(:,1) = lambda;
        Param_c(:,1) = Param;
        fnorm_c(:,1) = norm(f,inf);
        Param_old2 = Param_old1;
        Param_old1 = Param;

        temp_x = [count_pts,ArcLenParam.ds,Param,x(1:7)'];
        % fprintf(fid_sim,'%4.11f %4.11f %4.11f %4.11f %4.11f %4.11f %4.11f %4.11f %4.11f %4.11f \n',temp_x);
        sprintf('%4.11f %4.11f %4.11f %4.11f %4.11f %4.11f %4.11f %4.11f %4.11f %4.11f \n',temp_x)
        figure(2);
        plot(Param,x(N),'ro');
        hold on
        Param_out = Param;
        x_L1 = x;
        if Param > 99 || Param< -99
            flag = 666
            fclose(fid_sim);
            return;
        end
        if(ido_stab)
            stab_c(1) = ac_calc_stab(Jac);
        end
        count_pts = count_pts + 1; 
    end
end
fclose(fid_sim);%关闭文件
return;

function Jac = ac_calc_Jac(fun_name,x,theta,f,epsilon,Mission_Param, Environment_Param)
N = length(x);  Jac = zeros(N,N);
x_new = zeros(N,1);
f_new = zeros(N,1);
for k=1:N
    x_new = x;
    x_new(k) = x(k) + epsilon;
    f_new = feval(fun_name,x_new,theta,Mission_Param, Environment_Param);
    Jac(:,k) = (f_new-f)/epsilon;
end
return;


% 计算对lambda的积分
function df_dlambda = ac_calc_df_dlambda(fun_name,x,lambda,Param_0,Param_1,f,epsilon,Mission_Param, Environment_Param)
Param0 = (1-lambda)*Param_0 + lambda*Param_1;
lambda_new = lambda + epsilon;
Param_new = (1-lambda_new)*Param_0 + lambda_new*Param_1;
f_new = feval(fun_name,x,Param_new,Mission_Param, Environment_Param);
df_dlambda = (f_new-f)/epsilon;
return;

%计算是否稳定
function istable = ac_calc_stab(Jac)

% 计算雅可比的特征值
eig_vals_real = real(eig(Jac));

% 是否大于0
iunstable = length(find(eig_vals_real > 0));

if(iunstable)
    istable = 0;
else
    istable = 1;
end

return;