//! Element conversions and the two-body relations.
//!
//! These run per measurement in a filter and per step in some force models, so a
//! slow conversion is not a rounding error in the budget. The point of tracking
//! them is to notice a regression, not to hit a number.

use std::hint::black_box;

use astrokit::kepler::{self, Degeneracy, Keplerian};
use criterion::{criterion_group, criterion_main, Criterion};
use nalgebra::Vector3;

const MU: f64 = 3.986004415e14;

/// Inclined, near-circular LEO. Just outside the degeneracy tolerance, so it
/// takes the general branch.
fn leo() -> (Vector3<f64>, Vector3<f64>) {
    (
        Vector3::new(
            4.263868426884883e6,
            5.146189057155391e6,
            1.1310208421331816e6,
        ),
        Vector3::new(-5923.454461876975, 4496.802639690076, 1870.3893008991558),
    )
}

/// GEO: circular and equatorial, so it takes the doubly-degenerate branch.
fn geo() -> (Vector3<f64>, Vector3<f64>) {
    let radius = 4.2164e7;
    (
        Vector3::new(radius, 0.0, 0.0),
        Vector3::new(0.0, kepler::circular_speed(radius, MU), 0.0),
    )
}

fn bench_scalar_relations(c: &mut Criterion) {
    let mut group = c.benchmark_group("kepler/relations");
    let sma = 6.7781363e6;

    group.bench_function("period", |b| {
        b.iter(|| kepler::period(black_box(sma), black_box(MU)))
    });
    group.bench_function("semi_major_axis", |b| {
        b.iter(|| kepler::semi_major_axis(black_box(5553.6), black_box(MU)))
    });
    group.bench_function("mean_motion", |b| {
        b.iter(|| kepler::mean_motion(black_box(sma), black_box(MU)))
    });
    group.finish();
}

fn bench_anomalies(c: &mut Criterion) {
    let mut group = c.benchmark_group("kepler/anomalies");

    group.bench_function("eccentric_from_true", |b| {
        b.iter(|| kepler::eccentric_from_true(black_box(1.2), black_box(0.3)))
    });
    group.bench_function("mean_from_eccentric", |b| {
        b.iter(|| kepler::mean_from_eccentric(black_box(1.1), black_box(0.3)))
    });

    // The only iterative routine here, and the one worth watching. Near-circular
    // converges in a step or two; high eccentricity is the expensive case.
    for eccentricity in [0.001, 0.3, 0.9] {
        group.bench_function(format!("eccentric_from_mean/e={eccentricity}"), |b| {
            b.iter(|| kepler::eccentric_from_mean(black_box(1.0), black_box(eccentricity)))
        });
    }
    group.finish();
}

fn bench_conversions(c: &mut Criterion) {
    let mut group = c.benchmark_group("kepler/conversions");

    let (r, v) = leo();
    group.bench_function("from_cartesian/leo", |b| {
        b.iter(|| Keplerian::from_cartesian(black_box(r), black_box(v), black_box(MU)))
    });

    let (r_geo, v_geo) = geo();
    group.bench_function("from_cartesian/geo", |b| {
        b.iter(|| Keplerian::from_cartesian(black_box(r_geo), black_box(v_geo), black_box(MU)))
    });

    let elements = Keplerian {
        semi_major_axis: 2.0e7,
        eccentricity: 0.3,
        inclination: 0.8,
        raan: 0.5,
        aop: 1.0,
        true_anomaly: 2.0,
        degeneracy: Degeneracy::None,
    };
    group.bench_function("to_cartesian", |b| {
        b.iter(|| black_box(elements).to_cartesian(black_box(MU)))
    });

    group.bench_function("round_trip", |b| {
        b.iter(|| {
            let elements = Keplerian::from_cartesian(black_box(r), black_box(v), MU);
            black_box(elements.to_cartesian(MU))
        })
    });
    group.finish();
}

fn bench_invariants(c: &mut Criterion) {
    let mut group = c.benchmark_group("kepler/invariants");
    let (r, v) = leo();

    group.bench_function("specific_energy", |b| {
        b.iter(|| kepler::specific_energy(black_box(r), black_box(v), black_box(MU)))
    });
    group.bench_function("specific_angular_momentum", |b| {
        b.iter(|| kepler::specific_angular_momentum(black_box(r), black_box(v)))
    });
    group.finish();
}

criterion_group!(
    benches,
    bench_scalar_relations,
    bench_anomalies,
    bench_conversions,
    bench_invariants
);
criterion_main!(benches);