planner.hpp

// SPDX-License-Identifier: MIT
// SPDX-FileCopyrightText: 2022 Ivan Baidakou
 
#pragma once
 
#include "definitions.hpp"
#include "rotor-light-bsp.h"
#include <cstddef>
#include <type_traits>
 
namespace rotor_light {
 
struct TimeEvent {
  TimeEvent() = default;
  TimeEvent(TimeEvent &&) = delete;
  TimeEvent(const TimeEvent &) = delete;
 
  TimeEvent &operator=(const TimeEvent &other) = default;
 
  Callback callback = nullptr;
 
  void *data;
 
  TimePoint when = 0;
 
  EventId event_id = 0;
};
 
struct PlannerBase;
 
struct EventConsumer {
  void call_next();
 
  operator bool() const;
 
  ~EventConsumer();
 
private:
  friend struct PlannerBase;
  EventConsumer(TimePoint deadline, PlannerBase *watchdog);
  TimePoint deadline;
  PlannerBase *watchdog;
  int32_t index;
};
 
struct PlannerBase {
  PlannerBase(TimeEvent *events, int32_t events_count);
  PlannerBase(const PlannerBase &) = delete;
  PlannerBase(PlannerBase &&) = delete;
 
  template <typename Ctx>
  EventId add_event(TimePoint when, Callback callback, void *data) {
    if constexpr (std::is_same_v<Ctx, ctx::thread>) {
      ROTOR_LIGHT_DISABLE_INTERRUPTS();
    }
    EventId r;
    if (last_event + 1 >= events_count) {
      r = 0;
    } else {
 
      EventId future_id = next_id + 1;
      bool recheck = false;
      int i = last_event + 1;
      // find the insertion point (skip previous events)
      for (int j = 0; j <= last_event; ++j) {
        auto &event = events[j];
        if (event.event_id == future_id) {
          ++future_id;
          recheck = true;
        }
        if (event.when <= when) {
          continue;
        } else {
          i = j;
        }
      }
      while (recheck || !future_id) {
        recheck = false;
        if (!future_id) {
          ++future_id;
        }
        for (int j = 0; j <= last_event; ++j) {
          auto &event = events[j];
          if (event.event_id == future_id) {
            ++future_id;
            recheck = true;
            break;
          }
        }
      }
      // copy next events into the next position
      for (int j = last_event; j >= i; --j) {
        events[j + 1] = events[j];
      }
      events[i] = TimeEvent{callback, data, when, next_id};
      next_id = future_id;
      ++last_event;
      r = events[i].event_id;
    }
 
    if constexpr (std::is_same_v<Ctx, ctx::thread>) {
      ROTOR_LIGHT_ENABLE_INTERRUPTS();
    }
    return r;
  }
 
  void remove_event(EventId event_id);
 
  EventConsumer consume(TimePoint deadline);
 
  TimePoint next_event();
 
private:
  friend struct EventConsumer;
  void commit(size_t index);
  TimeEvent *events;
  int32_t events_count;
  int32_t last_event;
  EventId next_id;
};
 
template <size_t TimeEventsCount> struct Planner : PlannerBase {
  static_assert(TimeEventsCount > 0, "at least one event have to be allocated");
  Planner() : PlannerBase(events_holder, TimeEventsCount) {}
 
  TimeEvent events_holder[TimeEventsCount];
};
 
} // namespace rotor_light