// Radar Altimeter
//
// Written by Vivian MEAZZA, started Feb 2008.
//
// SPDX-License-Identifier: GPL-2.0-or-later
// SPDX-FileCopyrightText: 2008 Vivian Meazza

#include "config.h"

#include "rad_alt.hxx"

#include <simgear/scene/material/mat.hxx>


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


RadarAltimeter::RadarAltimeter(SGPropertyNode *node) :
    _time(0.0),
    _interval(node->getDoubleValue("update-interval-sec", 1.0))
{
    _name = node->getStringValue("name", "radar-altimeter");
    _num = node->getIntValue("number", 0);
}

RadarAltimeter::~RadarAltimeter()
{
}

void
RadarAltimeter::init ()
{

    std::string branch = "/instrumentation/" + _name;
    _Instrument = fgGetNode(branch, _num, true);

    _sceneryLoaded = fgGetNode("/sim/sceneryloaded", true);
    _serviceable_node = _Instrument->getNode("serviceable", true);

    _user_alt_agl_node     = fgGetNode("/position/altitude-agl-ft", true);
    _rad_alt_warning_node   = fgGetNode("/sim/alarms/rad-alt-warning", true);

    _Instrument->setFloatValue("tilt",-85);
    _Instrument->setStringValue("status","RA");

    _Instrument->getDoubleValue("elev-limit", true);
    _Instrument->getDoubleValue("elev-step-deg", true);
    _Instrument->getDoubleValue("az-limit-deg", true);
    _Instrument->getDoubleValue("az-step-deg", true);
    _Instrument->getDoubleValue("max-range-m", true);
    _Instrument->getDoubleValue("min-range-m", true);
    _Instrument->getDoubleValue("tilt", true);
    _Instrument->getDoubleValue("set-height-ft", true);
    _Instrument->getDoubleValue("set-excursion-percent", true);

    _antennaOffset = SGVec3d(_Instrument->getDoubleValue("antenna/x-offset-m"),
                             _Instrument->getDoubleValue("antenna/y-offset-m"),
                             _Instrument->getDoubleValue("antenna/z-offset-m"));
}

void
RadarAltimeter::update (double delta_time_sec)
{
    if (!_sceneryLoaded->getBoolValue())
        return;

    if ( ! _serviceable_node->getBoolValue() ) {
        _Instrument->setStringValue("status","");
        return;
    }

    _time += delta_time_sec;
    if (_time < _interval)
        return;

    _time -= _interval;

    update_altitude();
    updateSetHeight();
}

double
RadarAltimeter::getDistanceAntennaToHit(const SGVec3d& nearestHit) const
{
    return norm(nearestHit - getCartAntennaPos());
}

void
RadarAltimeter::updateSetHeight()
{
    double set_ht_ft   = _Instrument->getDoubleValue("set-height-ft", 9999);
    double set_excur   = _Instrument->getDoubleValue("set-excursion-percent", 0);
    if (set_ht_ft == 9999) {
        _rad_alt_warning_node->setIntValue(9999);
        return;
    }

    double radarAltFt = _min_radalt * SG_METER_TO_FEET;
    if (radarAltFt < set_ht_ft * (100 - set_excur)/100)
        _rad_alt_warning_node->setIntValue(-1);
    else if (radarAltFt > set_ht_ft * (100 + set_excur)/100)
        _rad_alt_warning_node->setIntValue(1);
    else
        _rad_alt_warning_node->setIntValue(0);
}

void
RadarAltimeter::update_altitude()
{
    double el_limit    = _Instrument->getDoubleValue("elev-limit", 15);
    double el_step     = _Instrument->getDoubleValue("elev-step-deg", 15);
    double az_limit    = _Instrument->getDoubleValue("az-limit-deg", 15);
    double az_step     = _Instrument->getDoubleValue("az-step-deg", 15);
    double max_range   = _Instrument->getDoubleValue("max-range-m", 1500);
    double min_range   = _Instrument->getDoubleValue("min-range-m", 0.001);


    _min_radalt = max_range;
    bool haveHit = false;
    SGVec3d cartantennapos = getCartAntennaPos();

    for(double brg = -az_limit; brg <= az_limit; brg += az_step){
        for(double elev = el_limit; elev >= - el_limit; elev -= el_step){
            SGVec3d userVec = rayVector(brg, elev);

            SGVec3d nearestHit;
            globals->get_scenery()->get_cart_ground_intersection(cartantennapos, userVec, nearestHit);
            double measuredDistance = dist(cartantennapos, nearestHit);

            if (measuredDistance >= min_range && measuredDistance <= max_range) {
                if (measuredDistance < _min_radalt) {
                    _min_radalt = measuredDistance;
                    haveHit = true;
                }
            } // of hit within permissible range
        } // of elevation step
    } // of azimuth step

    _Instrument->setDoubleValue("radar-altitude-ft", _min_radalt * SG_METER_TO_FEET);
    if (!haveHit) {
        _rad_alt_warning_node->setIntValue(9999);
    }
}

SGVec3d
RadarAltimeter::getCartAntennaPos() const
{
    double yaw, pitch, roll;
    globals->get_aircraft_orientation(yaw, pitch, roll);

    // Transform to the right coordinate frame, configuration is done in
    // the x-forward, y-right, z-up coordinates (feet), computation
    // in the simulation usual body x-forward, y-right, z-down coordinates
    // (meters) )

    // Transform the user position to the horizontal local coordinate system.
    SGQuatd hlTrans = SGQuatd::fromLonLat(globals->get_aircraft_position());

    // and postrotate the orientation of the user model wrt the horizontal
    // local frame
    hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,pitch,roll);

    // The offset converted to the usual body fixed coordinate system
    // rotated to the earth-fixed coordinates axis
    SGVec3d ecfOffset = hlTrans.backTransform(_antennaOffset);

    // Add the position offset of the user model to get the geocentered position
    return globals->get_aircraft_position_cart() + ecfOffset;
}

SGVec3d RadarAltimeter::rayVector(double az, double el) const
{
    double yaw, pitch, roll;
    globals->get_aircraft_orientation(yaw, pitch, roll);

    double tilt = _Instrument->getDoubleValue("tilt");
    bool roll_stab = false,
        pitch_stab = false;

    SGQuatd offset = SGQuatd::fromYawPitchRollDeg(az, el + tilt, 0);

    // Transform the antenna position to the horizontal local coordinate system.
    SGQuatd hlTrans = SGQuatd::fromLonLat(globals->get_aircraft_position());

    // and postrotate the orientation of the radar wrt the horizontal
    // local frame
    hlTrans *= SGQuatd::fromYawPitchRollDeg(yaw,
                                            pitch_stab ? 0 :pitch,
                                            roll_stab ? 0 : roll);
    hlTrans *= offset;

    // now rotate the rotation vector back into the
    // earth centered frames coordinates
    SGVec3d angleaxis(1,0,0);
    return hlTrans.backTransform(angleaxis);
}


// Register the subsystem.
#if 0
SGSubsystemMgr::InstancedRegistrant<RadarAltimeter> registrantRadarAltimeter(
    SGSubsystemMgr::FDM,
    {{"instrumentation", SGSubsystemMgr::Dependency::HARD}});
#endif
