/*
   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/>.
 */
//	Code by Jon Challinger
//  Modified by Paul Riseborough
//


#include "AP_FW_Controller.h"
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Scheduler/AP_Scheduler.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Math/AP_Math.h>

AP_FW_Controller::AP_FW_Controller(const AP_FixedWing &parms, const AC_PID::Defaults &defaults, AP_AutoTune::ATType _autotune_type)
    : aparm(parms),
      rate_pid(defaults),
      autotune_type(_autotune_type)
{
    rate_pid.set_slew_limit_scale(45);
}

// Return true if input shaping should be used
bool AP_FW_Controller::should_apply_input_shaping() const
{
    // Must be using rate limits
    if (!should_apply_rate_limits()) {
        return false;
    }

    // Accel limit must be set
    if (!is_positive(accel_limit.get())) {
        return false;
    }

    // auto-tune must not be running
    if ((autotune != nullptr) && autotune->running) {
        return false;
    }

    return true;
}

// Run angle controller
float AP_FW_Controller::run_angle_control(int32_t desired_angle_cd, float scaler, bool disable_integrator, bool ground_mode)
{
    // Ensure tau is valid
    if (gains.tau < 0.05f) {
        gains.tau.set(0.05f);
    }
    const float desired_angle_deg = wrap_180(desired_angle_cd * 0.01);

    if (!should_apply_input_shaping()) {
        // Calculate rate directly from angle error with no input shaping
        angle_err_deg = wrap_180(desired_angle_deg - get_measured_angle_deg());
        float desired_rate_degs = angle_err_deg / gains.tau;

        // Reset input shaping set points
        reset_input_shaping_deg(desired_angle_deg, desired_rate_degs);

        // Add coordination offset
        desired_rate_degs += get_rate_target_offset_degs();

        // Apply rate limits if enabled
        if (should_apply_rate_limits()) {
            desired_rate_degs = rate_limit_degs(desired_rate_degs);
        }

        // Run rate controller
        return run_axis_rate_control(desired_rate_degs, scaler, disable_integrator, ground_mode);
    }

    // Apply input shaping to desired angle
    const float dt = AP::scheduler().get_loop_period_s();

    const float accel_max = accel_limit.get();
    const float jerk_limit = accel_max / MAX(gains.tau.get(), 0.1);

    // Ensure the shortest path is taken
    const float angle_error = wrap_180(desired_angle_deg - angle_target_deg);

    // Apply input shaping updating the accel target
    shape_pos_vel_accel(
        angle_error, 0.0, 0.0, // desired pos, vel and accel
        0.0, rate_target_degs, accel_target_degss, // current shaped target
        -get_negative_rate_limit_degs(), get_positive_rate_limit_degs(), // velocity limits
        -accel_max, accel_max, // accel limits
        jerk_limit, // jerk limit
        dt, true
    );

    // Integrate pos and vel from updated accel target
    angle_target_deg += rate_target_degs * dt + accel_target_degss * 0.5 * sq(dt);
    rate_target_degs += accel_target_degss * dt;

    // Make sure target remains in the range +-180
    angle_target_deg = wrap_180(angle_target_deg);

    // Calculate angle error
    angle_err_deg = wrap_180(angle_target_deg - get_measured_angle_deg());

    // Use 1 / tau if angle gain is not set
    float angle_gain = 1.0 / gains.tau.get();
    if (is_positive(angle_p.get())) {
        angle_gain = angle_p.get();
    }

    // Apply gain using sqrt controller
    float desired_rate_degs = sqrt_controller(angle_err_deg, angle_gain, accel_max * 0.5, dt);

    // Add feed forward rate demand and offset then constrain to rate limit
    desired_rate_degs = rate_limit_degs(desired_rate_degs + rate_target_degs + get_rate_target_offset_degs());

    // Run rate controller
    return run_axis_rate_control(desired_rate_degs, scaler, disable_integrator, ground_mode);
}

/*
  AC_PID based rate controller
*/
float AP_FW_Controller::run_rate_control(float desired_rate_degs, float scaler, bool disable_integrator, bool ground_mode)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    // Check that the controller is called once per loop and no more
    const uint32_t ticks = AP::scheduler().ticks32();
    if (last_run_ticks != 0) {
        if (last_run_ticks == ticks) {
            AP_HAL::panic("FW rate control must be run only once per loop");
        }
        if ((last_run_ticks + 1) != ticks) {
            AP_HAL::panic("FW rate control must be run or reset every loop");
        }
    }
    last_run_ticks = ticks;
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL

    const float dt = AP::scheduler().get_loop_period_s();

    const float eas2tas = AP::ahrs().get_EAS2TAS();
    bool limit_I = fabsf(_last_out) >= 45;
    const float rate_rads = get_measured_rate_rads();
    const float old_I = rate_pid.get_i();

    const bool underspeed = is_underspeed();
    if (underspeed) {
        limit_I = true;
    }

    // the PID elements are scaled by sq(scaler). To use an
    // unmodified AC_PID object we scale the inputs (target and measurement)
    //
    // note that we run AC_PID in radians so that the normal scaling
    // range for IMAX in AC_PID applies (usually an IMAX value less than 1.0)
    rate_pid.update_all(radians(desired_rate_degs) * scaler * scaler, rate_rads * scaler * scaler, dt, limit_I);

    if (underspeed) {
        // when underspeed we lock the integrator
        rate_pid.set_integrator(old_I);
    }

    // FF and DFF should be scaled by scaler/eas2tas, but since we have scaled
    // the AC_PID target above by scaler*scaler we need to instead
    // divide by scaler*eas2tas to get the right scaling
    const float ff = degrees(ff_scale * rate_pid.get_ff_component() / (scaler * eas2tas));
    const float dff = degrees(ff_scale * rate_pid.get_dff_component() / (scaler * eas2tas));
    ff_scale = 1.0;

    if (disable_integrator) {
        rate_pid.reset_I();
    }

    // convert AC_PID info object to same scale as old controller
    _pid_info = rate_pid.get_pid_info();
    auto &pinfo = _pid_info;

    const float deg_scale = degrees(1);
    pinfo.FF = ff;
    pinfo.P *= deg_scale;
    pinfo.I *= deg_scale;
    pinfo.D *= deg_scale;
    pinfo.DFF = dff;

    // fix the logged target and actual values to not have the scalers applied
    pinfo.target = desired_rate_degs;
    pinfo.actual = degrees(rate_rads);

    // sum components
    float out = pinfo.FF + pinfo.P + pinfo.I + pinfo.D + pinfo.DFF;
    if (ground_mode) {
        // when on ground suppress D and half P term to prevent oscillations
        out -= pinfo.D + 0.5*pinfo.P;
    }

    // remember the last output to trigger the I limit
    _last_out = out;

    if (autotune != nullptr && autotune->running && get_airspeed() > aparm.airspeed_min) {
        // let autotune have a go at the values
        autotune->update(pinfo, scaler, angle_err_deg);
    }

    // output is scaled to notional centidegrees of deflection
    return constrain_float(out * 100, -4500, 4500);
}

/*
 Function returns an equivalent control surface deflection in centi-degrees in the range from -4500 to 4500
*/
float AP_FW_Controller::run_rate_control(float desired_rate_degs, float scaler)
{
    // Zero angle error in pure rate control
    angle_err_deg = 0.0;

    if (!should_apply_input_shaping()) {
        // Reset input shaping set points
        reset_input_shaping_deg(get_measured_angle_deg(), desired_rate_degs);

        // run rate control with no input shaping
        return run_rate_control(desired_rate_degs, scaler, false, false);
    }

    // Apply input shaping to desired rate
    const float dt = AP::scheduler().get_loop_period_s();

    // Reset the input shaping target angle
    angle_target_deg = get_measured_angle_deg();

    const float accel_max = accel_limit.get();
    const float jerk_limit = accel_max / MAX(gains.tau.get(), 0.1);

    // Apply input shaping updating the accel target
    shape_pos_vel_accel(
        0.0, desired_rate_degs, 0.0, // desired pos, vel and accel
        0.0, rate_target_degs, accel_target_degss, // current shaped target
        -get_negative_rate_limit_degs(), get_positive_rate_limit_degs(), // velocity limits
        -accel_max, accel_max, // accel limits
        jerk_limit, // jerk limit
        dt, true
    );

    rate_target_degs += accel_target_degss * dt;

    // Run rate controller
    return run_rate_control(rate_target_degs, scaler, false, false);
}

// Reset I term
void AP_FW_Controller::reset_I()
{
    rate_pid.reset_I();
    _last_out = 0.0;
}

/*
    reduce the integrator, used when we have a low scale factor in a quadplane hover
*/
void AP_FW_Controller::decay_I()
{
    // this reduces integrator by 95% over 2s
    _pid_info.I *= 0.995f;
    rate_pid.set_integrator(rate_pid.get_i() * 0.995);
}

/*
  restore autotune gains
 */
void AP_FW_Controller::autotune_restore(void)
{
    if (autotune != nullptr) {
        autotune->stop();
    }
}

/*
  start an autotune
 */
void AP_FW_Controller::autotune_start(void)
{
    if (autotune == nullptr) {
        autotune = NEW_NOTHROW AP_AutoTune(gains, autotune_type, aparm, rate_pid);
        if (autotune == nullptr) {
            if (!failed_autotune_alloc) {
                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "AutoTune: failed %s allocation", AP_AutoTune::axis_string(autotune_type));
            }
            failed_autotune_alloc = true;
        }
    }
    if (autotune != nullptr) {
        autotune->start();
    }
}

// Return the airspeed in m/s
float AP_FW_Controller::get_airspeed() const
{
    float aspeed;
    if (!AP::ahrs().airspeed_EAS(aspeed)) {
        // If no airspeed available use average of min and max
        aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_max));
    }
    return aspeed;
}

// Reset controller
void AP_FW_Controller::reset()
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    // Reset tick tracking
    last_run_ticks = 0;
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL

    // Reset PID
    rate_pid.reset_I();
    rate_pid.reset_filter();
    _last_out = 0.0;

    // Reset input shaping
    reset_input_shaping_deg(get_measured_angle_deg(), degrees(get_measured_rate_rads()));
}

// Apply positive and negative rate limits to passed in value
float AP_FW_Controller::rate_limit_degs(float rate_degs) const
{
    const float pos_rate_limit = get_positive_rate_limit_degs();
    if (is_positive(pos_rate_limit)) {
        rate_degs = MIN(rate_degs, pos_rate_limit);
    }

    const float neg_rate_limit = get_negative_rate_limit_degs();
    if (is_positive(neg_rate_limit)) {
        rate_degs = MAX(rate_degs, -neg_rate_limit);
    }

    return rate_degs;
}

// Reset input shaping applying rate limits
void AP_FW_Controller::reset_input_shaping_deg(const float angle_deg, const float rate_degs)
{
    // No angle limits at the controller level, reset to the passed in angle
    angle_target_deg = angle_deg;

    // Reset to passed in rate and apply rate limits
    rate_target_degs = rate_limit_degs(rate_degs);

    // Reset accel to zero
    accel_target_degss = 0.0;
}

// Get input shaping angle, rate and accel for logging
void AP_FW_Controller::get_input_shaping(float &angle_deg, float &rate_degs, float &accel_degss) const
{
    angle_deg = angle_target_deg;
    rate_degs = rate_target_degs;
    accel_degss = accel_target_degss;
}

// Reset the attitude target to such that a change in attitude due to an ahrs change is smooth
void AP_FW_Controller::ahrs_reset()
{
    // Update the target angle such that the angle error remains the same before and after the change in measured angle from the ahrs
    angle_target_deg = wrap_180(get_measured_angle_deg() + angle_err_deg);
}

// Get angle P gain
float AP_FW_Controller::get_angle_p() const
{
    if (should_apply_input_shaping() && is_positive(angle_p.get())) {
        // Angle gain is in use and configured
        return angle_p.get();
    }

    // angle gain = 1 / tau
    return 1.0 / gains.tau.get();
}