// bath mat fluffer bot
// vije miller

#include "FastLED.h"
#define NUM_LEDS 5
#define LED_TYPE WS2812B
#define COLOR_ORDER GRB
CRGB leds[NUM_LEDS];

#include <Servo.h>
Servo wiper;

// pins
const int ena = 2;
const int in1 = 3;
const int in2 = 4;
const int in3 = 5;
const int in4 = 6;
const int enb = 7;
const int ult = 8;
const int led = 9;
#define PIN 10
const int irs = 11;
const int ser = 12;

// var
const int wing_in = 5;
const int wing_out = 180;
const int forward_in = 100;
const int left_turn = 300;
const int forward_mark = 100; 

void setup() {
  Serial.begin(9600);
  pinMode(ena, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(enb, OUTPUT);
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
  pinMode(irs, INPUT);
  wiper.attach(ser);
  wiper.write(wing_in);
  FastLED.addLeds<LED_TYPE, PIN, COLOR_ORDER>(leds, NUM_LEDS).setCorrection( TypicalLEDStrip );
  pinMode(led, OUTPUT);
  for (int x = 0; x < 10; x++) {
    digitalWrite(led, HIGH);
    delay(500);
    digitalWrite(led, LOW);
    delay(500);
  }
}

void loop() {
  long duration, cm;
  pinMode(ult, OUTPUT);
  digitalWrite(ult, LOW);
  delayMicroseconds(2);
  digitalWrite(ult, HIGH);
  delayMicroseconds(5);
  digitalWrite(ult, LOW);
  pinMode(ult, INPUT);
  duration = pulseIn(ult, HIGH);
  cm = mtc(duration);
  // if active delay (5) mins
  if (cm < 250) {
    delay(1000 * 60 * 5);
    // check if occupied
    long duration, cm;
    pinMode(ult, OUTPUT);
    digitalWrite(ult, LOW);
    delayMicroseconds(2);
    digitalWrite(ult, HIGH);
    delayMicroseconds(5);
    digitalWrite(ult, LOW);
    pinMode(ult, INPUT);
    duration = pulseIn(ult, HIGH);
    cm = mtc(duration);
    if (cm > 250) {
      fluff();
    }
  }
  delay(500);
}

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

void fluff() {
  // turn on lights
  neoshow(0, 0, 255, 200);
  // forward
  analogWrite(ena, 180);
  analogWrite(enb, 180);
  // run until sensor on
  while (digitalRead(irs) == LOW) {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
  }
  // run mat until sensor off
  while (digitalRead(irs) == HIGH) {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
  }
  // stop
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
  // turn 180 degrees
  // until sensor on
  while (digitalRead(irs) == LOW) {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW);
  }
  // deploy arm
  for (int p = wing_in; p <= wing_out; p++) {
    wiper.write(p);
    delay(10);
  }
  // run mat until sensor off
  while (digitalRead(irs) == HIGH) {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
  }
    // stop
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
  // turn 180 degrees
  // until sensor on
  while (digitalRead(irs) == LOW) {
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
  }
  // retract arm
  for (int p = wing_out; p >= wing_in; p--) {
    wiper.write(p);
    delay(10);
  }
  // run mat until sensor off
  while (digitalRead(irs) == HIGH) {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
  }
    // stop
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
  // turn 180 degrees
  // until sensor on
  while (digitalRead(irs) == LOW) {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW);
  }
  // deploy arm
  for (int p = wing_in; p <= wing_out; p++) {
    wiper.write(p);
    delay(10);
  }
  // run mat until sensor off
  while (digitalRead(irs) == HIGH) {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
  }
  // retract arm
  for (int p = wing_out; p >= wing_in; p--) {
    wiper.write(p);
    delay(10);
  }
  // run forward
  for (int t = 0; t <= forward_in; t++) {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
  }
  // run turn
  for (int t = 0; t <= left_turn; t++) {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW);
  }
  // run to mark
  for (int t = 0; t <= forward_mark; t++) {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
  }
  neoshow(0, 0, 255, 200);
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
  for (int x = 0; x < 10; x++) {
    digitalWrite(led, HIGH);
    delay(500);
    digitalWrite(led, LOW);
    delay(500);
  }
}

void neoshow(int r, int g, int b, int brightness) {
  FastLED.setBrightness(brightness);
  for (int i = 0; i < NUM_LEDS; i++ )
  {
    leds[i] = CRGB(r, g, b);
  }
  FastLED.show();
}
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();
}