flightplanner/crates/core/src/routing/hybrid.rs

//! Dense free-route (DCT) routing over the navdata point cloud.
//!
//! Modern European enroute is Free Route Airspace: you may fly DCT between
//! (almost) any two published points, subject to length limits. Our RAD-derived
//! FRA graph ([`super::fra_graph`]) only has the *explicitly published* VIA pairs,
//! so it's far too sparse to find real routes (e.g. Paris–Nice `LATRA DCT LAMUT
//! … NISAR`). This router instead builds a **k-nearest-neighbour DCT graph** over
//! every waypoint/navaid in the dep→dest corridor and runs A* — yielding the
//! near-great-circle path through real points (the optimal free-route shape).
//! RAD-forbidden directs are skipped; the IFPUV oracle repairs residual issues
//! (e.g. TMA segments where DCT is banned → airway splice) in [`super::discover`].

use petgraph::algo::astar;
use petgraph::graph::{DiGraph, NodeIndex};
use rusqlite::{params, Connection};

use crate::error::Result;
use crate::model::LatLon;
use crate::rad::RadData;

use super::{airport_pos, Leg, Route};

/// Max length of a single DCT edge (nm). Most FRA areas allow long directs; TMAs
/// don't, but the oracle repairs those. Keeps the kNN graph sparse.
const MAX_DCT_NM: f64 = 220.0;
/// Neighbours per node in the DCT graph.
const K: usize = 10;
/// How many nearest corridor points an airport connects to (when no SID/STAR).
const CONNECT_K: usize = 12;

/// Plan a dense free-route (DCT) path from `from` to `to`. `dep_conn`/`dest_conn`
/// are SID exit / STAR entry fix idents used to anchor the terminal connection.
pub fn plan_hybrid(
    conn: &Connection,
    from: &str,
    to: &str,
    dep_conn: &[String],
    dest_conn: &[String],
    cruise_fl: i32,
    rad: Option<&RadData>,
) -> Result<Option<Route>> {
    let dep = airport_pos(conn, from)?;
    let dst = airport_pos(conn, to)?;
    let pts = load_corridor(conn, dep, dst, 2.5)?;
    if pts.len() < 2 {
        return Ok(None);
    }

    let mut g: DiGraph<(String, LatLon), f64> = DiGraph::new();
    let node: Vec<NodeIndex> = pts.iter().map(|(id, p)| g.add_node((id.clone(), *p))).collect();

    // kNN DCT edges (bidirectional), skipping RAD-forbidden directs at this FL.
    for i in 0..pts.len() {
        let mut nbrs: Vec<(f64, usize)> = (0..pts.len())
            .filter(|&j| j != i)
            .map(|j| (pts[i].1.distance_nm(&pts[j].1), j))
            .filter(|(d, _)| *d <= MAX_DCT_NM)
            .collect();
        nbrs.sort_by(|a, b| a.0.total_cmp(&b.0));
        for (d, j) in nbrs.into_iter().take(K) {
            if let Some(rad) = rad {
                if rad.forbidden_dct(&pts[i].0, &pts[j].0, cruise_fl).is_some() {
                    continue;
                }
            }
            g.add_edge(node[i], node[j], d);
        }
    }

    // Terminal connection: airport → SID exit fixes (or nearest corridor points).
    let dep_ix = g.add_node((from.to_uppercase(), dep));
    let dst_ix = g.add_node((to.to_uppercase(), dst));
    connect(&mut g, &pts, &node, dep_ix, dep, dep_conn, true);
    connect(&mut g, &pts, &node, dst_ix, dst, dest_conn, false);

    let result = astar(
        &g,
        dep_ix,
        |n| n == dst_ix,
        |e| *e.weight(),
        |n| g[n].1.distance_nm(&dst),
    );
    let Some((total_nm, path)) = result else {
        return Ok(None);
    };
    if path.len() < 2 {
        return Ok(None);
    }

    let mut legs = Vec::with_capacity(path.len() - 1);
    for w in path.windows(2) {
        let (a, b) = (&g[w[0]], &g[w[1]]);
        legs.push(Leg {
            from: a.0.clone(),
            to: b.0.clone(),
            airway: "DCT".to_owned(),
            dist_nm: a.1.distance_nm(&b.1),
        });
    }
    Ok(Some(Route { legs, total_nm, via_airways: false }))
}

/// Wire an airport node to its SID/STAR connector fixes (matched by ident), or to
/// the nearest corridor points when none resolve. `outbound` = airport→fix.
fn connect(
    g: &mut DiGraph<(String, LatLon), f64>,
    pts: &[(String, LatLon)],
    node: &[NodeIndex],
    apt_ix: NodeIndex,
    apt: LatLon,
    conn_fixes: &[String],
    outbound: bool,
) {
    let mut targets: Vec<usize> = conn_fixes
        .iter()
        .filter_map(|f| pts.iter().position(|(id, _)| id.eq_ignore_ascii_case(f)))
        .collect();
    if targets.is_empty() {
        let mut near: Vec<(f64, usize)> =
            pts.iter().enumerate().map(|(i, (_, p))| (apt.distance_nm(p), i)).collect();
        near.sort_by(|a, b| a.0.total_cmp(&b.0));
        targets = near.into_iter().take(CONNECT_K).map(|(_, i)| i).collect();
    }
    for i in targets {
        let d = apt.distance_nm(&pts[i].1);
        if outbound {
            g.add_edge(apt_ix, node[i], d);
        } else {
            g.add_edge(node[i], apt_ix, d);
        }
    }
}

/// A filable **enroute** waypoint ident: a 5-letter name code (5LNC), all
/// alphabetic. Terminal/procedure fixes (e.g. `AT410`, `MA13`, `DE27R`, `PG271`)
/// contain digits and are NOT valid enroute points — IFPS rejects them.
fn is_enroute_wpt(ident: &str) -> bool {
    ident.len() == 5 && ident.chars().all(|c| c.is_ascii_uppercase())
}

/// Navdata points inside the dep/dest bounding box (+`margin`°) usable for
/// enroute DCT: 5LNC waypoints + all navaids. One per ident (nearest the corridor
/// mid when repeated).
fn load_corridor(conn: &Connection, a: LatLon, b: LatLon, margin: f64) -> Result<Vec<(String, LatLon)>> {
    let mid = LatLon::new((a.lat + b.lat) / 2.0, (a.lon + b.lon) / 2.0);
    let (min_lat, max_lat) = (a.lat.min(b.lat) - margin, a.lat.max(b.lat) + margin);
    let (min_lon, max_lon) = (a.lon.min(b.lon) - margin, a.lon.max(b.lon) + margin);
    let mut best: std::collections::HashMap<String, LatLon> = std::collections::HashMap::new();
    for table in ["waypoints", "navaids"] {
        let enroute_only = table == "waypoints";
        let mut stmt = conn.prepare(&format!(
            "SELECT ident, lat, lon FROM {table} WHERE lat BETWEEN ?1 AND ?2 AND lon BETWEEN ?3 AND ?4"
        ))?;
        let rows = stmt.query_map(params![min_lat, max_lat, min_lon, max_lon], |r| {
            Ok((r.get::<_, String>(0)?, LatLon::new(r.get(1)?, r.get(2)?)))
        })?;
        for row in rows.flatten() {
            let (ident, p) = row;
            if enroute_only && !is_enroute_wpt(&ident) {
                continue;
            }
            best.entry(ident)
                .and_modify(|cur| {
                    if p.distance_nm(&mid) < cur.distance_nm(&mid) {
                        *cur = p;
                    }
                })
                .or_insert(p);
        }
    }
    Ok(best.into_iter().collect())
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn enroute_ident_filter() {
        assert!(is_enroute_wpt("KESAX") && is_enroute_wpt("LATRA") && is_enroute_wpt("NISAR"));
        assert!(!is_enroute_wpt("AT410") && !is_enroute_wpt("MA13") && !is_enroute_wpt("DE27R"));
        assert!(!is_enroute_wpt("RBT")); // navaids come from the navaids table
    }

    /// Prints the offline hybrid route for a few pairs (needs real.db). Ignored.
    /// `cargo test -p flightplanner-core prints_hybrid -- --ignored --nocapture`
    #[test]
    #[ignore = "needs real.db"]
    fn prints_hybrid_routes() {
        let Ok(conn) = Connection::open("../../real.db") else { return };
        for (f, t) in [("LFPG", "EGLL"), ("LFPG", "LFMN"), ("LFPG", "EDDF")] {
            match plan_hybrid(&conn, f, t, &[], &[], 360, None) {
                Ok(Some(r)) => {
                    let n = r.legs.len();
                    let item15: Vec<String> =
                        r.legs.iter().take(n.saturating_sub(1)).map(|l| l.to.clone()).collect();
                    eprintln!("{f}->{t} ({:.0} nm, {n} legs): {}", r.total_nm, item15.join(" DCT "));
                }
                other => eprintln!("{f}->{t}: {other:?}"),
            }
        }
    }
}