/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

#include "Plane.h"

/*
  is_flying and crash detection logic
 */

/*
  Do we think we are flying?
  Probabilistic method where a bool is low-passed and considered a probability.
*/
void Plane::update_is_flying_5Hz(void)
{
    float aspeed;
    bool is_flying_bool;
    uint32_t now_ms = AP_HAL::millis();

    uint32_t ground_speed_thresh_cm = (g.min_gndspeed_cm > 0) ? ((uint32_t)(g.min_gndspeed_cm*0.9f)) : 500;
    bool gps_confirmed_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_3D) &&
                                    (gps.ground_speed_cm() >= ground_speed_thresh_cm);

    // airspeed at least 75% of stall speed?
    bool airspeed_movement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= (aparm.airspeed_min*0.75f));

    if(arming.is_armed()) {
        // when armed assuming flying and we need overwhelming evidence that we ARE NOT flying

        // short drop-outs of GPS are common during flight due to banking which points the antenna in different directions
        bool gps_lost_recently = (gps.last_fix_time_ms() > 0) && // we have locked to GPS before
                        (gps.status() < AP_GPS::GPS_OK_FIX_2D) && // and it's lost now
                        (now_ms - gps.last_fix_time_ms() < 5000); // but it wasn't that long ago (<5s)

        if ((auto_state.last_flying_ms > 0) && gps_lost_recently) {
            // we've flown before, remove GPS constraints temporarily and only use airspeed
            is_flying_bool = airspeed_movement; // moving through the air
        } else {
            // we've never flown yet, require good GPS movement
            is_flying_bool = airspeed_movement || // moving through the air
                                gps_confirmed_movement; // locked and we're moving
        }

        if (control_mode == AUTO) {
            /*
              make is_flying() more accurate during various auto modes
             */

            switch (flight_stage)
            {
            case AP_SpdHgtControl::FLIGHT_TAKEOFF:
                // while on the ground, an uncalibrated airspeed sensor can drift to 7m/s so
                // ensure we aren't showing a false positive. If the throttle is suppressed
                // we are definitely not flying, or at least for not much longer!
                if (throttle_suppressed) {
                    is_flying_bool = false;
                    crash_state.is_crashed = false;
                }
                break;

            case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
            case AP_SpdHgtControl::FLIGHT_NORMAL:
                // TODO: detect ground impacts
                break;

            case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
                // TODO: detect ground impacts
                if (fabsf(auto_state.sink_rate) > 0.2f) {
                    is_flying_bool = true;
                }
                break;

            case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
                break;
            } // switch
        }
    } else {
        // when disarmed assume not flying and need overwhelming evidence that we ARE flying
        is_flying_bool = airspeed_movement && gps_confirmed_movement;

        if ((control_mode == AUTO) &&
            ((flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) ||
             (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL)) ) {
            is_flying_bool = false;
        }
    }

    // low-pass the result.
    // coef=0.15f @ 5Hz takes 3.0s to go from 100% down to 10% (or 0% up to 90%)
    isFlyingProbability = (0.85f * isFlyingProbability) + (0.15f * (float)is_flying_bool);

    /*
      update last_flying_ms so we always know how long we have not
      been flying for. This helps for crash detection and auto-disarm
     */
    bool new_is_flying = is_flying();

    // we are flying, note the time
    if (new_is_flying) {

        auto_state.last_flying_ms = now_ms;

        if (!previous_is_flying) {
            // just started flying in any mode
            started_flying_ms = now_ms;
        }

        if ((control_mode == AUTO) &&
            ((auto_state.started_flying_in_auto_ms == 0) || !previous_is_flying) ) {

            // We just started flying, note that time also
            auto_state.started_flying_in_auto_ms = now_ms;
        }
    }
    previous_is_flying = new_is_flying;

    if (should_log(MASK_LOG_MODE)) {
        Log_Write_Status();
    }
}

/*
  return true if we think we are flying. This is a probabilistic
  estimate, and needs to be used very carefully. Each use case needs
  to be thought about individually.
 */
bool Plane::is_flying(void)
{
    if (hal.util->get_soft_armed()) {
        // when armed, assume we're flying unless we probably aren't
        return (isFlyingProbability >= 0.1f);
    }

    // when disarmed, assume we're not flying unless we probably are
    return (isFlyingProbability >= 0.9f);
}

/*
 * Determine if we have crashed
 */
void Plane::crash_detection_update(void)
{
    if (control_mode != AUTO)
    {
        // crash detection is only available in AUTO mode
        crash_state.debounce_timer_ms = 0;
        return;
    }

    uint32_t now_ms = AP_HAL::millis();
    bool auto_launch_detected;
    bool crashed_near_land_waypoint = false;
    bool crashed = false;
    bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) &&
                            (now_ms - auto_state.started_flying_in_auto_ms >= 2500);

    if (!is_flying())
    {
        switch (flight_stage)
        {
        case AP_SpdHgtControl::FLIGHT_TAKEOFF:
            auto_launch_detected = !throttle_suppressed && (g.takeoff_throttle_min_accel > 0);

            if (been_auto_flying || // failed hand launch
                auto_launch_detected) { // threshold of been_auto_flying may not be met on auto-launches

                // has launched but is no longer flying. That's a crash on takeoff.
                crashed = true;
            }
            break;

        case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
        case AP_SpdHgtControl::FLIGHT_NORMAL:
            if (been_auto_flying) {
                crashed = true;
            }
            // TODO: handle auto missions without NAV_TAKEOFF mission cmd
            break;

        case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
            if (been_auto_flying) {
                crashed = true;
            }
            // when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL
            // so ground crashes most likely can not be triggered from here. However,
            // a crash into a tree, for example, would be.
            break;

        case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
            // We should be nice and level-ish in this flight stage. If not, we most
            // likely had a crazy landing. Throttle is inhibited already at the flare
            // but go ahead and notify GCS and perform any additional post-crash actions.
            // Declare a crash if we are oriented more that 60deg in pitch or roll
            if (been_auto_flying &&
                !crash_state.checkHardLanding && // only check once
                (fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) {
                crashed = true;

                // did we "crash" within 75m of the landing location? Probably just a hard landing
                crashed_near_land_waypoint =
                        get_distance(current_loc, mission.get_current_nav_cmd().content.location) < 75;

                // trigger hard landing event right away, or never again. This inhibits a false hard landing
                // event when, for example, a minute after a good landing you pick the plane up and
                // this logic is still running and detects the plane is on its side as you carry it.
                crash_state.debounce_timer_ms = now_ms + 2500;
            }
            crash_state.checkHardLanding = true;
            break;
        } // switch
    } else {
        crash_state.checkHardLanding = false;
    }

    if (!crashed) {
        // reset timer
        crash_state.debounce_timer_ms = 0;

    } else if (crash_state.debounce_timer_ms == 0) {
        // start timer
        crash_state.debounce_timer_ms = now_ms;

    } else if ((now_ms - crash_state.debounce_timer_ms >= 2500) && !crash_state.is_crashed) {
        crash_state.is_crashed = true;

        if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) {
            if (crashed_near_land_waypoint) {
                gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected. No action taken");
            } else {
                gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash detected. No action taken");
            }
        }
        else {
            if (g.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) {
                disarm_motors();
            }
            auto_state.land_complete = true;
            if (crashed_near_land_waypoint) {
                gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected");
            } else {
                gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash detected");
            }
        }
    }
}