#include <Servo.h>;
int button = 2;
int LED = 4;
int elbowPin = 3;
int wristPin = 5;

Servo Elbow;
Servo Wrist;

// float elbowAngle[]; //Array of angles to draw line
// float wristAngle[]; //Array of angles to draw line
int Elbow_PW;
int Wrist_PW;
int buttonState;
unsigned long debounceDuration = 10; //Wait to validate second change in button state
unsigned int lastTimeButtonStateChanged; //Check last time state was changed
byte lastButtonState;

void setup() {
  // put your setup code here, to run once:
  pinMode(LED, OUTPUT);
  pinMode(button, INPUT);
  Elbow.attach(elbowPin);
  Wrist.attach(wristPin);

  Serial.begin(9600);
}

//Function that moves Servos
int moveServo(Servo servo, int refPulseWidth, float servoRadAngle){
  const float Pi = 3.142;
  float minPulseWidth = float(refPulseWidth)-1000.0;
  int cmdSignal = (servoRadAngle+(Pi/2))*(2000.0/Pi)+minPulseWidth;
  servo.writeMicroseconds(cmdSignal);
  return cmdSignal;
}

void loop() {
  // put your main code here, to run repeatedly:
  //Debouncing
  if (millis()-lastTimeButtonStateChanged > debounceDuration) {
    buttonState = digitalRead(button);
    if (buttonState != lastButtonState) {
      lastTimeButtonStateChanged = millis();
      lastButtonState = buttonState;
    }
  }

  //Setting angles to go to
  float elbowAngle[] = {1.9123, 1.9104, 1.9084, 1.9065, 1.9046, 1.9026, 1.9006, 1.8987, 1.8967, 1.8927, 1.8908, 1.8888, 1.8868, 1.8848, 1.8827, 1.8807, 1.8787, 1.8767, 1.8746, 1.8726};
  float wristAngle[] = {2.5420, 2.5320, 2.5223, 2.5127, 2.5033, 2.4940, 2.4849, 2.4759, 2.4671, 2.4584, 2.4498, 2.4414, 2.4330, 2.4248, 2.4167, 2.4087, 2.4008, 2.3853, 2.3777, 2.3702};
  
  //Turning system on and off
  switch (buttonState) {
    case 1:
      digitalWrite(LED, HIGH);
      
      //Move to Initial Position to Draw
      Elbow_PW = moveServo(Elbow, 1453, 1.000); //Stationary
      Wrist_PW = moveServo(Wrist, 1566, 1.000); //Stationary
      delay(1000);
      Serial.print ("Elbow Angle = ");
      Serial.println("1.000");
      Serial.print ("Wrist Angle = ");
      Serial.println("1.000");
      
      //Draw line
      for (int i=0; i<21; i++) {
        Elbow_PW = moveServo(Elbow, 1453, elbowAngle[i]);
        Wrist_PW = moveServo(Wrist, 1566, wristAngle[i]);
        Serial.println(buttonState);
        Serial.print ("Elbow Angle = ");
        Serial.print(Elbow_PW);
        Serial.print (" Wrist Angle = ");
        Serial.println(Wrist_PW);
        delay(100);
      }
      delay(1000);
     
      //Move back to Initial Position
      Elbow_PW = moveServo(Elbow, 1453, 1.000); //Stationary
      Wrist_PW = moveServo(Wrist, 1566, 1.000); //Stationary
      delay(1000);
      Serial.print ("Elbow Angle = ");
      Serial.print("1.000");
      Serial.print (" Wrist Angle = ");
      Serial.println("1.000");
     
      //Move to Folded Position
      Elbow_PW = moveServo(Elbow, 1453, 0); //Stationary
      Wrist_PW = moveServo(Wrist, 1566, 0); //Stationary
      delay(1000);
      Serial.print ("Elbow Angle = ");
      Serial.print("0.000");
      Serial.print (" Wrist Angle = ");
      Serial.println("0.000");

      break;

    default:
      digitalWrite(LED, LOW);
      Elbow_PW = moveServo(Elbow, 1453, 0); //Stationary
      Wrist_PW = moveServo(Wrist, 1566, 0); //Stationary
      Serial.println(buttonState);

      break;
  }
}