#include <Arduino.h>
#include <Servo.h>
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

int frequencies[11];
int numFrequencies = 0;
int delaynum=0;
bool shouldStart = false;
int base_angle=25;
int angle=0;
#define dirPin 29
//#define Angle 45
#define stepPin 0
#define stepsPerRevolution 200
#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);
Servo myservo;

void setup() {
  Serial.begin(9600);
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  //pinMode(20, OUTPUT);
  //pinMode(8, OUTPUT);
  //pinMode(9, OUTPUT);
  myservo.attach(27);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
}

void loop() {
  digitalWrite(dirPin, LOW);
  //digitalWrite(20, LOW);
  //digitalWrite(8, LOW);
  //digitalWrite(9, LOW);
  delaynum = analogRead(26);
  //if (delaynum > 500){
  //delaynum =500;//}
  if (Serial.available() > 0) {
    String data = Serial.readStringUntil('\n');
    if (data.startsWith("start")) {
      shouldStart = true;
      myservo.write(110);
    } else {
      parseData(data);
    }
  }

  if (shouldStart) {
    executeFrequencies();
    shouldStart = false;
    returning();
  }
  
}

void parseData(String data) {
  int index = 0;
  while (data.length() > 0) {
    int commaIndex = data.indexOf(',');
    if (commaIndex == -1) {
      int frequency = data.toInt();
      if (index < 11) {
        frequencies[index] = frequency;
      }
      break;
    } else {
      String numString = data.substring(0, commaIndex);
      int frequency = numString.toInt();
      if (index < 11) {
        frequencies[index] = frequency;
      }
      data = data.substring(commaIndex + 1);
    }
    index++;
  }
  numFrequencies = index;
}

void executeFrequencies() {
  for (int i = 0; i <= numFrequencies; i++) {
    if (frequencies[i] == 0) {
      continue;
    }
    callFrequencyFunction(i, frequencies[i]-2);
    //delay(1000); // Add a delay between each function call if needed
  }
}

void callFrequencyFunction(int index, int frequency) {
  switch(index) {
    case 0:
      baselineFunction(frequency+2);
      break;
  
    case 1:
      frequency1Function(frequency);
      break;
    case 2:
      frequency2Function(frequency);
      break;
    case 3:
      frequency3Function(frequency);
      break;
    case 4:
      frequency4Function(frequency);
      break;
    case 5:
      frequency5Function(frequency);
      break;
    case 6:
      frequency6Function(frequency);
      break;
    case 7:
      frequency7Function(frequency);
      break;
    case 8:
      frequency8Function(frequency);
      break;
    case 9:
      frequency9Function(frequency);
      break;
    case 10:
      frequency10Function(frequency);
      break;
    default:
      Serial.println("Invalid index");
      break;
  }
}


void baselineFunction(int frequency) {
  Serial.print("Baseline duration: ");
  Serial.println(frequency);
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print("speed: ");
  display.print(0);
  display.print("%");
  display.display();
  display.clearDisplay();
  delay(1000 * frequency); // Delay for the baseline duration
}

// Functions for handling frequencies
void frequency1Function(int frequency) {
  Serial.print("Executing function for motor 1 with frequency: ");
  Serial.println(frequency);
  int steps = abs(base_angle-angle) * stepsPerRevolution / 360;
  digitalWrite(dirPin, LOW);
  myservo.write(10);
  for(int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(delaynum); // Adjust for motor speed
    digitalWrite(stepPin, LOW);
    delayMicroseconds(delaynum); // Adjust for motor speed
  }
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print("speed: ");
  display.print(5);
  display.print("%");
  display.display();
  display.clearDisplay();
  delay(1000 * frequency);
  angle=base_angle;
}

void frequency2Function(int frequency) {
  Serial.print("Executing function for motor 2 with frequency: ");
  Serial.println(frequency);
  int steps = abs(base_angle*2-angle) * stepsPerRevolution / 360;
  digitalWrite(dirPin, LOW);
  myservo.write(10);
  for(int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(delaynum); // Adjust for motor speed
    digitalWrite(stepPin, LOW);
    delayMicroseconds(delaynum); // Adjust for motor speed
  }
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print("speed: ");
  display.print(10);
  display.print("%");
  display.display();
  display.clearDisplay();
  delay(1000 * frequency);
  angle=base_angle*2;
}

void frequency3Function(int frequency) {
  Serial.print("Executing function for motor 3 with frequency: ");
  Serial.println(frequency);
  int steps = abs(base_angle*3-angle) * stepsPerRevolution / 360;
  digitalWrite(dirPin, LOW);
  myservo.write(10);
  for(int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(delaynum); // Adjust for motor speed
    digitalWrite(stepPin, LOW);
    delayMicroseconds(delaynum); // Adjust for motor speed
  }
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print("speed: ");
  display.print(15);
  display.print("%");
  display.display();
  display.clearDisplay();
  delay(1000 * frequency);
  angle=base_angle*3;
}

void frequency4Function(int frequency) {
  Serial.print("Executing function for motor 4 with frequency: ");
  Serial.println(frequency);
  int steps = abs(base_angle*4-angle) * stepsPerRevolution/ 360;
  digitalWrite(dirPin, LOW);
  myservo.write(10);
  for(int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(delaynum); // Adjust for motor speed
    digitalWrite(stepPin, LOW);
    delayMicroseconds(delaynum); // Adjust for motor speed
  }
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print("speed: ");
  display.print(20);
  display.print("%");
  display.display();
  display.clearDisplay();
  delay(1000 * frequency);
  angle=base_angle*4;
}

void frequency5Function(int frequency) {
  Serial.print("Executing function for motor 5 with frequency: ");
  Serial.println(frequency);
  int steps = abs(base_angle*5-angle) * stepsPerRevolution / 360;
  digitalWrite(dirPin, LOW);
  myservo.write(10);
  for(int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(delaynum); // Adjust for motor speed
    digitalWrite(stepPin, LOW);
    delayMicroseconds(delaynum); // Adjust for motor speed
  }
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print("speed: ");
  display.print(25);
  display.print("%");
  display.display();
  display.clearDisplay();
  delay(1000 * frequency);
  angle=base_angle*5;
}

void frequency6Function(int frequency) {
  Serial.print("Executing function for motor 6 with frequency: ");
  Serial.println(frequency);
  int steps = abs(base_angle*6-angle) * stepsPerRevolution / 360;
  digitalWrite(dirPin, LOW);
  myservo.write(10);
  for(int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(delaynum); // Adjust for motor speed
    digitalWrite(stepPin, LOW);
    delayMicroseconds(delaynum); // Adjust for motor speed
  }
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print("speed: ");
  display.print(30);
  display.print("%");
  display.display();
  display.clearDisplay();
  delay(1000 * frequency);
  angle=base_angle*6;
}

void frequency7Function(int frequency) {
  Serial.print("Executing function for motor 7 with frequency: ");
  Serial.println(frequency);
  int steps = abs(base_angle*7-angle) * stepsPerRevolution / 360;
  digitalWrite(dirPin, LOW);
  myservo.write(10);
  for(int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(delaynum); // Adjust for motor speed
    digitalWrite(stepPin, LOW);
    delayMicroseconds(delaynum); // Adjust for motor speed
  }
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print("speed: ");
  display.print(35);
  display.print("%");
  display.display();
  display.clearDisplay();
  delay(1000 * frequency);
  angle=base_angle*7;
}

void frequency8Function(int frequency) {
  Serial.print("Executing function for motor 8 with frequency: ");
  Serial.println(frequency);
  int steps = abs(base_angle*8-angle) * stepsPerRevolution / 360;
  digitalWrite(dirPin, LOW);
  myservo.write(10);
  for(int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(delaynum); // Adjust for motor speed
    digitalWrite(stepPin, LOW);
    delayMicroseconds(delaynum); // Adjust for motor speed
  }
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print("speed: ");
  display.print(40);
  display.print("%");
  display.display();
  display.clearDisplay();
  delay(1000 * frequency);
  angle=base_angle*8;
}

void frequency9Function(int frequency) {
  Serial.print("Executing function for motor 9 with frequency: ");
  Serial.println(frequency);
  int steps = abs(base_angle*9-angle) * stepsPerRevolution / 360;
  digitalWrite(dirPin, LOW);
  myservo.write(10);
  for(int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(delaynum); // Adjust for motor speed
    digitalWrite(stepPin, LOW);
    delayMicroseconds(delaynum); // Adjust for motor speed
  }
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print("speed: ");
  display.print(45);
  display.print("%");
  display.display();
  display.clearDisplay();
  delay(1000 * frequency);
  angle=base_angle*9;
}

void frequency10Function(int frequency) {
  Serial.print("Executing function for motor 10 with frequency: ");
  Serial.println(frequency);
  int steps = abs(base_angle*10-angle) * stepsPerRevolution / 360;
  digitalWrite(dirPin, LOW);
  myservo.write(10);
  for(int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(delaynum); // Adjust for motor speed
    digitalWrite(stepPin, LOW);
    delayMicroseconds(delaynum); // Adjust for motor speed
  }
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print("speed: ");
  display.print(50);
  display.print("%");
  display.display();
  display.clearDisplay();
  delay(1000 * frequency);
  angle=base_angle*10;
}

void returning() {

  int steps = abs(angle) * stepsPerRevolution / 360;
  digitalWrite(dirPin, HIGH);
  myservo.write(80);
  for(int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(delaynum); // Adjust for motor speed
    digitalWrite(stepPin, LOW);
    delayMicroseconds(delaynum); // Adjust for motor speed
  }
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.print("speed: ");
  display.print(0);
  display.print("%");
  display.display();
  display.clearDisplay();
  angle=0;
  }