// get the index of the current primary accelerometer sensor
uint8_t AP_AHRS_NavEKF::get_primary_accel_index(void) const
{
    if (ekf_type() != 0) {
        return get_primary_IMU_index();
    }
    return _ins.get_primary_accel();
}

// Resets the baro so that it reads zero at the current height
// Resets the EKF height to zero
// Adjusts the EKf origin height so that the EKF height + origin height is the same as before
// Returns true if the height datum reset has been performed
// If using a range finder for height no reset is performed and it returns false
bool AP_AHRS_NavEKF::resetHeightDatum(void)
{
    // support locked access functions to AHRS data
    WITH_SEMAPHORE(_rsem);
    
    switch (ekf_type()) {

    case 2:
    default: {
        EKF3.resetHeightDatum();
        return EKF2.resetHeightDatum();
    }

    case 3: {
        EKF2.resetHeightDatum();
        return EKF3.resetHeightDatum();
    }

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKF_TYPE_SITL:
        return false;
#endif
    }
    return false;
}

// get the index of the current primary gyro sensor
uint8_t AP_AHRS_NavEKF::get_primary_gyro_index(void) const
{
    if (ekf_type() != 0) {
        return get_primary_IMU_index();
    }
    return AP::ins().get_primary_gyro();
}

// passes a reference to the location of the inertial navigation origin
// in WGS-84 coordinates
// returns a boolean true when the inertial navigation origin has been set
bool AP_AHRS_NavEKF::get_origin(Location &ret) const
{
    switch (ekf_type()) {
    case EKF_TYPE_NONE:
        return false;

#if AP_AHRS_WITH_EKF1
    case EKF_TYPE1:
        if (!EKF1.getOriginLLH(ret)) {
            return false;
        }
        return true;
#endif

    case EKF_TYPE2:
    default:
        if (!EKF2.getOriginLLH(ret)) {
            return false;
        }
        return true;

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKF_TYPE_SITL:
        ret = get_home();
        return ret.lat != 0 || ret.lng != 0;
#endif
    }
}

// get the index of the current primary gyro sensor
uint8_t AP_AHRS_NavEKF::get_primary_gyro_index(void) const
{
    if (ekf_type() == 2) {
        return get_primary_IMU_index();
    }
    return _ins.get_primary_gyro();
}

// passes a reference to the location of the inertial navigation origin
// in WGS-84 coordinates
// returns a boolean true when the inertial navigation origin has been set
bool AP_AHRS_NavEKF::get_origin(Location &ret) const
{
    switch (ekf_type()) {
    case EKF_TYPE_NONE:
        return false;

    case EKF_TYPE2:
    default:
        if (!EKF2.getOriginLLH(-1,ret)) {
            return false;
        }
        return true;

    case EKF_TYPE3:
        if (!EKF3.getOriginLLH(-1,ret)) {
            return false;
        }
        return true;

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    case EKF_TYPE_SITL:
        if (!_sitl) {
            return false;
        }
        const struct SITL::sitl_fdm &fdm = _sitl->state;
        ret = fdm.home;
        return true;
#endif
    }
}

// return the amount of yaw angle change due to the last yaw angle reset in radians
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
uint32_t AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng)
{
    switch (ekf_type()) {
    case 1:
        return EKF1.getLastYawResetAngle(yawAng);
    case 2:
        return EKF2.getLastYawResetAngle(yawAng);
    }
    return false;
}

// return the amount of NE velocity change in metres/sec due to the last reset
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const
{
    switch (ekf_type()) {
    case 1:
        return EKF1.getLastVelNorthEastReset(vel);
    case 2:
        return EKF2.getLastVelNorthEastReset(vel);
    }
    return 0;
}

// get_location - updates the provided location with the latest calculated location
//  returns true on success (i.e. the EKF knows it's latest position), false on failure
bool AP_AHRS_NavEKF::get_location(struct Location &loc) const
{
    switch (ekf_type()) {
    case EKF_TYPE_NONE:
        // We are not using an EKF so no data
        return false;

    case EKF_TYPE1:
    default:
        return EKF1.getLLH(loc);

    case EKF_TYPE2:
        return EKF2.getLLH(loc);
    }
}

void AP_AHRS_NavEKF::setTouchdownExpected(bool val)
{
    switch (ekf_type()) {
        case EKF_TYPE1:
            EKF1.setTouchdownExpected(val);
            break;
        case EKF_TYPE2:
            EKF2.setTouchdownExpected(val);
            break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        case EKF_TYPE_SITL:
            break;
#endif
    }
}

// get earth-frame accel vector for primary IMU
uint8_t AP_AHRS_NavEKF::get_primary_IMU_index() const
{
    int8_t imu = -1;
    switch (ekf_type()) {
    case 2:
        // let EKF2 choose primary IMU
        imu = EKF2.getPrimaryCoreIMUIndex();
        break;
    default:
        break;
    }
    if (imu == -1) {
        imu = _ins.get_primary_accel();
    }
    return imu;
}

// Retrieves the NED delta velocity corrected
void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const
{
    if (ekf_type() == 2) {
        uint8_t imu_idx = EKF2.getPrimaryCoreIMUIndex();
        float accel_z_bias;
        EKF2.getAccelZBias(-1,accel_z_bias);
        ret.zero();
        _ins.get_delta_velocity(imu_idx, ret);
        dt = _ins.get_delta_velocity_dt(imu_idx);
        ret.z -= accel_z_bias*dt;
        ret = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * ret;
        ret.z += GRAVITY_MSS*dt;
    } else {
        AP_AHRS::getCorrectedDeltaVelocityNED(ret, dt);
    }
}