/*
  tacobot (0.2)
  vgmlr
  0 TX BT
  1 RX BT
  2 Sleep
  3 (8) Servo Disp
  4 (7) Neopixel Disp
  5 (6) Servo Flip
  6 Dir Y-Axis (14->A0)
  7 (4) Servo Rotate
  8 (3) Neopixel Plate
  9 TIP120 1 (Sauce 1)
  10 TIP120 2 (Sauce 2)
  11 (2) Coms (AG 0) -->
  12 (1) Coms (AG 1) -->
  13 (0) Limit Plate
  A0 (5) Light Sensor
  15 Step Y-Axis
  16 Dir X-Axis
  17 Step X-Axis
  18 Dir Disp
  19 Step Disp
  new_order[start(1), shell(1|2), order(1-9]
  86 cancel
  68 fire
  77 add-on
  99 repeat
*/
#include "AccelStepper.h"
#include "VarSpeedServo.h"
#include "FastLED.h"
// stepper data
// (driver, step, dir)
AccelStepper disp_step(1, 19, 18);
AccelStepper plate_x(1, 17, 16);
AccelStepper plate_y(1, 15, 6);
const int sleep = 2;
const int limit_plate = 13;
// long disp_homing = -1;
long plate_homing = -1;
// 1/4 steps 1600 2400
int disp_pos[9] = {0, 0, 400, 800, 1200, 1600, 2000, 2400, 2800};
// 1/4 1600 2400
const long plate_tort_one = 4250;
const long plate_feed = 3500;
const long plate_disp = 0;
// 1/4 13000 20000
const long plate_in = 0;
const long plate_out = 13500;
// servo data
VarSpeedServo servo_disp;
VarSpeedServo servo_rotate;
VarSpeedServo servo_flip;
const int servo_disp_pin = 3;
const int servo_rotate_pin = 7;
const int servo_flip_pin = 5;
const int s_r_forward = 107;
const int s_r_turn = 12;
const int s_f_down = 158;
const int s_f_up = 115;
const int s_d_top = 6;
const int s_d_bottom = 179;
const int sauce_one = 9;
const int sauce_two = 10;
const int s_d_sauce_one = 300;
const int s_d_sauce_two = 300;
int servo_disp_pos = 0;
// sensor data
const int light = A0;
const int light_covered = 380;
const int light_uncovered = 250;
// display data
#define num_leds_disp 7
CRGB leds_disp[num_leds_disp];
#define leds_pin_disp 4
#define num_leds_plate 2
CRGB leds_plate[num_leds_plate];
#define leds_pin_plate 8
// communication
int bt_int;
int start = 0;
int shell = 0;
int new_order[16];
int last_order[16];
int array_pos = 0;
int where = 0;
int when = 0;
int addon = 0;
const int com_one = 11;
const int com_two = 12;
void setup() {
  Serial.begin(9600);
  Serial.println("Tacobot: Loading");
  // prevent a4988 float
  pinMode(sleep, OUTPUT);
  digitalWrite(sleep, LOW);
  digitalWrite(15, LOW);
  digitalWrite(17, LOW);
  digitalWrite(19, LOW);
  delay(500);
  pinMode(limit_plate, INPUT_PULLUP);
  pinMode(sauce_one, OUTPUT);
  pinMode(sauce_two, OUTPUT);
  digitalWrite(sauce_one, HIGH);
  digitalWrite(sauce_two, HIGH);
  pinMode(com_one, OUTPUT);
  pinMode(com_two, OUTPUT);
  digitalWrite(com_one, LOW);
  digitalWrite(com_two, LOW);
  Serial.println("Tacobot: Zeroing");
  FastLED.addLeds< WS2812B, leds_pin_disp, RGB >(leds_disp, num_leds_disp);
  FastLED.addLeds< WS2812B, leds_pin_plate, RGB >(leds_plate, num_leds_plate);
  setAll_plate(0, 0, 0xff);
  colorWipe(0, 0, 0xff, 100);
  // home servos
  // zero steppers
  homing();
  setAll_plate(0, 0, 0);
  Serial.println("Tacobot: Ready");
}
void loop() {
  // waiting display
  colorWipe(0, 0, 0xff, 200);
  colorWipe(0, 0, 0, 200);
  // listen
  if (Serial.available() > 0) {
    bt_int = Serial.parseInt();
  }
  // add items until '68'
  if (shell != 0 && bt_int != 0) {
    // escape = 86
    if (bt_int == 86) {
      // clear array
      for (byte p = 0; p < 16; p++) {
        new_order[p] = 0;
      }
      // reset values
      bt_int = 0;
      shell = 0;
      start = 0;
      addon = 0;
      where = 0;
      when = 0;
      array_pos = 0;
      Serial.println("Tacobot: 86'd");
    }
    // fire order = 68
    if (bt_int == 68) {
      Serial.println("Tacobot: Fired");
      fire_order();
      // copy to last_order
      memmove(last_order, new_order, 15);
      // flush new_order
      for (byte p = 0; p < 16; p++) {
        new_order[p] = 0;
      }
      bt_int = 0;
      shell = 0;
      start = 0;
      addon = 0;
      where = 0;
      when = 0;
      array_pos = 0;
    }
    // com add-on = 77
    if (bt_int == 77) {
      addon = 1;
    }
    if (bt_int > 0 && bt_int < 15) {
      Serial.println(bt_int);
      new_order[array_pos] = bt_int;
      array_pos++;
    }
  }
  // repeat = 99
  if (bt_int == 99) {
    Serial.print("Tacobot: Reorder");
    // copy last to new
    memmove(new_order, last_order, 15);
    fire_order();
    // copy to last_order
    memmove(last_order, new_order, 15);
    // flush new_order
    for (byte p = 0; p < 16; p++) {
      new_order[p] = 0;
    }
    bt_int = 0;
    shell = 0;
    start = 0;
    addon = 0;
    where = 0;
    when = 0;
    array_pos = 0;
  }
  // actions reversed due to if sequence
  // 1 = soft
  // 2 = hard
  if (start == 1 && shell == 0) {
    if (bt_int == 1 || bt_int == 2) {
      shell = bt_int;
      if (bt_int == 1) {
        Serial.println("Soft Shell");
      } else if (bt_int == 2) {
        Serial.println("Hard Shell");
      }
    }
  }
  // initiate process
  // bt_int = 1
  if (new_order[0] == 0 && start == 0 && bt_int == 1) {
    start = 1;
    Serial.println("Tacobot: Ordering");
  }
  // flush
  bt_int = 0;
  delay(1);
}
void fire_order() {
  colorWipe(0, 0, 0, 100);
  // signal shell
  // soft now only
  digitalWrite(com_one, HIGH);
  delay(750);
  digitalWrite(com_one, LOW);
  // turn plate
  servo_rotate.attach(servo_rotate_pin);
  servo_flip.attach(servo_flip_pin);
  servo_disp.attach(servo_disp_pin);
  delay(50);
  servo_rotate.write(s_r_turn, 60, true);
  // move to rail
  digitalWrite(sleep, HIGH);
  setAll_plate(0, 0, 0xff);
  plate_x.moveTo(plate_tort_one);
  while (plate_x.distanceToGo() != 0) {
    plate_x.run();
  }
  // extend plate
  plate_y.moveTo(plate_out);
  while (plate_y.distanceToGo() != 0) {
    plate_y.run();
  }
  digitalWrite(sleep, LOW);
  // wait for light sensor
  while (analogRead(light) < light_covered) {
    // ...
  }
  digitalWrite(sleep, HIGH);
  delay(10);
  // retract plate
  plate_y.moveTo(plate_in);
  while (plate_y.distanceToGo() != 0) {
    plate_y.run();
  }
  // move back
  plate_x.moveTo(plate_feed);
  while (plate_x.distanceToGo() != 0) {
    plate_x.run();
  }
  // turn and flip
  servo_flip.write(s_f_up, 60, false);
  servo_rotate.write(s_r_forward, 60, true);
  delay(10);
  // move to disp
  plate_x.moveTo(plate_disp);
  while (plate_x.distanceToGo() != 0) {
    plate_x.run();
  }
  colorWipe(0, 0, 0xff, 100);
  // cycle order
  for (int q = 0; q < 15; q++) {
    where = new_order[q];
    // 0 = null
    // 5 = sauce 1
    // 10 = sauce 2 -> when = 5
    if (where > 0 && where != 5 && where != 10) {
      when = disp_pos[where];
      disp_step.moveTo(when);
      while (disp_step.distanceToGo() != 0) {
        disp_step.run();
      }
      delay(750);
      servo_drop_food();
      delay(750);
    } else if (where == 5 || where == 10) {
      when = disp_pos[5];
      disp_step.moveTo(when);
      while (disp_step.distanceToGo() != 0) {
        disp_step.run();
      }
      delay(800);
      servo_drop_sauce(where);
      delay(800);
    }
    delay(50);
  }
  // reset disp pos
  disp_step.moveTo(0);
  while (disp_step.distanceToGo() != 0) {
    disp_step.run();
  }
  colorWipe(0, 0, 0, 100);
  setAll_plate(0, 0, 0xff);
  // move tray back
  plate_x.moveTo(plate_feed);
  while (plate_x.distanceToGo() != 0) {
    plate_x.run();
  }
  // add on?
  if (addon == 1) {
    digitalWrite(com_two, HIGH);
    delay(1000);
    digitalWrite(com_two, LOW);
  }
  // extend tray
  plate_y.moveTo(plate_out);
  while (plate_y.distanceToGo() != 0) {
    plate_y.run();
  }
  delay(10);
  digitalWrite(sleep, LOW);
  // wait for light sensor
  while (analogRead(light) > light_uncovered) {
    // ...
  }
  // flip down
  servo_flip.write(s_f_down, 60, true);
  digitalWrite(sleep, HIGH);
  delay(10);
  // retract tray
  plate_y.moveTo(plate_in);
  while (plate_y.distanceToGo() != 0) {
    plate_y.run();
  }
  digitalWrite(sleep, LOW);
  servo_disp.detach();
  servo_flip.detach();
  servo_rotate.detach();
}
void servo_drop_food() {
  if (servo_disp_pos == 0) {
    servo_disp.write(s_d_bottom, 200, true);
    servo_disp_pos = 1;
  } else if (servo_disp_pos == 1) {
    servo_disp.write(s_d_top, 200, true);
    servo_disp_pos = 0;
  }
}
void servo_drop_sauce(int which) {
  if (which == 5) {
    digitalWrite(sauce_one, LOW);
    delay(s_d_sauce_one);
    digitalWrite(sauce_one, HIGH);
  } else if (which == 10) {
    digitalWrite(sauce_two, LOW);
    delay(s_d_sauce_two);
    digitalWrite(sauce_two, HIGH);
  }
}
void showStrip() {
  FastLED.show();
}
// neo disp
void setPixel_disp(int Pixel_disp, byte red_disp, byte green_disp, byte blue_disp) {
  leds_disp[Pixel_disp].r = red_disp;
  leds_disp[Pixel_disp].g = green_disp;
  leds_disp[Pixel_disp].b = blue_disp;
}
void setAll_disp(byte red_disp, byte green_disp, byte blue_disp) {
  for (int i = 0; i < num_leds_disp; i++ ) {
    setPixel_disp(i, red_disp, green_disp, blue_disp);
  }
  showStrip();
}
void colorWipe(byte red_disp, byte green_disp, byte blue_disp, int SpeedDelay_disp) {
  for (uint16_t i = 0; i < num_leds_disp; i++) {
    setPixel_disp(i, red_disp, green_disp, blue_disp);
    showStrip();
    delay(SpeedDelay_disp);
  }
}
// neo plate
void setPixel_plate(int Pixel_plate, byte red_plate, byte green_plate, byte blue_plate) {
  leds_plate[Pixel_plate].r = red_plate;
  leds_plate[Pixel_plate].g = green_plate;
  leds_plate[Pixel_plate].b = blue_plate;
}
void setAll_plate(byte red_plate, byte green_plate, byte blue_plate) {
  for (int j = 0; j < num_leds_plate; j++ ) {
    setPixel_plate(j, red_plate, green_plate, blue_plate);
  }
  showStrip();
}
void homing() {
  // home servos
  servo_disp.attach(servo_disp_pin);
  servo_rotate.attach(servo_rotate_pin);
  servo_flip.attach(servo_flip_pin);
  delay(100);
  servo_disp.write(s_d_top, 200, true);
  servo_rotate.write(s_r_forward, 60, true);
  servo_flip.write(s_f_down, 60, true);
  delay(500);
  servo_disp.detach();
  servo_flip.detach();
  delay(1000);
  Serial.println("Servos Adjusted");
  // home disp
  colorWipe(0, 0xff, 0, 100);
  digitalWrite(sleep, HIGH);
  delay(500);
  disp_step.setCurrentPosition(0);
  disp_step.setMaxSpeed(1600);
  disp_step.setAcceleration(2400);
  disp_step.moveTo(disp_pos[4]);
  while (disp_step.distanceToGo() != 0) {
    disp_step.run();
  }
  delay(500);
  disp_step.moveTo(0);
  while (disp_step.distanceToGo() != 0) {
    disp_step.run();
  }
  digitalWrite(sleep, LOW);
  colorWipe(0, 0, 0, 100);
  delay(500);
  // home plate
  setAll_plate(0, 0xff, 0);
  plate_x.setMaxSpeed(400.0);
  plate_x.setAcceleration(400.0);
  digitalWrite(sleep, HIGH);
  delay(100);
  while (digitalRead(limit_plate)) {
    plate_x.moveTo(plate_homing);
    plate_homing--;
    plate_x.run();
  }
  plate_x.setCurrentPosition(0);
  plate_x.setMaxSpeed(400.0);
  plate_x.setAcceleration(400.0);
  plate_homing = 1;
  while (!digitalRead(limit_plate)) {
    plate_x.moveTo(plate_homing);
    plate_x.run();
    plate_homing++;
  }
  plate_x.setCurrentPosition(0);
  plate_x.setMaxSpeed(1600);
  plate_x.setAcceleration(2400);
  plate_feed_pos();
  servo_rotate.write(s_r_forward, 60, true);
  delay(250);
  servo_rotate.detach();
  plate_y.setCurrentPosition(0);
  plate_y.setMaxSpeed(13000);
  plate_y.setAcceleration(20000);
}
void plate_feed_pos() {
  digitalWrite(sleep, HIGH);
  plate_x.moveTo(plate_feed);
  while (plate_x.distanceToGo() != 0) {
    plate_x.run();
  }
  delay(50);
  digitalWrite(sleep, LOW);
}