%% L19_Project2Starter.m
% ECE 495 -- Electromagnetic Warfare and Survivability
% Block 2, Project 2 -- B-21 Standoff Emitter Geolocation
%
% This starter gives you a WORKING pipeline for ONE emitter from a
% THREE-CUT collection: read the scenario, fly a sample leg, collect RWR
% bearings, fix the emitter by least squares (the helper at the bottom of
% this file, lsq_fix), and draw its 1-sigma location-error ellipse with the
% resulting CEP. Everything past that point is yours: fly more cuts and a
% better geometry, geolocate every emitter, and DESIGN the collection that
% meets the CEP spec while staying outside every keep-out.

clear; clc; close all;
rng(495);   % reproducible draws for checking your pipeline against this starter; comment out for fresh noise

%% Step 1 -- Scenario and spec
S          = readtable('IADS_Scenario.csv');
sigma_deg  = 0.5;        % RWR bearing 1-sigma [deg] (matches the test sim)
CEP_spec   = 0.5;        % required location CEP [km] (half the 1 km seeker basket)
target_id  = 'E3';       % the emitter we geolocate in this example (TTR)

%% Step 2 -- A SAMPLE collection leg (redesign this!)
% A LEGAL but MEDIOCRE leg: a straight E-W pass 80 km south of the cluster
% with only a 30 km baseline -- long standoff, short baseline -- and three
% cuts along it. It clears every keep-out circle, and it will NOT meet the
% CEP spec. Designing a collection that does is your job.
acN = [-80, -80, -80];   % km North
acE = [-15,   0,  15];   % km East

%% Step 3 -- Collect RWR bearings along the leg
meas = rwr_collection_sim(acN, acE);

rows = strcmp(meas.emitter_id, target_id);   % emitter_id is a cell array of char: == 'E3' errors; use strcmp or a string "E3"
az   = meas.az_deg(rows);                 % bearings to the target, in order of wp
P    = [acN(:), acE(:)];                  % aircraft positions [N E]

%% Step 4 -- Fix the emitter: least squares over all the cuts (L14 geometry)
% Each bearing is a line of position; lsq_fix stacks them as A*p = b and
% solves in the least-squares sense. With exactly two cuts this IS L14's
% cross-fix (two lines, one crossing); with more it is the best-fit crossing.
phat = lsq_fix(P, az);                    % estimated emitter position [N;E]

%% Step 5 -- Location-error ellipse from geometry + bearing noise
sig = deg2rad(sigma_deg);                 % J needs sigma in RADIANS: degrees here makes the ellipse ~57x too small
J   = zeros(2);
for k = 1:size(P,1)
    dN = phat(1)-P(k,1); dE = phat(2)-P(k,2); R2 = dN^2 + dE^2;
    H  = [-dE/R2, dN/R2];                  % d(az)/d[N,E]
    J  = J + (H.'*H)/sig^2;
end
C = inv(J);                               % 2x2 covariance [km^2]
[V,D] = eig(C); s = sqrt(diag(D));        % 1-sigma semi-axes [km]
CEP = 0.59*(s(1)+s(2));                    % approx CEP [km]

%% Step 6 -- Report
fprintf('Target %s  prior [N=%.1f E=%.1f] km\n', target_id, ...
        S.prior_N_km(strcmp(S.emitter_id,target_id)), ...
        S.prior_E_km(strcmp(S.emitter_id,target_id)));
fprintf('Three-cut LS fix  [N=%.2f E=%.2f] km\n', phat(1), phat(2));
if CEP <= CEP_spec, verdict = 'PASS'; else, verdict = 'needs work'; end
fprintf('1-sigma ellipse  a=%.0f m  b=%.0f m   CEP=%.0f m   spec=%.0f m  -> %s\n', ...
        max(s)*1e3, min(s)*1e3, CEP*1e3, CEP_spec*1e3, verdict);
fprintf('\nNow improve it: more cuts along the leg, a longer baseline, or a\n');
fprintf('heading change all shrink the ellipse. How close can you fly and stay safe?\n');

%% Step 7 -- Plot the picture
figure; hold on; grid on; axis equal;
th = linspace(0,2*pi,100);
ell = V*[s(1)*cos(th); s(2)*sin(th)];   % pair each semi-axis with ITS eigenvector (eig returns ascending)
% Only the chosen target is drawn: its prior, its keep-out circle, the leg,
% the fix and the ellipse. Your full picture (Task 5) shows all three.
i = strcmp(S.emitter_id, target_id);
plot(S.prior_E_km(i), S.prior_N_km(i), 'ks', 'MarkerFaceColor','y');   % prior
plot(S.prior_E_km(i)+S.keepout_km(i)*cos(th), ...
     S.prior_N_km(i)+S.keepout_km(i)*sin(th), 'r:');                   % keep-out
plot(acE, acN, 'b-o','LineWidth',1.4);                             % flight leg
for k = 1:size(P,1)                                                % the three bearing lines
    L  = 1.15*hypot(phat(1)-P(k,1), phat(2)-P(k,2));               % run each a little past the fix
    vis = 'off'; if k==1, vis = 'on'; end                          % one legend entry for all cuts
    plot(P(k,2)+[0 L*sind(az(k))], P(k,1)+[0 L*cosd(az(k))], ...
         '-', 'Color',[0.45 0.45 0.45], 'LineWidth',0.9, 'HandleVisibility',vis);
end
plot(phat(2), phat(1), 'r+','MarkerSize',12,'LineWidth',1.6);      % fix
plot(phat(2)+ell(2,:), phat(1)+ell(1,:), 'r-','LineWidth',1.2);    % ellipse
xlabel('East [km]'); ylabel('North [km]');
legend([target_id ' prior'],'keep-out','flight leg','bearing cuts','fix','1\sigma ellipse', ...
       'Location','best');
title(['Project 2 starter: three-cut standoff geolocation of ' target_id]);

%% Local helper -- the handout's linear least-squares fix (A\b) for N cuts
function phat = lsq_fix(P, az_deg)
%LSQ_FIX  Emitter fix from N bearings by linear least squares.
%   P      : N-by-2 aircraft positions [N E] (km), one row per cut
%   az_deg : N-by-1 bearings (deg, North toward East) to ONE emitter
%   phat   : 2-by-1 fix [N; E] (km)
%   Each bearing pins the emitter to its line of position,
%       sin(az)*N_e - cos(az)*E_e = sin(az)*N_k - cos(az)*E_k,
%   so stack the N lines as A*p = b and solve in the least-squares sense.
%   Good for a well-spread collection; on a short leg at long standoff the
%   linear stack is biased toward the aircraft (handout) -- refine with
%   Gauss-Newton on the atan2 model if your look angles are narrow.
    az   = az_deg(:);
    A    = [sind(az), -cosd(az)];
    b    = sind(az).*P(:,1) - cosd(az).*P(:,2);
    phat = A \ b;
end