function p_queue = TRI_CY_insert_p_queue(p_queue_in,k,patients,TB, model, combine_time)
% TRI_insert_p_queue - insert one patient in to the schedule queue
% Call:
%      p_queue.first = 0;
%      p_queue.last = 0;
%      p_queue.queue = [];
%      patients = TRI_CY_arrivals(2,24,ones(5,1)/5,1,1,1);
%      TB = [0,3; 3,12; 12,24];
%      k = any number
%      p_queue = TRI_CY_insert_p_queue(p_queue,k,patients,TB);
% 

p_queue = p_queue_in;
first = p_queue.first;
last = p_queue.last;
queue = p_queue.queue;
k_data = patients(k,:);
k_arrive = k_data(1);
k_priority = k_data(2);
k_duration = k_data(3);

% If queue is empty, this patient will be the first one.
if isempty(queue)
    % queue 
    % col 1: the privious patient surgery end time, if this is first
    % patient then before this time is free otherwise, it is busy
    % col 2: the index of the patinet
    % col 3: the privious patient index
    % col 4: the next patient index
    queue(1,1) = k_arrive;
    queue(1,2) = k;
    queue(1,3) = 0;
    queue(1,4) = 0;
    queue(1,5) = 0;
    % first patinet index and the last patient index
    p_queue.first = 1;
    p_queue.last = 1;
    p_queue.queue = queue;
    return
end

% Calculate the last patinet surgery end time
t_next = queue(last,1) + patients(queue(last,2),3);

% If the last patient of the queue surgery end time less than new patient
% arrive time. Then, just add this patinet at the tail of
% the queue.
if t_next <= k_arrive
    queue(end+1,:) = [k_arrive,k,last,0,0];
    queue(last,4) = k;
    p_queue.last = k;
    p_queue.queue = queue;
    return
end

% disp(queue)

% Find insertion place, loop from tail of the queue to head
ptr = last;
done = 0;
while done==0

    % get the index of (ptr) patient
    ptr_index = queue(ptr,2);

    % get the (ptr) patient surgery begin time
    ptr_time = queue(ptr,1);

    ptr_arrive = patients(ptr_index,1);
    ptr_priority = patients(ptr_index,2);
    ptr_duration = patients(ptr_index,3);
    
    % If: TRI_CY_Evaluate_Patient == 1
    % Then: let ptr = ptr.privious, which means insert k patient before
    % (ptr) patient
    % Otherwise, just let k patient after the (ptr) patient
    insert_patient = Patients(k_arrive, 0, 0, k_duration, k_priority);
    scheduled_patient = Patients(ptr_arrive, ptr_time, queue(ptr,5), ptr_duration, ptr_priority);

    if TRI_CY_Evaluate_Patient(insert_patient, scheduled_patient, TB, model, combine_time)
        ptr = queue(ptr,3);
    else
        done = 1;
    end

end

start = ptr;

% get the next index of patient of (ptr) patient
ptr_next = queue(start,4);

insert_start_time = queue(start,1) + patients(start, 3);
% Check whether the patient will be delay although it was insert early
if TRI_CY_Check_Delay(insert_start_time, k_arrive, TB(k_priority, :))
    ptr_next = 0;
end
    

% insert this k patient behind the start == ptr, if start is last one then
% just insert it at last, otherwise insert it between (start) patient and
% (start + 1) patient
if ptr_next == 0
    delay = TRI_CY_Check_Delay(t_next, k_arrive, TB(k_priority, :));
    queue = [queue; t_next, k, last, 0, delay];
    queue(last,4) = k;
else
    delay = TRI_CY_Check_Delay(queue(ptr_next,1), k_arrive, TB(k_priority, :));
    queue = [queue;queue(ptr_next,1),k,start,ptr_next, delay];
    queue(start,4) = k;
    queue(ptr_next,3) = k;

    % Update start times after the inserted patient
    ptr = k;
    done = 0;
    while done == 0

        % get the surgery end time of inserted patient, and set it as start
        % time for next patient, loop to end of the patient queue
        start_time = queue(ptr,1);
        duration = patients(queue(ptr,2),3);
        ptr = queue(ptr,4);
        queue(ptr,1) = start_time + duration;
        queue(ptr,5) = TRI_CY_Check_Delay(queue(ptr,1), patients(ptr,1), TB(patients(ptr,2), :));

        if queue(ptr,4)== 0
            done = 1;
        end

    end
end

% Refind first and last patient of the queue
ptr = first;
while ptr ~= 0
    last = ptr;
    ptr = queue(ptr,4);
end

p_queue.last = last;
p_queue.queue = queue;

return