function [b,min_d,t_cpa] = LEM_CPA(p11,p12,s1,t11,p21,p22,s2,t21,h_x)
% LEM_CPA - closest point of approach
% On input:
%     p11 (1x3 vector): start endpoint for first object
%     p12 (1x3 vector): end endpoint for first object
%     s1 (float): speed of object 1
%     t11 (loat): start time of object 1
%     p21 (1x3 vector): start endpoint for second moving object
%     p22 (1x3 vector): end endpoint for second moving object
%     s2 (float): speed of object 2
%     t21 (float): start time of object 2
%     hd (float): headway distance
% On output:
%     b (Boolean): 0 not closer; 1 closer than headway
%     min_d (oat): minimum distance during traversal
%     t_cpa (float): time of minimum distance
% Call:
%     [b,min_d,t_cpa] = LEM_CPA(p11,p12,s1,t11,p21,p22,s2,t21,hd);
% Author:
%     T. Henderson
%     UU
%     Fall 2020
%

b = 0;
min_d = -1;
t_cpa = -1;

d1 = norm(p12-p11);
d2 = norm(p22-p21);
dir1 = p12 - p11;
dir1 = dir1/norm(dir1);
u = dir1*s1;
dir2 = p22 - p21;
dir2 = dir2/norm(dir2);
v = dir2*s2;
t12 = t11 + d1/s1;
t22 = t21 + d2/s2;

d = LEM_dist_line_line([p11;p12],[p21;p22]);

if (t12<t21)|(t22<t11)|d>h_x
    return
end

if t11<=t21
    p0 = p11 + (t21-t11)*u;
    q0 = p21;
    len1 = norm(p12-p0);
    len2 = d2;
    t0 = t21;
else
    p0 = p11;
    q0 = p21 + (t11-t21)*v;
    len1 = d1;
    len2 = norm(p22-q0);
    t0 = t11;
end

t = -dot(p0-q0,u-v)/norm(u-v)^2;
t_cpa = t + t0;
p_t = p0 + t*u;
q_t = q0 + t*v;
min_d = norm(p_t-q_t);
if (t_cpa>=t11)&(t_cpa<=t12)&(t_cpa>=t21)&(t_cpa<=t22)&(min_d<h_x)
    b = 1;
end
