// Arduino Tipbot
// Vije Miller

#include <FastLED.h>
#include <VarSpeedServo.h>

// neopixels
#define NUM_LEDS 11
CRGB leds[NUM_LEDS];
#define PIN 9

// random
int fun_ran = 0;

// ping
const int us_pin = 8;

// left front
VarSpeedServo one_u;
const int one_u_pin = 2;
VarSpeedServo one_d;
const int one_d_pin = 3;

// right front
VarSpeedServo two_u;
const int two_u_pin = 6;
VarSpeedServo two_d;
const int two_d_pin = 7;

// rear
VarSpeedServo thr_u;
const int thr_u_pin = 4;
VarSpeedServo thr_d;
const int thr_d_pin = 5;

// ir
int ir_pin = 10;

// values
int slow = 20;
int reg = 40;
int fast = 60;
int state = 0;
int other = 0;
int coins = 0;

void setup() {
  Serial.begin(9600);

  FastLED.addLeds<WS2812B, PIN, GRB>(leds, NUM_LEDS).setCorrection(TypicalLEDStrip);

  one_u.attach(one_u_pin);
  one_d.attach(one_d_pin);
  two_u.attach(two_u_pin);
  two_d.attach(two_d_pin);
  thr_u.attach(thr_u_pin);
  thr_d.attach(thr_d_pin);

  pinMode(ir_pin, INPUT);

  setAll(0, 0, 0);
  for (int s = 0; s < 11; s++) {
    setPixel(s, 0, 255, 0);
    showStrip();
    delay(100);
  }

  up();
  delay(1000);
  down();
  delay(1000);
  stand();

  for (int s = 0; s < 11; s++) {
    setPixel(s, 0, 0, 0);
    showStrip();
    delay(10);
  }
  setAll(0, 0, 0);
  showStrip();

  delay(1000);

}

void loop() {

  // standing?
  if (state == 0) {
    stand();
    state = 1;
  }

  // coins?
  coin_check();

  // distance
  long dur, cm;
  pinMode(us_pin, OUTPUT);
  digitalWrite(us_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(us_pin, HIGH);
  delayMicroseconds(5);
  digitalWrite(us_pin, LOW);
  pinMode(us_pin, INPUT);
  dur = pulseIn(us_pin, HIGH);
  cm = mstocm(dur);
  Serial.println(cm);

  // coins?
  coin_check();

  if (cm < 50 && cm > 7) {
    look_up();
    delay(4000);
    look_down();
    delay(3000);
    stand();
    state = 1;
  } else if (cm < 7) {
    scared();
    delay(3000);
    save();
    state = 0;
  } else {
    fun_ran = random(0, 100);
    if (fun_ran == 0) {
      up();
      delay(3000);
      state = 0;
    } else if (fun_ran == 1) {
      down();
      delay(3000);
      state = 0;
    } else if (fun_ran == 2) {
      bounce();
      delay(3000);
      state = 0;
    } else if (fun_ran == 3) {
      left();
      delay(3000);
      state = 0;
    } else if (fun_ran == 4) {
      right();
      delay(3000);
      state = 0;
    } else if (fun_ran == 5) {
      look_up();
      delay(3000);
      state = 0;
    } else if (fun_ran == 6) {
      look_down();
      delay(3000);
      state = 0;
    }
  }

  // coins?
  coin_check();

  delay(5);
}

void coin_check() {
  coins = digitalRead(ir_pin);
  if (coins == LOW) {
    dance();
  }
}

void stand() {
  one_u.write(45, slow, false);
  one_d.write(50, slow, false);
  two_u.write(55, slow, false);
  two_d.write(50, slow, false);
  thr_u.write(45, slow, false);
  thr_d.write(50, slow, true);

  for (int s = 255; s > 0; s--) {
    setAll(0, s, 0);
    showStrip();
    delay(1);
  }
  for (int s = 0; s < 255; s++) {
    setAll(0, s, 0);
    showStrip();
    delay(1);
  }
}

void up() {
  one_u.write(90, reg, false);
  one_d.write(20, reg, false);
  two_u.write(100, reg, false);
  two_d.write(20, reg, false);
  thr_u.write(90, reg, false);
  thr_d.write(20, reg, true);
  setAll(0, 255, 0);
  showStrip();
}

void down() {
  one_u.write(30, reg, false);
  one_d.write(60, reg, false);
  two_u.write(35, reg, false);
  two_d.write(60, reg, false);
  thr_u.write(30, reg, false);
  thr_d.write(60, reg, true);
  setAll(0, 255, 0);
  showStrip();
}

void bounce() {
  for (int x = 0; x < 5; x++) {
    up();
    glitter(80);
    down();
  }
  stand();
}

void dance() {
  for (int x = 0; x < 5; x++) {
    one_u.write(30, reg, false);
    one_d.write(60, reg, true);

    two_u.write(30, reg, false);
    two_d.write(60, reg, false);
    one_u.write(45, reg, false);
    one_d.write(50, reg, true);

    thr_u.write(30, reg, false);
    thr_d.write(60, reg, false);
    two_u.write(45, reg, false);
    two_d.write(50, reg, true);

    thr_u.write(45, reg, false);
    thr_d.write(50, reg, true);
  }
  stand();
}

void left() {
  one_u.write(90, reg, false);
  one_d.write(20, reg, false);
  two_u.write(40, reg, false);
  two_d.write(40, reg, false);
  thr_u.write(30, reg, false);
  thr_d.write(60, reg, true);
  setAll(0, 255, 0);
  showStrip();
}

void right() {
  one_u.write(40, reg, false);
  one_d.write(40, reg, false);
  two_u.write(90, reg, false);
  two_d.write(20, reg, false);
  thr_u.write(30, reg, false);
  thr_d.write(60, reg, true);
  setAll(0, 255, 0);
  showStrip();
}

void scared() {
  one_u.write(5, fast, false);
  one_d.write(60, fast, false);
  two_u.write(5, fast, false);
  two_d.write(60, fast, false);
  thr_u.write(5, fast, false);
  thr_d.write(60, fast, true);
  setAll(0, 0, 255);
  showStrip();
}

void look_up() {
  // front up
  one_u.write(90, reg, false);
  one_d.write(20, reg, false);
  two_u.write(90, reg, false);
  two_d.write(20, reg, false);
  // back down
  thr_u.write(30, reg, false);
  thr_d.write(60, reg, true);
  setAll(0, 255, 0);
  showStrip();
}

void look_down() {
  // front down
  one_u.write(30, reg, false);
  one_d.write(60, reg, false);
  two_u.write(30, reg, false);
  two_d.write(60, reg, false);
  // back up
  thr_u.write(90, reg, false);
  thr_d.write(20, reg, true);
  setAll(0, 255, 0);
  showStrip();
}

long mstocm(long microseconds) {
  return microseconds / 29 / 2;
}

void save() {
  one_d.write(80, reg, false);
  one_u.write(20, reg, false);
  two_d.write(80, reg, false);
  two_u.write(20, reg, false);
  thr_d.write(80, reg, false);
  thr_u.write(20, reg, true);
}

void glitter(fract8 chance) {
  if (random8() < chance) {
    leds[random16(NUM_LEDS)] += CRGB::White;
  }
}

void showStrip() {
#ifdef ADAFRUIT_NEOPIXEL_H
  // NeoPixel
  strip.show();
#endif
#ifndef ADAFRUIT_NEOPIXEL_H
  // FastLED
  FastLED.show();
#endif
}

void setPixel(int Pixel, byte red, byte green, byte blue) {
#ifdef ADAFRUIT_NEOPIXEL_H
  // NeoPixel
  strip.setPixelColor(Pixel, strip.Color(red, green, blue));
#endif
#ifndef ADAFRUIT_NEOPIXEL_H
  // FastLED
  leds[Pixel].r = red;
  leds[Pixel].g = green;
  leds[Pixel].b = blue;
#endif
}

void setAll(byte red, byte green, byte blue) {
  for (int i = 0; i < NUM_LEDS; i++ ) {
    setPixel(i, red, green, blue);
  }
  showStrip();
}