// Simone Slouchy (3)
// vgmlr
#include "Servo.h"
#include "Stepper.h"
// Hand
const int stepsPerRevolution = 64;
Stepper hander(stepsPerRevolution, 19, 17, 18, 16);
// Shoulder
Servo shoulder;
// Elbow (
Servo elbow;
// Sensor
#define trigPin 2
#define echoPin 3
// Servo Position
int pos = 30;
// What Action Next?
int state = 0;
// Distance Adjust (Version 2)
// int newdis = 0;
// Serial For  Testing
char incom = 0;
void setup() {
  Serial.begin(9600);
  shoulder.attach(12);
  elbow.attach(5);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  // Set Start Position (Servo)
  shoulder.write(15);
  elbow.write(30);
  // Stepper Starts 0
}
void loop() {
  // Sensor Sequence
  long duration, distance;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  // Centimeter Conversion
  distance = (duration / 2) / 29.1;
  // Print for Testing
  Serial.print(distance); Serial.println(" cm");
  // IF Slouching || > 5cm
  // But Not NOT In Chair || < 40cm
  if (distance > 5 && distance < 40) {
    if (state == 0) {
      tap();
      // Change State
      state = 1;
    }
    else if (state == 1) {
      grab();
      state = 2;
    }
    else if (state == 2) {
      assault();
      // Reset
      state = 0;
    }
  }
  delay(5000);
  // Serial For Testing
  //  if (Serial.available() > 0) {
  //    incom = Serial.read();
  //    if (incom == '1') {
  //      tap();
  //    }
  //   else if (incom == '2') {
  //      grab();
  //    }
  //    else if (incom == '3') {
  //      assault();
  //    }
  //  }
  // Reset Value (Else)
  incom = 0;
}
void tap() {
  // Map Distance for Extension (Coming Soon)
  // newdis = map(distance, 5, 20, 70, 110);
  // Extend Shoulder and Forearm
  // Open Hand
  for (int i = 0; i <= 600; i++) {
    hander.step(-1);
    Serial.println("Open");
  }
  for (pos = 15; pos <= 30; pos += 1) {
    shoulder.write(pos);
    delay(20);
  }
  for (pos = 30; pos <= 90; pos += 1) {
    shoulder.write(pos);
    elbow.write(pos);
    delay(20);
  }
  for (pos = 90; pos <= 145; pos += 1) {
    elbow.write(pos);
    delay(20);
  }
  for (pos = 90; pos <= 110; pos += 1) {
    shoulder.write(pos);
    delay(20);
  }
  delay(1000);
  // Tap
  for (pos = 145; pos >= 110; pos -= 1) {
    elbow.write(pos);
    delay(10);
  }
  for (pos = 110; pos <= 145; pos += 1) {
    elbow.write(pos);
    delay(10);
  }
  for (pos = 145; pos >= 110; pos -= 1) {
    elbow.write(pos);
    delay(10);
  }
  for (pos = 110; pos <= 145; pos += 1) {
    elbow.write(pos);
    delay(10);
  }
  // Fold Back
  for (pos = 110; pos >= 70; pos -= 1) {
    shoulder.write(pos);
    delay(50);
  }
  delay(750);
  for (pos = 145; pos >= 70; pos -= 1) {
    elbow.write(pos);
    delay(20);
  }
  for (pos = 70; pos >= 30; pos -= 1) {
    shoulder.write(pos);
    elbow.write(pos);
    delay(30);
  }
  for (pos = 30; pos >= 15; pos -= 1) {
    shoulder.write(pos);
    delay(30);
  }
  for (int i = 0; i <= 600; i++) {
    hander.step(1);
    Serial.println("Close");
  }
}
void grab() {
  // Open Hand
  for (int i = 0; i <= 600; i++) {
    hander.step(-1);
    Serial.println("Open");
  }
  for (pos = 15; pos <= 30; pos += 1) {
    shoulder.write(pos);
    delay(20);
  }
  for (pos = 30; pos <= 90; pos += 1) {
    shoulder.write(pos);
    elbow.write(pos);
    delay(20);
  }
  for (pos = 90; pos <= 145; pos += 1) {
    elbow.write(pos);
    delay(20);
  }
  for (pos = 90; pos <= 110; pos += 1) {
    shoulder.write(pos);
    delay(20);
  }
  delay(1000);
  // Close Hand
  for (int i = 0; i <= 250; i++) {
    hander.step(1);
    Serial.println("Close");
  }
  // Lower On Sloucher
  for (pos = 145; pos >= 115; pos -= 1) {
    elbow.write(pos);
    delay(30);
  }
  // Pull Back
  for (pos = 115; pos >= 80; pos -= 1) {
    shoulder.write(pos);
    elbow.write(pos);
    delay(30);
  }
  // Open Hand
  for (int i = 0; i <= 250; i++) {
    hander.step(-1);
    Serial.println("Open");
  }
  // Extend Forearm
  for (pos = 80; pos <= 145; pos += 1) {
    elbow.write(pos);
    delay(30);
  }
  // Fold Back
  for (pos = 80; pos >= 50; pos -= 1) {
    shoulder.write(pos);
    delay(40);
  }
  for (pos = 145; pos >= 50; pos -= 1) {
    elbow.write(pos);
    delay(30);
  }
  for (pos = 50; pos >= 30; pos -= 1) {
    elbow.write(pos);
    shoulder.write(pos);
    delay(40);
  }
  for (pos = 30; pos >= 15; pos -= 1) {
    shoulder.write(pos);
    delay(40);
  }
  // Close Hand
  for (int i = 0; i <= 600; i++) {
    hander.step(1);
    Serial.println("Close");
  }
}
void assault() {
  for (int i = 0; i <= 600; i++) {
    hander.step(-1);
    Serial.println("Open");
  }
  for (pos = 15; pos <= 30; pos += 1) {
    shoulder.write(pos);
    delay(20);
  }
  for (pos = 30; pos <= 90; pos += 1) {
    shoulder.write(pos);
    elbow.write(pos);
    delay(20);
  }
  for (pos = 90; pos <= 145; pos += 1) {
    elbow.write(pos);
    delay(20);
  }
  for (pos = 90; pos <= 110; pos += 1) {
    shoulder.write(pos);
    delay(20);
  }
  delay(1000);
  for (int i = 0; i <= 250; i++) {
    hander.step(1);
    Serial.println("Close");
  }
  for (pos = 145; pos >= 115; pos -= 1) {
    elbow.write(pos);
    delay(30);
  }
  for (pos = 115; pos >= 60; pos -= 1) {
    shoulder.write(pos);
    elbow.write(pos);
    delay(30);
  }
  for (int i = 0; i <= 250; i++) {
    hander.step(-1);
    Serial.println("Open");
  }
  for (pos = 60; pos <= 80; pos += 1) {
    elbow.write(pos);
    delay(30);
  }
  delay(1500);
  for (pos = 60; pos <= 80; pos += 1) {
    shoulder.write(pos);
    delay(20);
  }
  for (pos = 80; pos <= 115; pos += 1) {
    shoulder.write(pos);
    elbow.write(pos);
    delay(5);
  }
  for (pos = 115; pos <= 135; pos += 1) {
    elbow.write(pos);
    delay(5);
  }
  delay(500);
  for (pos = 115; pos >= 80; pos -= 1) {
    shoulder.write(pos);
    delay(10);
  }
  for (pos = 135; pos <= 80; pos += 1) {
    elbow.write(pos);
    delay(30);
  }
  for (pos = 80; pos >= 30; pos -= 1) {
    shoulder.write(pos);
    elbow.write(pos);
    delay(40);
  }
  for (pos = 30; pos >= 15; pos -= 1) {
    shoulder.write(pos);
    delay(40);
  }
  for (int i = 0; i <= 600; i++) {
    hander.step(1);
    Serial.println("Close");
  }
}