classdef rrtstar
    % RRTSTAR Summary of this class goes here
    % Detailed explanation goes here

properties %(Access = private)
    A_ = [];
    B_ = [];
    c_ = [];
    R_ = [];
    h_ = [];
    facet_ = [];
    index_ = [];
    vertex_ = [];
%     best_arrival = [];
%     t = sym('t', 'positive'); %符号变量't'为正
%     x = sym('x', 'positive'); %used for integration
%     t_s = sym('t_s', 'real'); %optimal arrival time
%     min_dist = sym('d', 'real');
% 
%     x0 = [];
%     x1 = [];
% 
%     states_eq = [];
%     control_eq = [];
%     cost_eq = [];
%     distance_eq = [];

    timings = [];

%     eval_arrival_internal; % 以x0、x1为参数的函数句柄（多项式）
%     eval_cost_internal;    % 以t_s、x0、x1为参数的代价函数句柄
%     eval_states_internal;  % 以t、t_s、x0、x1为参数的状态函数句柄
%     eval_control_internal; % 以t、t_s、x0、x1为参数的控制量函数句柄
%     eval_time_for_distance_internal; % 以d、t_s、x0、x1为参数

    %termination conditions (per segment)
    max_it = 150;  %设置迭代次数
    max_time = 0;  % ???

    max_radius = 1000;  % 寻找临近状态节点(node)用的代价半径r
    tauStarSolve;
    optTraj;

end
    
methods
function obj = rrtstar(A, B, c, R, h)

    if rank(ctrb(A,B)) ~= size(A,1)  %用ctrb()判断系统的可控性
        disp('System is not controllable - aborting');
        return;
    end

    obj.A_ = A;
    obj.B_ = B;
    obj.c_ = c;
    obj.R_ = R;
    obj.h_ = h;
    obj.tauStarSolve = @tauStarSolve;
    obj.optTraj = @optTraj;
    load('433Eros.mat','facet','index','vertex')
    obj.index_ = index;
    obj.facet_ = facet;
    obj.vertex_ = vertex;
%     state_dims = size(A,1);
%     % 生成符号变量组成的state_dims*1向量
%     obj.x0 = sym('x0',[state_dims,1]); 
%     assume(obj.x0, 'real'); % obj.x0的元素都是实数
%     obj.x1 = sym('x1',[state_dims,1]);
%     assume(obj.x1, 'real');
%     % 以下判断中'var'表示在工作区变量中查找dist_idxs是否存在
%     if ~exist('dist_idxs','var') || isempty(dist_idxs) 
%         dist_idxs = 1:size(B,1); % 生成距离索引
%     end
% 
%     [obj.best_arrival, obj.states_eq, obj.control_eq,...
%         obj.cost_eq, obj.distance_eq] = obj.calc_equations(obj.A_,...
%         obj.B_,obj.R_,obj.c_,dist_idxs);


    %%calculate the factors for the resulting polynomial explicitly
%     p1 = [];
%     it = 0;
%     %just so mupad knows it's a polynomial
%     while it < 20 && length(p1) <= 1 
%         % feval(symengine,F,x1,...,xn)将x1,...,xn作为参数传递给F(MuPAD函数名)，
%         % coeff(p,x,All)求符号变量表示的多项式的关于变量x的所有系数(包括0系数)
%         % simplifyFraction(expr)简化符号有理式
%         % obj.best_arrival是代价函数c(τ)对τ的导数
%         p1 = feval(symengine,'coeff',...
%             simplifyFraction(obj.best_arrival*obj.t^it),...
%             obj.t, 'All');
%         it = it+1;
%     end
%     if it > 20
%         disp('either the result is not a polynomial or the degree is too high');
%     end
%     % B = fliplr(A) 返回 A，围绕垂直轴按左右方向翻转其各列。
%     p([obj.x0', obj.x1']) = fliplr(p1);
%     % matlabFunction(p, 'file', 'arrival_time.m'); %to write to file
%     obj.eval_arrival_internal = matlabFunction(p);
%     obj.eval_cost_internal = matlabFunction(obj.cost_eq);
%     obj.eval_states_internal = matlabFunction(obj.states_eq);
%     obj.eval_control_internal = matlabFunction(obj.control_eq);

    %%calculate the factors for the resulting polynomial explicitly
%     p1 = [];
%     it = 0;
%     %just so mupad knows it's a polynomial
%     while it < 20 && length(p1) <= 1
%         p1 = feval(symengine,'coeff',simplifyFraction(...
%             obj.distance_eq*obj.t^it), obj.t, 'All');
%         it = it+1;
%     end
%     if it > 20
%         disp('either the result is not a polynomial or the degree is too high');
%     end
% 
%     p([obj.min_dist, obj.t_s, obj.x0', obj.x1']) = fliplr(p1);
%     obj.eval_time_for_distance_internal = matlabFunction(p);

end

function [time] = evaluate_arrival_time(obj,x0_, x1_, h_)
    %arr = subs(obj.best_arrival, obj.x0, x0_);
    %arr = subs(arr, obj.x1, x1_);
    %time_a = min(eval(solve(arr, 'Real', true))); %symbolical version

%     in = num2cell([x0_', x1_']);
%     time = roots(obj.eval_arrival_internal(in{:}));
%     time = time(imag(time)==0);
%     time = min(time(time>=0));
    [time, ~] = obj.tauStarSolve(x0_, x1_, h_);
end

function [cost, time] = evaluate_cost(obj, x0_, x1_, h_)

%     if ~exist('time','var') || isempty(time)
%         time = obj.evaluate_arrival_time(x0_, x1_);
%     end
    %c = subs(obj.cost_eq, obj.x0, x0_);
    %c = subs(c, obj.x1, x1_);
    %c = subs(c, obj.t_s, time);
    %cost = eval(c);

%     in = num2cell([time, x0_', x1_']);
%     cost = obj.eval_cost_internal(in{:});
    [time, cost, ~, ~] = obj.tauStarSolve(x0_, x1_, h_);
    
end

function [states, inputs] = evaluate_states_and_inputs(obj,x0_,x1_,h_,B_,R_)

%     if ~exist('time','var') || isempty(time)
%         time = obj.evaluate_arrival_time(x0_, x1_);
%     end

    %s = subs(obj.states_eq, obj.x0, x0_);
    %s = subs(s, obj.x1, x1_);
    %states = subs(s, obj.t_s, time);

    %c = subs(obj.control_eq, obj.x0, x0_);
    %c = subs(c, obj.x1, x1_);
    %inputs = subs(c, obj.t_s, time);

%     in = num2cell([time, x0_', x1_']);
%     states = @(t)obj.eval_states_internal(t, in{:});  %以t为参数的状态函数句柄
%     inputs = @(t)obj.eval_control_internal(t, in{:}); %以t为参数的输入函数句柄
    [tauStar, ~, input, state] = obj.optTraj(x0_,x1_,h_,B_,R_);
%     states = state';   % 使输出的states每一列是一个时刻是状态，不同列代表不同时刻
%     inputs = input';
    n = floor(size(input,1)/10);
    partI = zeros(n+1, 3);
    partS = zeros(n+1, 6);
    partI(1,:) = input(1, :);
    partS(1,:) = state(1, :);
    for i = 2:n
        partI(i,:) = input((i-1)*10, :);
        partS(i,:) = state((i-1)*10, :);
    end
    partI(n+1,:) = input(end, :);
    partS(n+1,:) = state(end, :);
    for i = 1:size(partI,1)
        X = state(i,1:3)';
        g = Poly_gravity(X,obj.vertex_,obj.facet_,obj.index_);
        partI(i,:) = partI(i,:) - g';
    end
    l = linspace(0,tauStar,n+1);
    inputs = @(t) interp1(l,partI,t);
    states = @(t) interp1(l,partS,t);

end

function [close_time] = get_time_for_distance_equal(obj, x0_, x1_,...
        time, distance) 

    if isempty(obj.distance_eq)
       close_time = obj.evaluate_arrival_time(x0_, x1_, obj.h_);
       return;
    end

    if ~exist('time','var') || isempty(time)    
        time = obj.evaluate_arrival_time(x0_, x1_, obj.h_);
    end

    in = num2cell([distance, time, x0_', x1_']);
    close_time = roots(obj.eval_time_for_distance_internal(in{:}));
    close_time = close_time(imag(close_time)==0);
    close_time = min(close_time(close_time>=0));

end

function [obj] = set_termination_conditions(obj, iterations, time)
    obj.max_it = iterations;
    obj.max_time = time;
end

function [path_states, closest_end_state] = run(obj, sample_free_state,...
        is_state_free, is_input_free, start, goal, display, max_distance)
    T = [start];  % 放入起始点
    costs = [0];  % 初始化代价为0，存放每个节点(node)的代价
    times = [0];  % 初始化时间为0,
    parents = [-1];
    children = {[]};
    is_terminal = [false];
    cost_to_goal = [inf];  % 
    time_to_goal = 0;      % 

    goal_cost = inf;  % 从起始点到达目标点的代价，初始化为inf
    goal_parent = 0;  % 
    goal_time = inf;  % 从起始点到达目标点的时间，初始化为inf

    it_limit = obj.max_it;  % 迭代次数

    [cost, time] = evaluate_cost(obj,start, goal, obj.h_); % 求s->g最小代价和最优时间
    % 求最优轨迹对应的状态states(t)和u(t)
    [states, u] = evaluate_states_and_inputs(obj,start,goal,obj.h_,obj.B_,obj.R_);
    if is_state_free(states,[0,time]) && is_input_free(u,[0,time])
        disp('goal is reachable from the start node (optimal solution)');
        it_limit = 0;
        goal_parent = 1;
        display(-1, obj, start, -1, goal, cost, 1);
    end

    display_scratch = -1; % ???
    it = 0;
    while it < it_limit && goal_time > obj.max_time
        it = it + 1;
        disp(['iteration ', num2str(it), ' of ', num2str(it_limit),...
            '(time:',num2str(goal_time),')']);
        sample_ok = false;
        tic;  % 开始计时
        while ~sample_ok

            x_i = sample_free_state(); % 采样
            min_idx = 1;     % 
            min_cost = inf;  % 初始化起点到终点的最小代价
            min_time = inf;  % 初始化起点到终点的最短时间

            queue = 1;
            while ~isempty(queue)
                node_idx = queue(1);
                %switch with stack to perform DFS instead of BFS
                queue = queue(2:end);
                node = T(:,node_idx);
                if is_terminal(node_idx)
                   continue;
                end

                [cost, time] = evaluate_cost(obj, node, x_i, obj.h_);
                if cost < obj.max_radius && costs(node_idx)+cost <...
                        min_cost && costs(node_idx)+cost < goal_cost
                    [states, u] = evaluate_states_and_inputs(obj,node,x_i,obj.h_,obj.B_,obj.R_);
                    if  is_state_free(states,[0,time]) && is_input_free(u,[0,time])
                        sample_ok = true;
                        min_idx = node_idx;
                        min_cost = costs(node_idx)+cost;
                        min_time = times(node_idx)+time;
                        continue;
                    end
                end
                %if we arrive here it means there is no trajectory from node to new x_i
                %however child nodes might be able to form a connection
                queue = [queue, children{node_idx}];
            end
        end

        parents = [parents, min_idx];
        children{min_idx} = [children{min_idx},it+1];
        children{it+1} = [];
        costs = [costs, min_cost];
        times = [times, min_time];
        T = [T,x_i];
        is_terminal = [is_terminal, false];
        cost_to_goal = [cost_to_goal, inf];
        time_to_goal = [time_to_goal, inf];

        %update the tree with new shorter paths (the root node is checked
        %here even though it's not necessary)
        stack(1).index = 1;
        stack(1).improvement = 0;
        stack(1).time_improvement = 0;

        while ~isempty(stack)
            node = stack(end);
            stack = stack(1:end-1);
            state = T(:,node.index);

            costs(node.index) = costs(node.index)-node.improvement;
            times(node.index) = times(node.index)-node.time_improvement;
            if is_terminal(node.index)
                if costs(node.index)+cost_to_goal(node.index) < goal_cost
                    goal_cost = costs(node.index)+cost_to_goal(node.index);
                    goal_time = times(node.index)+time_to_goal(node.index);
                    goal_parent = node.index;
                end
                continue;
            end
            diff = node.improvement;
            time_diff = node.time_improvement;

            [cost, time] = evaluate_cost(obj, x_i,state,obj.h_);
            if cost < obj.max_radius && costs(end)+cost < costs(node.index)
                [states, u] = evaluate_states_and_inputs(obj,x_i,state,obj.h_,obj.B_,obj.R_);
                if is_state_free(states,[0,time]) && is_input_free(u,[0,time])

                    old_cost = costs(node.index);
                    old_time = times(node.index);
                    old_parent = parents(node.index);
                    costs(node.index) = costs(end)+cost;
                    parents(node.index) = size(parents,2);
                    ch = children{old_parent};
                    children{old_parent} = ch(ch~=node.index);

                    diff = old_cost-costs(node_idx);
                    time_diff = old_time-times(node_idx);
                end
            end

            for jj=1:length(children{node.index})
               stack(end+1).index = children{node.index}(jj);
               stack(end).improvement = diff;
               stack(end).time_improvement = time_diff;
            end

        end

        [cost, time] = evaluate_cost(obj, x_i, goal, obj.h_);
        if costs(end)+cost < goal_cost
            [states, u] = evaluate_states_and_inputs(obj, x_i,goal,obj.h_,obj.B_,obj.R_);
            if is_state_free(states,[0,time]) && is_input_free(u,[0,time])
                goal_cost = costs(end)+cost;
                goal_time = times(end)+time;
                goal_parent = it+1;
                is_terminal(end) = true;
                cost_to_goal(end) = cost;
            end
        end

        obj.timings = [obj.timings, toc()];
        display_scratch = display(display_scratch, obj, T, parents,...
            goal, goal_cost, goal_parent);

        if mod(length(obj.timings),50) == 1 && length(obj.timings) > 50
           disp(['time for the last 50 iterations: ',...
               num2str(sum(obj.timings(end-50:end))),' seconds']); 
        end

    end

    next = goal_parent;
    path_states = goal;
    while next ~= -1
        path_states = [T(:,next) ,path_states];
        next = parents(next);
    end

    if ~exist('max_distance','var') || isempty(max_distance)
        closest_end_state = goal;
    else
        src = T(:,goal_parent);
        best_t = obj.evaluate_arrival_time(src, goal, obj.h_);
        close_t = get_time_for_distance_equal(obj, src, goal, best_t,...
            max_distance);
        [s, ~] = evaluate_states_and_inputs(obj, src, goal, obj.h_,obj.B_,obj.R_);
        closest_end_state = s(close_t);
    end
end

function [path, time] = find_path(obj, sample_free_state, is_state_free,...
        is_input_free, waypoints, display, max_distance)
    time = 0;
    path = [waypoints(:,1)];
    for ii=2:size(waypoints,2)
        figure('name', [num2str(ii-1),' to ', num2str(ii)]);
        [path_states, closest_end_state] = obj.run(sample_free_state,...
            is_state_free, is_input_free, path(:,end), waypoints(:,ii),...
            display, max_distance);
        path = [path, path_states(:,2:end)];
        if ii ~= size(waypoints,2)
            path(:,end) = closest_end_state;
        end
    end

    for ii=2:size(path,2)
        time = time + obj.evaluate_arrival_time(path(:,ii-1),path(:,ii),obj.h_);
    end
end

end

% methods (Access = protected)
% 
% function [tau_star, states, control, cost, sq_distance] = ...
%         calc_equations(obj, A, B, R, c, dist_idxs)
%     % the solution of the equation 'tau_star = 0' gives the best 
%     % arrival time. states, control, and cost depend on t_s which
%     % is the solution from above
%     state_dims = size(A,1);
% 
%     G = int(expm(A*(obj.t-obj.x))*B/R*B'*expm(A'*(obj.t-obj.x)),...
%         obj.x, 0, obj.t);
%     x_bar = expm(A*obj.t)*obj.x0+int(expm(A*(obj.t-obj.x))*c,...
%         obj.x, 0, obj.t);
% 
%     d = G\(obj.x1-x_bar);
%     tau_star = 1-2*(A*obj.x1+c)'*d-d'*B/R*B'*d; % 代价函数对τ的导数
% 
%     solution = expm([A, B/R*B';zeros(state_dims), -A']*(obj.t-obj.t_s))...
%         *[obj.x1;subs(d,obj.t,obj.t_s)]+ ...
%         int(expm([A, B/R*B';zeros(state_dims), -A']*(obj.t-obj.x))...
%         *[c; zeros(state_dims,1)],obj.x,obj.t_s,obj.t);
% 
%     control = R\B'*solution(state_dims+1:2*state_dims,:);
%     states = solution(1:state_dims);
% 
%     if exist('dist_idxs','var')
%         sq_distance = sum((obj.x1(dist_idxs)-states(dist_idxs)).^2)...
%             -obj.min_dist^2;
%     end
% 
%     cost = int(1+control'*R*control, obj.t, 0, obj.t_s);
% 
% end
% end
end