DroppedSphereSteadyWind.cpp

// ------------------------------------------------------------------------------
// Project: Aetherion
// Copyright(c) 2025-2026, Onur Tuncer, PhD, Istanbul Technical University
//
// SPDX-License-Identifier: MIT
// License-Filename: LICENSE
// ------------------------------------------------------------------------------
//
// DroppedSphereSteadyWind.cpp  —  NASA TM-2015-218675 Atmospheric Scenario 7
//

#include <Aetherion/Examples/DroppedSphereSteadyWind/DroppedSphereSteadyWindApplication.h>

#include <Aetherion/Simulation/Log.h>
#include <Aetherion/Simulation/Snapshot1.h>
#include <Aetherion/Simulation/MakeSnapshot1.h>
#include <Aetherion/Serialization/LoadConfig.h>
#include <Aetherion/RigidBody/BuildInitialState.h>
#include <Aetherion/RigidBody/StateLayout.h>

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

namespace Aetherion::Examples::DroppedSphereSteadyWind {

    void DroppedSphereSteadyWindApplication::logStartupBanner() const
    {
        AE_CORE_INFO("=======================================================");
        AE_CORE_INFO("Aetherion - NASA TM-2015-218675 Atmospheric Scenario 7");
        AE_CORE_INFO("Dropped sphere - steady east wind - J2 gravity");
        AE_CORE_INFO("=======================================================");
    }

    void DroppedSphereSteadyWindApplication::prepareSimulation() const
    {
        const auto& cfg = getConfig();
        const RigidBody::Config rb_cfg = loadVehicleConfig();
        const double            theta0 = computeInitialERA(cfg.startTime);
        const RigidBody::StateD x0     = buildInitialState(rb_cfg, theta0);

        AE_CORE_INFO("Initial ECI position  = [{:.3f}, {:.3f}, {:.3f}] m",
            x0.g.p.x(), x0.g.p.y(), x0.g.p.z());
        AE_CORE_INFO("Wind NED = [{:.4f}, {:.4f}, {:.4f}] m/s",
            rb_cfg.wind.north_mps, rb_cfg.wind.east_mps, rb_cfg.wind.down_mps);
        AE_CORE_INFO("CD={:.4f}  S_ref={:.6f} m²",
            rb_cfg.aerodynamicParameters.CD, rb_cfg.aerodynamicParameters.S);

        m_Simulator = constructSimulator(rb_cfg, x0, theta0);
    }

    void DroppedSphereSteadyWindApplication::writeInitialSnapshot(std::ofstream& csv) const
    {
        const auto snap = m_Simulator->snapshot();
        Simulation::Snapshot1_WriteCsvRow(csv, snap);
        AE_CORE_INFO("t={:.6f} s  alt={:.3f} m  |v_NED|={:.6f} m/s  TAS={:.4f} m/s",
            snap.time, snap.altitudeMsl_m,
            snap.feVelocity_m_s.norm(), snap.trueAirspeed_m_s);
    }

    auto DroppedSphereSteadyWindApplication::stepAndRecord(
        std::ofstream& csv, double h, bool doWrite) const -> StepObservation
    {
        const auto res  = m_Simulator->step(h);
        const auto snap = m_Simulator->snapshot();
        if (doWrite)
            Simulation::Snapshot1_WriteCsvRow(csv, snap);

        AE_CORE_TRACE("  t={:.6f} s  alt={:.4f} m  v={:.6f} m/s  TAS={:.4f} m/s",
            snap.time, snap.altitudeMsl_m,
            snap.feVelocity_m_s.norm(), snap.trueAirspeed_m_s);

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

    void DroppedSphereSteadyWindApplication::logFinalSummary() const
    {
        const auto snap = m_Simulator->snapshot();
        constexpr double kToDeg = 180.0 / std::numbers::pi;

        AE_CORE_INFO("=======================================================");
        AE_CORE_INFO("Simulation complete.");
        AE_CORE_INFO("  Final time      : {:.6f} s",  snap.time);
        AE_CORE_INFO("  Final altitude  : {:.4f} m",  snap.altitudeMsl_m);
        AE_CORE_INFO("  Final |v_NED|   : {:.6f} m/s", snap.feVelocity_m_s.norm());
        AE_CORE_INFO("  Final TAS       : {:.6f} m/s", snap.trueAirspeed_m_s);
        AE_CORE_INFO("  Final latitude  : {:.8f} deg", snap.latitude_rad  * kToDeg);
        AE_CORE_INFO("  Final longitude : {:.8f} deg", snap.longitude_rad * kToDeg);
        AE_CORE_INFO("Output written to '{}'", getConfig().outputFileName);
        AE_CORE_INFO("=======================================================");
    }

    // =========================================================================

    RigidBody::Config DroppedSphereSteadyWindApplication::loadVehicleConfig() const
    {
        const auto& fname = getConfig().inputFileName;
        if (fname.empty())
            throw std::runtime_error("No --inputFileName specified.");
        AE_CORE_INFO("Loading vehicle config from '{}'", fname);
        return Serialization::LoadConfig(fname);
    }

    double DroppedSphereSteadyWindApplication::computeInitialERA(double t0) noexcept
    {
        return DroppedSphereSteadyWindSimulator::kOmegaEarth_rad_s * t0;
    }

    RigidBody::StateD DroppedSphereSteadyWindApplication::buildInitialState(
        const RigidBody::Config& rb_cfg, double theta0)
    {
        AE_CORE_INFO("Building initial state vector (ECI frame)...");
        const auto x0 = RigidBody::BuildInitialStateVector(rb_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<DroppedSphereSteadyWindSimulator>
        DroppedSphereSteadyWindApplication::constructSimulator(
            const RigidBody::Config& rb_cfg,
            const RigidBody::StateD& x0,
            double                   theta0)
    {
        constexpr double kDeg = std::numbers::pi / 180.0;
        const double lat0 = rb_cfg.pose.lat_deg * kDeg;
        const double lon0 = rb_cfg.pose.lon_deg * kDeg;

        AE_CORE_INFO("Constructing DroppedSphereSteadyWindSimulator...");
        return std::make_unique<DroppedSphereSteadyWindSimulator>(
            rb_cfg.inertialParameters,
            rb_cfg.aerodynamicParameters,
            rb_cfg.wind,
            lat0, lon0,
            x0, theta0);
    }

} // namespace Aetherion::Examples::DroppedSphereSteadyWind

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

#include <Aetherion/Simulation/EntryPoint.h>