function queue_out = NE_insert_priority_queue(T,queue)
% NE_insert_priority_queue- insert triangle into priority queue
%     priority is distance of midpoint from PD origin
% On input:
%     T (3x2 array): triangle vertexes
%     queue (queue struct vector): triangle queue
%       (k).corners (3x2 array): vertexes of triangle
% On output:
%     queue_out (queue struct vector): triangle queue with T inserted
%       (k).corners (3x2 array): vertexes of triangle
% Call:
%     q = NE_insert_priority_queue(T,q);
% Author:
%     T. Henderson
%     UU
%     Summer 2023
%

if isempty(queue)
    queue_out.corners = T;
    return
end

len_queue = length(queue);
queue_in = zeros(len_queue,6);
for k = 1:len_queue
    corn = queue(k).corners;
    queue_in(k,:) = corn(:)';
end

queue_out = queue_in;

queue_out(len_queue+1,:) = T(:);

dists = zeros(len_queue+1,1);
for c = 1:len_queue
    Tc = reshape(queue_in(c,:),3,2);
    dists(c) = norm(mean(Tc));
end
dists(end) = norm(mean(T));
[vals,indexes] = sort(dists);
queue_out = queue_out(indexes,:);

for k = 1:length(indexes)
    qo(k).corners = reshape(queue_out(k,:),3,2);
end
queue_out = qo;
