flight_plotter_v32.py

import math
import numpy as np
from datetime import datetime, timezone, timedelta


speedOfLight_c = 299792458.0
uplinkFreq = 1646652500
downlinkFreq = 3615152500
btoBias = -495679
BFO_bias = 150
elip_a = 6378137.0
elip_f = 1.0 / 298.257223563
elip_b = elip_a * (1.0 - elip_f)
elip_e2 = (2 * elip_f) - (elip_f * elip_f)

def degToRad(d):
    return d * math.pi / 180.0

def radToDeg(r):
    return r * 180.0 / math.pi

def feetToM(ft):
    return ft * 0.3048
satLoc = (
    (("18:25:00"), (18136.7, 38071.8, 1148.5), (0.00188, -0.00117, 0.02690), 11, 142, 12520),
    (("18:40:00"), (18138.21, 38070.79, 1170.29), (0.001892134, -0.001142665, 0.02137832), 8, 88, None),
    (("19:40:00"), (18145.1, 38067.0, 1206.3), (0.00189, -0.00092, -0.00148), -1, 111, 11500),
    (("20:40:00"), (18152.1, 38064.0, 1159.7), (0.00200, -0.00077, -0.02422), -1, 141, 11740),
    (("21:40:00"), (18159.5, 38061.3, 1033.8), (0.00212, -0.00076, -0.04531), -18, 168, 12780),
    (("22:40:00"), (18167.2, 38058.3, 837.2), (0.00211, -0.00096, -0.06331), -29, 204, 14540),
    (("24:10:00"), (18177.5, 38051.7, 440.0), (0.00160, -0.00151, -0.08188), -38, 252, 18040),
    (("24:20:00"), (18178.4, 38050.8, 390.5), (0.00150, -0.00158, -0.08321), -38, 182, 18400)
    )
gsPerth = (-2368.8, 4881.1, -3342.0)
nominalSatLoc = (0, 64.5, 36000000)

def N_phi(lat_rad):
    sinp = math.sin(lat_rad)
    return elip_a / math.sqrt(1.0 - elip_e2 * sinp * sinp)

def latLonToECEF(lat_deg, lon_deg, h_m):
    lat = degToRad(lat_deg)
    lon = degToRad(lon_deg)
    Np = N_phi(lat)
    x = (Np + h_m) * math.cos(lat) * math.cos(lon)
    y = (Np + h_m) * math.cos(lat) * math.sin(lon)
    z = (Np * (1 - elip_e2) + h_m) * math.sin(lat)
    return np.array([x, y, z], dtype=float)

def distBetECEF(p1, p2):
    d = p1 - p2
    return float(np.sqrt(np.dot(d, d)))

def ENU2ECEFvelo(vE, vN, vU, lat_rad, lon_rad):
    sL, cL = (math.sin(lat_rad), math.cos(lat_rad))
    sO, cO = (math.sin(lon_rad), math.cos(lon_rad))
    R = np.array([[-sO, -sL * cO, cL * cO], [cO, -sL * sO, cL * sO], [0.0, cL, sL]], dtype=float)
    return R @ np.array([vE, vN, vU], dtype=float)

def toECEFunitVector(p_from, p_to):
    v = p_to - p_from
    n = np.linalg.norm(v)
    return v / n if n != 0 else v

def rhumb_direct(start_latlon, bearing_deg, distance_m):
    lat1 = degToRad(start_latlon[0])
    lon1 = degToRad(start_latlon[1])
    brg = degToRad(bearing_deg)
    R = elip_a
    d = distance_m / R
    dphi = d * math.cos(brg)
    lat2 = lat1 + dphi
    if abs(lat2) > math.pi / 2 - 1e-12:
        lat2 = math.copysign(math.pi / 2 - 1e-12, lat2)
    dpsi = math.log(math.tan(lat2 / 2 + math.pi / 4) / math.tan(lat1 / 2 + math.pi / 4))
    q = dphi / dpsi if abs(dpsi) > 1e-12 else math.cos(lat1)
    dlon = d * math.sin(brg) / q
    lon2 = lon1 + dlon
    lon2 = (lon2 + math.pi) % (2 * math.pi) - math.pi
    return (radToDeg(lat2), radToDeg(lon2))

def vincenty_direct(latLon, alpha1_deg, s):
    lat1 = degToRad(latLon[0])
    lon1 = degToRad(latLon[1])
    alpha1 = degToRad(alpha1_deg)
    U1 = math.atan((1 - elip_f) * math.tan(lat1))
    sigma1 = math.atan2(math.tan(U1), math.cos(alpha1))
    sin_alpha = math.cos(U1) * math.sin(alpha1)
    cos2_alpha = 1 - sin_alpha ** 2
    elip_b = elip_a * (1.0 - elip_f)
    u2 = cos2_alpha * (elip_a ** 2 - elip_b ** 2) / elip_b ** 2
    A = 1 + u2 / 16384 * (4096 + u2 * (-768 + u2 * (320 - 175 * u2)))
    B = u2 / 1024 * (256 + u2 * (-128 + u2 * (74 - 47 * u2)))
    sigma = s / (elip_b * A)
    tol = 1e-12
    for _ in range(200):
        cos2sigma_m = math.cos(2 * sigma1 + sigma)
        sin_sigma = math.sin(sigma)
        cos_sigma = math.cos(sigma)
        delta_sigma = B * sin_sigma * (cos2sigma_m + B / 4 * (cos_sigma * (-1 + 2 * cos2sigma_m ** 2) - B / 6 * cos2sigma_m * (-3 + 4 * sin_sigma ** 2) * (-3 + 4 * cos2sigma_m ** 2)))
        sigma_new = s / (elip_b * A) + delta_sigma
        if abs(sigma_new - sigma) < tol:
            sigma = sigma_new
            break
        sigma = sigma_new
    lat2 = math.atan2(math.sin(U1) * cos_sigma + math.cos(U1) * sin_sigma * math.cos(alpha1), (1 - elip_f) * math.sqrt(sin_alpha ** 2 + (math.sin(U1) * sin_sigma - math.cos(U1) * cos_sigma * math.cos(alpha1)) ** 2))
    lam = math.atan2(sin_sigma * math.sin(alpha1), math.cos(U1) * cos_sigma - math.sin(U1) * sin_sigma * math.cos(alpha1))
    C = elip_f / 16 * cos2_alpha * (4 + elip_f * (4 - 3 * cos2_alpha))
    L = lam - (1 - C) * elip_f * sin_alpha * (sigma + C * sin_sigma * (cos2sigma_m + C * cos_sigma * (-1 + 2 * cos2sigma_m ** 2)))
    lon2 = lon1 + L
    alpha2 = math.atan2(sin_alpha, -math.sin(U1) * sin_sigma + math.cos(U1) * cos_sigma * math.cos(alpha1))
    return ([radToDeg(lat2), radToDeg(lon2)], (radToDeg(alpha2) + 360) % 360)
SIDEREAL_DAY_S = 86164.0
OMEGA = 2.0 * math.pi / SIDEREAL_DAY_S
SAT_EPOCH_UTC = datetime(2014, 3, 7, 16, 30, 0, tzinfo=timezone.utc)



from datetime import datetime, timedelta, timezone

def _safe_float(x, default=0.0):
    try:
        return float(x)
    except (TypeError, ValueError):
        return default

def _extract_time_string(label_field):
    if isinstance(label_field, (tuple, list)):
        label = label_field[0]
        if isinstance(label, (tuple, list)):
            return str(label[0])
        return str(label)
    return str(label_field)

def _time_label_to_dt(label: str, epoch: datetime) -> datetime:
    hh, mm, ss = map(int, label.split(":"))
    day_offset = hh // 24
    hh = hh % 24
    dt = datetime(epoch.year, epoch.month, epoch.day, hh, mm, ss, tzinfo=epoch.tzinfo)
    if day_offset:
        dt += timedelta(days=day_offset)
    return dt

def _extract_time_string(tfield):
    if isinstance(tfield, (tuple, list)):
        return str(tfield[0])
    return str(tfield)

def _build_sat_time_seconds():
    times_sec = []
    for entry in satLoc:
        tstr = _extract_time_string(entry[0])
        try:
            h, m, s = map(int, tstr.split(':'))
        except Exception:
            continue
        add_day = 0
        if h == 24:
            h = 0
            add_day = 1
        try:
            dt = datetime(SAT_EPOCH_UTC.year, SAT_EPOCH_UTC.month, SAT_EPOCH_UTC.day, h, m, s, tzinfo=timezone.utc)
        except ValueError:
            continue
        sec = (dt - SAT_EPOCH_UTC).total_seconds() + add_day * 86400.0
        times_sec.append(sec)
    return np.array(times_sec, dtype=float)
_sat_time_seconds = _build_sat_time_seconds()

def seconds_since_sat_epoch(t_utc):
    return (t_utc - SAT_EPOCH_UTC).total_seconds()

def calculateBTO_at_time(acLat, acLon, acAlt, time_utc):
    """
    Compute BTO (microseconds) using the *same* satellite geometry as BFO:
    nearest satLoc entry to time_utc, with positions in km converted to meters.
    
    """
    # Aircraft ECEF (meters)
    acECEF = latLonToECEF(acLat, acLon, acAlt)

    # Nearest satLoc satellite ECEF (meters)
    idx_nearest = nearest_measured_for_time(time_utc)[0]
    satECEF = 1000.0 * np.array(satLoc[idx_nearest][1], float)

    # Perth ground station ECEF (meters)
    gsECEF = 1000.0 * np.array(gsPerth, float)

    # One-way distances (meters)
    d1 = distBetECEF(acECEF, satECEF)  # AC -> SAT
    d2 = distBetECEF(satECEF, gsECEF)  # SAT -> GS

    # Round-trip signal time (microseconds), plus constant system bias
    bto_us = (2.0 * (d1 + d2) / speedOfLight_c) * 1e6 + btoBias
    return bto_us

def calculateBFO_at_time(flightV_mps, track_deg, acLat, acLon, acAlt, acVS_mps, time_utc):
    """
    Predict BFO using the satellite state from satLoc that is NEAREST to time_utc.
    Includes δF_sat + δF_AFC from satLoc[idx][3] and BFO_bias.
    """
    # --- aircraft velocity in ENU and ECEF ---
    vE = flightV_mps * math.sin(degToRad(track_deg))
    vN = flightV_mps * math.cos(degToRad(track_deg))
    vU = acVS_mps
    acECEF = latLonToECEF(acLat, acLon, acAlt)  # meters
    vECEF  = ENU2ECEFvelo(vE, vN, vU, degToRad(acLat), degToRad(acLon))  # m/s

    # --- find nearest satLoc entry for this UTC ---
    idx_nearest = nearest_measured_for_time(time_utc)[0]

    # --- satellite state from satLoc (convert km → m, km/s → m/s) ---
    sat_pos_km   = np.array(satLoc[idx_nearest][1], float)
    sat_vel_km_s = np.array(satLoc[idx_nearest][2], float)
    satECEF      = 1000.0 * sat_pos_km           # m
    satV_ECEF    = 1000.0 * sat_vel_km_s         # m/s

    # --- ground station (Perth) ECEF in meters ---
    gsECEF = 1000.0 * np.array(gsPerth, float)

    # --- line-of-sight unit vectors ---
    u_ac_sat = toECEFunitVector(acECEF, satECEF)
    u_sat_gs = toECEFunitVector(satECEF, gsECEF)

    # --- relative LOS velocities ---
    v_rel_ac_sat = float(np.dot(vECEF - satV_ECEF, u_ac_sat))   # m/s
    v_rel_sat_gs = float(np.dot(satV_ECEF, u_sat_gs))           # m/s  (GS static in ECEF)

    # --- Doppler terms ---
    Fup   = uplinkFreq   * (v_rel_ac_sat / speedOfLight_c)
    Fdown = downlinkFreq * (v_rel_sat_gs / speedOfLight_c)

    # --- aircraft frequency compensation toward nominal sat location ---
    nomECEF = latLonToECEF(nominalSatLoc[0], nominalSatLoc[1], nominalSatLoc[2])
    u_ac_nom = toECEFunitVector(acECEF, nomECEF)
    v_rel_ac_nom = float(np.dot(vECEF, u_ac_nom))
    FcompAC = uplinkFreq * (v_rel_ac_nom / speedOfLight_c)

    # --- δF_sat + δF_AFC from satLoc (index 3), safe-cast in case of None ---
    fsat_afc = float(satLoc[idx_nearest][3]) if satLoc[idx_nearest][3] is not None else 0.0

    # --- final BFO ---
    return Fup + Fdown - FcompAC + fsat_afc + BFO_bias

def nearest_measured_for_time(t_utc):
    """
    Return (idx, label_str, meas_bfo_hz, meas_bto_us) for the satLoc entry whose time label
    is nearest to t_utc. Guard against None in measured fields.
    """
    times = []
    for i, row in enumerate(satLoc):
        label_str = _extract_time_string(row[0])
        try:
            dt = _time_label_to_dt(label_str, SAT_EPOCH_UTC)
        except Exception:
            continue
        times.append((i, dt, label_str))
    if not times:
        raise RuntimeError("No valid satLoc times to compare.")
    idx, dt_near, label = min(times, key=lambda it: abs((t_utc - it[1]).total_seconds()))
    meas_bfo = _safe_float(satLoc[idx][4], float("nan"))
    meas_bto = _safe_float(satLoc[idx][5], float("nan"))
    return (idx, label, meas_bfo, meas_bto)

def nearest_measured_with_bto_for_time(t_utc):
    """Like nearest_measured_for_time, but guarantees meas_bto is not None if possible."""
    best = None
    best_dt = None
    for i, row in enumerate(satLoc):
        # row[0] is a time label like "19:40:00"
        row_dt = SAT_EPOCH_UTC + timedelta(seconds=_sat_time_seconds[i])
        dt = abs((t_utc - row_dt).total_seconds())
        meas_bto = row[5]
        if meas_bto is None:
            continue
        if best is None or dt < best_dt:
            best = (i, row[0], row[4], meas_bto)
            best_dt = dt
    if best is not None:
        return best
    # Fall back: return nearest even if BTO is None
    idx, meas_time_str, meas_bfo, meas_bto = nearest_measured_for_time(t_utc)
    return (idx, meas_time_str, meas_bfo, meas_bto)



def auto_tune_heading_to_bto(current_lat, current_lon, alt_m, current_time, base_heading_deg,
                            speed_knots, leg_minutes, model_sel, search_half_width_deg=5.0,
                            heading_step_deg=0.1):
    """Search around base_heading_deg to minimize |measured_bto - calculated_bto| at leg end.
    Returns dict with best_heading, best_lat, best_lon, best_time, best_bto, best_delta_bto, meas_time_str.
    """
    speed_mps = speed_knots * 0.514444
    distance_m = speed_mps * (leg_minutes * 60.0)
    target_time = current_time + timedelta(minutes=leg_minutes)

    # get measurement reference (prefer one with non-None BTO)
    meas_idx, meas_time_str, meas_bfo, meas_bto = nearest_measured_with_bto_for_time(target_time)

    # If measurement BTO is still None (should be rare), we cannot tune.
    if meas_bto is None:
        return {
            "ok": False,
            "reason": "No measured BTO available near target time",
            "best_heading": base_heading_deg,
            "meas_time_str": meas_time_str,
        }

    def wrap(h):
        h = h % 360.0
        return h + 360.0 if h < 0 else h

    best = None
    # number of steps on each side
    n = int(round(search_half_width_deg / heading_step_deg))
    for k in range(-n, n + 1):
        hdg = wrap(base_heading_deg + k * heading_step_deg)
        if model_sel == '1':
            (lat2, lon2), _ = vincenty_direct((current_lat, current_lon), hdg, distance_m)
        else:
            lat2, lon2 = rhumb_direct((current_lat, current_lon), hdg, distance_m)

        bto_val = calculateBTO_at_time(lat2, lon2, alt_m, target_time)
        delta_bto = meas_bto - bto_val
        score = abs(delta_bto)
        if best is None or score < best["score"]:
            best = {
                "ok": True,
                "score": score,
                "best_heading": hdg,
                "best_lat": lat2,
                "best_lon": lon2,
                "best_time": target_time,
                "best_bto": bto_val,
                "best_delta_bto": delta_bto,
                "meas_time_str": meas_time_str,
                "meas_bto": meas_bto,
            }
    return best


def auto_tune_heading_speed_to_bto(current_lat, current_lon, alt_m, current_time,
                                  base_heading_deg, base_speed_knots, leg_minutes, model_sel,
                                  heading_half_width_deg=8.0, heading_step_deg=0.2,
                                  speed_half_width_knots=80.0, speed_step_knots=2.0,
                                  refine_heading_half_width_deg=1.0, refine_heading_step_deg=0.05,
                                  refine_speed_half_width_knots=10.0, refine_speed_step_knots=0.5):
    """Search around (base_heading_deg, base_speed_knots) to minimize |ΔBTO| at leg end.

    Two-stage search:
      1) coarse grid over heading±heading_half_width_deg and speed±speed_half_width_knots
      2) refine around best using smaller steps

    Returns dict with best_heading, best_speed_knots, best_lat, best_lon, best_time, best_bto,
    best_delta_bto, meas_time_str, meas_bto.
    """
    target_time = current_time + timedelta(minutes=leg_minutes)
    meas_idx, meas_time_str, meas_bfo, meas_bto = nearest_measured_with_bto_for_time(target_time)

    if meas_bto is None:
        return {
            "ok": False,
            "reason": "No measured BTO available near target time.",
            "best_heading": base_heading_deg,
            "best_speed_knots": base_speed_knots,
            "best_lat": None,
            "best_lon": None,
            "best_time": target_time,
            "best_bto": None,
            "best_delta_bto": None,
            "meas_time_str": meas_time_str,
            "meas_bto": None,
        }

    def eval_candidate(hdg_deg: float, spd_knots: float):
        speed_mps = spd_knots * 0.514444
        distance_m = speed_mps * (leg_minutes * 60.0)
        if model_sel == '1':
            (latlon2, _az2) = vincenty_direct((current_lat, current_lon), hdg_deg, distance_m)
            lat2, lon2 = latlon2
        else:
            lat2, lon2 = rhumb_direct((current_lat, current_lon), hdg_deg, distance_m)
        bto_val = calculateBTO_at_time(lat2, lon2, alt_m, target_time)
        delta_bto = meas_bto - bto_val
        return lat2, lon2, bto_val, delta_bto

    def wrap_heading(h):
        h = h % 360.0
        return h + 360.0 if h < 0 else h

    best = None

    # ---- stage 1: coarse grid ----
    h0 = base_heading_deg
    s0 = base_speed_knots
    h_min = h0 - heading_half_width_deg
    h_max = h0 + heading_half_width_deg
    s_min = max(0.0, s0 - speed_half_width_knots)
    s_max = s0 + speed_half_width_knots

    # Build grids (inclusive ends)
    headings = np.arange(h_min, h_max + 1e-9, heading_step_deg)
    speeds = np.arange(s_min, s_max + 1e-9, speed_step_knots)

    for spd in speeds:
        for hdg in headings:
            hdg_w = wrap_heading(float(hdg))
            lat2, lon2, bto_val, delta_bto = eval_candidate(hdg_w, float(spd))
            score = abs(delta_bto)
            if best is None or score < best["score"]:
                best = {
                    "ok": True,
                    "score": score,
                    "best_heading": hdg_w,
                    "best_speed_knots": float(spd),
                    "best_lat": lat2,
                    "best_lon": lon2,
                    "best_time": target_time,
                    "best_bto": bto_val,
                    "best_delta_bto": delta_bto,
                    "meas_time_str": meas_time_str,
                    "meas_bto": meas_bto,
                }

    # ---- stage 2: refine around the best ----
    if best is None:
        return {
            "ok": False,
            "reason": "Search failed.",
            "best_heading": base_heading_deg,
            "best_speed_knots": base_speed_knots,
            "best_lat": None,
            "best_lon": None,
            "best_time": target_time,
            "best_bto": None,
            "best_delta_bto": None,
            "meas_time_str": meas_time_str,
            "meas_bto": meas_bto,
        }

    h1 = best["best_heading"]
    s1 = best["best_speed_knots"]

    headings2 = np.arange(h1 - refine_heading_half_width_deg, h1 + refine_heading_half_width_deg + 1e-9, refine_heading_step_deg)
    speeds2 = np.arange(max(0.0, s1 - refine_speed_half_width_knots), s1 + refine_speed_half_width_knots + 1e-9, refine_speed_step_knots)

    for spd in speeds2:
        for hdg in headings2:
            hdg_w = wrap_heading(float(hdg))
            lat2, lon2, bto_val, delta_bto = eval_candidate(hdg_w, float(spd))
            score = abs(delta_bto)
            if score < best["score"]:
                best.update({
                    "score": score,
                    "best_heading": hdg_w,
                    "best_speed_knots": float(spd),
                    "best_lat": lat2,
                    "best_lon": lon2,
                    "best_bto": bto_val,
                    "best_delta_bto": delta_bto,
                })

    return best




def repl_fly_from_radar_fix():
    print('=== MH370 REPL (radar fix) ===')
    print('Start: 2014-03-07 18:22:12Z @ 6.578°N, 96.341°E, FL350')
    current_lat = 6.578
    current_lon = 96.341
    current_alt_m = feetToM(35000)
    current_time = datetime(2014, 3, 7, 18, 22, 12, tzinfo=timezone.utc)
    path_points = [(current_lat, current_lon, current_time.isoformat())]
    while True:
        try:
            heading = float(input('HEADING (deg): ').strip())
            speed_knots = float(input('SPEED (knots): ').strip())
            model_sel = input('Enter 1 for VINCENTY or 2 for RHUMB: ').strip()
            model_sel = '1' if model_sel not in ('1', '2') else model_sel
            leg_str = input('Leg duration in minutes (default 10): ').strip()
            leg_minutes = float(leg_str) if leg_str else 10.0
        except Exception as e:
            print('Invalid input, try again.', e)
            continue
        auto_sel = input('Auto-tune: (h) heading, (b) heading+speed, Enter = none: ').strip().lower()

        speed_mps = speed_knots * 0.514444
        distance_m = speed_mps * (leg_minutes * 60.0)

        if auto_sel in ('b', 'both'):
            tuned = auto_tune_heading_speed_to_bto(
                current_lat=current_lat,
                current_lon=current_lon,
                alt_m=current_alt_m,
                current_time=current_time,
                base_heading_deg=heading,
                base_speed_knots=speed_knots,
                leg_minutes=leg_minutes,
                model_sel=model_sel,
            )
            if tuned.get('ok'):
                heading = tuned['best_heading']
                speed_knots = tuned['best_speed_knots']
                # update distance for subsequent printing/metrics
                speed_mps = speed_knots * 0.514444
                distance_m = speed_mps * (leg_minutes * 60.0)
                lat2 = tuned['best_lat']
                lon2 = tuned['best_lon']
                new_time = tuned['best_time']
                print(f"[Auto-tune] Best heading: {heading:.2f}° | Best speed: {speed_knots:.2f} kt | Expected ΔBTO vs {tuned['meas_time_str']}: {tuned['best_delta_bto']:+.3f} µs")
            else:
                if model_sel == '1':
                    (lat2, lon2), _ = vincenty_direct((current_lat, current_lon), heading, distance_m)
                else:
                    lat2, lon2 = rhumb_direct((current_lat, current_lon), heading, distance_m)
                new_time = current_time + timedelta(minutes=leg_minutes)

        elif auto_sel in ('h', 'heading', 'y', 'yes'):
            tuned = auto_tune_heading_to_bto(
                current_lat=current_lat,
                current_lon=current_lon,
                alt_m=current_alt_m,
                current_time=current_time,
                base_heading_deg=heading,
                speed_knots=speed_knots,
                leg_minutes=leg_minutes,
                model_sel=model_sel,
                search_half_width_deg=5.0,
                heading_step_deg=0.1,
            )
            if tuned.get('ok'):
                heading = tuned['best_heading']
                lat2 = tuned['best_lat']
                lon2 = tuned['best_lon']
                new_time = tuned['best_time']
                print(f"[Auto-tune] Best heading: {heading:.2f}° | Expected ΔBTO vs {tuned['meas_time_str']}: {tuned['best_delta_bto']:+.3f} µs")
            else:
                if model_sel == '1':
                    (lat2, lon2), _ = vincenty_direct((current_lat, current_lon), heading, distance_m)
                else:
                    lat2, lon2 = rhumb_direct((current_lat, current_lon), heading, distance_m)
                new_time = current_time + timedelta(minutes=leg_minutes)

        else:
            if model_sel == '1':
                (lat2, lon2), _ = vincenty_direct((current_lat, current_lon), heading, distance_m)
            else:
                lat2, lon2 = rhumb_direct((current_lat, current_lon), heading, distance_m)
            new_time = current_time + timedelta(minutes=leg_minutes)
        bto_val = calculateBTO_at_time(lat2, lon2, current_alt_m, new_time)
        tsec = seconds_since_sat_epoch(new_time)
        bfo_val = calculateBFO_at_time(speed_mps, heading, lat2, lon2, current_alt_m, 0.0, new_time)
        idx, meas_time_str, meas_bfo, meas_bto = nearest_measured_for_time(new_time)
        delta_bto = meas_bto - bto_val
        delta_bfo = meas_bfo - bfo_val
        print()
        print(f'New coordinates: {lat2:.6f}, {lon2:.6f} | Calculated BTO: {bto_val:.3f} (Δ vs nearest@{meas_time_str}: {delta_bto:.3f}) | Calculated BFO: {bfo_val:.1f} (Δ vs nearest@{meas_time_str}: {delta_bfo:.1f}) | Distance travelled: {distance_m / 1000.0:.2f} km | Timing (UTC): {new_time.isoformat()}')
        print()
        path_points.append((lat2, lon2, new_time.isoformat()))
        print('Would you like to:')
        print('A) Output the flight path generated so far')
        print('B) Input a new heading and ground speed for calculating the flight path to handshake no. 3')
        print('C) Restart calculations from an earlier handshake (Handshake 1 to 2)')
        print('D) Proceed to calculate a flight path to handshake no. 4')
        print('E) Manually input coordinates and speed for BTO & BFO calculations at handshake 3')
        print('F) Exit the program')
        selection = input('Selection: ').strip().upper()

        # --- Menu actions (dispatch table) ---
        advance_after_menu = True  # if False, do not overwrite current_* with the just-computed leg

        def action_A():
            print('\nFlight path so far (index: lat, lon, utc):')
            for i, (la, lo, ts) in enumerate(path_points):
                print(f'{i:02d}: {la:.6f}, {lo:.6f}, {ts}')

        def action_B():
            # Placeholder: keep behavior as a no-op (you can wire this to a heading/speed editor later).
            pass

        def action_C():
            idx_str = input(f'Restart from index (0..{len(path_points) - 1}): ').strip()
            try:
                i_idx = int(idx_str)
                if 0 <= i_idx < len(path_points):
                    la, lo, ts = path_points[i_idx]
                    return ('restart', i_idx, la, lo, ts)
                else:
                    print('Index out of range.')
            except Exception as e:
                print('Invalid index.', e)
            return None

        def action_D():
            return 'continue'

        def action_E():
            try:
                la = float(input('Latitude (deg): '))
                lo = float(input('Longitude (deg): '))
                spd_kn = float(input('Speed (knots): '))
                hdg2 = float(input('Heading (deg): '))
                t_str = input('UTC time (YYYY-MM-DDTHH:MM:SSZ, blank = nearest handshake to current time): ').strip()
                if t_str:
                    if t_str.endswith('Z'):
                        t_str = t_str[:-1] + '+00:00'
                    t_utc = datetime.fromisoformat(t_str)
                else:
                    nearest_idx, meas_ts, *_ = nearest_measured_for_time(current_time)
                    t_utc = SAT_EPOCH_UTC + timedelta(seconds=_sat_time_seconds[nearest_idx])
                spd_mps = spd_kn * 0.514444
                bto_v = calculateBTO_at_time(la, lo, current_alt_m, t_utc)
                bfo_v = calculateBFO_at_time(spd_mps, hdg2, la, lo, current_alt_m, 0.0, t_utc)
                idxn, m_ts, m_bfo, m_bto = nearest_measured_for_time(t_utc)
                print(f'Manual point BTO: {bto_v:.3f} (meas@{m_ts}: {m_bto}, Δ: {m_bto - bto_v:.3f})')
                print(f'Manual point BFO: {bfo_v:.1f} (meas@{m_ts}: {m_bfo}, Δ: {m_bfo - bfo_v:.1f})')
            except Exception as e:
                print('Bad manual input.', e)

        def action_F():
            return 'exit'

        actions = {
            'A': action_A,
            'B': action_B,
            'C': action_C,
            'D': action_D,
            'E': action_E,
            'F': action_F,
        }

        action = actions.get(selection)
        if action is None:
            # Unknown / blank input: do nothing.
            pass
        else:
            result = action()
            if result == 'exit':
                print('Exiting.')
                break
            if result == 'continue':
                current_lat, current_lon, current_time = (lat2, lon2, new_time)
                continue
            if isinstance(result, tuple) and result and result[0] == 'restart':
                _, i_idx, la, lo, ts = result
                current_lat, current_lon = (la, lo)
                current_time = datetime.fromisoformat(ts.replace('Z', '+00:00'))
                path_points = path_points[:i_idx + 1]
                print(f'Restarted from index {i_idx}.')
                advance_after_menu = False

        if advance_after_menu:
            current_lat, current_lon, current_time = (lat2, lon2, new_time)
if __name__ == '__main__':
    repl_fly_from_radar_fix()


def satloc_labels():
    return [_extract_time_string(r[0]) for r in satLoc]