Practical Arduino C

Page 194

#include "Arduino.h"
#include "FiFoQ.h"
struct FiFoNode 
{
  void *data;
  struct FiFoNode* next;
};
struct FiFoQ
{
  struct FiFoNode* head;
  struct FiFoNode* tail;
  int count;
  size_t dataSize;
};

Page 195

struct FiFoQ* newQueue(size_t memSize){
  struct FiFoQ* p = malloc(sizeof(struct FiFoQ));
  p->head = p->tail = NULL;
  p->dataSize = memSize;
  p->count = 0;
  return p;      
}
bool queueAdd(struct FiFoQ* fq, void* dta) {
  struct FiFoNode* node = malloc(sizeof(struct FiFoNode));
  if(node == NULL) {
    return false;
  }
  node->next = NULL;
  void* dat = malloc(fq->dataSize);
  if(dat == NULL) {
    free(node);
    return false;
  }
  memcpy(dat, dta, fq->dataSize);
  node->data = dat;
  if(fq->head == NULL) {
    fq->head = fq->tail = node;
  } else {
    fq->tail->next = node;
    fq->tail = node;
  }
  fq->count++;
  return true;
}

Page 196

void* readFirst(const struct FiFoQ* fp) {
  struct FiFoNode* p = fp->head;
  if(p) {
    return p->data;
  } else {
    return NULL;
  }
}
bool deQueue(struct FiFoQ* fp){
  if(fp->head == NULL) {
    // already empty
    return false;
  }
  struct FiFoNode* node = fp->head;
  struct FiFoNode* nxt = node->next;
  free(node->data);
  free(node);
  fp->head = nxt;
  if(nxt == NULL) {
    fp->tail = NULL;
  }
  fp->count--;
  return true;
}
int getQCount(const struct FiFoQ* fp) {
  return fp->count;
}

Page 197

void zapQueue(struct FiFoQ* fp){
  bool res = deQueue(fp);
  while(res){
    res = deQueue(fp);
  }
  free(fp);
}

#ifndef FiFoQ_h
#define FiFoQ_h
struct FiFoNode;
struct FiFoQ;
typedef struct FiFoQ* FiFoQPointer;
struct FiFoQ* newQueue(size_t memSize);
bool queueAdd(struct FiFoQ* fq, void* dta);
void* readFirst(const struct FiFoQ* fp);
bool deQueue(struct FiFoQ* fp);
void zapQueue(struct FiFoQ* fp);
int getQCount(const struct FiFoQ* fp);
#endif

extern "C"{
  #include "FiFoQ.h"
}
template<class T> inline Print &operator <<(Print &obj, T arg) { obj.print(arg); return obj; } 
struct MyData {
  int myVal;
  int yourVal;
};
FiFoQPointer myQ;

Page 198

void setup() {
  Serial.begin(115200);
  MyData myData;
  MyData* mDta;
  myQ = newQueue(sizeof(MyData));
  for(int i = 1; i < 21; i++) {
    myData.myVal = i << 2;
    myData.yourVal = i;
    queueAdd(myQ, &myData);
  }
  Serial << "Queue has " << getQCount(myQ) << " items loaded\n";
  mDta = readFirst(myQ);
  while(mDta) {
    Serial << "Read myval: " << mDta->myVal << " yourVal: " << 
           mDta->yourVal << "\n";
    deQueue(myQ);
    mDta = readFirst(myQ);
  }
  Serial << "Queue now has " << getQCount(myQ) << " items\n";
  zapQueue(myQ);
}

extern "C"{
  #include <FiFoQ.h>
}