Add central function scheduler (#609)

* Add central function scheduler

* Avoid unnecessary copies

* Lint

* Prevent more copies, store pointers

* Add never update_interval

esphome/core/application.cpp

@@ -37,6 +37,7 @@ void Application::setup() {
       continue;
 
     component->call_setup();
+    this->scheduler.process_to_add();
     if (component->can_proceed())
       continue;
 
@@ -45,6 +46,7 @@ void Application::setup() {
 
     do {
       uint32_t new_app_state = STATUS_LED_WARNING;
+      this->scheduler.call();
       for (uint32_t j = 0; j <= i; j++) {
         if (!this->components_[j]->is_failed()) {
           this->components_[j]->call_loop();
@@ -63,6 +65,8 @@ void Application::setup() {
 void Application::loop() {
   uint32_t new_app_state = 0;
   const uint32_t start = millis();
+
+  this->scheduler.call();
   for (Component *component : this->components_) {
     if (!component->is_failed()) {
       component->call_loop();
@@ -72,6 +76,7 @@ void Application::loop() {
     this->feed_wdt();
   }
   this->app_state_ = new_app_state;
+
   const uint32_t end = millis();
   if (end - start > 200) {
     ESP_LOGV(TAG, "A component took a long time in a loop() cycle (%.1f s).", (end - start) / 1e3f);
@@ -87,6 +92,12 @@ void Application::loop() {
     uint32_t delay_time = this->loop_interval_;
     if (now - this->last_loop_ < this->loop_interval_)
       delay_time = this->loop_interval_ - (now - this->last_loop_);
+
+    uint32_t next_schedule = this->scheduler.next_schedule_in().value_or(delay_time);
+    // next_schedule is max 0.5*delay_time
+    // otherwise interval=0 schedules result in constant looping with almost no sleep
+    next_schedule = std::max(next_schedule, delay_time / 2);
+    delay_time = std::min(next_schedule, delay_time);
     delay(delay_time);
   }
   this->last_loop_ = now;

esphome/core/application.h

@@ -6,6 +6,7 @@
 #include "esphome/core/preferences.h"
 #include "esphome/core/component.h"
 #include "esphome/core/helpers.h"
+#include "esphome/core/scheduler.h"
 
 #ifdef USE_BINARY_SENSOR
 #include "esphome/components/binary_sensor/binary_sensor.h"
@@ -197,6 +198,8 @@ class Application {
   }
 #endif
 
+  Scheduler scheduler;
+
  protected:
   friend Component;
 

esphome/core/component.cpp

@@ -1,5 +1,3 @@
-#include <algorithm>
-
 #include "esphome/core/component.h"
 #include "esphome/core/helpers.h"
 #include "esphome/core/esphal.h"
@@ -45,51 +43,19 @@ void Component::setup() {}
 void Component::loop() {}
 
 void Component::set_interval(const std::string &name, uint32_t interval, std::function<void()> &&f) {  // NOLINT
-  const uint32_t now = millis();
-  // only put offset in lower half
-  uint32_t offset = 0;
-  if (interval != 0)
-    offset = (random_uint32() % interval) / 2;
-  ESP_LOGVV(TAG, "set_interval(name='%s', interval=%u, offset=%u)", name.c_str(), interval, offset);
-
-  if (!name.empty()) {
-    this->cancel_interval(name);
-  }
-  struct TimeFunction function = {
-      .name = name,
-      .type = TimeFunction::INTERVAL,
-      .interval = interval,
-      .last_execution = now - interval - offset,
-      .f = std::move(f),
-      .remove = false,
-  };
-  this->time_functions_.push_back(function);
+  App.scheduler.set_interval(this, name, interval, std::move(f));
 }
 
 bool Component::cancel_interval(const std::string &name) {  // NOLINT
-  return this->cancel_time_function_(name, TimeFunction::INTERVAL);
+  return App.scheduler.cancel_interval(this, name);
 }
 
 void Component::set_timeout(const std::string &name, uint32_t timeout, std::function<void()> &&f) {  // NOLINT
-  const uint32_t now = millis();
-  ESP_LOGVV(TAG, "set_timeout(name='%s', timeout=%u)", name.c_str(), timeout);
-
-  if (!name.empty()) {
-    this->cancel_timeout(name);
-  }
-  struct TimeFunction function = {
-      .name = name,
-      .type = TimeFunction::TIMEOUT,
-      .interval = timeout,
-      .last_execution = now,
-      .f = std::move(f),
-      .remove = false,
-  };
-  this->time_functions_.push_back(function);
+  return App.scheduler.set_timeout(this, name, timeout, std::move(f));
 }
 
 bool Component::cancel_timeout(const std::string &name) {  // NOLINT
-  return this->cancel_time_function_(name, TimeFunction::TIMEOUT);
+  return App.scheduler.cancel_timeout(this, name);
 }
 
 void Component::call_loop() {
@@ -97,17 +63,6 @@ void Component::call_loop() {
   this->loop();
 }
 
-bool Component::cancel_time_function_(const std::string &name, TimeFunction::Type type) {
-  // NOLINTNEXTLINE
-  for (auto iter = this->time_functions_.begin(); iter != this->time_functions_.end(); iter++) {
-    if (!iter->remove && iter->name == name && iter->type == type) {
-      ESP_LOGVV(TAG, "Removing old time function %s.", iter->name.c_str());
-      iter->remove = true;
-      return true;
-    }
-  }
-  return false;
-}
 void Component::call_setup() {
   this->setup_internal_();
   this->setup();
@@ -116,34 +71,6 @@ uint32_t Component::get_component_state() const { return this->component_state_;
 void Component::loop_internal_() {
   this->component_state_ &= ~COMPONENT_STATE_MASK;
   this->component_state_ |= COMPONENT_STATE_LOOP;
-
-  for (unsigned int i = 0; i < this->time_functions_.size(); i++) {  // NOLINT
-    const uint32_t now = millis();
-    TimeFunction *tf = &this->time_functions_[i];
-    if (tf->should_run(now)) {
-#ifdef ESPHOME_LOG_HAS_VERY_VERBOSE
-      const char *type =
-          tf->type == TimeFunction::INTERVAL ? "interval" : (tf->type == TimeFunction::TIMEOUT ? "timeout" : "defer");
-      ESP_LOGVV(TAG, "Running %s '%s':%u with interval=%u last_execution=%u (now=%u)", type, tf->name.c_str(), i,
-                tf->interval, tf->last_execution, now);
-#endif
-
-      tf->f();
-      // The vector might have reallocated due to new items
-      tf = &this->time_functions_[i];
-
-      if (tf->type == TimeFunction::INTERVAL && tf->interval != 0) {
-        const uint32_t amount = (now - tf->last_execution) / tf->interval;
-        tf->last_execution += (amount * tf->interval);
-      } else if (tf->type == TimeFunction::DEFER || tf->type == TimeFunction::TIMEOUT) {
-        tf->remove = true;
-      }
-    }
-  }
-
-  this->time_functions_.erase(std::remove_if(this->time_functions_.begin(), this->time_functions_.end(),
-                                             [](const TimeFunction &tf) -> bool { return tf.remove; }),
-                              this->time_functions_.end());
 }
 void Component::setup_internal_() {
   this->component_state_ &= ~COMPONENT_STATE_MASK;
@@ -155,29 +82,20 @@ void Component::mark_failed() {
   this->component_state_ |= COMPONENT_STATE_FAILED;
   this->status_set_error();
 }
-void Component::defer(std::function<void()> &&f) { this->defer("", std::move(f)); }  // NOLINT
+void Component::defer(std::function<void()> &&f) {  // NOLINT
+  App.scheduler.set_timeout(this, "", 0, std::move(f));
+}
 bool Component::cancel_defer(const std::string &name) {  // NOLINT
-  return this->cancel_time_function_(name, TimeFunction::DEFER);
+  return App.scheduler.cancel_timeout(this, name);
 }
 void Component::defer(const std::string &name, std::function<void()> &&f) {  // NOLINT
-  if (!name.empty()) {
-    this->cancel_defer(name);
-  }
-  struct TimeFunction function = {
-      .name = name,
-      .type = TimeFunction::DEFER,
-      .interval = 0,
-      .last_execution = 0,
-      .f = std::move(f),
-      .remove = false,
-  };
-  this->time_functions_.push_back(function);
+  App.scheduler.set_timeout(this, name, 0, std::move(f));
 }
 void Component::set_timeout(uint32_t timeout, std::function<void()> &&f) {  // NOLINT
-  this->set_timeout("", timeout, std::move(f));
+  App.scheduler.set_timeout(this, "", timeout, std::move(f));
 }
 void Component::set_interval(uint32_t interval, std::function<void()> &&f) {  // NOLINT
-  this->set_interval("", interval, std::move(f));
+  App.scheduler.set_timeout(this, "", interval, std::move(f));
 }
 bool Component::is_failed() { return (this->component_state_ & COMPONENT_STATE_MASK) == COMPONENT_STATE_FAILED; }
 bool Component::can_proceed() { return true; }
@@ -203,7 +121,9 @@ void Component::status_momentary_error(const std::string &name, uint32_t length)
 }
 void Component::dump_config() {}
 float Component::get_actual_setup_priority() const {
-  return this->setup_priority_override_.value_or(this->get_setup_priority());
+  if (isnan(this->setup_priority_override_))
+    return this->get_setup_priority();
+  return this->setup_priority_override_;
 }
 void Component::set_setup_priority(float priority) { this->setup_priority_override_ = priority; }
 
@@ -240,12 +160,4 @@ void Nameable::calc_object_id_() {
 }
 uint32_t Nameable::get_object_id_hash() { return this->object_id_hash_; }
 
-bool Component::TimeFunction::should_run(uint32_t now) const {
-  if (this->remove)
-    return false;
-  if (this->type == DEFER)
-    return true;
-  return this->interval != 4294967295UL && now - this->last_execution > this->interval;
-}
-
 }  // namespace esphome

esphome/core/component.h

@@ -2,7 +2,7 @@
 
 #include <string>
 #include <functional>
-#include <vector>
+#include "Arduino.h"
 
 #include "esphome/core/optional.h"
 
@@ -207,31 +207,8 @@ class Component {
   void loop_internal_();
   void setup_internal_();
 
-  /// Internal struct for storing timeout/interval functions.
-  struct TimeFunction {
-    std::string name;                             ///< The name/id of this TimeFunction.
-    enum Type { TIMEOUT, INTERVAL, DEFER } type;  ///< The type of this TimeFunction. Either TIMEOUT, INTERVAL or DEFER.
-    uint32_t interval;                            ///< The interval/timeout of this function.
-    /// The last execution for interval functions and the time, SetInterval was called, for timeout functions.
-    uint32_t last_execution;
-    std::function<void()> f;  ///< The function (or callback) itself.
-    bool remove;
-
-    bool should_run(uint32_t now) const;
-  };
-
-  /// Cancel an only time function. If name is empty, won't do anything.
-  bool cancel_time_function_(const std::string &name, TimeFunction::Type type);
-
-  /** Storage for interval/timeout functions.
-   *
-   * Intentionally a vector despite its map-like nature, because of the
-   * memory overhead.
-   */
-  std::vector<TimeFunction> time_functions_;
-
   uint32_t component_state_{0x0000};  ///< State of this component.
-  optional<float> setup_priority_override_;
+  float setup_priority_override_{NAN};
 };
 
 /** This class simplifies creating components that periodically check a state.

esphome/core/scheduler.cpp

@@ -0,0 +1,193 @@
+#include "scheduler.h"
+#include "esphome/core/log.h"
+#include "esphome/core/helpers.h"
+#include <algorithm>
+
+namespace esphome {
+
+static const char *TAG = "scheduler";
+
+static const uint32_t SCHEDULER_DONT_RUN = 4294967295UL;
+
+void HOT Scheduler::set_timeout(Component *component, const std::string &name, uint32_t timeout,
+                                std::function<void()> &&func) {
+  const uint32_t now = millis();
+
+  if (!name.empty())
+    this->cancel_timeout(component, name);
+
+  if (timeout == SCHEDULER_DONT_RUN)
+    return;
+
+  ESP_LOGVV(TAG, "set_timeout(name='%s', timeout=%u)", name.c_str(), timeout);
+
+  auto *item = new SchedulerItem();
+  item->component = component;
+  item->name = name;
+  item->type = SchedulerItem::TIMEOUT;
+  item->timeout = timeout;
+  item->last_execution = now;
+  item->f = std::move(func);
+  item->remove = false;
+  this->push_(item);
+}
+bool HOT Scheduler::cancel_timeout(Component *component, const std::string &name) {
+  return this->cancel_item_(component, name, SchedulerItem::TIMEOUT);
+}
+void HOT Scheduler::set_interval(Component *component, const std::string &name, uint32_t interval,
+                                 std::function<void()> &&func) {
+  const uint32_t now = millis();
+
+  if (!name.empty())
+    this->cancel_interval(component, name);
+
+  if (interval == SCHEDULER_DONT_RUN)
+    return;
+
+  // only put offset in lower half
+  uint32_t offset = 0;
+  if (interval != 0)
+    offset = (random_uint32() % interval) / 2;
+
+  ESP_LOGVV(TAG, "set_interval(name='%s', interval=%u, offset=%u)", name.c_str(), interval, offset);
+
+  auto *item = new SchedulerItem();
+  item->component = component;
+  item->name = name;
+  item->type = SchedulerItem::INTERVAL;
+  item->interval = interval;
+  item->last_execution = now - offset;
+  item->f = std::move(func);
+  item->remove = false;
+  this->push_(item);
+}
+bool HOT Scheduler::cancel_interval(Component *component, const std::string &name) {
+  return this->cancel_item_(component, name, SchedulerItem::INTERVAL);
+}
+optional<uint32_t> HOT Scheduler::next_schedule_in() {
+  if (!this->peek_())
+    return {};
+  auto *item = this->items_[0];
+  const uint32_t now = millis();
+  uint32_t next_time = item->last_execution + item->interval;
+  if (next_time < now)
+    return 0;
+  return next_time - now;
+}
+void ICACHE_RAM_ATTR HOT Scheduler::call() {
+  const uint32_t now = millis();
+  this->process_to_add();
+
+  while (true) {
+    bool has_item = this->peek_();
+    if (!has_item)
+      // No more item left, done!
+      break;
+
+    // Don't copy-by value yet
+    auto *item = this->items_[0];
+    if ((now - item->last_execution) < item->interval)
+      // Not reached timeout yet, done for this call
+      break;
+
+    // Don't run on failed components
+    if (item->component != nullptr && item->component->is_failed()) {
+      this->pop_raw_();
+      delete item;
+      continue;
+    }
+
+#ifdef ESPHOME_LOG_HAS_VERY_VERBOSE
+    const char *type = item->type == SchedulerItem::INTERVAL ? "interval" : "timeout";
+    ESP_LOGVV(TAG, "Running %s '%s' with interval=%u last_execution=%u (now=%u)", type, item->name.c_str(),
+              item->interval, item->last_execution, now);
+#endif
+
+    // Warning: During f(), a lot of stuff can happen, including:
+    //  - timeouts/intervals get added, potentially invalidating vector pointers
+    //  - timeouts/intervals get cancelled
+    item->f();
+
+    // Only pop after function call, this ensures we were reachable
+    // during the function call and know if we were cancelled.
+    this->pop_raw_();
+
+    if (item->remove) {
+      // We were removed/cancelled in the function call, stop
+      delete item;
+      continue;
+    }
+
+    if (item->type == SchedulerItem::INTERVAL) {
+      if (item->interval != 0) {
+        const uint32_t amount = (now - item->last_execution) / item->interval;
+        item->last_execution += amount * item->interval;
+      }
+      this->push_(item);
+    } else {
+      delete item;
+    }
+  }
+
+  this->process_to_add();
+}
+void HOT Scheduler::process_to_add() {
+  for (auto &it : this->to_add_) {
+    if (it->remove) {
+      delete it;
+      continue;
+    }
+
+    this->items_.push_back(it);
+    std::push_heap(this->items_.begin(), this->items_.end());
+  }
+  this->to_add_.clear();
+}
+void HOT Scheduler::cleanup_() {
+  while (!this->items_.empty()) {
+    auto item = this->items_[0];
+    if (!item->remove)
+      return;
+
+    delete item;
+    this->pop_raw_();
+  }
+}
+bool HOT Scheduler::peek_() {
+  this->cleanup_();
+  return !this->items_.empty();
+}
+void HOT Scheduler::pop_raw_() {
+  std::pop_heap(this->items_.begin(), this->items_.end());
+  this->items_.pop_back();
+}
+void HOT Scheduler::push_(Scheduler::SchedulerItem *item) { this->to_add_.push_back(item); }
+bool HOT Scheduler::cancel_item_(Component *component, const std::string &name, Scheduler::SchedulerItem::Type type) {
+  bool ret = false;
+  for (auto *it : this->items_)
+    if (it->component == component && it->name == name && it->type == type) {
+      it->remove = true;
+      ret = true;
+    }
+  for (auto &it : this->to_add_)
+    if (it->component == component && it->name == name && it->type == type) {
+      it->remove = true;
+      ret = true;
+    }
+
+  return ret;
+}
+
+bool HOT Scheduler::SchedulerItem::operator<(const Scheduler::SchedulerItem &other) const {
+  // min-heap
+  uint32_t this_next_exec = this->last_execution + this->timeout;
+  bool this_overflow = this_next_exec < this->last_execution;
+  uint32_t other_next_exec = other.last_execution + other.timeout;
+  bool other_overflow = other_next_exec < other.last_execution;
+  if (this_overflow == other_overflow)
+    return this_next_exec > other_next_exec;
+
+  return this_overflow;
+}
+
+}  // namespace esphome

esphome/core/scheduler.h

@@ -0,0 +1,49 @@
+#pragma once
+
+#include "esphome/core/component.h"
+#include <vector>
+
+namespace esphome {
+
+class Component;
+
+class Scheduler {
+ public:
+  void set_timeout(Component *component, const std::string &name, uint32_t timeout, std::function<void()> &&func);
+  bool cancel_timeout(Component *component, const std::string &name);
+  void set_interval(Component *component, const std::string &name, uint32_t interval, std::function<void()> &&func);
+  bool cancel_interval(Component *component, const std::string &name);
+
+  optional<uint32_t> next_schedule_in();
+
+  void call();
+
+  void process_to_add();
+
+ protected:
+  struct SchedulerItem {
+    Component *component;
+    std::string name;
+    enum Type { TIMEOUT, INTERVAL } type;
+    union {
+      uint32_t interval;
+      uint32_t timeout;
+    };
+    uint32_t last_execution;
+    std::function<void()> f;
+    bool remove;
+
+    bool operator<(const SchedulerItem &other) const;
+  };
+
+  void cleanup_();
+  bool peek_();
+  void pop_raw_();
+  void push_(SchedulerItem *item);
+  bool cancel_item_(Component *component, const std::string &name, SchedulerItem::Type type);
+
+  std::vector<SchedulerItem *> items_;
+  std::vector<SchedulerItem *> to_add_;
+};
+
+}  // namespace esphome