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

#include "module.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 read;  // queue index
  int write; // queue index
  event_t event[QUEUE_SIZE];
} queue_t;

static int clock; // current time

static queue_t queue[NUM_PORTS];

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


/*
 * queue_free
 * ----------
 * Returns the number of free entries.
 */
static int queue_free(int port)
{
  int used = queue[port].write - queue[port].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(int port)
{
  for(;;)
  {
    if(queue[port].read == queue[port].write) // empty
      return QUEUE_SIZE-1;

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

      if(++queue[port].read >= QUEUE_SIZE)
        queue[port].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)
{
  int port = cmd >> 28;

  if(queue_free(port) <= 0) // full
    return -1;

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

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

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

  if(++queue[port].write >= QUEUE_SIZE)
    queue[port].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(port);
}


/*
 * queue_check
 * -----------
 * Returns non zero if the queue will empty within 10ms
 */
int queue_check(int port)
{
  // check time of newest entry
  if(queue[port].write == queue[port].read)
    return 0;

  int newest = queue[port].write - 1;
  if(newest < 0)
    newest = QUEUE_SIZE - 1;

  return queue[port].event[newest].time <= (clock + 10);
}


/*
 * queue_clear
 * -----------
 * Clears the queue.
 * Returns the number of free entries.
 */
int queue_clear(int port)
{
  queue[port].read = 0;
  queue[port].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 clock += incr;
}


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

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

  return prev;
}

