Using an array as a queue I recently created a state machine for my latest Arduino project. It is a simple LED control, but it has many different modes and functions. I created a template for my state machine so I can use it in the future, but I am looking to take it to the next level. Here is a copy of the current template for reference:

/*Setup Variables here*/
int nextState;
int currState;

/*Fill enum with all case names*/
enum cases{

  init;
  idle;
  case1;
  case2;
  leave;

};

/*Begin Program*/
void setup(){
  Serial.begin(9600);
  currState = init;
}

void loop(){
  switch(currState){

    case init:
      /*Prepare program here*/
      nextState = idle;
      break;

    case idle:
      if (input1){
        nextState = case1;
      }else if (input2){
        nextState = case2;
      }else{
        nextState = idle;
      }   

    case case1:
      //Code for case1 here
      break;

    case case2:
      //Code for case2 here
      break;

  }

  currState = nextState;
}


I am trying to make a Queued State Machine (QSM). This means that instead of a single variable nextState, I can add several states to be executed. Is the best way to do this an array? I have not found much in my research about how to add and remove elements easily, so if this is the best way, could you point me towards a good tutorial on these types of array functions?

Thanks in advanced for all the help!
Have an array with fixed size, and have two indexes. One pointing to the first/current item in the queue, the other to the last item in the queue.

(It actually better/easier to have the second index point to where the next item has to be written)

To add an item to the queue:


Check that that the queue isn't full (second index is the same as the first)
If not write to second index.
Increment second index, and modulo array/queue size, so it wraps around.


To read from the queue


Read value at the first index
Increment first index, modulo array/queue size