// steering sneeze saver
// vije miller

#include <SoftwareSerial.h>
SoftwareSerial blue(2, 3);

#include <VarSpeedServo.h>
VarSpeedServo upper_arm;
VarSpeedServo lower_arm;
const int upper_pin = 4;
const int lower_pin = 5;

int upper_in = 170;
int upper_out = 40;

int lower_in = 10;
int lower_out = 140;

const int led_one = 6;
const int led_two = 7;
const int led_thr = 8;

void setup() {

  Serial.begin(9600);
  blue.begin(9600);

  upper_arm.attach(upper_pin);
  lower_arm.attach(lower_pin);
  upper_arm.write(upper_in, 255, false);
  lower_arm.write(lower_in, 255, true);

  pinMode(led_one, OUTPUT);
  pinMode(led_two, OUTPUT);
  pinMode(led_thr, OUTPUT);

  // run leds to alert activation
  digitalWrite(led_one, LOW);
  digitalWrite(led_two, LOW);
  digitalWrite(led_thr, LOW);
  digitalWrite(led_one, HIGH);
  delay(1000);
  digitalWrite(led_two, HIGH);
  delay(1000);
  digitalWrite(led_thr, HIGH);
  delay(1000);
  digitalWrite(led_two, LOW);
  digitalWrite(led_thr, LOW);
}

void loop() {
  if (blue.available()) {
    char sneeze;
    sneeze = blue.read();
    if (sneeze == '1') {
      capture();
      sneeze = 0;
    } else if (sneeze == '2') {
      undo();
      sneeze = 0;
    }
  }
  delay(5);
}

void capture() {
  digitalWrite(led_two, HIGH);
  upper_arm.write(upper_out, 255, true);
  lower_arm.write(lower_out, 255, true);
  delay(500);
  digitalWrite(led_thr, HIGH);
}

void undo() {
  digitalWrite(led_thr, LOW);
  lower_arm.write(lower_in, 255, true);
  upper_arm.write(upper_in, 255, true);
  delay(500);
  digitalWrite(led_two, LOW);
}