F16LateralSideStep.cpp

// ------------------------------------------------------------------------------
// Project: Aetherion
// Copyright(c) 2025-2026, Onur Tuncer, PhD, Istanbul Technical University
//
// SPDX-License-Identifier: MIT
// License-Filename: LICENSE
// ------------------------------------------------------------------------------
//
// F16LateralSideStep.cpp  —  NASA TM-2015-218675 Atmospheric Scenario 13.4
//
// F-16 subsonic lateral side step: closed-loop LQR SAS + autopilot commands a
// 2 000 ft right-of-course offset from the Case-11 trim condition at t = 20 s.
//
// The lateral offset error (latOffset) is computed dynamically at each step
// from the aircraft's position relative to the original 45° NE courseline;
// the commanded offset (2 000 ft) is subtracted after t = 20 s.  As the
// aircraft crosses to the new parallel track, latOffset → 0 and the heading
// autopilot returns the aircraft to ψ ≈ 45°.
//
// Recommended step size: --timeStep 0.02
//

#include <Aetherion/Examples/F16LateralSideStep/F16LateralSideStepApplication.h>
#include <Aetherion/Examples/F16AltitudeChange/F16AltitudeChangeSimulator.h>

#include <Aetherion/Simulation/Log.h>
#include <Aetherion/Simulation/SnapshotTraits.h>
#include <Aetherion/RigidBody/BuildInitialState.h>
#include <Aetherion/FlightDynamics/Trim/TrimSolver.h>
#include <Aetherion/FlightDynamics/Trim/TrimBodyRates.h>
#include <Aetherion/FlightDynamics/Trim/TrimWeight.h>
#include <Aetherion/Serialization/DAVEML/DAVEMLAeroModel.h>
#include <Aetherion/Serialization/DAVEML/DAVEMLPropModel.h>
#include <Aetherion/Serialization/DAVEML/DAVEMLControlModel.h>
#include <Aetherion/Serialization/DAVEML/LoadInertiaFromDAVEML.h>
#include <Aetherion/RigidBody/Config.h>
#include <Aetherion/RigidBody/GeodeticPoseNED.h>
#include <Aetherion/RigidBody/StateLayout.h>
#include <Aetherion/Environment/Atmosphere.h>

#include <numbers>
#include <cmath>
#include <stdexcept>

// ── DML file paths — injected by CMake ───────────────────────────────────────
// AV Rule 30 deviation (#define for constants): see CODING_STANDARDS.md,
// Pre-Processing Directives.
#ifndef F16_INERTIA_FILE
#define F16_INERTIA_FILE ""
#endif
#ifndef F16_AERO_FILE
#define F16_AERO_FILE ""
#endif
#ifndef F16_PROP_FILE
#define F16_PROP_FILE ""
#endif
#ifndef F16_CONTROL_FILE
#define F16_CONTROL_FILE ""
#endif

// ── Scenario 13.4 constants ───────────────────────────────────────────────────
namespace {
    constexpr double kLat_deg = 36.01917;
    constexpr double kLon_deg = -75.67444;
    constexpr double kAlt_ft = 10013.0;
    constexpr double kAlt_m = kAlt_ft * 0.3048;
    constexpr double kHeading_deg = 45.0;
    constexpr double kRoll_deg = 0.0; // wings-level, as NASA sims 04/05 (sim 02 starts at −0.172°)
    constexpr double kVelocity_fps = 400.0;
    constexpr double kVelocity_mps = kVelocity_fps * 0.3048;
    constexpr double kTAS_fps = 565.685;
    constexpr double kLbf_N = 4.448221615260751;
    constexpr double kXcgFromAC_m = (35.0 - 25.0) / 100.0 * 11.32 * 0.3048;

    // ── Scenario 13.4 autopilot commands ─────────────────────────────────────
    // Hold altitude and airspeed at trim; command a 2 000 ft lateral side step
    // to the right of the 45° NE courseline at t = 20 s.
    // NASA TM-2015-218675 §B-13.4 specifies the step is applied at t = 20 s.
    constexpr double kAltCmd_ft = 10013.0;   // hold initial altitude [ft]
    constexpr double kBaseChiCmd_deg = 45.0; // hold NE course [deg]
    constexpr double kLatStep_ft = 2000.0;   // lateral step right of course [ft]
    constexpr double kLatStepTime_s = 20.0;  // step applied at t = 20 s
}

namespace Aetherion::Examples::F16LateralSideStep {

    using Sim = F16AltitudeChange::F16AltitudeChangeSimulator;

    // ── logStartupBanner ──────────────────────────────────────────────────────────

    void F16LateralSideStepApplication::logStartupBanner() const
    {
        AE_CORE_INFO("=======================================================");
        AE_CORE_INFO("Aetherion - NASA TM-2015-218675 Atmospheric Scenario 13.4");
        AE_CORE_INFO("F-16 subsonic lateral side step  ({:.0f} ft right at t={:.0f} s)",
            kLatStep_ft, kLatStepTime_s);
        AE_CORE_INFO("  Location : {:.5f} N, {:.5f} E  (Kitty Hawk, NC)", kLat_deg, kLon_deg);
        AE_CORE_INFO("  Altitude : {:.0f} ft ({:.2f} m)", kAlt_ft, kAlt_m);
        AE_CORE_INFO("  Course   : {:.1f} deg NE  (held throughout)", kHeading_deg);
        AE_CORE_INFO("  TAS init : {:.2f} ft/s (335.15 KTAS)", kTAS_fps);
        AE_CORE_INFO("  LatStep  : {:.0f} ft right of course at t = {:.0f} s",
            kLatStep_ft, kLatStepTime_s);
        AE_CORE_INFO("=======================================================");
    }

    // ── writeCsvHeader ────────────────────────────────────────────────────────────

    void F16LateralSideStepApplication::writeCsvHeader(std::ofstream& csv) const
    {
        Simulation::Snapshot2Traits::write_header(csv);
    }

    // ── prepareSimulation ─────────────────────────────────────────────────────────

    void F16LateralSideStepApplication::prepareSimulation() const
    {
        const auto& simCfg = getConfig();

        // ── 1. Load models ────────────────────────────────────────────────────────
        const std::string inertiaPath = F16_INERTIA_FILE;
        const std::string aeroPath = F16_AERO_FILE;
        const std::string propPath = F16_PROP_FILE;
        const std::string controlPath = F16_CONTROL_FILE;
        if (inertiaPath.empty()) throw std::runtime_error("F16_INERTIA_FILE not configured.");
        if (aeroPath.empty() || propPath.empty())
            throw std::runtime_error("F16_AERO_FILE or F16_PROP_FILE not configured.");
        if (controlPath.empty()) throw std::runtime_error("F16_CONTROL_FILE not configured.");

        AE_CORE_INFO("Loading inertia from '{}'", inertiaPath);
        const RigidBody::InertialParameters ip = Serialization::LoadInertiaFromDAVEML(inertiaPath);
        AE_CORE_INFO("  mass = {:.4f} kg", ip.mass_kg);

        AE_CORE_INFO("Loading aero    from '{}'", aeroPath);
        auto aero_model = std::make_shared<const Serialization::DAVEMLAeroModel>(aeroPath);
        AE_CORE_INFO("Loading prop    from '{}'", propPath);
        auto prop_model = std::make_shared<const Serialization::DAVEMLPropModel>(propPath);
        AE_CORE_INFO("Loading control from '{}'", controlPath);
        auto ctrl_model = std::make_shared<const Serialization::DAVEMLControlModel>(controlPath);

        // ── 2. Trim solver ────────────────────────────────────────────────────────
        // Apparent weight at the trim point: J2 attraction — the same gravity
        // J2GravityPolicy applies during integration — less the centripetal relief of
        // level flight over the rotating Earth.  Trimming against the static weight
        // would leave the vehicle with excess lift on the first step.
        const double weight_lbf = FlightDynamics::LevelFlightTrimWeight_lbf(
            ip.mass_kg, kLat_deg, kAlt_m, kVelocity_mps, kVelocity_mps);
        FlightDynamics::TrimInputs tin{};
        tin.vt_fps = kTAS_fps;
        tin.alt_ft = kAlt_ft;
        tin.weight_lbf = weight_lbf;
        // Aero damping at the rate F16AeroPolicy will actually see — the transport
        // rate.  Pitch attitude is not known yet and q does not depend on it.
        tin.bodyRates = FlightDynamics::LevelFlightBodyRates(
            kLat_deg, kAlt_m, kVelocity_mps, kVelocity_mps, kHeading_deg, 0.0, kRoll_deg);

        AE_CORE_INFO("Running trim solver ...");
        const double xcg_ft = kXcgFromAC_m / FlightDynamics::TrimSolver::kFt_m;
        FlightDynamics::TrimSolver solver(*aero_model, *prop_model, xcg_ft);
        const FlightDynamics::TrimPoint trim = solver.solve(tin);
        if (!trim.converged) throw std::runtime_error("Trim solver did not converge.");

        AE_CORE_INFO("Trim converged:");
        AE_CORE_INFO("  alpha = {:.4f} deg", trim.alpha_deg);
        AE_CORE_INFO("  el    = {:.4f} deg", trim.el_deg);
        AE_CORE_INFO("  pwr   = {:.4f} %%", trim.pwr_pct);

        // ── 3. Initial state ──────────────────────────────────────────────────────
        RigidBody::Config cfg{};
        cfg.pose.lat_deg = kLat_deg;
        cfg.pose.lon_deg = kLon_deg;
        cfg.pose.alt_m = kAlt_m;
        cfg.pose.azimuth_deg = kHeading_deg;
        cfg.pose.zenith_deg = 90.0 - trim.alpha_deg;
        cfg.pose.roll_deg = kRoll_deg;
        cfg.velocityNED.north_mps = kVelocity_mps;
        cfg.velocityNED.east_mps = kVelocity_mps;
        cfg.velocityNED.down_mps = 0.0;
        // Attitude held fixed against the local-level frame, which tips forward at the
        // transport rate as the vehicle moves over the curved Earth.
        cfg.bodyRates = FlightDynamics::LevelFlightBodyRates(
            kLat_deg, kAlt_m, kVelocity_mps, kVelocity_mps, kHeading_deg, trim.alpha_deg, kRoll_deg);
        cfg.inertialParameters = ip;

        const double theta0 = computeInitialERA(simCfg.startTime);
        const RigidBody::StateD x0 = buildInitialState(cfg, theta0);

        // ── 4. Autopilot commands ─────────────────────────────────────────────────
        // keasCmd computed from US1976 atmosphere so deltaVequiv ≈ 0 at t=0.
        const auto atm_trim = Environment::US1976Atmosphere(kAlt_m);
        constexpr double kRhoSL_kg_m3 = 1.225;
        constexpr double kKt_mps = 0.5144444;
        const double tas_mps = kVelocity_mps * std::numbers::sqrt2;
        const double keas_kt = (tas_mps / kKt_mps) * std::sqrt(atm_trim.rho / kRhoSL_kg_m3);

        Sim::AutopilotCmds cmds;
        cmds.altCmd_ft = kAltCmd_ft;
        cmds.keasCmd_kt = keas_kt;
        cmds.baseChiCmd_deg = kBaseChiCmd_deg;
        cmds.latOffset_ft = 0.0; // dynamic; overridden by latStepOffset_ft
        cmds.latStepTime_s = kLatStepTime_s;
        cmds.latStepOffset_ft = kLatStep_ft;
        cmds.coursePsi_deg = kHeading_deg;

        AE_CORE_INFO("Autopilot commands:");
        AE_CORE_INFO("  altCmd      = {:.0f} ft  (hold)", cmds.altCmd_ft);
        AE_CORE_INFO("  keasCmd     = {:.4f} kt  (computed from US1976 atm at {:.0f} ft)",
            keas_kt, kAlt_ft);
        AE_CORE_INFO("  chiCmd      = {:.1f} deg  (hold NE course)", cmds.baseChiCmd_deg);
        AE_CORE_INFO("  latStep     = {:.0f} ft right of course at t = {:.0f} s",
            cmds.latStepOffset_ft, cmds.latStepTime_s);

        m_Simulator = constructSimulator(ip, trim, aero_model, prop_model, ctrl_model,
            x0, theta0, cmds, kXcgFromAC_m);
    }

    // ── writeInitialSnapshot ──────────────────────────────────────────────────────

    void F16LateralSideStepApplication::writeInitialSnapshot(std::ofstream& csv) const
    {
        const auto s2 = m_Simulator->snapshot2();
        Simulation::Snapshot2Traits::write_row(csv, s2);
        AE_CORE_INFO("t={:.6f} s  alt={:.3f} m  TAS={:.4f} m/s  Mach={:.4f}",
            s2.time, s2.altitudeMsl_m, s2.trueAirspeed_m_s, s2.mach);
    }

    // ── stepAndRecord ─────────────────────────────────────────────────────────────

    auto F16LateralSideStepApplication::stepAndRecord(
        std::ofstream& csv, double h, bool doWrite) const -> StepObservation
    {
        const auto res = m_Simulator->step(h);
        const auto s2 = m_Simulator->snapshot2();

        if (doWrite)
            Simulation::Snapshot2Traits::write_row(csv, s2);

        AE_CORE_TRACE("  t={:.4f} s  alt={:.2f} m  yaw={:.4f} deg",
            s2.time, s2.altitudeMsl_m,
            s2.eulerAngle_rad_Yaw * (180.0 / std::numbers::pi));

        return StepObservation{
            .time_s = s2.time,
            .altitude_m = s2.altitudeMsl_m,
            .speed_mps = s2.feVelocity_m_s.norm(),
            .latitude_rad = s2.latitude_rad,
            .longitude_rad = s2.longitude_rad,
            .converged = res.converged,
            .residual = res.residual_norm,
        };
    }

    // ── logFinalSummary ───────────────────────────────────────────────────────────

    void F16LateralSideStepApplication::logFinalSummary() const
    {
        const auto s2 = m_Simulator->snapshot2();
        constexpr double kToDeg = 180.0 / std::numbers::pi;
        constexpr double kFt = 1.0 / 0.3048;

        // Compute lateral displacement from initial position (for reporting)
        constexpr double kRE_m = 6'371'000.0;
        constexpr double kLat0 = kLat_deg * (std::numbers::pi / 180.0);
        constexpr double kLon0 = kLon_deg * (std::numbers::pi / 180.0);
        constexpr double kPsi0 = kHeading_deg * (std::numbers::pi / 180.0);
        const double dp_N_m = (s2.latitude_rad - kLat0) * kRE_m;
        const double dp_E_m = (s2.longitude_rad - kLon0) * kRE_m * std::cos(kLat0);
        const double lat_dev_ft = (-dp_N_m * std::sin(kPsi0) + dp_E_m * std::cos(kPsi0)) * kFt;

        AE_CORE_INFO("=======================================================");
        AE_CORE_INFO("Simulation complete.");
        AE_CORE_INFO("  Final time      : {:.4f} s", s2.time);
        AE_CORE_INFO("  Final altitude  : {:.2f} m  ({:.1f} ft)",
            s2.altitudeMsl_m, s2.altitudeMsl_m * kFt);
        AE_CORE_INFO("  Final TAS       : {:.4f} m/s  ({:.2f} kt)",
            s2.trueAirspeed_m_s, s2.trueAirspeed_m_s / 0.5144444);
        AE_CORE_INFO("  Final Mach      : {:.6f}", s2.mach);
        AE_CORE_INFO("  Final yaw       : {:.4f} deg", s2.eulerAngle_rad_Yaw * kToDeg);
        AE_CORE_INFO("  Final roll      : {:.4f} deg", s2.eulerAngle_rad_Roll * kToDeg);
        AE_CORE_INFO("  Lateral dev     : {:.1f} ft  (cmd = {:.0f} ft)",
            lat_dev_ft, kLatStep_ft);
        AE_CORE_INFO("Output written to '{}'", getConfig().outputFileName);
        AE_CORE_INFO("=======================================================");
    }

    // ── Private helpers ───────────────────────────────────────────────────────────

    double F16LateralSideStepApplication::computeInitialERA(double t0) noexcept
    {
        constexpr double kOmegaE = 7.2921150e-5;
        return kOmegaE * t0;
    }

    RigidBody::StateD F16LateralSideStepApplication::buildInitialState(
        const RigidBody::Config& cfg, double theta0)
    {
        const auto x0 = RigidBody::BuildInitialStateVector(cfg, theta0);

        RigidBody::StateD state;
        using Layout = RigidBody::StateLayout;

        state.g.p = Eigen::Vector3d(
            x0[Layout::IDX_P], x0[Layout::IDX_P + 1], x0[Layout::IDX_P + 2]);

        const Eigen::Quaterniond q(
            x0[Layout::IDX_Q], x0[Layout::IDX_Q + 1],
            x0[Layout::IDX_Q + 2], x0[Layout::IDX_Q + 3]);
        state.g.q = q.normalized();
        state.g.R = state.g.q.toRotationMatrix();

        state.nu_B << x0[Layout::IDX_W], x0[Layout::IDX_W + 1], x0[Layout::IDX_W + 2],
            x0[Layout::IDX_V], x0[Layout::IDX_V + 1], x0[Layout::IDX_V + 2];

        state.m = x0[Layout::IDX_M];
        return state;
    }

    std::unique_ptr<Sim>
    F16LateralSideStepApplication::constructSimulator(
        const RigidBody::InertialParameters& ip,
        const FlightDynamics::TrimPoint& trim,
        const std::shared_ptr<const Serialization::DAVEMLAeroModel>& aero,
        const std::shared_ptr<const Serialization::DAVEMLPropModel>& prop,
        const std::shared_ptr<const Serialization::DAVEMLControlModel>& ctrl,
        const RigidBody::StateD& x0,
        double theta0,
        const Sim::AutopilotCmds& cmds,
        double xcg_from_ac_m)
    {
        AE_CORE_INFO("Constructing F16LateralSideStep simulator (reuses F16AltitudeChangeSimulator)...");
        AE_CORE_INFO("  xcg_from_ac = {:.4f} m", xcg_from_ac_m);
        return std::make_unique<Sim>(ip, trim, aero, prop, ctrl, x0, theta0,
            cmds, xcg_from_ac_m);
    }

} // namespace Aetherion::Examples::F16LateralSideStep

// ── Entry point ───────────────────────────────────────────────────────────────

namespace Aetherion::Simulation {
    Application* CreateApplication(int argc, char** argv)
    {
        return new Examples::F16LateralSideStep::F16LateralSideStepApplication(argc, argv);
    }
}

#include <Aetherion/Simulation/EntryPoint.h>