#include "GenericQueue.h"

enum {
  RED,
  YELLOW,
  GREEN
};

typedef struct {
  String name;       // Light name
  uint32_t duration; // Duration time in milliseconds
} Light;

const uint8_t numberOfLists = 3;  // The number of light in the list
const Light lightLists[numberOfLists] = {
  {"RED  ", 2000},    // 2 seconds
  {"YELLOW", 2000},   // 2 seconds
  {"GREEN  ", 2000},  // 2 seconds
};

// Define the light led pin
const uint8_t lightPins[numberOfLists] = {4, 3, 2};

// Define the maximum of queues
const size_t numberOfQueues = 3;
// Initializes a new queue of the numbers
// that are empty and have the specified 5-capacity.
GenericQueue<uint8_t> queues(numberOfQueues);   //

// Declare the start time (in milliseconds).
uint32_t startTime;
uint8_t state;

void setup() {
  Serial.begin(115200);

  // Set the LED pinmode.
  for (uint8_t index = 0; index < numberOfLists; index ++ ) {
    pinMode(lightPins[index], OUTPUT);
  }
}

void loop () {
  uint32_t currTime = millis();
  if (!queues.isEmpty()) {
    uint32_t elapsedTime = currTime - startTime;      // Calculate the elapsed time.
    uint8_t index = queues.peek();                    // Get a current light number.
    if (elapsedTime < lightLists[index].duration) {   // If the cooking time is In-Progress,
      digitalWrite(lightPins[index], HIGH);           // Turn the light on.
    } else {
      digitalWrite(lightPins[index], LOW);            // Turn the light off.
      queues.dequeue();                               // Dequeue the light.
      startTime = currTime;                           // Set the start time
      if (!queues.isEmpty())                          // If the queue is not empty,
        PloatInfo();                                   // Plot the light info.
    }
  } else {
    Enqueue();                // Enqueue the traffic light.
    startTime = currTime;     // Set the start time
    PloatInfo();              // Plot the light info.
  }
}

void Enqueue() {
  // Add the triffic light.
  queues.enqueue(RED);            // Enqueue the red light.
  queues.enqueue(YELLOW);         // Enqueue the yellow light.
  queues.enqueue(GREEN);          // Enqueue the green light.
}

void PloatInfo() {
  uint8_t index = queues.peek();  // Get a current light number.
  Serial.print("Light:");
  Serial.print(lightLists[index].name);
  Serial.print("\tDuration:");
  Serial.print(lightLists[index].duration);
  Serial.println(" milliseconds.");
}