// SPDX-License-Identifier: MIT
// SPDX-FileCopyrightText: 2022 Ivan Baidakou

#include "rotor-light/planner.hpp"

#include <cassert>

using namespace rotor_light;

PlannerBase::PlannerBase(TimeEvent *events_, int32_t events_count_)
    : events{events_}, events_count{events_count_}, last_event{-1}, next_id{1} {
}

void PlannerBase::remove_event(EventId event_id) {
  int from = -1;
  ROTOR_LIGHT_DISABLE_INTERRUPTS();
  for (int i = 0; i <= last_event; ++i) {
    auto &e = events[i];
    if (e.event_id == event_id) {
      if (from == -1) {
        from = i;
        break;
      }
    }
  }
  if (from >= 0) {
    for (int i = from; (i + 1) <= last_event; ++i) {
      events[i] = events[i + 1];
    }
    --last_event;
  }
  ROTOR_LIGHT_ENABLE_INTERRUPTS();
}

EventConsumer PlannerBase::consume(TimePoint deadline) {
  return EventConsumer(deadline, this);
}

void PlannerBase::commit(size_t index) {
  assert((int32_t)index <= last_event);
  for (int i = index + 1; i <= last_event; ++i) {
    events[i - (index + 1)] = events[i];
  }
  last_event -= (index + 1);
}

TimePoint PlannerBase::next_event() {
  if (last_event >= 0) {
    return events[0].when;
  }
  return 0;
}

EventConsumer::EventConsumer(TimePoint deadline_, PlannerBase *watchdog_)
    : deadline{deadline_}, watchdog{watchdog_}, index{0} {}

EventConsumer::operator bool() const {
  return (index <= watchdog->last_event) &&
         (watchdog->events[index].when <= deadline);
}

void EventConsumer::call_next() {
  auto &event = watchdog->events[index++];
  event.callback(event.data);
}

EventConsumer::~EventConsumer() {
  if (--index >= 0) {
    watchdog->commit(index);
  }
}
