#include <Servo.h>
#include <limits.h>

#define SAVE_BUTTON A3
#define PLAY_BUTTON A4
#define ARM_BUTTON 10

#define ELBOW_PIN 3
#define BASE_PIN 5
#define SHOULDER_PIN 6
#define ARM_PIN 9

#define ELBOW_INPUT A0
#define BASE_INPUT A1
#define SHOULDER_INPUT A2

Servo servo_elbow;
Servo servo_base;
Servo servo_shoulder;
Servo servo_arm;

int led = 13;
//Servo starting positions, you can change them if you will
int elbowPos = 90, basePos = 90, shoulderPos = 180, armState = 0;
//values from the input
int elbow, base, shoulder, arm,

//Utility variables
rotationDirection, cmpCount, cmpLimit, cmpDiff, 
arrID = 0, currentPos = 1, valueCount = 1;
long startTime = LONG_MAX;
int incr = 1, decr = -1;

//Servo arrays
int elbow_arr[100];
int base_arr[100];
int shoulder_arr[100];
int arm_arr[100];

boolean record = true, playback = false;

void setup() {
  pinMode(led, OUTPUT);
  pinMode(ARM_BUTTON, INPUT);
  
  servo_elbow.attach(ELBOW_PIN);
  servo_base.attach(BASE_PIN);
  servo_shoulder.attach(SHOULDER_PIN);
  servo_arm.attach(ARM_PIN);
  
  Serial.begin(9600);
  Serial.println("System up and running");
}

void readVal() {
  elbow = analogRead(ELBOW_INPUT);
  base = analogRead(BASE_INPUT);
  shoulder = analogRead(SHOULDER_INPUT);
  
  elbow = map(elbow, 0, 1023, 3, -3);
  base = map(base, 0, 1023, 3, -3);
  shoulder = map(shoulder, 0, 1023, -3, 3);
  
  //If you don't use pulldown resistors replace this row with the following if(analogRead(ARM_BUTTON)=800 && (millis()-startTime)>=300)
  if (digitalRead(ARM_BUTTON) && (millis() - startTime) >= 300) {
    if (armState == 1)
      armState = 0;
    else armState = 1;
    startTime = millis();
  }
}

//Sends the values to the servos
void write_val() {
  
  //Moves the servos
  servo_elbow.write(elbowPos + elbow);
  servo_base.write(basePos + base);
  servo_shoulder.write(shoulderPos + shoulder);
  
  elbowPos += elbow;
  basePos += base;
  shoulderPos += shoulder;

  //Engages the arm
  if (armState) {
    //The value which tensions the string, you can adjust it by changing 180 to something other
    servo_arm.write(180);
  }
  else {
    //The value which loosens the string, you can adjust it by changing 120 to something other
    servo_arm.write(100);
  }
  //Setting boundries
  if (elbowPos > 180)
    elbowPos = 180;
  if (elbowPos < 0)
    elbowPos = 0;
  if (basePos > 180)
    basePos = 180;
  if (basePos < 0)
    basePos = 0;
  if (shoulderPos > 180)
    shoulderPos = 180;
  if (shoulderPos < 70)
    shoulderPos = 70;
  delay(20);
}
//Blinks the LED
void blnk(int time, int pin, int repeats) {
  for (int u = 0; u < repeats ; u++) {
    digitalWrite(pin, HIGH);
    delay(time);
    digitalWrite(pin, LOW);
    delay(time);
  }
}

//Compares the current and recent value to determine to which side should the servo rotate
void compare( int first, int second) {
  if (first > second) {
    cmpCount = first;
    cmpLimit = second;
    rotationDirection = -1;
    cmpDiff = second - first;
  }
  else if (first < second) {
    cmpCount = first;
    cmpLimit = second;
    rotationDirection = 1;
    cmpDiff = first - second;
  }
}

//Moves the servos slowly, itterating from the current to the next position
void moveServo(int array_name[], int currentPos, Servo servo_name) {
  compare(array_name[currentPos - 1], array_name[currentPos]);
  if (rotationDirection > 0) {
    compare(array_name[currentPos - 1], array_name[currentPos]);
    for (cmpCount; cmpCount <= cmpLimit; cmpCount++) {
      servo_name.write(array_name[currentPos - 1] + incr);
      incr += 1;
      delay(15);
      if (cmpCount == cmpLimit)
      {
        incr = 1;
      }
    }
  }
  else if (rotationDirection < 0) {
    compare(array_name[currentPos - 1], array_name[currentPos]);
    for (cmpCount; cmpCount >= cmpLimit; cmpCount--) {
      servo_name.write(array_name[currentPos - 1] + decr);
      decr -= 1;
      delay(15);
      if (cmpCount == cmpLimit) {
        decr = -1;
      }
    }
  }
}

//Moves the servos in the debounce function
void moveDebounce( int array_name[], Servo servo_name) {
  currentPos = servo_name.read();
  compare(currentPos, array_name[0]);
  if (rotationDirection > 0) {
    compare(currentPos, array_name[0]);
    for (cmpCount; cmpCount <= cmpLimit; cmpCount++) {
      servo_name.write(currentPos + incr);
      incr += 1;
      delay(15);
      if (cmpCount == cmpLimit) {
        incr = 1;
      }
    }
  }
  else if (rotationDirection < 0) {
    compare(currentPos, shoulder_arr[0]);
    for (cmpCount; cmpCount >= cmpLimit; cmpCount--) {
      servo_name.write(currentPos + decr);
      decr -= 1;
      delay(15);
      if (cmpCount == cmpLimit) {
        decr = -1;
      }
    }
  }
}

//Itterates from the last to the first positon in the array
void debounce() {
  moveDebounce(shoulder_arr, servo_shoulder);
  moveDebounce(elbow_arr, servo_elbow);
  moveDebounce(base_arr, servo_base);
  if (arm_arr[0]) {
    //The value which tensions the string, you can adjust it by changing 180 to something other
    servo_arm.write(180);
  }
  else {
    //The value which loosens the string, you can adjust it by changing 100 to something other
    servo_arm.write(100);
  }
  delay(350);
}

//Plays the saved positions
int play() {
  record = false;
  playback = true;
  Serial.println("Playback");
  delay(300);
}

//Saves the current position
void save() {
  blnk(200, 13, 2);
  readVal();
  elbow_arr[arrID] = elbowPos;
  base_arr[arrID] = basePos;
  shoulder_arr[arrID] = shoulderPos;
  arm_arr[arrID] = armState;
  Serial.print("Saved Value");
  Serial.println(valueCount);
  valueCount++;
  arrID++;
}

void loop() {
  while (record == true) {
beginning:
    readVal();
    write_val();
    //Play button
    if (analogRead(PLAY_BUTTON) >= 800) {
      //or if(analogRead(PLAY_BUTTON)>=800) if you don't have a pulldown resistor on the switch.
      play();
      goto play;
    }

    //Save button
    if (digitalRead(SAVE_BUTTON)) {
      //or if(analogRead(SAVE_BUTTON)>=800)if you don't have a pulldown resistor on the switch.
      save();
    }
    else
      goto beginning;

    while (playback == true) {
play:
      debounce();
      currentPos = 1;
      for (currentPos; currentPos <= valueCount - 2; currentPos++) {
        //Moving the servos gradually
        moveServo(shoulder_arr, currentPos, servo_shoulder);
        moveServo(elbow_arr, currentPos, servo_elbow);
        moveServo(base_arr, currentPos, servo_base);
        
        //Engaging the arm
        if (arm_arr[currentPos]) {
          //The value which tensions the string, you can adjust it by changing 180 to something other
          servo_arm.write(180);
          delay(350);
        }
        else {
          //The value which loosens the string, you can adjust it by changing 100 to something other
          servo_arm.write(100);
          delay(350);
        }
        //If this is the last position, this will move back to the first
        if (currentPos == valueCount - 2) {
          debounce();
          currentPos = 0;
          goto play;
        }
      }
    }
  }
}