#include <ESP32Servo.h>

Servo servo1; // Servos
Servo servo2;
Servo servo3;
Servo servo4;


const int button1 = 23; // Buttons
const int button2 = 22;

int button1Presses = 0; // Button values
boolean button2Pressed = false;

const int pot1 = 34; // Potentiometers
const int pot2 = 35;
const int pot3 = 32;
const int pot4 = 33;

int pot1Val; // Potentiometer values
int pot2Val;
int pot3Val;
int pot4Val;
int pot1Angle;
int pot2Angle;
int pot3Angle;
int pot4Angle;

int servo1PosSaves[] = {1, 1, 1, 1, 1}; // Position saves
int servo2PosSaves[] = {1, 1, 1, 1, 1};
int servo3PosSaves[] = {1, 1, 1, 1, 1};
int servo4PosSaves[] = {1, 1, 1, 1, 1};

void setup()
{
  servo1.attach(5); // Set up everything and will run once; attach servos and define the pin modes
  servo2.attach(2); // Change the pin numbers based on your ESP32 board
  servo3.attach(4);
  servo4.attach(14);


  pinMode(button1, INPUT);
  pinMode(button2, INPUT);

  Serial.begin(9600);
}

void loop()
{
  // put your main code here, to run repeatedly:
  pot1Val = analogRead(pot1); // This will read the values from the potentiometers and store it...
  pot1Angle = map(pot1Val, 0, 4095, 0, 179); // ... and this will map the values from the potentiometers to values the servos can use and store it for later use
  pot2Val = analogRead(pot2);
  pot2Angle = map(pot2Val, 0, 4095, 0, 179);
  pot3Val = analogRead(pot3);
  pot3Angle = map(pot3Val, 0, 4095, 0, 179);
  pot4Val = analogRead(pot4);
  pot4Angle = map(pot4Val, 0, 4095, 0, 179);

  servo1.write(pot1Angle); // These will make the servos move to the mapped angles
  servo2.write(pot2Angle);
  servo3.write(pot3Angle);
  servo4.write(pot4Angle);

  if (digitalRead(button1) == HIGH)
  { // This will check how many times button1 is pressed and save the positions to an array depending on how many times it is pressed; switch/case works like an if statement
    button1Presses++;
    switch (button1Presses)
    {
    case 1:
      servo1PosSaves[0] = pot1Angle;
      servo2PosSaves[0] = pot2Angle;
      servo3PosSaves[0] = pot3Angle;
      servo4PosSaves[0] = pot4Angle;
      Serial.println("Pos 1 Saved");
      break;
    case 2:
      servo1PosSaves[1] = pot1Angle;
      servo2PosSaves[1] = pot2Angle;
      servo3PosSaves[1] = pot3Angle;
      servo4PosSaves[0] = pot4Angle;
      Serial.println("Pos 2 Saved");
      break;
    case 3:
      servo1PosSaves[2] = pot1Angle;
      servo2PosSaves[2] = pot2Angle;
      servo3PosSaves[2] = pot3Angle;
      servo4PosSaves[0] = pot4Angle;
      Serial.println("Pos 3 Saved");
      break;
    case 4:
      servo1PosSaves[3] = pot1Angle;
      servo2PosSaves[3] = pot2Angle;
      servo3PosSaves[3] = pot3Angle;
      servo4PosSaves[0] = pot4Angle;
      Serial.println("Pos 4 Saved");
      break;
    case 5:
      servo1PosSaves[4] = pot1Angle;
      servo2PosSaves[4] = pot2Angle;
      servo3PosSaves[4] = pot3Angle;
      servo4PosSaves[0] = pot4Angle;
      Serial.println("Pos 5 Saved");
      break;
    }
  }

  if (digitalRead(button2) == HIGH)
  { // Pretty self-explanatory here
    button2Pressed = true;
  }

  if (button2Pressed)
  { // if the boolean button2Press is true, then the servos will run through all their saved positions
    for (int i = 0; i < 5; i++)
    {
      servo1.write(servo1PosSaves[i]);
      servo2.write(servo2PosSaves[i]);
      servo3.write(servo3PosSaves[i]);
      servo4.write(servo4PosSaves[i]);
      Serial.println(" Potentiometer Angles: ");
      Serial.println(servo1PosSaves[i]);
      Serial.println(servo2PosSaves[i]);
      Serial.println(servo3PosSaves[i]);
      Serial.println(servo4PosSaves[i]);
      delay(1050);
    }
  }
  delay(300);
}