// flight.cxx -- a general interface to the various flight models
//
// Written by Curtis Olson, started May 1997.

// SPDX-FileCopyrightText: (C) 1997  Curtis L. Olson - http://www.flightgear.org/~curt
// SPDX-License-Identifier: GPL-2.0-or-later

#include "config.h"
#include "simgear/math/SGGeoc.hxx"
#include "simgear/math/SGVec2.hxx"
#include "simgear/structure/exception.hxx"

#include "flight.hxx"

#include <cmath>
#include <simgear/constants.h>
#include <simgear/debug/logstream.hxx>
#include <simgear/io/iochannel.hxx>
#include <simgear/scene/material/mat.hxx>
#include <simgear/timing/timestamp.hxx>

#include <FDM/groundcache.hxx>
#include <Main/fg_props.hxx>
#include <Main/globals.hxx>
#include <Scenery/scenery.hxx>

namespace {

/**
 * A little helper class to update the track if
 * the position has changed. In the constructor,
 * create a copy of the current position and store
 * references to the position object and the track
 * variable to update.
 * The destructor, called at TrackComputer's end of
 * life/visibility, computes the track if the
 * position has changed.
 */
class TrackComputer
{
public:
    inline TrackComputer(double& track, double& path, const SGGeod& position) : _track(track),
                                                                                _path(path),
                                                                                _position(position),
                                                                                _prevPosition(position)
    {
    }

    inline ~TrackComputer()
    {
        if (_prevPosition == _position) return;
        //        _track = SGGeodesy::courseDeg( _prevPosition, _position );
        double d = .0;
        double distance = .0;
        if (SGGeodesy::inverse(_prevPosition, _position, _track, d, distance)) {
            d = _position.getElevationM() - _prevPosition.getElevationM();
            _path = atan2(d, distance) * SGD_RADIANS_TO_DEGREES;
        }
    }

private:
    double& _track;
    double& _path;
    const SGGeod& _position;
    const SGGeod _prevPosition;
};

} // namespace

static inline void assign(double* ptr, const SGVec3d& vec)
{
    ptr[0] = vec[0];
    ptr[1] = vec[1];
    ptr[2] = vec[2];
}

static bool isNANArray(const double* d, size_t count)
{
    for (int i = 0; i < count; ++i) {
        if (std::isnan(d[i])) {
            return true;
        }
    }

    return false;
}

// Constructor
FGInterface::FGInterface()
{
    _setup();
}

FGInterface::FGInterface(double dt)
{
    _setup();
}

// Destructor
FGInterface::~FGInterface() = default;

int FGInterface::_calc_multiloop(double dt)
{
    if (dt == 0.0) {
        return 0; // paused
    }

    // this method is now obsolete - multiloop is handled by
    // SGSubsystemGroup; the FDM group operates with a fixed time interval
    // (defined by /sim/model-hz), so at this level we always want to run
    // exactly one FDM iteration
    return 1;
}


/**
 * Set default values for the state of the FDM.
 *
 * This method is invoked by the constructors.
 */
void FGInterface::_setup()
{
    inited = false;
    bound = false;

    _state.d_cg_rp_body_v = SGVec3d::zeros();
    _state.v_dot_local_v = SGVec3d::zeros();
    _state.v_dot_body_v = SGVec3d::zeros();
    _state.a_cg_body_v = SGVec3d::zeros();
    _state.a_pilot_body_v = SGVec3d::zeros();
    _state.n_cg_body_v = SGVec3d::zeros();
    _state.v_local_v = SGVec3d::zeros();
    _state.v_local_rel_ground_v = SGVec3d::zeros();
    _state.v_local_airmass_v = SGVec3d::zeros();
    _state.v_body_v = SGVec3d::zeros();
    _state.omega_body_v = SGVec3d::zeros();
    _state.euler_rates_v = SGVec3d::zeros();
    _state.geocentric_rates_v = SGVec3d::zeros();
    _state.geodetic_position_v = SGGeod::fromRadM(0, 0, 0);
    _state.cartesian_position_v = SGVec3d::fromGeod(_state.geodetic_position_v);
    _state.geocentric_position_v = SGGeoc::fromCart(_state.cartesian_position_v);
    _state.euler_angles_v = SGVec3d::zeros();

    _state.nlf = 0;
    _state.v_rel_wind = _state.v_true_kts = 0;
    _state.v_ground_speed = _state.v_equiv_kts = 0;
    _state.v_calibrated_kts = 0;
    _state.alpha = _state.beta = 0;
    _state.gamma_vert_rad = 0;
    _state.density = _state.mach_number = 0;
    _state.static_pressure = _state.total_pressure = 0;
    _state.dynamic_pressure = 0;
    _state.static_temperature = _state.total_temperature = 0;
    _state.sea_level_radius = _state.earth_position_angle = 0;
    _state.runway_altitude = 0;
    _state.climb_rate = 0;
    _state.altitude_agl = 0;
    _state.track = 0;
}

void FGInterface::init() {}

/**
 * Initialize the state of the FDM.
 *
 * Subclasses of FGInterface may do their own, additional initialization,
 * but there is some that is common to all.  Normally, they should call
 * this before they begin their own init to make sure the basic structures
 * are set up properly.
 */
void FGInterface::common_init()
{
    SG_LOG(SG_FLIGHT, SG_INFO, "Start common FDM init");

    set_inited(true);

    ground_cache.set_cache_time_offset(globals->get_sim_time_sec());

    // Set initial position
    SG_LOG(SG_FLIGHT, SG_INFO, "...initializing position...");
    double lon = fgGetDouble("/sim/presets/longitude-deg") * SGD_DEGREES_TO_RADIANS;
    double lat = fgGetDouble("/sim/presets/latitude-deg") * SGD_DEGREES_TO_RADIANS;
    double alt_ft = fgGetDouble("/sim/presets/altitude-ft");
    double alt_m = alt_ft * SG_FEET_TO_METER;
    set_Longitude(lon);
    set_Latitude(lat);
    SG_LOG(SG_FLIGHT, SG_INFO, "Checking for lon = " << lon * SGD_RADIANS_TO_DEGREES << "deg, lat = " << lat * SGD_RADIANS_TO_DEGREES << "deg, alt = " << alt_ft << "ft");

    double ground_elev_m = get_groundlevel_m(lat, lon, alt_m);
    double ground_elev_ft = ground_elev_m * SG_METER_TO_FEET;
    _set_Runway_altitude(ground_elev_ft);

    // Set aircraft altitude
    if (fgGetBool("/sim/presets/onground") || alt_ft < ground_elev_ft) {
        fgSetDouble("/position/altitude-ft", ground_elev_ft + 0.1);
        set_Altitude(ground_elev_ft + 0.1);
    } else {
        set_Altitude(alt_ft);
    }

    // Set ground elevation
    SG_LOG(SG_FLIGHT, SG_INFO,
           "...initializing ground elevation to " << ground_elev_ft
                                                  << "ft...");

    // Set sea-level radius
    SG_LOG(SG_FLIGHT, SG_INFO, "...initializing sea-level radius...");
    SG_LOG(SG_FLIGHT, SG_INFO, " lat = " << fgGetDouble("/sim/presets/latitude-deg") << " alt = " << get_Altitude());
    double slr = SGGeodesy::SGGeodToSeaLevelRadius(_state.geodetic_position_v);
    _set_Sea_level_radius(slr * SG_METER_TO_FEET);

    // Set initial Euler angles
    SG_LOG(SG_FLIGHT, SG_INFO, "...initializing Euler angles...");
    set_Euler_Angles(fgGetDouble("/sim/presets/roll-deg") * SGD_DEGREES_TO_RADIANS,
                     fgGetDouble("/sim/presets/pitch-deg") * SGD_DEGREES_TO_RADIANS,
                     fgGetDouble("/sim/presets/heading-deg") * SGD_DEGREES_TO_RADIANS);

    // Set initial velocities
    SG_LOG(SG_FLIGHT, SG_INFO, "...initializing velocities...");
    if (!fgHasNode("/sim/presets/speed-set")) {
        set_V_calibrated_kts(0.0);
    } else {
        const std::string speedset = fgGetString("/sim/presets/speed-set");
        if (speedset == "knots" || speedset == "KNOTS") {
            set_V_calibrated_kts(fgGetDouble("/sim/presets/airspeed-kt"));
        } else if (speedset == "mach" || speedset == "MACH") {
            set_Mach_number(fgGetDouble("/sim/presets/mach"));
        } else if (speedset == "UVW" || speedset == "uvw") {
            set_Velocities_Body(
                fgGetDouble("/sim/presets/uBody-fps"),
                fgGetDouble("/sim/presets/vBody-fps"),
                fgGetDouble("/sim/presets/wBody-fps"));
        } else if (speedset == "NED" || speedset == "ned") {
            set_Velocities_Local(
                fgGetDouble("/sim/presets/speed-north-fps"),
                fgGetDouble("/sim/presets/speed-east-fps"),
                fgGetDouble("/sim/presets/speed-down-fps"));
        } else {
            SG_LOG(SG_FLIGHT, SG_ALERT,
                   "Unrecognized value for /sim/presets/speed-set: "
                       << speedset);
            set_V_calibrated_kts(0.0);
        }
    }

    if (fgHasNode("/sim/presets/glideslope-deg"))
        set_Gamma_vert_rad(fgGetDouble("/sim/presets/glideslope-deg") * SGD_DEGREES_TO_RADIANS);
    else if (fgHasNode("/sim/presets/speed-set") &&
             fgHasNode("/sim/presets/vertical-speed-fps")) {
        set_Climb_Rate(fgGetDouble("/sim/presets/vertical-speed-fps"));
    }

    reset_wake_group();

    SG_LOG(SG_FLIGHT, SG_INFO, "End common FDM init");
}

void FGInterface::property_associations(
    std::function<void(const std::string& from, const std::string& to)> fn)
{
    /* Do nothing by default. */
}


/**
 * Bind getters and setters to properties.
 *
 * The bind() method will be invoked after init().  Note that unlike
 * the usual implementations of FGSubsystem::bind(), this method does
 * not automatically pick up existing values for the properties at
 * bind time; instead, all values are set explicitly in the init()
 * method.
 */
void FGInterface::bind()
{
    bound = true;

    _tiedProperties.setRoot(globals->get_props());
    // Aircraft position
    _tiedProperties.Tie("/position/latitude-deg", this,
                        &FGInterface::get_Latitude_deg,
                        &FGInterface::set_Latitude_deg,
                        false);
    fgSetArchivable("/position/latitude-deg");
    _tiedProperties.Tie("/position/longitude-deg", this,
                        &FGInterface::get_Longitude_deg,
                        &FGInterface::set_Longitude_deg,
                        false);
    fgSetArchivable("/position/longitude-deg");
    _tiedProperties.Tie("/position/altitude-ft", this,
                        &FGInterface::get_Altitude,
                        &FGInterface::set_Altitude,
                        false);
    fgSetArchivable("/position/altitude-ft");
    _tiedProperties.Tie("/position/altitude-agl-ft", this,
                        &FGInterface::get_Altitude_AGL, &FGInterface::set_AltitudeAGL, false);
    fgSetArchivable("/position/ground-elev-ft");
    _tiedProperties.Tie("/position/ground-elev-ft", this,
                        &FGInterface::get_Runway_altitude); // read-only
    fgSetArchivable("/position/ground-elev-m");
    _tiedProperties.Tie("/position/ground-elev-m", this,
                        &FGInterface::get_Runway_altitude_m); // read-only
    _tiedProperties.Tie("/environment/ground-elevation-m", this,
                        &FGInterface::get_Runway_altitude_m); // read-only
    fgSetArchivable("/position/sea-level-radius-ft");
    _tiedProperties.Tie("/position/sea-level-radius-ft", this,
                        &FGInterface::get_Sea_level_radius,
                        &FGInterface::_set_Sea_level_radius, false);

    // Orientation
    _tiedProperties.Tie("/orientation/roll-deg", this,
                        &FGInterface::get_Phi_deg,
                        &FGInterface::set_Phi_deg, false);
    fgSetArchivable("/orientation/roll-deg");
    _tiedProperties.Tie("/orientation/pitch-deg", this,
                        &FGInterface::get_Theta_deg,
                        &FGInterface::set_Theta_deg, false);
    fgSetArchivable("/orientation/pitch-deg");
    _tiedProperties.Tie("/orientation/heading-deg", this,
                        &FGInterface::get_Psi_deg,
                        &FGInterface::set_Psi_deg, false);
    fgSetArchivable("/orientation/heading-deg");
    _tiedProperties.Tie("/orientation/true-heading-deg", this,
                        &FGInterface::get_Psi_deg,
                        &FGInterface::set_Psi_deg, false);
    fgSetArchivable("/orientation/true-heading-deg");
    _tiedProperties.Tie("/orientation/track-deg", this,
                        &FGInterface::get_Track); // read-only
    _tiedProperties.Tie("/orientation/path-deg", this,
                        &FGInterface::get_Path); // read-only

    // Body-axis "euler rates" (rotation speed, but in a funny
    // representation).
    _tiedProperties.Tie("/orientation/roll-rate-degps", this,
                        &FGInterface::get_Phi_dot_degps,
                        &FGInterface::set_Phi_dot_degps, false);
    _tiedProperties.Tie("/orientation/pitch-rate-degps", this,
                        &FGInterface::get_Theta_dot_degps,
                        &FGInterface::set_Theta_dot_degps, false);
    _tiedProperties.Tie("/orientation/yaw-rate-degps", this,
                        &FGInterface::get_Psi_dot_degps,
                        &FGInterface::set_Psi_dot_degps, false);

    _tiedProperties.Tie("/orientation/p-body", this, &FGInterface::get_P_body); // read-only
    _tiedProperties.Tie("/orientation/q-body", this, &FGInterface::get_Q_body); // read-only
    _tiedProperties.Tie("/orientation/r-body", this, &FGInterface::get_R_body); // read-only

    // Ground speed knots
    _tiedProperties.Tie("/velocities/groundspeed-kt", this,
                        &FGInterface::get_V_ground_speed_kt,
                        &FGInterface::set_V_ground_speed_kt); // read-only

    // Calibrated airspeed
    _tiedProperties.Tie("/velocities/airspeed-kt", this,
                        &FGInterface::get_V_calibrated_kts,
                        &FGInterface::set_V_calibrated_kts,
                        false);

    _tiedProperties.Tie("/velocities/equivalent-kt", this,
                        &FGInterface::get_V_equiv_kts); // read-only

    // Mach number
    _tiedProperties.Tie("/velocities/mach", this,
                        &FGInterface::get_Mach_number,
                        &FGInterface::set_Mach_number,
                        false);

    // Local velocities
    //   _tiedProperties.Tie("/velocities/speed-north-fps", this,
    //                       &FGInterface::get_V_north,
    //                       &FGInterface::set_V_north);
    //   fgSetArchivable("/velocities/speed-north-fps");
    //   _tiedProperties.Tie("/velocities/speed-east-fps", this,
    //                       &FGInterface::get_V_east,
    //                       &FGInterface::set_V_east);
    //   fgSetArchivable("/velocities/speed-east-fps");
    //   _tiedProperties.Tie("/velocities/speed-down-fps", this,
    //                       &FGInterface::get_V_down,
    //                       &FGInterface::set_V_down);
    //   fgSetArchivable("/velocities/speed-down-fps");

    // FIXME: Temporarily read-only, until the
    // incompatibilities between JSBSim and
    // LaRCSim are fixed (LaRCSim adds the
    // earth's rotation to the east velocity).
    _tiedProperties.Tie("/velocities/speed-north-fps", this,
                        &FGInterface::get_V_north, &FGInterface::set_V_north, false);
    _tiedProperties.Tie("/velocities/speed-east-fps", this,
                        &FGInterface::get_V_east, &FGInterface::set_V_east, false);
    _tiedProperties.Tie("/velocities/speed-down-fps", this,
                        &FGInterface::get_V_down, &FGInterface::set_V_down, false);

    _tiedProperties.Tie("/velocities/north-relground-fps", this,
                        &FGInterface::get_V_north_rel_ground); // read-only
    _tiedProperties.Tie("/velocities/east-relground-fps", this,
                        &FGInterface::get_V_east_rel_ground); // read-only
    _tiedProperties.Tie("/velocities/down-relground-fps", this,
                        &FGInterface::get_V_down_rel_ground); // read-only

    // ECEF velocity in body axis
    // FIXME: temporarily archivable, until the NED problem is fixed.
    _tiedProperties.Tie("/velocities/uBody-fps", this,
                        &FGInterface::get_uBody,
                        &FGInterface::set_uBody,
                        false);
    fgSetArchivable("/velocities/uBody-fps");
    _tiedProperties.Tie("/velocities/vBody-fps", this,
                        &FGInterface::get_vBody,
                        &FGInterface::set_vBody,
                        false);
    fgSetArchivable("/velocities/vBody-fps");
    _tiedProperties.Tie("/velocities/wBody-fps", this,
                        &FGInterface::get_wBody,
                        &FGInterface::set_wBody,
                        false);
    fgSetArchivable("/velocities/wBody-fps");

    // Climb and slip (read-only)
    _tiedProperties.Tie("/velocities/vertical-speed-fps", this,
                        &FGInterface::get_Climb_Rate,
                        &FGInterface::set_Climb_Rate, false);
    _tiedProperties.Tie("/velocities/glideslope", this,
                        &FGInterface::get_Gamma_vert_rad,
                        &FGInterface::set_Gamma_vert_rad, false);
    _tiedProperties.Tie("/orientation/side-slip-rad", this,
                        &FGInterface::get_Beta, &FGInterface::_set_Beta, false);
    _tiedProperties.Tie("/orientation/side-slip-deg", this,
                        &FGInterface::get_Beta_deg); // read-only
    _tiedProperties.Tie("/orientation/alpha-deg", this,
                        &FGInterface::get_Alpha_deg, &FGInterface::set_Alpha_deg, false);
    _tiedProperties.Tie("/accelerations/nlf", this,
                        &FGInterface::get_Nlf); // read-only

    // NED accelerations
    _tiedProperties.Tie("/accelerations/ned/north-accel-fps_sec",
                        this, &FGInterface::get_V_dot_north); // read-only
    _tiedProperties.Tie("/accelerations/ned/east-accel-fps_sec",
                        this, &FGInterface::get_V_dot_east); // read-only
    _tiedProperties.Tie("/accelerations/ned/down-accel-fps_sec",
                        this, &FGInterface::get_V_dot_down); // read-only

    // Pilot accelerations
    _tiedProperties.Tie("/accelerations/pilot/x-accel-fps_sec",
                        this, &FGInterface::get_A_X_pilot, &FGInterface::set_A_X_pilot, false);
    _tiedProperties.Tie("/accelerations/pilot/y-accel-fps_sec",
                        this, &FGInterface::get_A_Y_pilot, &FGInterface::set_A_Y_pilot, false);
    _tiedProperties.Tie("/accelerations/pilot/z-accel-fps_sec",
                        this, &FGInterface::get_A_Z_pilot, &FGInterface::set_A_Z_pilot, false);

    _tiedProperties.Tie("/accelerations/n-z-cg-fps_sec",
                        this, &FGInterface::get_N_Z_cg); // read-only
}


/**
 * Unbind any properties bound to this FDM.
 *
 * This method allows the FDM to release properties so that a new
 * FDM can bind them instead.
 */
void FGInterface::unbind()
{
    _tiedProperties.Untie();
    bound = false;
}

/**
 * Update the state of the FDM (i.e. run the equations of motion).
 */
void FGInterface::update(double dt)
{
    SG_LOG(SG_FLIGHT, SG_ALERT, "dummy update() ... SHOULDN'T BE CALLED!");
}

bool FGInterface::readState(SGIOChannel* io)
{
    FlightState buf;
    int length = sizeof(FlightState);

    if (io->get_type() == sgFileType) {
        if (io->read((char*)(&buf), length) == length) {
            SG_LOG(SG_IO, SG_DEBUG, "Success reading data.");
        } else {
            return false;
        }
    } else {
        while (io->read((char*)(&buf), length) == length) {
            SG_LOG(SG_IO, SG_DEBUG, "Success reading data.");
        }
    }

    _state = buf; // copy the read state over
    return true;
}

bool FGInterface::writeState(SGIOChannel* io)
{
    if (!bound || !inited) {
        return false;
    }

    int length = sizeof(FlightState);
    if (!io->write((char*)(&_state), length)) {
        SG_LOG(SG_IO, SG_ALERT, "Error writing data.");
        return false;
    }

    return true;
}

void FGInterface::_set_Altitude(double altitude)
{
    if (std::isnan(altitude)) {
        throw sg_range_exception("FGInterface::_set_Altitude: invalid value");
    }

    auto newGeod = _state.geodetic_position_v;
    newGeod.setElevationFt(altitude);
    _updatePosition(newGeod);
}

void FGInterface::_set_Geodetic_Position(double lat, double lon, double alt)
{
    _updatePosition(SGGeod::fromRadFt(lon, lat, alt));
}

void FGInterface::_set_Geocentric_Position(double lat, double lon, double rad)
{
    _updatePosition(SGGeoc::fromRadFt(lon, lat, rad));
}

void FGInterface::_updatePositionM(const SGVec3d& cartPos)
{
    if (isNaN(cartPos)) {
        throw sg_range_exception("FGInterface::_updatePositionM: passed invalid position");
    }

    const SGGeod geod = SGGeod::fromCart(cartPos);
    if (!geod.isValid()) {
        throw sg_range_exception("FGInterface::_updatePositionM: position is not convertible to valid SGGeod");
    }

    TrackComputer tracker(_state.track, _state.path, _state.geodetic_position_v);
    _state.cartesian_position_v = cartPos;
    _state.geodetic_position_v = geod;
    _state.geocentric_position_v = SGGeoc::fromCart(_state.cartesian_position_v);
    _set_Sea_level_radius(SGGeodesy::SGGeodToSeaLevelRadius(_state.geodetic_position_v) * SG_METER_TO_FEET);
    _update_ground_elev_at_pos();
}


void FGInterface::_updatePosition(const SGGeod& geod)
{
    if (!geod.isValid()) {
        throw sg_range_exception("FGInterface::_updatePosition: passed invalid position");
    }

    TrackComputer tracker(_state.track, _state.path, _state.geodetic_position_v);
    _state.geodetic_position_v = geod;
    _state.cartesian_position_v = SGVec3d::fromGeod(_state.geodetic_position_v);
    _state.geocentric_position_v = SGGeoc::fromCart(_state.cartesian_position_v);

    _set_Sea_level_radius(SGGeodesy::SGGeodToSeaLevelRadius(_state.geodetic_position_v) * SG_METER_TO_FEET);
    _update_ground_elev_at_pos();
}


void FGInterface::_updatePosition(const SGGeoc& geoc)
{
    const SGGeod geod = SGGeod::fromGeoc(geoc);
    if (!geod.isValid()) {
        throw sg_range_exception("FGInterface::_updatePositionM: position is not convertible to valid SGGeod");
    }

    TrackComputer tracker(_state.track, _state.path, _state.geodetic_position_v);
    _state.geocentric_position_v = geoc;
    _state.cartesian_position_v = SGVec3d::fromGeoc(_state.geocentric_position_v);
    _state.geodetic_position_v = geod;

    _set_Sea_level_radius(SGGeodesy::SGGeodToSeaLevelRadius(_state.geodetic_position_v) * SG_METER_TO_FEET);
    _update_ground_elev_at_pos();
}


void FGInterface::_updateGeodeticPosition(double lat, double lon, double alt)
{
    _updatePosition(SGGeod::fromRadFt(lon, lat, alt));
}


void FGInterface::_updateGeocentricPosition(double lat, double lon,
                                            double alt)
{
    _updatePosition(SGGeoc::fromRadFt(lon, lat, get_Sea_level_radius() + alt));
}

void FGInterface::_update_ground_elev_at_pos(void)
{
    double groundlevel_m = get_groundlevel_m(_state.geodetic_position_v);
    _set_Runway_altitude(groundlevel_m * SG_METER_TO_FEET);
}

// Positions
void FGInterface::set_Latitude(double lat)
{
    if (std::isnan(lat)) {
        throw sg_range_exception("FGInterface::set_Latitude: passed NaN");
    }
    _state.geodetic_position_v.setLatitudeRad(lat);
}

void FGInterface::set_Longitude(double lon)
{
    if (std::isnan(lon)) {
        throw sg_range_exception("FGInterface::set_Longitude: passed NaN");
    }
    _state.geodetic_position_v.setLongitudeRad(lon);
}

void FGInterface::set_Altitude(double alt)
{
    if (std::isnan(alt)) {
        throw sg_range_exception("FGInterface::set_Altitude: passed NaN");
    }
    _state.geodetic_position_v.setElevationFt(alt);
}

void FGInterface::set_AltitudeAGL(double altagl)
{
    if (std::isnan(altagl)) {
        throw sg_range_exception("FGInterface::set_AltitudeAGL: passed NaN");
    }
    _state.altitude_agl = altagl;
}

// Velocities
void FGInterface::set_V_calibrated_kts(double vc)
{
    _state.v_calibrated_kts = vc;
}

void FGInterface::set_Mach_number(double mach)
{
    _state.mach_number = mach;
}

void FGInterface::set_Velocities_Local(double north,
                                       double east,
                                       double down)
{
    _state.v_local_v[0] = north;
    _state.v_local_v[1] = east;
    _state.v_local_v[2] = down;
}

void FGInterface::set_Velocities_Body(double u,
                                      double v,
                                      double w)
{
    _state.v_body_v[0] = u;
    _state.v_body_v[1] = v;
    _state.v_body_v[2] = w;
}

void FGInterface::_set_Euler_Angles(double phi,
                                    double theta,
                                    double psi)
{
    if (std::isnan(phi) || std::isnan(theta) || std::isnan(psi)) {
        throw sg_range_exception("FGInterface::set_Euler_Angles: passed NaNs");
    }

    _state.euler_angles_v[0] = phi;
    _state.euler_angles_v[1] = theta;
    _state.euler_angles_v[2] = psi;
}

// Euler angles
void FGInterface::set_Euler_Angles(double phi,
                                   double theta,
                                   double psi)
{
    _set_Euler_Angles(phi, theta, psi);
}

// Flight Path
void FGInterface::set_Climb_Rate(double roc)
{
    _state.climb_rate = roc;
}

void FGInterface::set_Gamma_vert_rad(double gamma)
{
    _state.gamma_vert_rad = gamma;
}

void FGInterface::set_Static_pressure(double p) { _state.static_pressure = p; }
void FGInterface::set_Static_temperature(double T) { _state.static_temperature = T; }
void FGInterface::set_Density(double rho) { _state.density = rho; }

void FGInterface::set_Velocities_Local_Airmass(double wnorth,
                                               double weast,
                                               double wdown)
{
    _state.v_local_airmass_v[0] = wnorth;
    _state.v_local_airmass_v[1] = weast;
    _state.v_local_airmass_v[2] = wdown;
}


void FGInterface::_busdump(void)
{
    SG_LOG(SG_FLIGHT, SG_INFO, "d_cg_rp_body_v: " << _state.d_cg_rp_body_v);
    SG_LOG(SG_FLIGHT, SG_INFO, "v_dot_local_v: " << _state.v_dot_local_v);
    SG_LOG(SG_FLIGHT, SG_INFO, "v_dot_body_v: " << _state.v_dot_body_v);
    SG_LOG(SG_FLIGHT, SG_INFO, "a_cg_body_v: " << _state.a_cg_body_v);
    SG_LOG(SG_FLIGHT, SG_INFO, "a_pilot_body_v: " << _state.a_pilot_body_v);
    SG_LOG(SG_FLIGHT, SG_INFO, "n_cg_body_v: " << _state.n_cg_body_v);
    SG_LOG(SG_FLIGHT, SG_INFO, "v_local_v: " << _state.v_local_v);
    SG_LOG(SG_FLIGHT, SG_INFO, "v_local_rel_ground_v: " << _state.v_local_rel_ground_v);
    SG_LOG(SG_FLIGHT, SG_INFO, "v_local_airmass_v: " << _state.v_local_airmass_v);
    SG_LOG(SG_FLIGHT, SG_INFO, "v_body_v: " << _state.v_body_v);
    SG_LOG(SG_FLIGHT, SG_INFO, "omega_body_v: " << _state.omega_body_v);
    SG_LOG(SG_FLIGHT, SG_INFO, "euler_rates_v: " << _state.euler_rates_v);
    SG_LOG(SG_FLIGHT, SG_INFO, "geocentric_rates_v: " << _state.geocentric_rates_v);
    SG_LOG(SG_FLIGHT, SG_INFO, "geocentric_position_v: " << _state.geocentric_position_v);
    SG_LOG(SG_FLIGHT, SG_INFO, "geodetic_position_v: " << _state.geodetic_position_v);
    SG_LOG(SG_FLIGHT, SG_INFO, "euler_angles_v: " << _state.euler_angles_v);

    SG_LOG(SG_FLIGHT, SG_INFO, "nlf: " << _state.nlf);
    SG_LOG(SG_FLIGHT, SG_INFO, "v_rel_wind: " << _state.v_rel_wind);
    SG_LOG(SG_FLIGHT, SG_INFO, "v_true_kts: " << _state.v_true_kts);
    SG_LOG(SG_FLIGHT, SG_INFO, "v_ground_speed: " << _state.v_ground_speed);
    SG_LOG(SG_FLIGHT, SG_INFO, "v_equiv_kts: " << _state.v_equiv_kts);
    SG_LOG(SG_FLIGHT, SG_INFO, "v_calibrated_kts: " << _state.v_calibrated_kts);
    SG_LOG(SG_FLIGHT, SG_INFO, "alpha: " << _state.alpha);
    SG_LOG(SG_FLIGHT, SG_INFO, "beta: " << _state.beta);
    SG_LOG(SG_FLIGHT, SG_INFO, "gamma_vert_rad: " << _state.gamma_vert_rad);
    SG_LOG(SG_FLIGHT, SG_INFO, "density: " << _state.density);
    SG_LOG(SG_FLIGHT, SG_INFO, "mach_number: " << _state.mach_number);
    SG_LOG(SG_FLIGHT, SG_INFO, "static_pressure: " << _state.static_pressure);
    SG_LOG(SG_FLIGHT, SG_INFO, "total_pressure: " << _state.total_pressure);
    SG_LOG(SG_FLIGHT, SG_INFO, "dynamic_pressure: " << _state.dynamic_pressure);
    SG_LOG(SG_FLIGHT, SG_INFO, "static_temperature: " << _state.static_temperature);
    SG_LOG(SG_FLIGHT, SG_INFO, "total_temperature: " << _state.total_temperature);
    SG_LOG(SG_FLIGHT, SG_INFO, "sea_level_radius: " << _state.sea_level_radius);
    SG_LOG(SG_FLIGHT, SG_INFO, "earth_position_angle: " << _state.earth_position_angle);
    SG_LOG(SG_FLIGHT, SG_INFO, "runway_altitude: " << _state.runway_altitude);
    SG_LOG(SG_FLIGHT, SG_INFO, "climb_rate: " << _state.climb_rate);
    SG_LOG(SG_FLIGHT, SG_INFO, "altitude_agl: " << _state.altitude_agl);
}

bool FGInterface::prepare_ground_cache_m(double startSimTime, double endSimTime,
                                         const double pt[3], double rad)
{
    if (isNANArray(pt, 3)) {
        throw sg_range_exception("FGInterface::prepare_ground_cache_m: invalid WGS84 position");
    }

    return ground_cache.prepare_ground_cache(startSimTime, endSimTime,
                                             SGVec3d(pt), rad);
}

bool FGInterface::prepare_ground_cache_ft(double startSimTime, double endSimTime,
                                          const double pt[3], double rad)
{
    if (isNANArray(pt, 3)) {
        throw sg_range_exception("FGInterface::prepare_ground_cache_ft: invalid WGS84 position");
    }

    // Convert units and do the real work.
    SGVec3d pt_ft = SG_FEET_TO_METER * SGVec3d(pt);
    return ground_cache.prepare_ground_cache(startSimTime, endSimTime,
                                             pt_ft, rad * SG_FEET_TO_METER);
}

bool FGInterface::is_valid_m(double* ref_time, double pt[3], double* rad)
{
    SGVec3d _pt;
    bool valid = ground_cache.is_valid(*ref_time, _pt, *rad);
    assign(pt, _pt);
    return valid;
}

bool FGInterface::is_valid_ft(double* ref_time, double pt[3], double* rad)
{
    // Convert units and do the real work.
    SGVec3d _pt;
    bool found_ground = ground_cache.is_valid(*ref_time, _pt, *rad);
    assign(pt, SG_METER_TO_FEET * _pt);
    *rad *= SG_METER_TO_FEET;
    return found_ground;
}

double
FGInterface::get_cat_m(double t, const double pt[3],
                       double end[2][3], double vel[2][3])
{
    SGVec3d _end[2], _vel[2];
    double dist = ground_cache.get_cat(t, SGVec3d(pt), _end, _vel);
    for (int k = 0; k < 2; ++k) {
        assign(end[k], _end[k]);
        assign(vel[k], _vel[k]);
    }
    return dist;
}

double
FGInterface::get_cat_ft(double t, const double pt[3],
                        double end[2][3], double vel[2][3])
{
    // Convert units and do the real work.
    SGVec3d pt_m = SG_FEET_TO_METER * SGVec3d(pt);
    SGVec3d _end[2], _vel[2];
    double dist = ground_cache.get_cat(t, pt_m, _end, _vel);
    for (int k = 0; k < 2; ++k) {
        assign(end[k], SG_METER_TO_FEET * _end[k]);
        assign(vel[k], SG_METER_TO_FEET * _vel[k]);
    }
    return dist * SG_METER_TO_FEET;
}

bool FGInterface::get_body_m(double t, simgear::BVHNode::Id id,
                             double bodyToWorld[16], double linearVel[3],
                             double angularVel[3])
{
    SGMatrixd _bodyToWorld;
    SGVec3d _linearVel, _angularVel;
    bool _foundID = ground_cache.get_body(t, _bodyToWorld, _linearVel, _angularVel, id);

    assign(linearVel, _linearVel);
    assign(angularVel, _angularVel);
    for (unsigned int i = 0; i < 16; ++i)
        bodyToWorld[i] = _bodyToWorld.data()[i];

    if (!_foundID)
        return false;

    return true;
}

bool FGInterface::get_agl_m(double t, const double pt[3], double max_altoff,
                            double contact[3], double normal[3],
                            double linearVel[3], double angularVel[3],
                            simgear::BVHMaterial const*& material, simgear::BVHNode::Id& id)
{
    SGVec3d pt_m = SGVec3d(pt) - max_altoff * ground_cache.get_down();
    SGVec3d _contact, _normal, _linearVel, _angularVel;
    material = 0;
    bool ret = ground_cache.get_agl(t, pt_m, _contact, _normal, _linearVel,
                                    _angularVel, id, material);
    // correct the linear velocity, since the line intersector delivers
    // values for the start point and the get_agl function should
    // traditionally deliver for the contact point
    _linearVel += cross(_angularVel, _contact - pt_m);

    assign(contact, _contact);
    assign(normal, _normal);
    assign(linearVel, _linearVel);
    assign(angularVel, _angularVel);
    return ret;
}

bool FGInterface::get_agl_ft(double t, const double pt[3], double max_altoff,
                             double contact[3], double normal[3],
                             double linearVel[3], double angularVel[3],
                             simgear::BVHMaterial const*& material, simgear::BVHNode::Id& id)
{
    // Convert units and do the real work.
    SGVec3d pt_m = SGVec3d(pt) - max_altoff * ground_cache.get_down();
    pt_m *= SG_FEET_TO_METER;
    SGVec3d _contact, _normal, _linearVel, _angularVel;
    material = 0;
    bool ret = ground_cache.get_agl(t, pt_m, _contact, _normal, _linearVel,
                                    _angularVel, id, material);
    // correct the linear velocity, since the line intersector delivers
    // values for the start point and the get_agl function should
    // traditionally deliver for the contact point
    _linearVel += cross(_angularVel, _contact - pt_m);

    // Convert units back ...
    assign(contact, SG_METER_TO_FEET * _contact);
    assign(normal, _normal);
    assign(linearVel, SG_METER_TO_FEET * _linearVel);
    assign(angularVel, _angularVel);
    return ret;
}

bool FGInterface::get_nearest_m(double t, const double pt[3], double maxDist,
                                double contact[3], double normal[3],
                                double linearVel[3], double angularVel[3],
                                simgear::BVHMaterial const*& material,
                                simgear::BVHNode::Id& id)
{
    SGVec3d _contact, _linearVel, _angularVel;
    material = 0;
    if (!ground_cache.get_nearest(t, SGVec3d(pt), maxDist, _contact, _linearVel,
                                  _angularVel, id, material))
        return false;

    assign(contact, _contact);
    assign(linearVel, _linearVel);
    assign(angularVel, _angularVel);
    return true;
}

bool FGInterface::get_nearest_ft(double t, const double pt[3], double maxDist,
                                 double contact[3], double normal[3],
                                 double linearVel[3], double angularVel[3],
                                 simgear::BVHMaterial const*& material,
                                 simgear::BVHNode::Id& id)
{
    SGVec3d _contact, _linearVel, _angularVel;
    material = 0;
    if (!ground_cache.get_nearest(t, SG_FEET_TO_METER * SGVec3d(pt),
                                  SG_FEET_TO_METER * maxDist, _contact, _linearVel,
                                  _angularVel, id, material))
        return false;

    assign(contact, SG_METER_TO_FEET * _contact);
    assign(linearVel, SG_METER_TO_FEET * _linearVel);
    assign(angularVel, _angularVel);
    return true;
}

double
FGInterface::get_groundlevel_m(double lat, double lon, double alt)
{
    return get_groundlevel_m(SGGeod::fromRadM(lon, lat, alt));
}

double
FGInterface::get_groundlevel_m(const SGGeod& geod)
{
    // Compute the cartesian position of the given lat/lon/alt.
    SGVec3d pos = SGVec3d::fromGeod(geod);

    // FIXME: how to handle t - ref_time differences ???
    SGVec3d cpos;
    double ref_time = 0, radius;
    // Prepare the ground cache for that position.
    if (!is_valid_m(&ref_time, cpos.data(), &radius)) {
        double startTime = ref_time;
        double endTime = startTime + 1;
        bool ok = prepare_ground_cache_m(startTime, endTime, pos.data(), 10);
        /// This is most likely the case when the given altitude is
        /// too low, try with a new altitude of 10000m, that should be
        /// sufficient to find a ground level below everywhere on our planet
        if (!ok) {
            pos = SGVec3d::fromGeod(SGGeod::fromGeodM(geod, 10000));
            /// If there is still no ground, return sea level radius
            if (!prepare_ground_cache_m(startTime, endTime, pos.data(), 10))
                return 0;
        }
    } else if (radius * radius <= distSqr(pos, cpos)) {
        double startTime = ref_time;
        double endTime = startTime + 1;

        /// We reuse the old radius value, but only if it is at least 10 Meters ..
        if (!(10 < radius)) // Well this strange compare is nan safe
            radius = 10;

        bool ok = prepare_ground_cache_m(startTime, endTime, pos.data(), radius);
        /// This is most likely the case when the given altitude is
        /// too low, try with a new altitude of 10000m, that should be
        /// sufficient to find a ground level below everywhere on our planet
        if (!ok) {
            pos = SGVec3d::fromGeod(SGGeod::fromGeodM(geod, 10000));
            /// If there is still no ground, return sea level radius
            if (!prepare_ground_cache_m(startTime, endTime, pos.data(), radius))
                return 0;
        }
    }

    double contact[3], normal[3], vel[3], angvel[3];
    const simgear::BVHMaterial* material;
    simgear::BVHNode::Id id;
    // Ignore the return value here, since it just tells us if
    // the returns stem from the groundcache or from the coarse
    // computations below the groundcache. The contact point is still something
    // valid, the normals and the other returns just contain some defaults.
    get_agl_m(ref_time, pos.data(), 2.0, contact, normal, vel, angvel,
              material, id);
    return SGGeod::fromCart(SGVec3d(contact)).getElevationM();
}

bool FGInterface::caught_wire_m(double t, const double pt[4][3])
{
    SGVec3d pt_m[4];
    for (int i = 0; i < 4; ++i)
        pt_m[i] = SGVec3d(pt[i]);

    return ground_cache.caught_wire(t, pt_m);
}

bool FGInterface::caught_wire_ft(double t, const double pt[4][3])
{
    // Convert units and do the real work.
    SGVec3d pt_m[4];
    for (int i = 0; i < 4; ++i)
        pt_m[i] = SG_FEET_TO_METER * SGVec3d(pt[i]);

    return ground_cache.caught_wire(t, pt_m);
}

bool FGInterface::get_wire_ends_m(double t, double end[2][3], double vel[2][3])
{
    SGVec3d _end[2], _vel[2];
    bool ret = ground_cache.get_wire_ends(t, _end, _vel);
    for (int k = 0; k < 2; ++k) {
        assign(end[k], _end[k]);
        assign(vel[k], _vel[k]);
    }
    return ret;
}

bool FGInterface::get_wire_ends_ft(double t, double end[2][3], double vel[2][3])
{
    // Convert units and do the real work.
    SGVec3d _end[2], _vel[2];
    bool ret = ground_cache.get_wire_ends(t, _end, _vel);
    for (int k = 0; k < 2; ++k) {
        assign(end[k], SG_METER_TO_FEET * _end[k]);
        assign(vel[k], SG_METER_TO_FEET * _vel[k]);
    }
    return ret;
}

void FGInterface::release_wire(void)
{
    ground_cache.release_wire();
}
