#include "AP_Baro.h"
#include <AP_AHRS/AP_AHRS.h>

#if HAL_BARO_WIND_COMP_ENABLED

// table of compensation coefficient parameters for one baro
const AP_Param::GroupInfo AP_Baro::WindCoeff::var_info[] = {

    // @Param: ENABLE
    // @DisplayName: Wind coefficient enable
    // @Description: This enables the use of wind coefficients for barometer compensation
    // @Values: 0:Disabled, 1:Enabled
    // @User: Advanced
    AP_GROUPINFO_FLAGS("ENABLE", 1, WindCoeff, enable, 0, AP_PARAM_FLAG_ENABLE),

    // @Param: FWD
    // @DisplayName: Pressure error coefficient in positive X direction (forward)
    // @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the X body axis. If the baro height estimate rises during forwards flight, then this will be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned.
    // @Range: -1.0 1.0
    // @Increment: 0.05
    // @User: Advanced
    AP_GROUPINFO("FWD", 2, WindCoeff, xp, 0.0),

    // @Param: BCK
    // @DisplayName: Pressure error coefficient in negative X direction (backwards)
    // @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the X body axis. If the baro height estimate rises during backwards flight, then this will be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned.
    // @Range: -1.0 1.0
    // @Increment: 0.05
    // @User: Advanced
    AP_GROUPINFO("BCK", 3, WindCoeff, xn, 0.0),

    // @Param: RGT
    // @DisplayName: Pressure error coefficient in positive Y direction (right)
    // @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the Y body axis. If the baro height estimate rises during sideways flight to the right, then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned.
    // @Range: -1.0 1.0
    // @Increment: 0.05
    // @User: Advanced
    AP_GROUPINFO("RGT", 4, WindCoeff, yp, 0.0),

    // @Param: LFT
    // @DisplayName: Pressure error coefficient in negative Y direction (left)
    // @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the Y body axis. If the baro height estimate rises during sideways flight to the left, then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned.
    // @Range: -1.0 1.0
    // @Increment: 0.05
    // @User: Advanced
    AP_GROUPINFO("LFT", 5, WindCoeff, yn, 0.0),

    // @Param: UP
    // @DisplayName: Pressure error coefficient in positive Z direction (up)
    // @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the Z body axis. If the baro height estimate rises above truth height during climbing flight (or forward flight with a high forwards lean angle), then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned.
    // @Range: -1.0 1.0
    // @Increment: 0.05
    // @User: Advanced
    AP_GROUPINFO("UP", 6, WindCoeff, zp, 0.0),

    // @Param: DN
    // @DisplayName: Pressure error coefficient in negative Z direction (down)
    // @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the Z body axis. If the baro height estimate rises above truth height during descending flight (or forward flight with a high backwards lean angle, eg braking manoeuvre), then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned.
    // @Range: -1.0 1.0
    // @Increment: 0.05
    // @User: Advanced
    AP_GROUPINFO("DN", 7, WindCoeff, zn, 0.0),

    AP_GROUPEND
};


/*
  return pressure correction for wind based on GND_WCOEF parameters
 */
float AP_Baro::wind_pressure_correction(uint8_t instance)
{
    const WindCoeff &wcoef = sensors[instance].wind_coeff;

    if (!wcoef.enable) {
        return 0;
    }

    auto &ahrs = AP::ahrs();

    // correct for static pressure position errors
    Vector3f airspeed_vec_bf;
    if (!ahrs.airspeed_vector_true(airspeed_vec_bf)) {
        return 0;
    }

    float error = 0.0;

    const float kp = 0.5 * SSL_AIR_DENSITY * ahrs.get_air_density_ratio();
    const float sqxp = sq(airspeed_vec_bf.x) * kp;
    const float sqyp = sq(airspeed_vec_bf.y) * kp;
    const float sqzp = sq(airspeed_vec_bf.z) * kp;

    if (is_positive(airspeed_vec_bf.x)) {
        sensors[instance].dynamic_pressure.x = sqxp;
        error += wcoef.xp * sqxp;
    } else {
        sensors[instance].dynamic_pressure.x = -sqxp;
        error += wcoef.xn * sqxp;
    }
    if (is_positive(airspeed_vec_bf.y)) {
        sensors[instance].dynamic_pressure.y = sqyp;
        error += wcoef.yp * sqyp;
    } else {
        sensors[instance].dynamic_pressure.y = -sqyp;
        error += wcoef.yn * sqyp;
    }
    if (is_positive(airspeed_vec_bf.z)) {
        sensors[instance].dynamic_pressure.z = sqzp;
        error += wcoef.zp * sqzp;
    } else {
        sensors[instance].dynamic_pressure.z = -sqzp;
        error += wcoef.zn * sqzp;
    }

    return error;
}
#endif // HAL_BARO_WIND_COMP_ENABLED