void Analyzer_Position_Estimate_Divergence::update_result(std::string name,
                                                          double delta)
{
    if (delta > _result[name]->max_delta()) {
        _result[name]->set_max_delta(delta);
        if (delta >= delta_fail()) {
            _result[name]->set_delta_threshold(delta_fail());
            _result[name]->set_status(analyzer_status_fail);
            _result[name]->set_severity_score(20);
        } else if (delta >= delta_warn()) {
            _result[name]->set_status(analyzer_status_warn);
            _result[name]->set_delta_threshold(delta_warn());
            _result[name]->set_severity_score(10);
        }
    }
}
void Analyzer_Position_Estimate_Divergence::evaluate_estimate(
    const std::string name,
    AnalyzerVehicle::Position position,
    AnalyzerVehicle::Position estimate)
{
    if (estimate.lat_modtime() == 0) {
        // No estimate for this  yet
        return;
    }
    double delta = estimate.horizontal_distance_to(position);

    bool failing = (delta > delta_warn());
    // ::fprintf(stderr, "%s: delta=%f delta_warn=%f: %s\n", name.c_str(), delta, delta_warn(), failing ? "FAILING" : "");

    if (_result.count(name) == 0 ||
        _result[name] == NULL) {
        // no current problem
        if (failing) {
            open_result(name, delta);
        }
    } else {
        // problem currently underway
        if (failing) {
            update_result(name, delta);
        } else {
            close_result(name);
        }
    }
}
void Analyzer_Attitude_Estimate_Divergence::evaluate_estimate(
    std::string name,
    AnalyzerVehicle::Attitude attitude,
    AnalyzerVehicle::Attitude estimate)
{
    if (estimate.roll_modtime() == 0) {
        // No estimate for this  yet
        return;
    }

    double delta_roll = fabs(angle_delta(estimate.roll(), attitude.roll()));
    double delta_pitch = fabs(angle_delta(estimate.pitch(), attitude.pitch()));
    // double delta_yaw = estimate.yaw() - attitude.yaw();

    double delta = delta_roll > delta_pitch ? delta_roll : delta_pitch;

    bool failing = (delta >= delta_warn());
    if (_result[name] == NULL) {
        if (failing) {
            open_result(name, delta);
        }
    } else {
        if (failing) {
            update_result(name, delta);
        } else {
            close_result(name);
        }
    }
}
void Analyzer_Estimate_Divergence::update_result_set_status(Analyzer_Estimate_Divergence_Result *result)
{
    if (fabs(result->max_delta()) >= delta_fail()) {
        result->set_status(analyzer_status_fail);
    } else if (fabs(result->max_delta()) >= delta_warn()) {
        result->set_status(analyzer_status_warn);
    }
 }
void Analyzer_Estimate_Divergence::update_result(std::string name, double delta)
{
    Analyzer_Estimate_Divergence_Result *result = _result[name];
    if (fabs(delta) > fabs(result->max_delta())) {
        result->set_max_delta(delta);

        update_result_set_status(result);

        if (result->status() == analyzer_status_fail) {
            result->set_delta_threshold(delta_fail());
            result->set_severity_score(20);
        } else if (result->status() == analyzer_status_warn) {
            result->set_delta_threshold(delta_warn());
            result->set_severity_score(10);
        }
    }
}
void Analyzer_Velocity_Estimate_Divergence::evaluate_estimate(
    const std::string name,
    AnalyzerVehicle::Velocity &velocity,
    AnalyzerVehicle::Velocity &estimate)
{
    if (estimate.velocity_modtime() == 0) {
        // No estimate for this  yet
        return;
    }

    // make sure we're comparing apples with apples:
    double velocity_size;
    if (estimate.is_2d()) {
        velocity_size = velocity.size_2d();
    } else {
        velocity_size = velocity.size();
    }
    double delta = estimate.size() - velocity_size;
    // ::fprintf(stderr, "delta: %f\n", delta);

    bool failing = (delta > delta_warn());
    // ::fprintf(stderr, "%s: delta=%f delta_warn=%f: %s\n", name.c_str(), delta, delta_warn(), failing ? "FAILING" : "");

    if (_result.count(name) == 0 ||
        _result[name] == NULL) {
        // no current problem
        if (failing) {
            open_result(name, delta);
        }
    } else {
        // problem currently underway
        if (failing) {
            update_result(name, delta);
        } else {
            close_result(name);
        }
    }
}