function [tauStar, cStar, xBarStar, GStar] = tauStarSolve(x0, x1, h)
% TAUSTARSOLVE 求解最优到达时间tauStar、最小代价cStar、xBarStar是时间为tauStar
% 时状态向量xBar的值，GStar是时间为tauStar时状态向量G的值
% 同文件夹下需要有x_bar_dot.m、G_dot.m和rungeKutta4c.m三个函数文件
% x0 = [0 0 0 0 0 0]'; % 初始位置列向量
% x1 = [10 10 10 10 10 10]'; % 末端位置列向量
% h  = 0.01; % 时间步长，四阶龙格库塔法等使用
% 注    意： 输入x0和x1必须是列向量！！！

% 作    者：鲁 鹏(北京理工大学宇航学院)
% 创建日期：2018年7月11日

% 版  本：1.1
% 更新日志：去掉cTau、xBartau、GTau的初始化语句，添加了调用函数时的注意事项
% 修 改 人：鲁 鹏
% 修改日期：2018年7月21日

t0 = 0;
t1 = h; %
cStar = inf; % 初始化最小代价
xBar0 = x0;
G0 = zeros(36,1);
% cTau = zeros(1,1000);
% xBartau = zeros(6, 1000);
% GTau = zeros(36, 1000);
count = 1;

% 当tau >= c_star结束
while t1 < cStar
    % 求无控制力时，系统的状态向量x_bar
    [T1,xBar] = rungeKutta4c(@x_bar_dot, t0, xBar0, h);
    
    % 求加权可控格拉姆矩阵G
    [~,G] = rungeKutta4c(@G_dot, t0, G0, h);
    
    % 求随到达时间tau的变化而变化的最优轨迹代价c_tau
    % n = size(G,1);
    % c_tau = zeros(n,1);
    % for i = 2:n %i从2开始，是因为i=1时，G(0)是零矩阵，没有逆
    %     G_tau = reshape(G(i,:),6,6);
    %     c_tau(i) = T1(i) + (x1'-x_bar(i,:))/G_tau*(x1'-x_bar(i,:))';
    % end
    % plot(t1(1500:end),c_tau(1500:end))
    xBartau(:,count) = xBar(2, :);
    GTau(:,count) = G(2, :);
    cTau(count) = T1(2) + (x1' - xBar(2, :))/reshape(G(2, :), 6, 6)*...
        (x1' - xBar(2, :))';
    
    if cTau(count) < cStar
        cStar = cTau(count);
    end
%     cStar = min(cStar, cTau(count));
    
    t0 = t1;
    t1=t1+h; % 若使用tau=tau+1很可能找不到最小代价c_star，而是求得了c_star近似值
    count = count + 1;
    xBar0 = xBar(2,:);
    G0 = G(2,:);
end
t = find(cTau == cStar);
tauStar = t * h;
xBarStar = xBartau(:, t);
GStar = reshape(GTau(:, t), 6, 6);
end