% analysisAndVisualization.m
%
% Perform analysis of TDOA data associated with an interference event on
% February 11, 2026.  Display various visualizations.  See readme.txt for
% details on the data stored in the struct iedata.

clear; clc;
%% Load data
load interferenceEventData;

%% Setup
dtAnimation = 0.01;
load coastlines;
r1 = iedata.NLR_ECEF;
r2 = iedata.TRO_ECEF;
cLight = physconst('LightSpeed');
meshDensity = 50;
axisLim = 5e7;
faceAlpha = 0.7;
rgbMat = orderedcolors("glow");
faceColor = rgbMat(5,:);
satelliteMarkerSize = 4;
viewElevation = 25;
% The TDOA measurement is defined as TGT1-TGT2, i.e., station 1's TOA minus
% station 2's TOA.
ctdoa1 = -iedata.tdoaVec(1)*cLight;
% Implicit function modeling the TDOA measurement
fTdoa = @(x,y,z) sqrt((x - r1(1)).^2 + (y - r1(2)).^2 + (z - r1(3)).^2) - ...
     sqrt((x - r2(1)).^2 + (y - r2(2)).^2 + (z - r2(3)).^2) - ctdoa1;

%% Preprocessing
svEcefMat = iedata.svPosCell{1};
svEcefMaskMat = iedata.svPosElMaskCell{1};
Nd = length(svEcefMaskMat);
residualVec = zeros(Nd,1);
for ii=1:Nd
  p = svEcefMaskMat(ii,:)';
  residualVec(ii) = fTdoa(p(1),p(2),p(3));
end

%% Plot Earth with two TDOA stations
f1 = figure(1);clf;
set(f1, 'Color', 'k');
E = wgs84Ellipsoid;
[SX,SY,SZ] = ellipsoid(0,0,0,E.SemimajorAxis, E.SemimajorAxis, E.SemiminorAxis,100);
hSurf = surf(SX,SY,SZ);
set(hSurf, 'FaceColor', rgbMat(1,:), 'EdgeColor', 'none');
camlight;
material dull;
lighting gouraud;
axis off;
hold on;
[CX,CY,CZ] = geodetic2ecef(E, coastlat, coastlon, 0);
plot3(CX, CY, CZ, 'Color', 'white', 'LineWidth', 1);
stationMat = 1.01*[iedata.NLR_ECEF'; iedata.TRO_ECEF'];
hStat = scatter3(stationMat(:,1), stationMat(:,2), stationMat(:,3), 50, 'filled');
hStat.MarkerFaceColor = rgbMat(2,:);
hStat.MarkerEdgeColor = rgbMat(2,:);
axis equal vis3d;
azAngleVec = [80:0.2:110];
sgtitle('Receiving stations', 'color', 'w');
for az = azAngleVec
  view(az, viewElevation);
  drawnow;     
  pause(dtAnimation); 
end

%% Plot hyperboloid
sgtitle('Surface defining solutions that exactly fit first TDOA measurement', 'color', 'w');
hHyp = fimplicit3(fTdoa,1.4e7*[-1 1 -1 1 0 1], 'MeshDensity', meshDensity, ...
                  'EdgeColor', 'none', 'FaceAlpha', faceAlpha, 'FaceColor', ...
                  faceColor);
for az = azAngleVec
  view(az, viewElevation);
  drawnow;     
  pause(dtAnimation); 
end
delete(hHyp);

%% Plot satellites with public ephemerides
sgtitle('All satellites with public ephemerides', 'color', 'w');
hScat = scatter3(svEcefMat(:,1), svEcefMat(:,2), svEcefMat(:,3),...
                 satelliteMarkerSize, 'filled');
hScat.MarkerFaceColor = 'w';
hScat.MarkerEdgeColor = 'w';
camproj('perspective');
xlim(axisLim*[-1,1]);
ylim(axisLim*[-1,1]);
zlim(axisLim*[-1,1]);
for az = azAngleVec
  view(az, viewElevation);
  drawnow;     
  pause(dtAnimation); 
end
delete(hScat);

%% Plot satellites satisfying elevation mask
Nf = length(svEcefMaskMat);
sgtitle([num2str(Nf) ' satellites satisfying the composite elevation mask'], ...
         'color', 'w');
hScat = scatter3(svEcefMaskMat(:,1), svEcefMaskMat(:,2), svEcefMaskMat(:,3),...
                 satelliteMarkerSize, 'filled');
hScat.MarkerFaceColor = 'w';
hScat.MarkerEdgeColor = 'w';
camproj('perspective');
xlim(axisLim*[-1,1]);
ylim(axisLim*[-1,1]);
zlim(axisLim*[-1,1]);
for az = azAngleVec
  view(az, viewElevation);
  drawnow;     
  pause(dtAnimation); 
end

%% Plot satellites within 100 km
[idxVec] = find(abs(residualVec) < 100e3);
Nf = length(idxVec);
sgtitle([num2str(Nf) ' satellites whose fit to first measurement is within 100 km'], 'color', 'w');
delete(hScat);
hHyp = fimplicit3(fTdoa,3.6e7*[-1 1 -1 1 0 1], 'MeshDensity', meshDensity, ...
                  'EdgeColor', 'none', 'FaceAlpha', faceAlpha, 'FaceColor', ...
                  faceColor);
hScat = scatter3(svEcefMaskMat(idxVec,1), svEcefMaskMat(idxVec,2), ...
                 svEcefMaskMat(idxVec,3), satelliteMarkerSize, 'filled');
hScat.MarkerFaceColor = 'w';
hScat.MarkerEdgeColor = 'w';
for az = azAngleVec
  view(az, viewElevation);
  drawnow;     
  pause(dtAnimation); 
end

%% Plot satellites within 50 km
[idxVec] = find(abs(residualVec) < 50e3);
Nf = length(idxVec);
sgtitle([num2str(Nf) ' satellites whose fit to first measurement is within 50 km'], 'color', 'w');
delete(hScat);
hScat = scatter3(svEcefMaskMat(idxVec,1), svEcefMaskMat(idxVec,2), ...
                 svEcefMaskMat(idxVec,3), satelliteMarkerSize, 'filled');
hScat.MarkerFaceColor = 'w';
hScat.MarkerEdgeColor = 'w';
for az = azAngleVec
  view(az, viewElevation);
  drawnow;     
  pause(dtAnimation); 
end

%% Plot satellites within 10 km
[idxVec] = find(abs(residualVec) < 10e3);
Nf = length(idxVec);
sgtitle([num2str(Nf) ' satellite whose fit to first measurement is within 10 km'], 'color', 'w');
delete(hScat);
hScat = scatter3(svEcefMaskMat(idxVec,1), svEcefMaskMat(idxVec,2), ...
                 svEcefMaskMat(idxVec,3), satelliteMarkerSize, 'filled');
hScat.MarkerFaceColor = 'w';
hScat.MarkerEdgeColor = 'w';
for az = azAngleVec
  view(az, viewElevation);
  drawnow;     
  pause(dtAnimation); 
end

%% Identify the remaining satellite and plot its residuals history
if (Nf == 1)
  idxCandidate = idxVec;
  res = abs(residualVec(idxVec));
  str = sprintf(['This satellite (NORAD ID: %d) fits first TDOA measurement ' ...
                 'to better than %0.2f km.'], ... 
                iedata.noradIdMaskVec(idxCandidate), res/1000);
  sgtitle(str, 'color', 'w');
  for az = azAngleVec
    view(az, viewElevation);
    drawnow;     
    pause(dtAnimation); 
  end
  % The residuals plotted here are evaluated at the optimal estimate for the
  % ephemeris error eta_s.
  t2Vec = iedata.tTGTVec;
  Nmeas = length(t2Vec);
  residualVec = zeros(Nmeas,1);
  for ii=1:Nmeas
    svEcefMaskMat = iedata.svPosElMaskCell{ii};
    ctdoa = -iedata.tdoaVec(ii)*cLight;
    p = svEcefMaskMat(idxCandidate,:)';
    residualVec(ii) = norm(p - r1) - norm(p - r2) - ctdoa;
  end
  figure(2);clf;
  plot(t2Vec - t2Vec(1), residualVec - mean(residualVec), 'linewidth', 2);
  xlabel('Time [sec]');
  ylabel('Residual [m]');
  title('Final solution residual over full measurement interval');
  ylim(5*[-1 1]);
  grid on;
end
