// Darkest Timeline Bot
// pkvi
// #SixSeasonsAndAMovie
#include <Servo.h>
#include <Adafruit_NeoPixel.h>
// Neopixel
#define PINPIXEL 6
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(3, PINPIXEL,
NEO_GRB + NEO_KHZ800);
// Sensor Values
Servo headservo;
Servo rollservo;
#define trigPin 9
#define echoPin 8
// Set Distance (cm)
int setdist = 40;
// Head Positions
int pos = 3;
int good = 3;
int evil = 158;
// Roll Positions
int forw = 75;
int halt = 90;
int back = 105;
int slow = 80;
// Bleu Cheese
char incom = 0;
void setup() {
  Serial.begin (9600);
  // Servo Pins
  headservo.attach(5);
  rollservo.attach(7);
  // Set Start Position
  headservo.write(good);
  rollservo.write(halt);
  // Sensor Pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  // Load Neopixel
  pixels.begin();
}
void loop() {
  // Happy Blue
  pixels.setPixelColor(0, pixels.Color(0, 0, 255));
  pixels.setPixelColor(1, pixels.Color(0, 0, 255));
  pixels.setPixelColor(2, pixels.Color(0, 0, 255));
  pixels.show();
  // Roll Bluetooth (Jacob)
  if (Serial.available() > 0) {
    incom = Serial.read();
    if (incom == '1') {
      cruel();
    }
    else if (incom == '2') {
      cool();
    }
    else if (incom == '3') {
      gloat();
    }
  }
  // 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 (distance > setdist) {
    forward();
  } else if (distance < setdist) {
    backward();
  } else {
  }
  delay(10);
}
void forward() {
  rollservo.write(forw);
  delay(1000);
  rollservo.write(halt);
}
void backward() {
  rollservo.write(back);
  delay(1000);
  rollservo.write(halt);
}
void cruel() {
  // Black
  pixels.setPixelColor(0, pixels.Color(0, 0, 0));
  pixels.setPixelColor(1, pixels.Color(0, 0, 0));
  pixels.setPixelColor(2, pixels.Color(0, 0, 0));
  pixels.show();
  // Become Evil
  for (pos = good; pos <= evil; pos += 1) {
    headservo.write(pos);
    delay(20);
  }
  delay(500);
  // Evil Red
  pixels.setPixelColor(0, pixels.Color(255, 0, 0));
  pixels.setPixelColor(1, pixels.Color(255, 0, 0));
  pixels.setPixelColor(2, pixels.Color(255, 0, 0));
  pixels.show();
  delay(1000);
  rollservo.write(slow);
  delay(5000);
  // Murder
  rollservo.write(halt);
  cool();
  incom = 0;
}
void cool() {
  // Black
  pixels.setPixelColor(0, pixels.Color(0, 0, 0));
  pixels.setPixelColor(1, pixels.Color(0, 0, 0));
  pixels.setPixelColor(2, pixels.Color(0, 0, 0));
  pixels.show();
  // Become Good
  for (pos = evil; pos >= good; pos -= 1) {
    headservo.write(pos);
    delay(20);
  }
  incom = 0;
}
void gloat() {
  rollservo.write(slow);
  delay(5000);
  rollservo.write(halt);
}