// soda shot two
// vgmlr
/*
0  rx nodemcu     1  tx 
2  step x         3  step y
4  step z         5  direction x
6  direction y    7  direction z
8  step enable    9  limit x
10 limit y        11 limit z
12 rgb leds (7)   13 limit shot
14 (A0) servo     15 (A1) limit servo
16 (A2) button    17 (A3) relay
18 (A4) SDA oled  19 (A5) SCL oled
*/
// stepper values
#include <AccelStepper.h>
#include <MultiStepper.h>
const int xstep = 2;
const int xdir = 5;
const int xlimit = 9;
const int ystep = 3;
const int ydir = 6;
const int ylimit = 10;
const int zstep = 4;
const int zdir = 7;
const int zlimit = 11;
const int motortype = 1;
AccelStepper xstepper = AccelStepper(motortype, xstep, xdir);
AccelStepper ystepper = AccelStepper(motortype, ystep, ydir);
AccelStepper zstepper = AccelStepper(motortype, zstep, zdir);
const int enable = 8;  // low=on
const int xopenpos = -2030;
const int xpourpos = -610;
const int yfrontpos = -2100;
const int ysetpos = -1400;
const int ztoppos = 960;
const int zopenpos = 210;
int plate_homing;
// servo values
#include < Servo.h >
Servo pourservo;
const int servopin = 14;
const int servoup = 86;
const int servostop = 90;
const int servodown = 92;
const int servolimit = 15;
const int servotime = 7300;
// rgb values
#include "FastLED.h"
#define NUM_LEDS 7
#define LED_TYPE WS2812B
#define COLOR_ORDER GRB
CRGB leds[NUM_LEDS];
const int rgb = 12;
// display
#include < Wire.h >
#include < Adafruit_SSD1306.h >
#include < Adafruit_GFX.h >
#define OLED_ADDR 0x3C
Adafruit_SSD1306 display(-1);
// misc
const int button = 16;
const int shotlimit = 13;
const int relay = 17;
const int oneoztime = 3100;
const int primetime = 350;
const int purgetime = 3100;
int state;
int ounces;
int purge;
int remote;
void setup() {
  Serial.begin(115200);
  Serial1.begin(115200);
  // declare display
  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);
  display.clearDisplay();
  display.display();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(26, 4);
  display.print("Soda Shot 2.1");
  display.display();
  display.setTextSize(2);
  display.setTextColor(WHITE, BLACK);
  display.setCursor(29, 15);
  display.print("Homing");
  display.display();
  // declare and detach
  pinMode(enable, OUTPUT);
  digitalWrite(enable, HIGH);  // off
  pourservo.attach(servopin);
  pourservo.write(servostop);
  pourservo.detach();
  // declare rgb
  FastLED.addLeds< LED_TYPE, rgb, COLOR_ORDER >(leds, NUM_LEDS).setCorrection(TypicalLEDStrip);
  // purple (loading)
  rgbshow(120, 0, 150);
  // declare the rest
  pinMode(xlimit, INPUT_PULLUP);
  pinMode(ylimit, INPUT_PULLUP);
  pinMode(zlimit, INPUT_PULLUP);
  pinMode(servolimit, INPUT_PULLUP);
  pinMode(shotlimit, INPUT_PULLUP);
  pinMode(button, INPUT_PULLUP);
  pinMode(relay, OUTPUT);
  digitalWrite(relay, LOW);
  // home x
  digitalWrite(enable, LOW);
  delay(100);
  xhoming();
  // home and raise z
  zhoming();
  zstepper.setMaxSpeed(800);
  zstepper.setAcceleration(800);
  zstepper.moveTo(ztoppos);
  zstepper.runToPosition();
  delay(5);
  // home and forward y
  yhoming();
  ystepper.setMaxSpeed(1600);
  ystepper.setAcceleration(1600);
  ystepper.moveTo(yfrontpos);
  ystepper.runToPosition();
  digitalWrite(enable, HIGH);
  // home and raise servo
  servohoming();
  servoupfunc();
  // green (ready)
  rgbshow(0, 255, 0);
}
void loop() {
  if (state == 0) {
    display.clearDisplay();
    display.display();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(26, 4);
    display.print("Soda Shot 2.1");
    display.display();
    display.setTextSize(2);
    display.setTextColor(WHITE, BLACK);
    display.setCursor(17, 15);
    display.print("Load Can");
    display.display();
    while (digitalRead(button) == HIGH) {
      delay(2);
    }
    display.clearDisplay();
    display.display();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(26, 4);
    display.print("Soda Shot 2.1");
    display.display();
    display.setTextSize(2);
    display.setTextColor(WHITE, BLACK);
    display.setCursor(23, 15);
    display.print("Loading");
    display.display();
    sodaload();
    state = 1;
    ounces = 0;
  } else if (state == 1) {
    while (digitalRead(button) == HIGH || remote == 1) {
      // wifi remote
      // add pour size variable
      // add pour size display value
      if (Serial1.available()) {
        remote = Serial1.read();
      }
    }
    pour();
    ounces++;
    remote = 0;
    if (ounces == 13) {
      // no display change
    } else if (ounces > 9 && ounces < 12) {
      display.clearDisplay();
      display.display();
      display.setTextSize(1);
      display.setTextColor(WHITE);
      display.setCursor(26, 4);
      display.print("Soda Shot 2.1");
      display.display();
      display.setTextSize(2);
      display.setTextColor(WHITE, BLACK);
      display.setCursor(17, 15);
      display.print(ounces);
      display.display();
      display.setTextSize(2);
      display.setTextColor(WHITE, BLACK);
      display.setCursor(53, 15);
      display.print("of 12");
      display.display();
    } else {
      display.clearDisplay();
      display.display();
      display.setTextSize(1);
      display.setTextColor(WHITE);
      display.setCursor(26, 4);
      display.print("Soda Shot 2.1");
      display.display();
      display.setTextSize(2);
      display.setTextColor(WHITE, BLACK);
      display.setCursor(23, 15);
      display.print(ounces);
      display.display();
      display.setTextSize(2);
      display.setTextColor(WHITE, BLACK);
      display.setCursor(47, 15);
      display.print("of 12");
      display.display();
    }
    if (ounces == 12) {
      display.clearDisplay();
      display.display();
      display.setTextSize(1);
      display.setTextColor(WHITE);
      display.setCursor(26, 4);
      display.print("Soda Shot 2.1");
      display.display();
      display.setTextSize(2);
      display.setTextColor(WHITE, BLACK);
      display.setCursor(35, 15);
      display.print("Purge");
      display.display();
      purge = 1;
    }
    if (ounces == 13) {
      exhume();
      state = 0;
      purge = 0;
      ounces = 0;
      remote = 0;
    }
  }
}
void xhoming() {
  xstepper.setCurrentPosition(0);
  xstepper.setMaxSpeed(300);
  xstepper.setAcceleration(300);
  plate_homing = 1;
  while (!digitalRead(xlimit)) {
    xstepper.moveTo(plate_homing);
    xstepper.run();
    plate_homing++;
  }
  xstepper.setCurrentPosition(0);
}
void yhoming() {
  ystepper.setCurrentPosition(0);
  ystepper.setMaxSpeed(300);
  ystepper.setAcceleration(300);
  plate_homing = 1;
  while (!digitalRead(ylimit)) {
    ystepper.moveTo(plate_homing);
    ystepper.run();
    plate_homing++;
  }
  ystepper.setCurrentPosition(0);
}
void zhoming() {
  zstepper.setCurrentPosition(0);
  zstepper.setMaxSpeed(300);
  zstepper.setAcceleration(300);
  plate_homing = 1;
  while (!digitalRead(zlimit)) {
    zstepper.moveTo(plate_homing);
    zstepper.run();
    plate_homing--;
  }
  zstepper.setCurrentPosition(0);
}
void servohoming() {
  pourservo.attach(servopin);
  while (digitalRead(servolimit) == HIGH) {
    pourservo.write(servodown);
  }
  pourservo.write(servostop);
  pourservo.detach();
}
void servoupfunc() {
  pourservo.attach(servopin);
  pourservo.write(servoup);
  // no limit - timed
  delay(servotime);
  pourservo.write(servostop);
  pourservo.detach();
}
void sodaload() {
  // red (active)
  rgbshow(255, 0, 0);
  digitalWrite(enable, LOW);
  // move y to set position
  ystepper.setMaxSpeed(1600);
  ystepper.setAcceleration(1600);
  ystepper.moveTo(ysetpos);
  ystepper.runToPosition();
  // move x to open
  xstepper.setMaxSpeed(1600);
  xstepper.setAcceleration(1600);
  xstepper.moveTo(xopenpos);
  xstepper.runToPosition();
  // move z to open
  display.setTextSize(2);
  display.setTextColor(WHITE, BLACK);
  display.setCursor(17, 30);
  display.print("Pop Fizz");
  display.display();
  zstepper.setMaxSpeed(500);
  zstepper.setAcceleration(500);
  zstepper.moveTo(zopenpos);
  zstepper.runToPosition();
  delay(5);
  // move z to top
  zstepper.setMaxSpeed(800);
  zstepper.setAcceleration(800);
  zstepper.moveTo(ztoppos);
  zstepper.runToPosition();
  delay(5);
  // move x to pour
  xstepper.moveTo(xpourpos);
  xstepper.runToPosition();
  // move z to lid
  display.clearDisplay();
  display.display();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(26, 4);
  display.print("Soda Shot 2.1");
  display.display();
  display.setTextSize(2);
  display.setTextColor(WHITE, BLACK);
  display.setCursor(23, 15);
  display.print("Sealing");
  display.display();
  zhoming();
  delay(5);
  digitalWrite(enable, HIGH);
  // move servo down
  display.clearDisplay();
  display.display();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(26, 4);
  display.print("Soda Shot 2.1");
  display.display();
  display.setTextSize(2);
  display.setTextColor(WHITE, BLACK);
  display.setCursor(23, 15);
  display.print("Tapping");
  display.display();
  servohoming();
  // prime
  display.clearDisplay();
  display.display();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(26, 4);
  display.print("Soda Shot 2.1");
  display.display();
  display.setTextSize(2);
  display.setTextColor(WHITE, BLACK);
  display.setCursor(23, 15);
  display.print("Priming");
  display.display();
  digitalWrite(relay, HIGH);
  delay(primetime);
  digitalWrite(relay, LOW);
  rgbshow(0, 0, 255);
  display.clearDisplay();
  display.display();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(26, 4);
  display.print("Soda Shot 2.1");
  display.display();
  display.setTextSize(2);
  display.setTextColor(WHITE, BLACK);
  display.setCursor(23, 15);
  display.print("0 of 12");
  display.display();
}
void exhume() {
  // red
  rgbshow(255, 0, 0);
  // lift servo
  servoupfunc();
  // lift z
  digitalWrite(enable, LOW);
  zstepper.setMaxSpeed(800);
  zstepper.setAcceleration(800);
  zstepper.moveTo(ztoppos);
  zstepper.runToPosition();
  // home x
  xhoming();
  // forward y
  ystepper.setMaxSpeed(1600);
  ystepper.setAcceleration(1600);
  ystepper.moveTo(yfrontpos);
  ystepper.runToPosition();
  digitalWrite(enable, HIGH);
  rgbshow(0, 255, 0);
}
void pour() {
  digitalWrite(relay, HIGH);
  if (purge == 1) {
    delay(purgetime);
  } else {
    // change here for size var
    delay(oneoztime);
  }
  digitalWrite(relay, LOW);
}
void rgbshow(int r, int g, int b) {
  for (int v = 0; v < NUM_LEDS; v++) {
    leds[v] = CRGB(r, g, b);
    FastLED.show();
  }
}