#include <Arduino.h>
#include "vector.h" // Include your vector implementation

// Define your functions
void function1() {
    Serial.println("Function 1 executed");
    while (true){
      if (millis()%10000){
        break;
      }
    }
    next();
    
}

void function2() {
    Serial.println("Function 2 executed");
}

void function3() {
    Serial.println("Function 3 executed");
}

void function4() {
    Serial.println("Function 4 executed");
}

void function5() {
    Serial.println("Function 5 executed");
}

// Define a struct to hold function pointer and suspension flag
struct FunctionWrapper {
    void (*function)();
    bool suspended;
};

// Define a vector to hold function wrappers
vector<FunctionWrapper> functionVector;

// Define current index
int currentIndex = 0;

// Function to execute next function
void next() {
    if (currentIndex < functionVector.size() - 1) {
        currentIndex++;
        if (functionVector[currentIndex].suspended) {
            next(); // Skip to the next function if the current one is suspended
        } else {
            functionVector[currentIndex].function();
        }
    } else {
        Serial.println("Already at the last function");
    }
}

// Function to execute previous function
void previous() {
    if (currentIndex > 0) {
        currentIndex--;
        if (functionVector[currentIndex].suspended) {
            previous(); // Skip to the previous function if the current one is suspended
        } else {
            functionVector[currentIndex].function();
        }
    } else {
        Serial.println("Already at the first function");
    }
}

void setup() {
    Serial.begin(115200); // Initialize serial communication

    // Populate the function vector
    functionVector.push_back({function1, false});
    functionVector.push_back({function2, false});
    functionVector.push_back({function3, true});
    functionVector.push_back({function4, true});
    functionVector.push_back({function5, true});
    function1();
}

void loop() {
    if(Serial.available()>0){
      String data = Serial.readStringUntil('\n');
      if(data == "n"){
        next();
      }
      else if(data == "p")
      {
        previous();
      }
    }
}