ObstacleModel.cpp

/*
  ObstacleModel.cpp
  ---------------------------------------------------------------------------
  The detector implementations and the engine that runs them.

  Scroll to DETECTORS[] at the bottom to see the full registry. That array is
  the single place where models are added or removed.
*/

#include "ObstacleModel.h"

// =============================================================================
// Small helpers
// =============================================================================

const char* severityName(DetectorSeverity severity) {
  switch (severity) {
    case DetectorSeverity::ADVISORY:
      return "advisory";

    case DetectorSeverity::HARD:
      return "hard";

    default:
      return "vote";
  }
}

const char* motionDirectionName(MotionDirection direction) {
  switch (direction) {
    case MotionDirection::OPEN:
      return "OPEN";

    case MotionDirection::CLOSE:
      return "CLOSE";

    default:
      return "NONE";
  }
}

static float medianOfCopy(float* values, uint8_t count) {
  if (count == 0) {
    return 0.0f;
  }

  // Insertion sort. Count is at most 40 and this runs at 10 Hz.
  for (uint8_t i = 1; i < count; i++) {
    float key = values[i];
    int16_t j = (int16_t)i - 1;

    while (j >= 0 && values[j] > key) {
      values[j + 1] = values[j];
      j--;
    }

    values[j + 1] = key;
  }

  if (count & 1) {
    return values[count / 2];
  }

  return 0.5f * (values[count / 2 - 1] + values[count / 2]);
}

// =============================================================================
// ObstacleDetector base
// =============================================================================

ObstacleDetector::ObstacleDetector(const char* name,
                                   const char* description,
                                   DetectorSeverity severity,
                                   bool enabled)
    : name_(name),
      description_(description),
      severity_(severity),
      enabled_(enabled),
      asserted_(false),
      tripped_(false),
      trippedAtMs_(0) {}

void ObstacleDetector::beginRun(MotionDirection direction) {
  asserted_ = false;
  tripped_ = false;
  trippedAtMs_ = 0;

  onRunStart(direction);
}

bool ObstacleDetector::run(const ModelContext& context) {
  if (!enabled_) {
    asserted_ = false;
    return false;
  }

  asserted_ = evaluate(context);

  if (asserted_ && !tripped_) {
    tripped_ = true;
    trippedAtMs_ = context.elapsedMs;
  }

  return asserted_;
}

// =============================================================================
// Detector: current sensor health
//
// Checks the measurement chain itself rather than the motor. Registered first
// so that when the front end is broken the reported cause is the front end,
// not the saturation and rail-sag symptoms it produces downstream.
//
// Does not lock the direction: a broken sensor says nothing about where the
// cover is, so demanding a reverse move to clear it would be nonsense.
// =============================================================================

class SensorHealthDetector : public ObstacleDetector {
 public:
  SensorHealthDetector()
      : ObstacleDetector("sensor",
                         "Current sensor DC midpoint out of range",
                         DetectorSeverity::HARD,
                         true) {}

  void onRunStart(MotionDirection) override { streak_ = 0; }

  bool evaluate(const ModelContext& context) override {
    if (!context.sampleUsable) {
      return false;
    }

    float deviation = context.sample.mean - SENSOR_MIDPOINT_EXPECTED;

    if (deviation < 0.0f) {
      deviation = -deviation;
    }

    if (deviation > SENSOR_MIDPOINT_TOLERANCE) {
      streak_++;
    } else {
      streak_ = 0;
    }

    return streak_ >= DET_SENSOR_CONSECUTIVE;
  }

  const char* reason() const override { return "current_sensor_fault"; }

  bool locksDirection() const override { return false; }

 private:
  uint8_t streak_ = 0;
};

// =============================================================================
// Detector 1: absolute RMS threshold
//
// The workhorse. Direction-specific ceiling sitting ~25-30% above the worst
// normal-travel sample ever recorded and well below the end-stop load.
// =============================================================================

class AbsoluteThresholdDetector : public ObstacleDetector {
 public:
  AbsoluteThresholdDetector()
      : ObstacleDetector("abs",
                         "RMS above a direction-specific ceiling",
                         DetectorSeverity::VOTE,
                         true) {}

  void onRunStart(MotionDirection direction) override {
    threshold_ = (direction == MotionDirection::OPEN)
                     ? DET_ABS_OPEN_COUNTS
                     : DET_ABS_CLOSE_COUNTS;
    streak_ = 0;
  }

  bool evaluate(const ModelContext& context) override {
    if (!context.pastBlanking || !context.sampleUsable) {
      streak_ = 0;
      return false;
    }

    if (context.sample.rms >= threshold_) {
      streak_++;
    } else {
      streak_ = 0;
    }

    return streak_ >= DET_ABS_CONSECUTIVE;
  }

  const char* reason() const override { return "current_above_limit"; }

 private:
  float threshold_ = DET_ABS_OPEN_COUNTS;
  uint8_t streak_ = 0;
};

// =============================================================================
// Detector 2: rate of rise
//
// Catches a fast-developing block before it reaches the absolute ceiling.
// Steady travel never moves more than ~66 counts per 300 ms; the end stop
// moves 200-300.
// =============================================================================

class SlopeDetector : public ObstacleDetector {
 public:
  SlopeDetector()
      : ObstacleDetector("slope",
                         "Current rising faster than travel ever does",
                         DetectorSeverity::VOTE,
                         true) {}

  void onRunStart(MotionDirection) override { streak_ = 0; }

  bool evaluate(const ModelContext& context) override {
    if (!context.pastBlanking || !context.sampleUsable || !context.slopeValid) {
      streak_ = 0;
      return false;
    }

    if (context.slope >= DET_SLOPE_COUNTS) {
      streak_++;
    } else {
      streak_ = 0;
    }

    return streak_ >= DET_SLOPE_CONSECUTIVE;
  }

  const char* reason() const override { return "current_rising_fast"; }

 private:
  uint8_t streak_ = 0;
};

// =============================================================================
// Detector 3: relative to rolling baseline
//
// Self-calibrating: adapts to a heavier cover, colder grease or a wetter day
// without new thresholds. Needs a settled baseline, so it arms later.
// =============================================================================

class RelativeBaselineDetector : public ObstacleDetector {
 public:
  RelativeBaselineDetector()
      : ObstacleDetector("rel",
                         "Current above a multiple of this run's own baseline",
                         DetectorSeverity::VOTE,
                         true) {}

  void onRunStart(MotionDirection) override { streak_ = 0; }

  bool evaluate(const ModelContext& context) override {
    if (!context.baselineValid || !context.sampleUsable ||
        context.elapsedMs < MODEL_BASELINE_ARM_MS) {
      streak_ = 0;
      return false;
    }

    if (context.ratioToBaseline >= DET_REL_FACTOR) {
      streak_++;
    } else {
      streak_ = 0;
    }

    return streak_ >= DET_REL_CONSECUTIVE;
  }

  const char* reason() const override { return "current_above_baseline"; }

 private:
  uint8_t streak_ = 0;
};

// =============================================================================
// Detector: slow drift against a latched baseline
//
// The rolling baseline adapts, which is exactly what makes it robust to a
// heavier cover or a cold morning - and exactly what lets a slowly developing
// problem drag the baseline along with it and never look abnormal.
//
// This one latches a baseline from early travel and never moves it, so a
// gradual climb has something fixed to be measured against.
// =============================================================================

class DriftDetector : public ObstacleDetector {
 public:
  DriftDetector()
      : ObstacleDetector("drift",
                         "Slow climb above a baseline latched early in the run",
                         DetectorSeverity::VOTE,
                         true) {}

  void onRunStart(MotionDirection) override { streak_ = 0; }

  bool evaluate(const ModelContext& context) override {
    if (!context.latchedValid || !context.sampleUsable ||
        !context.pastBlanking) {
      streak_ = 0;
      return false;
    }

    if (context.ratioToLatched >= DET_DRIFT_FACTOR) {
      streak_++;
    } else {
      streak_ = 0;
    }

    return streak_ >= DET_DRIFT_CONSECUTIVE;
  }

  const char* reason() const override { return "load_drifted_up"; }

 private:
  uint8_t streak_ = 0;
};

// =============================================================================
// Detector 4: measurement saturation
//
// Hitting the rail means current is beyond what this sensor and ADC can
// express. Outside the blanking window that only happens at a hard stop, so
// it stops the motor on its own.
// =============================================================================

class SaturationDetector : public ObstacleDetector {
 public:
  SaturationDetector()
      : ObstacleDetector("sat",
                         "Reading pinned at the measurable limit",
                         DetectorSeverity::HARD,
                         true) {}

  void onRunStart(MotionDirection) override { streak_ = 0; }

  bool evaluate(const ModelContext& context) override {
    // A starved window can rail on a single stray reading. Requiring a usable
    // window keeps a HARD detector from stopping the cover on bad data.
    if (!context.pastBlanking || !context.sampleUsable) {
      streak_ = 0;
      return false;
    }

    if (context.clipped) {
      streak_++;
    } else {
      streak_ = 0;
    }

    return streak_ >= DET_SAT_CONSECUTIVE;
  }

  const char* reason() const override { return "current_out_of_range"; }

 private:
  uint8_t streak_ = 0;
};

// =============================================================================
// Detector 5: waveform swing asymmetry
//
// The positive half of the waveform clips before the negative half, so the
// swing ratio keeps climbing after RMS has started to compress. This is the
// detector that still works when the reading is no longer linear.
// =============================================================================

class SwingAsymmetryDetector : public ObstacleDetector {
 public:
  SwingAsymmetryDetector()
      : ObstacleDetector("swing",
                         "Waveform lopsided, the positive half is clipping",
                         DetectorSeverity::VOTE,
                         true) {}

  void onRunStart(MotionDirection) override { streak_ = 0; }

  bool evaluate(const ModelContext& context) override {
    if (!context.pastBlanking || !context.sampleUsable) {
      streak_ = 0;
      return false;
    }

    // Only meaningful once there is real load; at low current the ratio is
    // dominated by quantisation noise.
    if (context.sample.rms < DET_SWING_MIN_RMS || context.swingRatio <= 0.0f) {
      streak_ = 0;
      return false;
    }

    if (context.swingRatio >= DET_SWING_RATIO) {
      streak_++;
    } else {
      streak_ = 0;
    }

    return streak_ >= DET_SWING_CONSECUTIVE;
  }

  const char* reason() const override { return "waveform_asymmetric"; }

 private:
  uint8_t streak_ = 0;
};

// =============================================================================
// Detector 6: supply rail sag
//
// The ACS712 zero point tracks its 5 V supply. Heavy motor load drags the rail
// down and the measured DC mean falls with it. Completely independent of the
// AC amplitude path, so it is a genuine second opinion.
// =============================================================================

class RailSagDetector : public ObstacleDetector {
 public:
  RailSagDetector()
      : ObstacleDetector("sag",
                         "Sensor supply sagging under load",
                         DetectorSeverity::VOTE,
                         true) {}

  void onRunStart(MotionDirection) override { streak_ = 0; }

  bool evaluate(const ModelContext& context) override {
    if (!context.baselineValid || !context.pastBlanking ||
        !context.sampleUsable) {
      streak_ = 0;
      return false;
    }

    if (context.meanSag >= DET_SAG_COUNTS) {
      streak_++;
    } else {
      streak_ = 0;
    }

    return streak_ >= DET_SAG_CONSECUTIVE;
  }

  const char* reason() const override { return "supply_sag"; }

 private:
  uint8_t streak_ = 0;
};

// =============================================================================
// Detector 7: clutch slip / load collapse
//
// At the CLOSE end stop the drive loads up, slips, and current collapses to
// the free-running value before rebuilding, roughly once a second. Seeing that
// collapse proves the cover already reached a stop, so it stops on its own.
//
// This is also the backstop against a rule that mistakes the collapse for
// "load returned to normal, carry on".
// =============================================================================

class SlipCycleDetector : public ObstacleDetector {
 public:
  SlipCycleDetector()
      : ObstacleDetector("slip",
                         "Load built up then collapsed: drive is slipping",
                         DetectorSeverity::HARD,
                         true) {}

  void onRunStart(MotionDirection) override {
    highAtMs_ = 0;
    sawHigh_ = false;
  }

  bool evaluate(const ModelContext& context) override {
    if (!context.baselineValid || !context.pastBlanking ||
        !context.sampleUsable) {
      return false;
    }

    if (context.ratioToBaseline >= DET_SLIP_HIGH_FACTOR) {
      sawHigh_ = true;
      highAtMs_ = context.elapsedMs;
      return false;
    }

    if (!sawHigh_) {
      return false;
    }

    if (context.elapsedMs - highAtMs_ > DET_SLIP_WINDOW_MS) {
      sawHigh_ = false;
      return false;
    }

    return context.ratioToBaseline <= DET_SLIP_LOW_FACTOR;
  }

  const char* reason() const override { return "drive_slipping"; }

 private:
  bool sawHigh_ = false;
  unsigned long highAtMs_ = 0;
};

// =============================================================================
// Detector 8: motor never started
//
// Idle is 12.7 counts and a spinning motor is at least ~320. Near-zero current
// after the relay closed means failed relay, blown fuse, open winding or a
// disconnected sensor. Running the timeout in that state is pointless.
// =============================================================================

class MotorStartDetector : public ObstacleDetector {
 public:
  MotorStartDetector()
      : ObstacleDetector("nostart",
                         "No motor current after the relay closed",
                         DetectorSeverity::HARD,
                         true) {}

  bool evaluate(const ModelContext& context) override {
    if (context.elapsedMs < DET_NOSTART_FROM_MS ||
        context.elapsedMs > DET_NOSTART_UNTIL_MS) {
      return false;
    }

    // A half-filled window would under-report RMS and look like a dead motor.
    if (!context.sampleUsable) {
      return false;
    }

    return context.sample.rms < DET_NOSTART_COUNTS;
  }

  const char* reason() const override { return "no_motor_current"; }

  // An electrical fault says nothing about where the cover is, so it must not
  // demand a reverse move before trying again.
  bool locksDirection() const override { return false; }
};

// =============================================================================
// Detector 9: hard travel limit
//
// Backstop for the case where every current-based detector misses. Sized at
// ~1.25x the longest observed run for that direction.
// =============================================================================

class TravelLimitDetector : public ObstacleDetector {
 public:
  TravelLimitDetector()
      : ObstacleDetector("timeout",
                         "Travelled longer than the hard limit",
                         DetectorSeverity::HARD,
                         true) {}

  void onRunStart(MotionDirection direction) override {
    limitMs_ = (direction == MotionDirection::OPEN) ? MODEL_MAX_OPEN_MS
                                                    : MODEL_MAX_CLOSE_MS;
  }

  bool evaluate(const ModelContext& context) override {
    return context.elapsedMs >= limitMs_;
  }

  const char* reason() const override { return "travel_timeout"; }

 private:
  unsigned long limitMs_ = MODEL_MAX_OPEN_MS;
};

// =============================================================================
// Detector 10: overrun advisory
//
// Never stops the motor. Flags that this run is already longer than any run in
// the training data, which is useful context in the logs when something else
// trips a moment later.
// =============================================================================

class OverrunAdvisoryDetector : public ObstacleDetector {
 public:
  OverrunAdvisoryDetector()
      : ObstacleDetector("overrun",
                         "Past the expected travel time for this direction",
                         DetectorSeverity::ADVISORY,
                         true) {}

  bool evaluate(const ModelContext& context) override {
    if (context.expectedMs == 0) {
      return false;
    }

    return context.elapsedMs >=
           (unsigned long)(context.expectedMs * MODEL_OVERRUN_FACTOR);
  }

  const char* reason() const override { return "past_expected_time"; }
};

// =============================================================================
// THE REGISTRY
//
// Add a model: create the instance above, add one line here.
// Remove a model: delete its line here (or use "model <name> off" at runtime).
// The order is the order of evaluation and of the telemetry flag bits.
// =============================================================================

static SensorHealthDetector detSensorHealth;
static AbsoluteThresholdDetector detAbsolute;
static SlopeDetector detSlope;
static RelativeBaselineDetector detRelative;
static DriftDetector detDrift;
static SaturationDetector detSaturation;
static SwingAsymmetryDetector detSwing;
static RailSagDetector detRailSag;
static SlipCycleDetector detSlip;
static MotorStartDetector detMotorStart;
static TravelLimitDetector detTravelLimit;
static OverrunAdvisoryDetector detOverrun;

static ObstacleDetector* const DETECTORS[] = {
    // First: a broken sensor must be reported as such, not as the saturation
    // and sag symptoms it causes.
    &detSensorHealth,
    &detAbsolute,
    &detSlope,
    &detRelative,
    &detDrift,
    &detSaturation,
    &detSwing,
    &detRailSag,
    &detSlip,
    &detMotorStart,
    &detTravelLimit,
    &detOverrun,
};

static const uint8_t DETECTOR_COUNT =
    sizeof(DETECTORS) / sizeof(DETECTORS[0]);

ObstacleModel obstacleModel;

// =============================================================================
// Engine
// =============================================================================

void ObstacleModel::begin() {
  historyCount_ = 0;
  historyHead_ = 0;
  windowIndex_ = 0;
  latchCount_ = 0;
  latchedBaseline_ = 0.0f;
  latched_ = false;
  flagString_[0] = '\0';
}

uint8_t ObstacleModel::detectorCount() const {
  return DETECTOR_COUNT;
}

ObstacleDetector* ObstacleModel::detectorAt(uint8_t index) const {
  if (index >= DETECTOR_COUNT) {
    return nullptr;
  }

  return DETECTORS[index];
}

ObstacleDetector* ObstacleModel::detectorByName(const char* name) const {
  for (uint8_t i = 0; i < DETECTOR_COUNT; i++) {
    if (strcmp(DETECTORS[i]->name(), name) == 0) {
      return DETECTORS[i];
    }
  }

  return nullptr;
}

uint8_t ObstacleModel::enabledCount() const {
  uint8_t count = 0;

  for (uint8_t i = 0; i < DETECTOR_COUNT; i++) {
    if (DETECTORS[i]->enabled()) {
      count++;
    }
  }

  return count;
}

void ObstacleModel::onRunStart(MotionDirection direction) {
  direction_ = direction;
  historyCount_ = 0;
  historyHead_ = 0;
  windowIndex_ = 0;
  latchCount_ = 0;
  latchedBaseline_ = 0.0f;
  latched_ = false;
  flagString_[0] = '\0';

  memset(&context_, 0, sizeof(context_));
  context_.direction = direction;

  for (uint8_t i = 0; i < DETECTOR_COUNT; i++) {
    DETECTORS[i]->beginRun(direction);
  }
}

void ObstacleModel::onRunEnd() {
  direction_ = MotionDirection::NONE;
}

void ObstacleModel::pushHistory(const CurrentSample& sample,
                                unsigned long elapsedMs) {
  history_[historyHead_].elapsedMs = elapsedMs;
  history_[historyHead_].rms = sample.rms;
  history_[historyHead_].mean = sample.mean;

  historyHead_ = (historyHead_ + 1) % HISTORY_CAPACITY;

  if (historyCount_ < HISTORY_CAPACITY) {
    historyCount_++;
  }
}

void ObstacleModel::updateBaseline() {
  context_.baselineValid = false;
  context_.baselineRms = 0.0f;
  context_.baselineMean = 0.0f;
  context_.ratioToBaseline = 0.0f;
  context_.meanSag = 0.0f;

  if (context_.elapsedMs < MODEL_BASELINE_SKIP_MS + 1000UL) {
    return;
  }

  float rmsValues[HISTORY_CAPACITY];
  float meanValues[HISTORY_CAPACITY];
  uint8_t count = 0;

  unsigned long newest = context_.elapsedMs - MODEL_BASELINE_SKIP_MS;
  unsigned long oldest =
      (newest > MODEL_BASELINE_SPAN_MS) ? newest - MODEL_BASELINE_SPAN_MS : 0;

  for (uint8_t i = 0; i < historyCount_; i++) {
    const HistoryEntry& entry = history_[i];

    if (entry.elapsedMs <= newest && entry.elapsedMs >= oldest) {
      rmsValues[count] = entry.rms;
      meanValues[count] = entry.mean;
      count++;
    }
  }

  // Fewer than a second of settled history is not a baseline.
  if (count < 10) {
    return;
  }

  context_.baselineRms = medianOfCopy(rmsValues, count);
  context_.baselineMean = medianOfCopy(meanValues, count);
  context_.baselineValid = context_.baselineRms > 1.0f;

  if (context_.baselineValid) {
    context_.ratioToBaseline = context_.sample.rms / context_.baselineRms;
    context_.meanSag = context_.baselineMean - context_.sample.mean;
  }
}

/*
  Collect samples from the early part of travel, then freeze their median for
  the rest of the run. Unlike the rolling baseline this never follows the
  signal, so a slow climb cannot hide inside it.
*/
void ObstacleModel::updateLatchedBaseline() {
  if (!latched_) {
    if (context_.elapsedMs >= MODEL_LATCH_FROM_MS &&
        context_.elapsedMs <= MODEL_LATCH_TO_MS &&
        context_.sampleUsable &&
        latchCount_ < LATCH_CAPACITY) {
      latchSamples_[latchCount_++] = context_.sample.rms;
    }

    if (context_.elapsedMs > MODEL_LATCH_TO_MS &&
        latchCount_ >= MODEL_LATCH_MIN_SAMPLES) {
      latchedBaseline_ = medianOfCopy(latchSamples_, latchCount_);
      latched_ = latchedBaseline_ > 1.0f;
    }
  }

  context_.latchedValid = latched_;
  context_.latchedBaseline = latchedBaseline_;
  context_.ratioToLatched =
      latched_ ? (context_.sample.rms / latchedBaseline_) : 0.0f;
}

void ObstacleModel::updateSlope() {
  context_.slope = 0.0f;
  context_.slopeValid = false;

  if (historyCount_ == 0) {
    return;
  }

  // Find the most recent history entry at least DET_SLOPE_SPAN_MS old.
  unsigned long target = DET_SLOPE_SPAN_MS;

  if (context_.elapsedMs < target) {
    return;
  }

  unsigned long cutoff = context_.elapsedMs - target;
  bool found = false;
  unsigned long bestAge = 0;
  float bestRms = 0.0f;

  for (uint8_t i = 0; i < historyCount_; i++) {
    const HistoryEntry& entry = history_[i];

    if (entry.elapsedMs > cutoff) {
      continue;
    }

    unsigned long age = context_.elapsedMs - entry.elapsedMs;

    if (!found || age < bestAge) {
      found = true;
      bestAge = age;
      bestRms = entry.rms;
    }
  }

  // Reject a stale comparison point: dropped windows would inflate the slope.
  if (!found || bestAge > (unsigned long)(DET_SLOPE_SPAN_MS + 200)) {
    return;
  }

  context_.slope = context_.sample.rms - bestRms;
  context_.slopeValid = true;
}

void ObstacleModel::buildContext(const CurrentSample& sample,
                                 unsigned long elapsedMs) {
  context_.direction = direction_;
  context_.elapsedMs = elapsedMs;
  context_.windowIndex = windowIndex_;
  context_.sample = sample;

  context_.negSwing = sample.mean - (float)sample.minRaw;
  context_.posSwing = (float)sample.maxRaw - sample.mean;
  context_.swingRatio =
      (context_.posSwing > 1.0f) ? (context_.negSwing / context_.posSwing) : 0.0f;

  context_.clipped =
      (sample.maxRaw >= DET_SAT_MAX_COUNTS) || (sample.minRaw <= DET_SAT_MIN_COUNTS);

  context_.expectedMs = (direction_ == MotionDirection::OPEN)
                            ? MODEL_EXPECTED_OPEN_MS
                            : MODEL_EXPECTED_CLOSE_MS;

  context_.pastBlanking = elapsedMs >= MODEL_BLANKING_MS;
  context_.sampleUsable = sample.sampleCount >= MODEL_MIN_ADC_SAMPLES;

  updateSlope();
  updateBaseline();
  updateLatchedBaseline();
}

void ObstacleModel::buildFlagString(uint16_t flags) {
  flagString_[0] = '\0';

  if (flags == 0) {
    return;
  }

  size_t used = 0;

  for (uint8_t i = 0; i < DETECTOR_COUNT && i < 16; i++) {
    if ((flags & (1u << i)) == 0) {
      continue;
    }

    const char* name = DETECTORS[i]->name();
    size_t need = strlen(name) + (used ? 1 : 0);

    if (used + need + 1 >= sizeof(flagString_)) {
      break;
    }

    if (used) {
      flagString_[used++] = ',';
    }

    strcpy(&flagString_[used], name);
    used += strlen(name);
  }

  flagString_[used] = '\0';
}

ModelResult ObstacleModel::update(const CurrentSample& sample,
                                  unsigned long elapsedMs) {
  ModelResult result;
  result.stop = false;
  result.detector = "";
  result.reason = "";
  result.votes = 0;
  result.flags = 0;
  result.evaluated = false;
  result.locksDirection = false;

  if (!MODEL_ENABLED || direction_ == MotionDirection::NONE) {
    flagString_[0] = '\0';
    return result;
  }

  buildContext(sample, elapsedMs);
  result.evaluated = true;

  ObstacleDetector* hardTrip = nullptr;
  ObstacleDetector* firstVote = nullptr;

  for (uint8_t i = 0; i < DETECTOR_COUNT; i++) {
    ObstacleDetector* detector = DETECTORS[i];

    if (!detector->run(context_)) {
      continue;
    }

    if (i < 16) {
      result.flags |= (uint16_t)(1u << i);
    }

    switch (detector->severity()) {
      case DetectorSeverity::HARD:
        if (hardTrip == nullptr) {
          hardTrip = detector;
        }
        break;

      case DetectorSeverity::VOTE:
        result.votes++;
        if (firstVote == nullptr) {
          firstVote = detector;
        }
        break;

      default:
        break;
    }
  }

  buildFlagString(result.flags);

  // History is pushed after evaluation so a detector never compares a sample
  // against itself.
  pushHistory(sample, elapsedMs);
  windowIndex_++;

  if (hardTrip != nullptr) {
    result.stop = true;
    result.detector = hardTrip->name();
    result.reason = hardTrip->reason();
    result.locksDirection = hardTrip->locksDirection();
    return result;
  }

  if (result.votes >= MODEL_REQUIRED_VOTES && firstVote != nullptr) {
    result.stop = true;
    result.detector = firstVote->name();
    result.reason = firstVote->reason();
    result.locksDirection = firstVote->locksDirection();
  }

  return result;
}