% THis function will interpolate between any two points on a given discretized trajectory
% Inputs
%   current_t - the current time step, or dependent variable of the trajectory
%   des_sim_traj - the entire vector/array of the wanted output trajectory
%   des_sim_time_traj - the corresponding dependent vector for des_sim_traj
% Outputs
%   interp_ref - the interpolated value between the points bounding current_t

% Created by Brevin Banks
% Modified 4/20/2023
function interp_ref = traj_interp(current_t,des_sim_traj,des_sim_time_traj)
 % find the closest point to time in the chirp function
    [ d, ix ] = min(abs(des_sim_time_traj-current_t));
    if ix <2
        ix =2; % can't interpolate starter position without more than 2 samples
    elseif ix>length(des_sim_traj)-1
        ix = length(des_sim_traj)-1; % can't interpolate end position without if more than 10000 samples
    end
    
    before_ref = des_sim_traj(ix-1);
    after_ref = des_sim_traj(ix+1);
    before_t = des_sim_time_traj(ix-1);
    after_t = des_sim_time_traj(ix+1);

    interp_ref = ((after_ref - before_ref)/(before_t-after_t))*current_t*(current_t-before_t)+before_ref;
end