#include <Servo.h>

//For Button Function Variables
bool buttonState[2];				        //for storing button current state
bool lastButtonState[2];			      //for storing last button state
unsigned long lastDebounceTime[2];	//for storing the last debounce time
const int debounceDelay = 50; 		  //the debounce delay set by the user/or the wait time in millis

Servo servo_A;
Servo servo_B;

int action_servo;
int action_pos;
int action_servo2;
int action_pos2;

int pot_A, pot_B;
int l_pot_A, l_pot_B;
int p_pot_A, p_pot_B;

int pos_saved[700];
int arrayindex = 0;

void setup() {
  Serial.begin(9600);

  pinMode(A0, INPUT_PULLUP);
  pinMode(A1, INPUT_PULLUP);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);

  servo_A.attach(5);
  servo_B.attach(6);
}

void ReadPot() {
  pot_A = map(analogRead(A2), 0, 1023, 1, 180);
  pot_B = map(analogRead(A3), 0, 1023, 1, 180);
}

void Record() {
  ReadPot();
  p_pot_A = pot_A;
  p_pot_B = pot_B;
  
  if (p_pot_A == pot_A) {
    servo_A.write(pot_A);
  } 
  if (p_pot_B == pot_B) {
    servo_B.write(pot_B);
  }

  if (ReadButton(A0, 0, LOW) == LOW) {
    //saves the position to the array
    //If both servos got changes,
    if (pot_A != l_pot_A && pot_B != l_pot_B) {
      pos_saved[arrayindex] = pot_A;           //by adding 1000, we can add an identifier for which servo this angle data will be
      pos_saved[arrayindex + 1] = pot_B;           //by adding 1000, we can add an identifier for which servo this angle data will be
      l_pot_A = pot_A;
      l_pot_B = pot_B;
      arrayindex+=2;
      Serial.println("Position is saved for A & B.");
    }

    //if only Servo A
    else if (pot_A != l_pot_A && pot_B == l_pot_B) {
      pos_saved[arrayindex] = pot_A;  //Stores the current value to the array
      l_pot_A = pot_A;                //Stores the current value to the last value read
      arrayindex+=2;                  //Jumps 2 since it is the first value and it increments by 2
      Serial.println("Position is saved for A.");
    }

    //if only Servo B
    else if (pot_B != l_pot_B && pot_A == l_pot_A) {
      pos_saved[arrayindex] = pot_B;  //Stores the current value to the array
      l_pot_B = pot_B;                //Stores the current value to the last value read
      arrayindex+=1;                  //Jumps 1 since it is the second value and the next value would be the A
      Serial.println("Position is saved for B.");
    }
  }
}

void Play() {
  //Improved. This is to make the servo run at the same time.
  for (int Play_action = 0; Play_action < arrayindex; Play_action+=2) //Navigate through every saved element in the array 
  {
    Serial.print("Play Action: ");
    Serial.println(Play_action);
      
    //First Servo
    action_pos = pos_saved[Play_action];    //The last three characters of the array element is split to know the servo postion 
    servo_A.write(action_pos);
    Serial.print("Servo A is at ");
    Serial.print(action_pos);
    Serial.println(" degrees.");
  
    //2nd Servo
    action_pos2 = pos_saved[Play_action + 1];    //The last three characters of the array element is split to know the servo postion 
    servo_B.write(action_pos2);
    Serial.print("Servo B is at ");
    Serial.print(action_pos2);
    Serial.println(" degrees.");

    delay(250);
  }
}

void loop() {
  Record();
  if (ReadButton(A1, 1, LOW) == LOW) {
    Serial.println("Playing...");
    Play();
    Serial.println("Done...");
  }

}

//parameters: button pin, assigned button id, expected on state (HIGH or LOW)
bool ReadButton(int btnPin, int btnID, bool onState) {

  // Read the current state of the button and store it in an array with the associated ID.
  buttonState[btnID] = digitalRead(btnPin);
  
  // Compare the current button state to the previous button state to check if
  // the button has changed state since the last read.
  if (buttonState[btnID] != lastButtonState[btnID]) {
    
    // If the button has changed state, record the current time to check
    // against the debounce delay.
    lastDebounceTime[btnID] = millis();

    // Wait for the debounce delay to ensure that the button state is stable.
    if (millis() - lastDebounceTime[btnID] > debounceDelay) {
      
      // If the button state is the target state, return true.
      if (buttonState[btnID] == onState) {
        return true;
      } else {
        // If the button state is not the target state, return false.
        return false;
      }
    }
  }

  //Once the execution above is complete, it will then store the read state of the button,
  //to the last button state so it will then be used to compare the values later on
  //whenever the function is called again. THis btnID will make every state unique and thhe
  //function will work accordingly without any interferrence with other buttons being\
  //pushed or pressed.
  lastButtonState[btnID] = buttonState[btnID];
}