/*
  command scheduler
  queue.c
  5/1/23
*/

#include <kernel.h>
#include <swis.h>
#include "main.h"

typedef struct event_s
{
  int cmd;  // command to send
  int time; // time to send it
} event_t;

#define QUEUE_SIZE 1024
// usable size is 1 less than actual size so we
// can tell the difference between full and empty.

typedef struct queue_s
{
  int clock; // current time
  int read;  // queue index
  int write; // queue index
  event_t event[QUEUE_SIZE];
} queue_t;

static queue_t queue;

// note. A 1ms resolution clock will overflow a signed int
//       in 24.8  days, long enough.

void tx_command(unsigned int cmd, int len); // module.c

/*
 * queue_free
 * ----------
 * Returns the number of free entries.
 */
static int queue_free(void)
{
  int used = queue.write - queue.read;
  if(used < 0)
    used += QUEUE_SIZE;
  return QUEUE_SIZE-1 - used;
}

/*
 * queue_read
 * ----------
 * Checks the oldest event in the queue.
 * If its time is up, reads, sends, and removes it from the queue.
 * Returns the number of free entries.
 */
int queue_read(void)
{
  for(;;)
  {
    if(queue.read == queue.write) // empty
      return QUEUE_SIZE-1;

    if(queue.event[queue.read].time > queue.clock) // all done
      return queue_free();
    {
      unsigned int cmd = queue.event[queue.read].cmd;
      tx_command(cmd, (cmd >> 24) & 3);

      if(++queue.read >= QUEUE_SIZE)
        queue.read = 0;

      if(mod.log && (mod.debug & (1<<DBG_SCHEDULER)))
        fprintf(mod.log, "%X: read %d, free %d\n", queue.clock, queue.read, queue_free());
    }
  }
}

/*
 * queue_write
 * -----------
 * Writes an event to the queue.
 * Returns the number of free entries or,
 * -1 = queue full
 * -2 = time is before the last event time
 */
int queue_write(int cmd, int time)
{
  if(queue_free() <= 0) // full
    return -1;

  if(queue.write != queue.read) // not empty
  {
    int last = queue.write - 1;
    if(last < 0)
      last = QUEUE_SIZE - 1;

    if(time < queue.event[last].time) // not consecutive
      return -2;
  }

  // write event
  queue.event[queue.write].cmd = cmd;
  queue.event[queue.write].time = time;

  if(++queue.write >= QUEUE_SIZE)
    queue.write = 0;

  if(mod.log && (mod.debug & (1<<DBG_SCHEDULER)))
    fprintf(mod.log, "%X: write %d, free %d\n", queue.clock, queue.write, queue_free());

  return queue_free();
}

/*
 * queue_clear
 * -----------
 * Clears the queue.
 * Returns the number of free entries.
 */
int queue_clear(void)
{
  queue.read = 0;
  queue.write = 0;
  return QUEUE_SIZE-1;
}

/*
 * queue_clock
 * -----------
 * Increments the clock by the given ammount.
 * Returns the new clock value.
 */
int queue_clock(int incr)
{
  return queue.clock += incr;
}

/*
 * queue_set
 * ---------
 * Sets the clock to the given time.
 * Returns the previous clock value.
 */
int queue_set(int time)
{
  int prev = queue.clock;
  queue.clock = time;

  if(mod.log && (mod.debug & (1<<DBG_SCHEDULER)))
    fprintf(mod.log, "clock = %d\n", time);

  return prev;
}

