function targets = LEM_radar(radars,objects,time)
% UTM_radar: returns objects detected by radars 
% On input:
%     radars (vector struct): radar position and orientation info
%        radars(k).x (float): x coordinate
%        radars(k).y (float): y coordinate
%        radars(k).z (float): z coordinate
%        radars(k).dx (float): x direction coordinate
%        radars(k).dy (float): y direction coordinate
%        radars(k).dz (float): z direction coordinate
%        radars(k).angle (float): cone angle
%        radars(k).max_range (float): maximum detectable range
%        radars(k).id (int): radar identifier
%        radars(k).pos_noise (3x3 array): variance
%     objects (vector struct): object info
%        (k).x (float): x coord of object
%        (k).y (float): y coord of object
%        (k).z (float): z coord of object
%        (k).dx (float): x direction coordinate
%        (k).dy (float): y direction coordinate
%        (k).dz (float): z direction coordinate
%        (k).speed (float): speed of object
%        (k).diam (float): diameter of object
%     time (float): The current time within the simulation.
% On output:
%     targets (vector struct): target info
%        (k).x (float): x coord of target
%        (k).y (float): y coord of target
%        (k).z (float): z coord of target
%        (k).s (float): speed of target
%        (k).id (int): radar identifier
% Call:
%     targets = UTM_radar(radars, objects, time);
% Author:
%     Vista & Tom
%     UU
%     Fall 2020
%
targets = [];
num_radars = length(radars);
num_objects = length(objects);
num_targets = 0;
mu = [0,0,0];

% Looping through all radars
for r = 1:num_radars
    rx = radars(r).x;
    ry = radars(r).y;
    rz = radars(r).z;
    rdx = radars(r).dx;
    rdy = radars(r).dy;
    rdz = radars(r).dz;
    rid = radars(r).id;
    ra = radars(r).angle;
    rr = radars(r).max_range;
    rpn = radars(r).pos_noise;    
    for k = 1:num_objects
        kx = objects(k).x;
        ky = objects(k).y;
        kz = objects(k).z;
        ks = objects(k).speed;
        ksize = objects(k).diam;
        v = [kx;ky;kz]-[rx;ry;rz];
        dist = norm(v);
        v1 = v/norm(v);
        w = [rdx;rdy;rdz];
        alpha = posori(acos(dot(v1,w)));
        x = mvnrnd(mu,rpn);
        if alpha<=ra&&dist<=rr
            num_targets = num_targets + 1;
            targets(num_targets).x = kx + x(1);
            targets(num_targets).y = ky + x(2);
            targets(num_targets).z = kz + x(3);
            targets(num_targets).s = ks + rand;
            targets(num_targets).diam = ksize + rand;
            targets(num_targets).time = time;
            targets(num_targets).id = rid;
        end
    end
end
