#define RED 1
#define YELLOW 5
#define GREEN 9

#define DIR_PIN 2
#define STEP_PIN 3

#ifndef _QUEUE_H_
#define _QUEUE_H_

#define QUEUE_SIZE 2000

void initQueue();
void rmQueue();
bool enqueue(int data);
int dequeue();
void clear();
void display();

#endif

int *array;
int entryCounter;
int writeIndex;
int readIndex;


void setup() {
  Serial1.begin(115200);
  
  // Print welcome messages
  Serial1.println("Hello, Raspberry Pi Pico! \nWelcome to the Stepper Motor Control System! \nThis system will control a stepper motor based on queue data. \nMotor will rotate CW for positive values and CCW for negative values.");


  pinMode(RED, OUTPUT);
  pinMode(YELLOW, OUTPUT);
  pinMode(GREEN, OUTPUT);

  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  digitalWrite(STEP_PIN, LOW);
}

void loop() {
  digitalWrite(GREEN, HIGH);
  delay(3000);

  digitalWrite(GREEN, LOW);
  digitalWrite(YELLOW, HIGH);
  delay(500);

  digitalWrite(YELLOW, LOW);
  digitalWrite(RED, HIGH);
  delay(2000);

  digitalWrite(YELLOW, HIGH);
  delay(500);
  digitalWrite(YELLOW, LOW);
  digitalWrite(RED, LOW);

  // Move 200 steps (one rotation) CW over one second
  digitalWrite(DIR_PIN, HIGH);
  for (int i = 0; i < 200; i++) {
    digitalWrite(STEP_PIN, HIGH);
    digitalWrite(STEP_PIN, LOW);
    delay(5); // 5 ms * 200 = 1 second
  }

  delay(500); // Wait half a second

  // Move 200 steps (one rotation) CCW over 400 millis
  digitalWrite(DIR_PIN, LOW); 
  for (int i = 0; i < 200; i++) {
    digitalWrite(STEP_PIN, HIGH);
    digitalWrite(STEP_PIN, LOW);
    delay(2); // 2 ms * 200 = 0.4 seconds
  }

  delay(1000); // Wait another second
}

void initQueue() {
    array = (int *)malloc(QUEUE_SIZE * sizeof(int));  // Allocate space for int
    if (array == NULL) {
        printf("Memory not allocated.\n");
        exit(1);
    }
    writeIndex = -1;
    readIndex = 0;
    entryCounter = 0;
    printf("Queue initialized!\n");
}

void rmQueue() {
    if (array != NULL) {
        free(array);
    }
}

bool enqueue(int data) {
    if (entryCounter >= QUEUE_SIZE) {
        printf("Queue is full.\n");
        return false;
    }
    writeIndex = (++writeIndex) % QUEUE_SIZE;
    array[writeIndex] = data;
    entryCounter++;
    return true;
}

int dequeue() {
    if (entryCounter <= 0) {
        printf("Queue is empty.\n");
        return 0;
    }
    int data = array[readIndex];
    readIndex = (++readIndex) % QUEUE_SIZE;
    entryCounter--;
    return data;
}

void clear() {
    for (int i = 0; i < QUEUE_SIZE; i++) {
        array[i] = 0;
    }
    entryCounter = 0;
    writeIndex = -1;
    readIndex = 0;
}

void display() {
    if (entryCounter == 0) {
        printf("Queue is empty!\n");
        return;
    }
    int index = readIndex;
    for (int i = 0; i < entryCounter; i++) {
        printf("%d\n", array[index]);
        index = (++index) % QUEUE_SIZE;
    }
    printf("\n");
}