/*
  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "AP_NavEKF_Source.h"
#include <AP_Math/AP_Math.h>
#include <AP_DAL/AP_DAL.h>

extern const AP_HAL::HAL& hal;

const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = {

    // @Param: 1_POSXY
    // @DisplayName: Position Horizontal Source (Primary)
    // @Description: Position Horizontal Source (Primary)
    // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
    // @User: Advanced
    AP_GROUPINFO("1_POSXY", 1, AP_NavEKF_Source, _source_set[0].posxy, (int8_t)AP_NavEKF_Source::SourceXY::GPS),

    // @Param: 1_VELXY
    // @DisplayName: Velocity Horizontal Source
    // @Description: Velocity Horizontal Source
    // @Values: 0:None, 3:GPS, 4:Beacon, 5:OpticalFlow, 6:ExternalNav, 7:WheelEncoder
    // @User: Advanced
    AP_GROUPINFO("1_VELXY", 2, AP_NavEKF_Source, _source_set[0].velxy, (int8_t)AP_NavEKF_Source::SourceXY::GPS),

    // @Param: 1_POSZ
    // @DisplayName: Position Vertical Source
    // @Description: Position Vertical Source
    // @Values: 0:None, 1:Baro, 2:RangeFinder, 3:GPS, 4:Beacon, 6:ExternalNav
    // @User: Advanced
    AP_GROUPINFO("1_POSZ", 3, AP_NavEKF_Source, _source_set[0].posz, (int8_t)AP_NavEKF_Source::SourceZ::BARO),

    // @Param: 1_VELZ
    // @DisplayName: Velocity Vertical Source
    // @Description: Velocity Vertical Source
    // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
    // @User: Advanced
    AP_GROUPINFO("1_VELZ", 4, AP_NavEKF_Source, _source_set[0].velz, (int8_t)AP_NavEKF_Source::SourceZ::GPS),

    // @Param: 1_YAW
    // @DisplayName: Yaw Source
    // @Description: Yaw Source
    // @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback
    // @User: Advanced
    AP_GROUPINFO("1_YAW", 5, AP_NavEKF_Source, _source_set[0].yaw, (int8_t)AP_NavEKF_Source::SourceYaw::COMPASS),

#if AP_NAKEKF_SOURCE_SET_MAX >= 2
    // @Param: 2_POSXY
    // @DisplayName: Position Horizontal Source (Secondary)
    // @Description: Position Horizontal Source (Secondary)
    // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
    // @User: Advanced
    AP_GROUPINFO("2_POSXY", 6, AP_NavEKF_Source, _source_set[1].posxy, (int8_t)AP_NavEKF_Source::SourceXY::NONE),

    // @Param: 2_VELXY
    // @DisplayName: Velocity Horizontal Source (Secondary)
    // @Description: Velocity Horizontal Source (Secondary)
    // @Values: 0:None, 3:GPS, 4:Beacon, 5:OpticalFlow, 6:ExternalNav, 7:WheelEncoder
    // @User: Advanced
    AP_GROUPINFO("2_VELXY", 7, AP_NavEKF_Source, _source_set[1].velxy, (int8_t)AP_NavEKF_Source::SourceXY::NONE),

    // @Param: 2_POSZ
    // @DisplayName: Position Vertical Source (Secondary)
    // @Description: Position Vertical Source (Secondary)
    // @Values: 0:None, 1:Baro, 2:RangeFinder, 3:GPS, 4:Beacon, 6:ExternalNav
    // @User: Advanced
    AP_GROUPINFO("2_POSZ", 8, AP_NavEKF_Source, _source_set[1].posz, (int8_t)AP_NavEKF_Source::SourceZ::BARO),

    // @Param: 2_VELZ
    // @DisplayName: Velocity Vertical Source (Secondary)
    // @Description: Velocity Vertical Source (Secondary)
    // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
    // @User: Advanced
    AP_GROUPINFO("2_VELZ", 9, AP_NavEKF_Source, _source_set[1].velz, (int8_t)AP_NavEKF_Source::SourceZ::NONE),

    // @Param: 2_YAW
    // @DisplayName: Yaw Source (Secondary)
    // @Description: Yaw Source (Secondary)
    // @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback
    // @User: Advanced
    AP_GROUPINFO("2_YAW", 10, AP_NavEKF_Source, _source_set[1].yaw, (int8_t)AP_NavEKF_Source::SourceYaw::NONE),
#endif

#if AP_NAKEKF_SOURCE_SET_MAX >= 3
    // @Param: 3_POSXY
    // @DisplayName: Position Horizontal Source (Tertiary)
    // @Description: Position Horizontal Source (Tertiary)
    // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
    // @User: Advanced
    AP_GROUPINFO("3_POSXY", 11, AP_NavEKF_Source, _source_set[2].posxy, (int8_t)AP_NavEKF_Source::SourceXY::NONE),

    // @Param: 3_VELXY
    // @DisplayName: Velocity Horizontal Source (Tertiary)
    // @Description: Velocity Horizontal Source (Tertiary)
    // @Values: 0:None, 3:GPS, 4:Beacon, 5:OpticalFlow, 6:ExternalNav, 7:WheelEncoder
    // @User: Advanced
    AP_GROUPINFO("3_VELXY", 12, AP_NavEKF_Source, _source_set[2].velxy, (int8_t)AP_NavEKF_Source::SourceXY::NONE),

    // @Param: 3_POSZ
    // @DisplayName: Position Vertical Source (Tertiary)
    // @Description: Position Vertical Source (Tertiary)
    // @Values: 0:None, 1:Baro, 2:RangeFinder, 3:GPS, 4:Beacon, 6:ExternalNav
    // @User: Advanced
    AP_GROUPINFO("3_POSZ", 13, AP_NavEKF_Source, _source_set[2].posz, (int8_t)AP_NavEKF_Source::SourceZ::BARO),

    // @Param: 3_VELZ
    // @DisplayName: Velocity Vertical Source (Tertiary)
    // @Description: Velocity Vertical Source (Tertiary)
    // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
    // @User: Advanced
    AP_GROUPINFO("3_VELZ", 14, AP_NavEKF_Source, _source_set[2].velz, (int8_t)AP_NavEKF_Source::SourceZ::NONE),

    // @Param: 3_YAW
    // @DisplayName: Yaw Source (Tertiary)
    // @Description: Yaw Source (Tertiary)
    // @Values: 0:None, 1:Compass, 2:External, 3:External with Compass Fallback
    // @User: Advanced
    AP_GROUPINFO("3_YAW", 15, AP_NavEKF_Source, _source_set[2].yaw, (int8_t)AP_NavEKF_Source::SourceYaw::NONE),
#endif

    // @Param: _OPTIONS
    // @DisplayName: EKF Source Options
    // @Description: EKF Source Options
    // @Bitmask: 0:FuseAllVelocities
    // @User: Advanced
    AP_GROUPINFO("_OPTIONS", 16, AP_NavEKF_Source, _options, (int16_t)SourceOptions::FUSE_ALL_VELOCITIES),

    AP_GROUPEND
};

AP_NavEKF_Source::AP_NavEKF_Source()
{
    AP_Param::setup_object_defaults(this, var_info);
}

void AP_NavEKF_Source::init()
{
    // ensure init is only run once
    if (initialised) {
        return;
    }

    // initialise active sources
    _active_source_set.posxy = (SourceXY)_source_set[0].posxy.get();
    _active_source_set.velxy = (SourceXY)_source_set[0].velxy.get();
    _active_source_set.posz = (SourceZ)_source_set[0].posz.get();
    _active_source_set.velz = (SourceZ)_source_set[0].velz.get();
    _active_source_set.yaw = (SourceYaw)_source_set[0].yaw.get();

    initialised = true;
}

// set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
void AP_NavEKF_Source::setPosVelYawSourceSet(uint8_t source_set_idx)
{
    // ensure init has been run
    init();

    // sanity check source idx
    if (source_set_idx >= AP_NAKEKF_SOURCE_SET_MAX) {
        return;
    }

    _active_source_set.posxy = (SourceXY)_source_set[source_set_idx].posxy.get();
    _active_source_set.velxy = (SourceXY)_source_set[source_set_idx].velxy.get();
    _active_source_set.posz = (SourceZ)_source_set[source_set_idx].posz.get();
    _active_source_set.velz = (SourceZ)_source_set[source_set_idx].velz.get();
    _active_source_set.yaw = (SourceYaw)_source_set[source_set_idx].yaw.get();
}

// true/false of whether velocity source should be used
bool AP_NavEKF_Source::useVelXYSource(SourceXY velxy_source) const
{
    if (velxy_source == _active_source_set.velxy) {
        return true;
    }

    // check for fuse all velocities
    if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
        for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
            if ((SourceXY)_source_set[i].velxy.get() == velxy_source) {
                return true;
            }
        }
    }

    // if we got this far source should not be used
    return false;
}

bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source) const
{
    if (velz_source == _active_source_set.velz) {
        return true;
    }

    // check for fuse all velocities
    if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
        for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
            if ((SourceZ)_source_set[i].velz.get() == velz_source) {
                return true;
            }
        }
    }

    // if we got this far source should not be used
    return false;
}

// true if a velocity source is configured
bool AP_NavEKF_Source::haveVelZSource() const
{
    if (_active_source_set.velz != SourceZ::NONE) {
        return true;
    }

    // check for fuse all velocities
    if (_options.get() & (uint16_t)(SourceOptions::FUSE_ALL_VELOCITIES)) {
        for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
            if ((SourceZ)_source_set[i].velz.get() != SourceZ::NONE) {
                return true;
            }
        }
    }

    // if we got this far no velocity z source has been configured
    return false;
}

// align position of inactive sources to ahrs
void AP_NavEKF_Source::align_inactive_sources()
{
    // align visual odometry
#if HAL_VISUALODOM_ENABLED

    auto *visual_odom = AP::dal().visualodom();
    if (!visual_odom || !visual_odom->enabled()) {
        return;
    }

    // consider aligning XY position:
    bool align_posxy = false;
    if ((getPosXYSource() == SourceXY::GPS) ||
        (getPosXYSource() == SourceXY::BEACON)) {
        // only align position if active source is GPS or Beacon
        for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
            if ((SourceXY)_source_set[i].posxy.get() == SourceXY::EXTNAV) {
                // ExtNav could potentially be used, so align it
                align_posxy = true;
                break;
            }
        }
    }

    // consider aligning Z position:
    bool align_posz = false;
    if ((getPosZSource() == SourceZ::BARO) ||
        (getPosZSource() == SourceZ::RANGEFINDER) ||
        (getPosZSource() == SourceZ::GPS) ||
        (getPosZSource() == SourceZ::BEACON)) {
        // ExtNav is not the active source; we do not want to align active source!
        for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
            if ((SourceZ)_source_set[i].posz.get() == SourceZ::EXTNAV) {
                // ExtNav could potentially be used, so align it
                align_posz = true;
                break;
            }
        }
    }
    visual_odom->align_position_to_ahrs(align_posxy, align_posz);
#endif
}

// sensor specific helper functions
bool AP_NavEKF_Source::usingGPS() const
{
    return getPosXYSource() == SourceXY::GPS ||
           getPosZSource() == SourceZ::GPS ||
           getVelXYSource() == SourceXY::GPS ||
           getVelZSource() == SourceZ::GPS;
}

// true if some parameters have been configured (used during parameter conversion)
bool AP_NavEKF_Source::configured_in_storage()
{
    if (config_in_storage) {
        return true;
    }

    // first source parameter is used to determine if configured or not
    config_in_storage = _source_set[0].posxy.configured_in_storage();

    return config_in_storage;
}

// mark parameters as configured in storage (used to ensure parameter conversion is only done once)
void AP_NavEKF_Source::mark_configured_in_storage()
{
    // save first parameter's current value to mark as configured
    return _source_set[0].posxy.save(true);
}

// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool AP_NavEKF_Source::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
{
    auto &dal = AP::dal();
    bool baro_required = false;
    bool beacon_required = false;
    bool compass_required = false;
    bool gps_required = false;
    bool rangefinder_required = false;
    bool visualodom_required = false;
    bool optflow_required = false;
    bool wheelencoder_required = false;

    // check source params are valid
    for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {

        // check posxy
        switch ((SourceXY)_source_set[i].posxy.get()) {
        case SourceXY::NONE:
            break;
        case SourceXY::GPS:
            gps_required = true;
            break;
        case SourceXY::BEACON:
            beacon_required = true;
            break;
        case SourceXY::EXTNAV:
            visualodom_required = true;
            break;
        case SourceXY::OPTFLOW:
        case SourceXY::WHEEL_ENCODER:
        default:
            // invalid posxy value
            hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_POSXY", (int)i+1);
            return false;
        }

        // check velxy
        switch ((SourceXY)_source_set[i].velxy.get()) {
        case SourceXY::NONE:
            break;
        case SourceXY::GPS:
            gps_required = true;
            break;
        case SourceXY::OPTFLOW:
            optflow_required = true;
            break;
        case SourceXY::EXTNAV:
            visualodom_required = true;
            break;
        case SourceXY::WHEEL_ENCODER:
            wheelencoder_required = true;
            break;
        case SourceXY::BEACON:
        default:
            // invalid velxy value
            hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_VELXY", (int)i+1);
            return false;
        }

        // check posz
        switch ((SourceZ)_source_set[i].posz.get()) {
        case SourceZ::BARO:
            baro_required = true;
            break;
        case SourceZ::RANGEFINDER:
            rangefinder_required = true;
            break;
        case SourceZ::GPS:
            gps_required = true;
            break;
        case SourceZ::BEACON:
            beacon_required = true;
            break;
        case SourceZ::EXTNAV:
            visualodom_required = true;
            break;
        case SourceZ::NONE:
        default:
            // invalid posz value
            hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_POSZ", (int)i+1);
            return false;
        }

        // check velz
        switch ((SourceZ)_source_set[i].velz.get()) {
        case SourceZ::NONE:
            break;
        case SourceZ::GPS:
            gps_required = true;
            break;
        case SourceZ::EXTNAV:
            visualodom_required = true;
            break;
        case SourceZ::BARO:
        case SourceZ::RANGEFINDER:
        case SourceZ::BEACON:
        default:
            // invalid velz value
            hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_VELZ", (int)i+1);
            return false;
        }

        // check yaw
        switch ((SourceYaw)_source_set[i].yaw.get()) {
        case SourceYaw::NONE:
        case SourceYaw::EXTERNAL:
            // valid yaw value
            break;
        case SourceYaw::COMPASS:
        case SourceYaw::EXTERNAL_COMPASS_FALLBACK:
            compass_required = true;
            break;
        default:
            // invalid yaw value
            hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_YAW", (int)i+1);
            return false;
        }
    }

    // check all required sensors are available
    const char* ekf_requires_msg = "EK3 sources require %s";
    if (baro_required && (dal.baro().num_instances() == 0)) {
        hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "Baro");
        return false;
    }

    if (beacon_required && (dal.beacon() == nullptr || !dal.beacon()->enabled())) {
        hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "Beacon");
        return false;
    }

    if (compass_required && dal.compass().get_num_enabled() == 0) {
        hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "Compass");
        return false;
    }

    if (gps_required && (dal.gps().num_sensors() == 0)) {
        hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "GPS");
        return false;
    }

    if (optflow_required && !dal.opticalflow_enabled()) {
        hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "OpticalFlow");
        return false;
    }

    if (rangefinder_required && (dal.rangefinder() == nullptr || !dal.rangefinder()->has_orientation(ROTATION_PITCH_270))) {
        hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "RangeFinder");
        return false;
    }

    if (visualodom_required) {
        bool visualodom_available = false;
#if HAL_VISUALODOM_ENABLED
        auto *vo = AP::dal().visualodom();
        visualodom_available = vo && vo->enabled();
#endif
        if (!visualodom_available) {
            hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "VisualOdom");
            return false;
        }
    }

    if (wheelencoder_required && !dal.wheelencoder_enabled()) {
        hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "WheelEncoder");
        return false;
    }

    return true;
}

// return true if ext nav is enabled on any source
bool AP_NavEKF_Source::ext_nav_enabled(void) const
{
    for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
        const auto &src = _source_set[i];
        if (SourceXY(src.posxy.get()) == SourceXY::EXTNAV) {
            return true;
        }
        if (SourceZ(src.posz.get()) == SourceZ::EXTNAV) {
            return true;
        }
        if (SourceXY(src.velxy.get()) == SourceXY::EXTNAV) {
            return true;
        }
        if (SourceZ(src.velz.get()) == SourceZ::EXTNAV) {
            return true;
        }
    }
    return false;
}

// return true if wheel encoder is enabled on any source
bool AP_NavEKF_Source::wheel_encoder_enabled(void) const
{
    for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
        const auto &src = _source_set[i];
        if (SourceXY(src.velxy.get()) == SourceXY::WHEEL_ENCODER) {
            return true;
        }
    }
    return false;
}

// return true if ext yaw is enabled on any source
bool AP_NavEKF_Source::ext_yaw_enabled(void) const
{
    for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
        const auto &src = _source_set[i];
        const SourceYaw yaw = SourceYaw(src.yaw.get());
        if (yaw == SourceYaw::EXTERNAL ||
            yaw == SourceYaw::EXTERNAL_COMPASS_FALLBACK) {
            return true;
        }
    }
    return false;
}