#include <Servo.h>
Servo servoJ1; //Base Servo
Servo servoJ2; //Shoulder Servo
Servo servoJ3; //Elbow Servo
const int LED1 = 2; //Save Position 1 LED
const int LED2 = 3; //Save Position 2 LED
const int LED3 = 4; //Save Position 3 LED
const int LED4 = 7; //Save Position 4 LED
const int LED5 = 8; //Save Position 5 LED
const int saveButton = 12; //Save Button
const int runButton = 13; //Run Button
int saveButtonPresses = 0; //Button Values
bool runButtonPressed = false;
const int basePot = A0; //Base Potentimeter
const int shoulderPot = A1; //Shoulder Potentimeter
const int elbowPot = A2; //Elbow Potentimeter
int basePotVal; //Base Potentimeter Values
int shoulderPotVal; //Shoulder Potentimeter Values
int elbowPotVal; //Elbow Potentimeter Values
int baseServoAngle; //Base Potentimeter Values
int shoulderServoAngle; //Shoulder Potentimeter Values
int elbowServoAngle; //Elbow Potentimeter Values
int buttonDelay = 3000;
int servoJ1PosSaves[] = {1, 1, 1, 1, 1}; //Base Servo Position Array
int servoJ2PosSaves[] = {1, 1, 1, 1, 1}; //Shoulder Servo Position Array
int servoJ3PosSaves[] = {1, 1, 1, 1, 1}; //Elbow Servo Position Array

void setup() {
  servoJ1.attach(5); //Attach Base Servo to the Arduino Pin
  servoJ2.attach(6); //Attach Shoulder Servo to the Arduino Pin
  servoJ3.attach(9); //Attach Elbow Servo to the Arduino Pin
  pinMode(LED1, OUTPUT); //LED Output Mode for First Save Location
  pinMode(LED2, OUTPUT); //LED Output Mode for Second Save Location
  pinMode(LED3, OUTPUT); //LED Output Mode for Third Save Location
  pinMode(LED4, OUTPUT); //LED Output Mode for Fourth Save Location
  pinMode(LED5, OUTPUT); //LED Output Mode for Fifth Save Location
  pinMode(saveButton, INPUT); //Save Button as Input
  pinMode(runButton, INPUT); //Run Button as Input
}

void loop() {
  int runButtonState = digitalRead(runButton);
  int saveButtonState = digitalRead(saveButton);

  if (runButtonState == HIGH && saveButtonState == HIGH) {
    delay(buttonDelay);

    runButtonState = digitalRead(runButton);
    saveButtonState = digitalRead(saveButton);

    if (runButtonState == HIGH && saveButtonState == HIGH) {
      runButtonPressed = false;
      saveButtonPresses = 0;

      for (int i = 0; i < 5; i++) {
        servoJ1PosSaves[i] = 0;
        servoJ2PosSaves[i] = 0;
        servoJ3PosSaves[i] = 0;
      }

      digitalWrite(LED1, LOW);
      digitalWrite(LED2, LOW);
      digitalWrite(LED3, LOW);
      digitalWrite(LED4, LOW);
      digitalWrite(LED5, LOW);
    }
  }

  basePotVal = analogRead(basePot);
  baseServoAngle = (150. / 1023.) * basePotVal + 20;
  shoulderPotVal = analogRead(shoulderPot);
  shoulderServoAngle = (150. / 1023.) * shoulderPotVal + 20;
  elbowPotVal = analogRead(elbowPot);
  elbowServoAngle = (150. / 1023.) * elbowPotVal + 20;

  servoJ1.write(baseServoAngle);
  servoJ2.write(shoulderServoAngle);
  servoJ3.write(elbowServoAngle);

  if (digitalRead(saveButton) == HIGH) {
    saveButtonPresses++;
    switch (saveButtonPresses) {
      case 1:
        servoJ1PosSaves[0] = baseServoAngle;
        servoJ2PosSaves[0] = shoulderServoAngle;
        servoJ3PosSaves[0] = elbowServoAngle;
        digitalWrite(LED1, HIGH);
        break;
      case 2:
        servoJ1PosSaves[1] = baseServoAngle;
        servoJ2PosSaves[1] = shoulderServoAngle;
        servoJ3PosSaves[1] = elbowServoAngle;
        digitalWrite(LED2, HIGH);
        break;
      case 3:
        servoJ1PosSaves[2] = baseServoAngle;
        servoJ2PosSaves[2] = shoulderServoAngle;
        servoJ3PosSaves[2] = elbowServoAngle;
        digitalWrite(LED3, HIGH);
        break;
      case 4:
        servoJ1PosSaves[3] = baseServoAngle;
        servoJ2PosSaves[3] = shoulderServoAngle;
        servoJ3PosSaves[3] = elbowServoAngle;
        digitalWrite(LED4, HIGH);
        break;
      case 5:
        servoJ1PosSaves[4] = baseServoAngle;
        servoJ2PosSaves[4] = shoulderServoAngle;
        servoJ3PosSaves[4] = elbowServoAngle;
        digitalWrite(LED5, HIGH);
        break;
    }
  }

  if (digitalRead(runButton) == HIGH) {
    runButtonPressed = true;
  }

  if (runButtonPressed) {
    for (int i = 0; i < 5; i++) {
      servoJ1.write(servoJ1PosSaves[i]);
      servoJ2.write(servoJ2PosSaves[i]);
      servoJ3.write(servoJ3PosSaves[i]);
      delay(1050);
    }
  }

  delay(300);
}