#include "press.h"
#include "blink.h"

static uint32_t next_send(press_t *s) {
  return s->timestamp + (1 << s->send_count) - 1;
}

static bool next_send_less_than(void *user_data, pheap_node_id_t a,
                                pheap_node_id_t b) {
  press_t *presses = (press_t *)user_data;
  return next_send(&presses[a]) < next_send(&presses[b]);
}

press_pile_t *create_press_pile() {
  press_pile_t *pp = (press_pile_t *)malloc(sizeof(press_pile_t));
  if (pp == NULL)
    signal_error_by_blinking();
  pp->presses = calloc(PICO_PHEAP_MAX_ENTRIES, sizeof(press_t));
  if (pp->presses == NULL)
    signal_error_by_blinking();
  pp->sleeps_heap =
      ph_create(PICO_PHEAP_MAX_ENTRIES, next_send_less_than, pp->presses);
  if (pp->sleeps_heap == NULL)
    signal_error_by_blinking();
  return pp;
}

void add_press(press_pile_t *pp, press_t *press) {
  pheap_node_id_t i = ph_new_node(pp->sleeps_heap);
  if (i == 0) {
    /* TODO: Don't drop new presses just because sleeps_heap is full of old
     * presses. */
    return;
  }
  pp->presses[i] = *press;
  ph_insert_node(pp->sleeps_heap, i);
}

int32_t next_scheduled_send(press_pile_t *pp) {
  pheap_node_id_t i = ph_peek_head(pp->sleeps_heap);
  if (i == 0)
    return -1;
  return next_send(&pp->presses[i]);
}

bool get_press_due_for_resend(press_pile_t *pp, uint32_t now, press_t *press) {
  pheap_node_id_t i = ph_peek_head(pp->sleeps_heap);
  if (i == 0 || next_send(&pp->presses[i]) > now)
    return false;
  if (ph_remove_head(pp->sleeps_heap, true) != i)
    signal_error_by_blinking();
  *press = pp->presses[i];
  return true;
}