// Darkest Timeline Bot
// VijeMiller
// 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);
}