function [ scratch ] = plot_field(scratch, obj, tree, parents, goal_state, goal_cost, goal_parent)
% PLOT_FIELD Summary of this function goes here
%   Detailed explanation goes here

clf; % clf清除当前窗口
hold on;
if true
    for ii=2:size(tree,2) %root node has no parents -> start with 2
        src = tree(:,parents(ii));
        dst = tree(:,ii);
        draw_trajectory(obj, src, dst, 'magenta', 0.5);
    end
end

%% 绘制433Eros
% h = trisurf(obj.facet_, obj.vertex_(:,1), obj.vertex_(:,2), obj.vertex_(:,3));  % 绘制三角形面组成的多面体
% h.EdgeColor = 'none';  %去掉棱的颜色
% h.FaceColor = [0.8,0.8,0.8];
% h.FaceLighting = 'gouraud';  %使表面光滑
% light; %添加光照效果
% axis equal

%% 绘制障碍物
grid on
view(-11,30)
% obstacles = [-21000,-1000,11000;    %Free Space的边界点坐标（每行为一个点的坐标）
%              -21000,-1000, 5189;
%              -7014, -1000, 5189;
%              -7014, -1000,11000;
%              -21000, 1000,11000;
%              -21000, 1000, 5189;
%              -7014,  1000, 5189;
%              -7014,  1000,11000];

X1 = [-21000,-21000,-7014,-7014];
Y1 = [-1000,-1000,-1000,-1000];
Z1 = [11000,5189,5189,11000];
p1 = patch(X1,Y1,Z1,[0.5 0.5 0.5]);
p1.FaceAlpha = 0.1;
p1.EdgeColor = [0.5 0.5 0.5];

X2 = [-21000,-21000,-7014,-7014];
Y2 = [  1000,  1000, 1000, 1000];
Z2 = [11000,5189,5189,11000];
p2 = patch(X2,Y2,Z2,[0.5 0.5 0.5]);
p2.FaceAlpha = 0.1;
p2.EdgeColor = [0.5 0.5 0.5];

X3 = [-21000 -21000 -21000 -21000];
Y3 = [-1000 -1000 1000 1000];
Z3 = [11000 5189 5189 11000];
p3 = patch(X3,Y3,Z3,[0.5 0.5 0.5]);
p3.FaceAlpha = 0.1;
p3.EdgeColor = [0.5 0.5 0.5];

X4 = [-7014 -7014 -7014 -7014];
Y4 = [-1000 -1000 1000 1000];
Z4 = [11000 5189 5189 11000];
p4 = patch(X4,Y4,Z4,[0.5 0.5 0.5]);
p4.FaceAlpha = 0.1;
p4.EdgeColor = [0.5 0.5 0.5];

X5 = [-21000 -7014 -7014 -21000];
Y5 = [-1000 -1000 1000 1000];
Z5 = [5189 5189 5189 5189];
p5 = patch(X5,Y5,Z5,[0.5 0.5 0.5]);
p5.FaceAlpha = 0.1;
p5.EdgeColor = [0.5 0.5 0.5];

X6 = [-18000,-18000,-18000,-18000];
Y6 = [-1000,-1000,1000,1000];
Z6 = [11000,8500,8500,11000];
p6 = patch(X6,Y6,Z6,[0.5 0.5 0.5]);
p6.FaceAlpha = 0.1;
p6.EdgeColor = [0.5 0.5 0.5];

X7 = [-18000,-16000,-16000,-18000];
Y7 = [-1000,-1000,1000,1000];
Z7 = [8500,8500,8500,8500];
p7 = patch(X7,Y7,Z7,[0.5 0.5 0.5]);
p7.FaceAlpha = 0.1;
p7.EdgeColor = [0.5 0.5 0.5];

X8 = [-16000,-16000,-16000,-16000];
Y8 = [-1000,-1000,1000,1000];
Z8 = [11000,8500,8500,11000];
p8 = patch(X8,Y8,Z8,[0.5 0.5 0.5]);
p8.FaceAlpha = 0.1;
p8.EdgeColor = [0.5 0.5 0.5];

% X9 = [];
% Y9 = [];
% Z9 = [];
% p9 = patch(X9,Y9,Z9,[0.5 0.5 0.5]);
% p9.FaceAlpha = 0.1;
% p9.EdgeColor = [0.5 0.5 0.5];

xlabel('x')
ylabel('y')
zlabel('z')
axis equal

%%
if goal_cost < inf
    p = goal_parent;
    h = draw_trajectory(obj, tree(:,p), goal_state, 'black', 2);
    uistack(h, 'top')
    c = p;
    p = parents(c);
    while p > 0
        h = draw_trajectory(obj, tree(:,p), tree(:,c), 'black', 2);
        uistack(h, 'top')
        c = p;
        p = parents(c);
    end
end
hold off;
title(['cost: ', num2str(goal_cost)]);
drawnow

end

function [h] = draw_trajectory(obj,x0,x1,color, thickness)

    t = obj.evaluate_arrival_time(x0,x1,obj.h_); % 使用obj.就不要添加参数obj
    [states, ~] = obj.evaluate_states_and_inputs(x0,x1,obj.h_,obj.B_,obj.R_);
    X = [];
    Y = [];
    Z = [];
    for jj=[0:obj.h_*5:t,t]
        p = states(jj);
        X = [X,p(1)];
        Y = [Y,p(2)];
        Z = [Z,p(3)];
    end
    h = line(X, Y, Z, 'Color', color, 'LineWidth', thickness);

end