#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
  Serial.begin(9600);
}

void loop() {
  if (runButton == HIGH && saveButton == HIGH) {
    runButtonPressed = false;
    delay(buttonDelay);

    if (runButton == HIGH && saveButton == HIGH) {
      digitalWrite(LED1, LOW);
      digitalWrite(LED1, LOW);
      digitalWrite(LED1, LOW);
      digitalWrite(LED1, LOW);
      digitalWrite(LED1, LOW);
    }
  }

  basePotVal = analogRead(basePot);                           //Reads the Base Potentimeter Value
  baseServoAngle = (150. / 1023.) * basePotVal + 20;          //Map the value from the Base Potentiometer (0 to 1023) to Base Servo Value (20 to 170)
  shoulderPotVal = analogRead(shoulderPot);                   //Reads the Shoulder Potentimeter Value
  shoulderServoAngle = (150. / 1023.) * shoulderPotVal + 20;  //Map the value from the Shoulder Potentiometer (0 to 1023) to Shoulder Servo Value (20 to 170)
  elbowPotVal = analogRead(elbowPot);                         //Reads the Elbow Potentimeter Value
  elbowServoAngle = (150. / 1023.) * elbowPotVal + 20;        //Map the value from the Elbow Potentiometer (0 to 1023) to Slbow Servo Value (20 to 170)
  servoJ1.write(baseServoAngle);                              //Writes Base Maped Value to the Servo
  servoJ2.write(shoulderServoAngle);                          //Writes Shoulder Maped Value to the Servo
  servoJ3.write(elbowServoAngle);                             //Writes Elbow Maped Value to the Servo
  
  if (digitalRead(saveButton) == HIGH) {                      //Using Switch Case to Save Each Saved Angle (0 - 4)
    saveButtonPresses++;

    switch (saveButtonPresses) {
      case 1:
        servoJ1PosSaves[0] = baseServoAngle;
        servoJ2PosSaves[0] = shoulderServoAngle;
        servoJ3PosSaves[0] = elbowServoAngle;
        digitalWrite(LED1, HIGH);
        Serial.println("Pos 1 Saved");
        break;
      case 2:
        servoJ1PosSaves[1] = baseServoAngle;
        servoJ2PosSaves[1] = shoulderServoAngle;
        servoJ3PosSaves[1] = elbowServoAngle;
        digitalWrite(LED2, HIGH);
        Serial.println("Pos 2 Saved");
        break;
      case 3:
        servoJ1PosSaves[2] = baseServoAngle;
        servoJ2PosSaves[2] = shoulderServoAngle;
        servoJ3PosSaves[2] = elbowServoAngle;
        digitalWrite(LED3, HIGH);
        Serial.println("Pos 3 Saved");
        break;
      case 4:
        servoJ1PosSaves[3] = baseServoAngle;
        servoJ2PosSaves[3] = shoulderServoAngle;
        servoJ3PosSaves[3] = elbowServoAngle;
        digitalWrite(LED4, HIGH);
        Serial.println("Pos 4 Saved");
        break;
      case 5:
        servoJ1PosSaves[4] = baseServoAngle;
        servoJ2PosSaves[4] = shoulderServoAngle;
        servoJ3PosSaves[4] = elbowServoAngle;
        digitalWrite(LED5, HIGH);
        Serial.println("Pos 5 Saved");
        break;
    }
  }

  if (digitalRead(runButton) == HIGH) {  //Check if Run Button is Pressed
    runButtonPressed = true;
  }

  if (runButtonPressed) {  //If Run Button is Pressed, Write to Servos with the Saved Array locations
    for (int i = 0; i < 5; i++) {
      servoJ1.write(servoJ1PosSaves[i]);
      servoJ2.write(servoJ2PosSaves[i]);
      servoJ3.write(servoJ3PosSaves[i]);
      Serial.println(" Potentimeter Angles: ");  //Output to the Serial Monitor for Each Servo
      Serial.print("Base Servo Angle = ");
      Serial.println(servoJ1PosSaves[i]);
      Serial.print("Shoulder Servo Angle = ");
      Serial.println(servoJ2PosSaves[i]);
      Serial.print("Elbow Servo Angle = ");
      Serial.println(servoJ3PosSaves[i]);
      delay(1050);
    }
  }

  delay(300);
}